add convex option to meshraw

Author: alaurenzi

include/xbot2_interface/collision.h

@@ -69,6 +69,7 @@ struct XBOT2IFC_API Shape
     {
         std::vector<Eigen::Vector3d> vertices;
         std::vector<Eigen::Vector3i> triangles;
+        bool convex = false;
     };
 
     using Variant = std::variant<

src/collision/collision.cpp

@@ -622,6 +622,12 @@ bool CollisionModel::Impl::addCollisionShape(string_const_ref name,
             bvhModel->addSubModel(vertices, triangles);
             bvhModel->endModel();
 
+            if(mr.convex)
+            {
+                bvhModel->buildConvexHull(true, "Qt");
+                fcl_geom = bvhModel->convex;
+            }
+
             std::cout << "mesh raw";
 
 
@@ -673,12 +679,6 @@ bool CollisionModel::Impl::addCollisionShape(string_const_ref name,
                 fcl_geom = bvhModel->convex;
             }
 
-            if(m.convex)
-            {
-                bvhModel->buildConvexHull(true, "Qt");
-                fcl_geom = bvhModel->convex;
-            }
-
             std::cout << "mesh";
 
             return true;
@@ -1242,7 +1242,7 @@ void CollisionModel::Impl::CollisionPairData::compute_distance(const ModelInterf
     if(threshold > 0 && aabb_dist > threshold)
     {
         dresult.min_distance = aabb_dist;
-        dresult.normal = (dresult.nearest_points[1]-dresult.nearest_points[0]).normalized();
+        dresult.normal = (dresult.nearest_points[1] - dresult.nearest_points[0]).normalized();
         return;
     }
 
@@ -1267,6 +1267,7 @@ void CollisionModel::Impl::CollisionPairData::compute_distance(const ModelInterf
          drequest,
          dresult);
 
+
     // hack to fix wrong distance when in deep collision
     if(dresult.min_distance < 0)
     {