[Convex] Add BVHModel::buildConvexRepresentation

include/hpp/fcl/BVH/BVH_model.h

@@ -52,6 +52,8 @@ namespace hpp
 namespace fcl
 {
 
+class ConvexBase;
+
 /// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
 template<typename BV>
 class BVHModel : public CollisionGeometry
@@ -81,6 +83,9 @@ public:
 
   /// @brief Fitting rule to fit a BV node to a set of geometry primitives
   boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
+
+  /// @brief Convex<Triangle> representation of this object
+  boost::shared_ptr< ConvexBase > convex;
   
   /// @brief Model type described by the instance
   BVHModelType getModelType() const
@@ -209,7 +214,12 @@ public:
   /// @brief Check the number of memory used
   int memUsage(int msg) const;
 
-/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent 
+  /// @brief Build this Convex<Triangle> representation of this model.
+  /// \note this only takes the points of this model. It does not check that the
+  ///       object is convex. It does not compute a convex hull.
+  void buildConvexRepresentation();
+
+  /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent 
   /// BV node. When traversing the BVH, this can save one matrix transformation.
   void makeParentRelative()
   {
@@ -275,7 +285,6 @@ private:
   int num_vertex_updated; /// for ccd vertex update
   unsigned int* primitive_indices;
 
-
   /// @brief Bounding volume hierarchy
   BVNode<BV>* bvs;
 

include/hpp/fcl/shape/convex.h

@@ -54,12 +54,15 @@ class Convex : public ConvexBase
 {
 public:
   /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information 
+  /// \param own_storage whether this class owns the pointers of points and
+  ///                    polygons. If owned, they are deleted upon destruction.
   /// \param points_ list of 3D points
   /// \param num_points_ number of 3D points
   /// \param polygons_ \copydoc Convex::polygons
   /// \param num_polygons_ the number of polygons.
   /// \note num_polygons_ is not the allocated size of polygons_.
-  Convex(Vec3f* points_, int num_points_,
+  Convex(bool ownStorage,
+         Vec3f* points_, int num_points_,
          PolygonT* polygons_, int num_polygons_);
 
   /// @brief Copy constructor 

include/hpp/fcl/shape/details/convex.hxx

@@ -46,11 +46,9 @@ namespace fcl
 {
 
 template <typename PolygonT>
-Convex<PolygonT>::Convex(Vec3f* points_,
-       int num_points_,
-       PolygonT* polygons_,
-       int num_polygons_) :
-  ConvexBase(points_, num_points_),
+Convex<PolygonT>::Convex(bool own_storage, Vec3f* points_, int num_points_,
+       PolygonT* polygons_, int num_polygons_) :
+  ConvexBase(own_storage, points_, num_points_),
   polygons     (polygons_),
   num_polygons (num_polygons_)
 {
@@ -62,11 +60,18 @@ Convex<PolygonT>::Convex(const Convex<PolygonT>& other) :
   ConvexBase   (other),
   polygons     (other.polygons),
   num_polygons (other.num_polygons)
-{}
+{
+  if (own_storage_) {
+    polygons = new PolygonT[num_polygons];
+    memcpy(polygons, other.polygons, sizeof(PolygonT) * num_polygons);
+  }
+}
 
 template <typename PolygonT>
 Convex<PolygonT>::~Convex()
-{}
+{
+  if (own_storage_) delete [] polygons;
+}
 
 template <typename PolygonT>
 Matrix3f Convex<PolygonT>::computeMomentofInertia() const
@@ -193,15 +198,15 @@ void Convex<PolygonT>::fillNeighbors()
     {
       size_type i = (j==0  ) ? n-1 : j-1;
       size_type k = (j==n-1) ? 0   : j+1;
-      index_type pi = polygon[i+1],
-                 pj = polygon[j+1],
-                 pk = polygon[k+1];
+      index_type pi = polygon[i],
+                 pj = polygon[j],
+                 pk = polygon[k];
       // Update neighbors of pj;
-      if (nneighbors[pj].count(pi) == (unsigned int)0) {
+      if (nneighbors[pj].count(pi) == 0) {
         c_nneighbors++;
         nneighbors[pj].insert(pi);
       }
-      if (nneighbors[pj].count(pk) == (unsigned int)0) {
+      if (nneighbors[pj].count(pk) == 0) {
         c_nneighbors++;
         nneighbors[pj].insert(pk);
       }

include/hpp/fcl/shape/geometric_shapes.h

@@ -300,7 +300,7 @@ protected:
   /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information 
   /// \param points_ list of 3D points
   /// \param num_points_ number of 3D points
-  ConvexBase(Vec3f* points_, int num_points_);
+  ConvexBase(bool ownStorage, Vec3f* points_, int num_points_);
 
   /// @brief Copy constructor 
   /// Only the list of neighbors is copied.
@@ -308,10 +308,14 @@ protected:
 
   unsigned int* nneighbors_;
 
+  bool own_storage_;
+
 private:
   void computeCenter();
 };
 
+template <typename PolygonT> class Convex;
+
 /// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
 /// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
 /// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space

src/BVH/BVH_model.cpp

@@ -37,6 +37,7 @@
 
 #include <hpp/fcl/BVH/BVH_model.h>
 #include <hpp/fcl/BV/BV.h>
+#include <hpp/fcl/shape/convex.h>
 #include <iostream>
 #include <string.h>
 
@@ -641,6 +642,13 @@ int BVHModel<BV>::memUsage(int msg) const
   return BVH_OK;
 }
 
+template<typename BV>
+void BVHModel<BV>::buildConvexRepresentation()
+{
+  if (!convex) {
+    convex.reset(new Convex<Triangle>(false, vertices, num_vertices, tri_indices, num_vertices));
+  }
+}
 
 template<typename BV>
 int BVHModel<BV>::buildTree()

src/shape/geometric_shapes.cpp

@@ -44,11 +44,11 @@ namespace hpp
 namespace fcl
 {
 
-ConvexBase::ConvexBase(Vec3f* points_,
-       int num_points_) :
+ConvexBase::ConvexBase(bool own_storage, Vec3f* points_, int num_points_) :
   ShapeBase(),
   points       (points_),
-  num_points   (num_points_)
+  num_points   (num_points_),
+  own_storage_ (own_storage)
 {
   computeCenter();
 }
@@ -57,8 +57,14 @@ ConvexBase::ConvexBase(const ConvexBase& other) :
   ShapeBase    (other),
   points       (other.points),
   num_points   (other.num_points),
-  center       (other.center)
+  center       (other.center),
+  own_storage_ (other.own_storage_)
 {
+  if (own_storage_) {
+    points = new Vec3f[num_points];
+    memcpy(points, other.points, sizeof(Vec3f) * num_points);
+  }
+
   neighbors = new Neighbors[num_points];
   memcpy(neighbors, other.neighbors, sizeof(Neighbors) * num_points);
 
@@ -72,6 +78,7 @@ ConvexBase::~ConvexBase ()
 {
   delete [] neighbors;
   delete [] nneighbors_;
+  if (own_storage_) delete [] points;
 }
 
 void ConvexBase::computeCenter()

test/convex.cpp

@@ -80,7 +80,7 @@ private:
 
 Convex<Quadrilateral> buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d)
 {
-  Vec3f pts[8];
+  Vec3f* pts = new Vec3f[8];
   pts[0] = Vec3f( l, w, d);
   pts[1] = Vec3f( l, w,-d);
   pts[2] = Vec3f( l,-w, d);
@@ -89,7 +89,8 @@ Convex<Quadrilateral> buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d)
   pts[5] = Vec3f(-l, w,-d);
   pts[6] = Vec3f(-l,-w, d);
   pts[7] = Vec3f(-l,-w,-d);
-  std::vector<Quadrilateral> polygons (6);
+
+  Quadrilateral* polygons = new Quadrilateral[6];
   polygons[0].set(0, 2, 3, 1); // x+ side
   polygons[1].set(2, 6, 7, 3); // y- side
   polygons[2].set(4, 5, 7, 6); // x- side
@@ -97,10 +98,10 @@ Convex<Quadrilateral> buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d)
   polygons[4].set(1, 3, 7, 5); // z- side
   polygons[5].set(0, 2, 6, 4); // z+ side
 
-  return Convex<Quadrilateral> (
+  return Convex<Quadrilateral> (true,
       pts, // points
       8, // num points
-      polygons.data(),
+      polygons,
       6 // number of polygons
       );
 }