From 46b8f40e0eb66283bce0535ae648e8e329f09fc5 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 9 Sep 2019 17:49:09 +0200
Subject: [PATCH] [Convex] Add BVHModel::buildConvexRepresentation
---
include/hpp/fcl/BVH/BVH_model.h | 13 +++++++++--
include/hpp/fcl/shape/convex.h | 5 +++-
include/hpp/fcl/shape/details/convex.hxx | 29 ++++++++++++++----------
include/hpp/fcl/shape/geometric_shapes.h | 6 ++++-
src/BVH/BVH_model.cpp | 8 +++++++
src/shape/geometric_shapes.cpp | 15 ++++++++----
test/convex.cpp | 9 ++++----
7 files changed, 61 insertions(+), 24 deletions(-)
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 2a7be848..7d01dec2 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/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;
diff --git a/include/hpp/fcl/shape/convex.h b/include/hpp/fcl/shape/convex.h
index 32785a5a..cafbddd0 100644
--- a/include/hpp/fcl/shape/convex.h
+++ b/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
diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx
index c2eeaa8d..b76eae24 100644
--- a/include/hpp/fcl/shape/details/convex.hxx
+++ b/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);
}
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 99ad0626..cb70da9f 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/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
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 350f8160..0920041e 100644
--- a/src/BVH/BVH_model.cpp
+++ b/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()
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index 95ce6647..ddf0cb4f 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/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()
diff --git a/test/convex.cpp b/test/convex.cpp
index e82950f0..a81b1734 100644
--- a/test/convex.cpp
+++ b/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
);
}
--
GitLab