From ec397df239abbfb67ae045cf3e27ee7b5ae9e499 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 10 Sep 2019 15:03:26 +0200
Subject: [PATCH] [BVH] Split BVHModel into two classes.
---
include/hpp/fcl/BVH/BVH_model.h | 168 +++++++++++++++++------------
include/hpp/fcl/fwd.hh | 3 +
src/BVH/BVH_model.cpp | 183 ++++++++++++++++----------------
3 files changed, 196 insertions(+), 158 deletions(-)
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 7d01dec2..40624e8b 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -54,11 +54,9 @@ 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
+/// @brief A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh)
+class BVHModelBase : public CollisionGeometry
{
-
public:
/// @brief Geometry point data
Vec3f* vertices;
@@ -78,12 +76,6 @@ public:
/// @brief The state of BVH building process
BVHBuildState build_state;
- /// @brief Split rule to split one BV node into two children
- boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
-
- /// @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;
@@ -99,66 +91,32 @@ public:
}
/// @brief Constructing an empty BVH
- BVHModel() : vertices(NULL),
- tri_indices(NULL),
- prev_vertices(NULL),
- num_tris(0),
- num_vertices(0),
- build_state(BVH_BUILD_STATE_EMPTY),
- bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
- bv_fitter(new BVFitter<BV>()),
- num_tris_allocated(0),
- num_vertices_allocated(0),
- num_bvs_allocated(0),
- num_vertex_updated(0),
- primitive_indices(NULL),
- bvs(NULL),
- num_bvs(0)
+ BVHModelBase() : vertices(NULL),
+ tri_indices(NULL),
+ prev_vertices(NULL),
+ num_tris(0),
+ num_vertices(0),
+ build_state(BVH_BUILD_STATE_EMPTY),
+ num_tris_allocated(0),
+ num_vertices_allocated(0),
+ num_vertex_updated(0)
{
}
/// @brief copy from another BVH
- BVHModel(const BVHModel& other);
+ BVHModelBase(const BVHModelBase& other);
/// @brief deconstruction, delete mesh data related.
- ~BVHModel()
+ virtual ~BVHModelBase ()
{
delete [] vertices;
delete [] tri_indices;
- delete [] bvs;
-
delete [] prev_vertices;
- delete [] primitive_indices;
- }
-
- /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
-
- /// @brief Access the bv giving the its index
- const BVNode<BV>& getBV(int id) const
- {
- assert (id < num_bvs);
- return bvs[id];
- }
-
- /// @brief Access the bv giving the its index
- BVNode<BV>& getBV(int id)
- {
- assert (id < num_bvs);
- return bvs[id];
- }
-
- /// @brief Get the number of bv in the BVH
- int getNumBVs() const
- {
- return num_bvs;
}
/// @brief Get the object type: it is a BVH
OBJECT_TYPE getObjectType() const { return OT_BVH; }
- /// @brief Get the BV type: default is unknown
- NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
-
/// @brief Compute the AABB for the BVH, used for broad-phase collision
void computeLocalAABB();
@@ -211,21 +169,16 @@ public:
/// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy
int endUpdateModel(bool refit = true, bool bottomup = true);
- /// @brief Check the number of memory used
- int memUsage(int msg) const;
-
/// @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();
+ void buildConvexRepresentation(bool share_memory);
+
+ virtual int memUsage(int msg) const = 0;
/// @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()
- {
- Matrix3f I (Matrix3f::Identity());
- makeParentRelativeRecurse(0, I, Vec3f());
- }
+ virtual void makeParentRelative() = 0;
Vec3f computeCOM() const
{
@@ -277,12 +230,95 @@ public:
return C.trace() * Matrix3f::Identity() - C;
}
-private:
+protected:
+ virtual void deleteBVs() = 0;
+ virtual bool allocateBVs() = 0;
+
+ /// @brief Build the bounding volume hierarchy
+ virtual int buildTree() = 0;
+
+ /// @brief Refit the bounding volume hierarchy
+ virtual int refitTree(bool bottomup) = 0;
int num_tris_allocated;
int num_vertices_allocated;
- int num_bvs_allocated;
int num_vertex_updated; /// for ccd vertex update
+};
+
+/// @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 BVHModelBase
+{
+
+public:
+ /// @brief Split rule to split one BV node into two children
+ boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
+
+ /// @brief Fitting rule to fit a BV node to a set of geometry primitives
+ boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
+
+ /// @brief Constructing an empty BVH
+ BVHModel() : BVHModelBase (),
+ bv_splitter(new BVSplitter<BV>(SPLIT_METHOD_MEAN)),
+ bv_fitter(new BVFitter<BV>()),
+ num_bvs_allocated(0),
+ primitive_indices(NULL),
+ bvs(NULL),
+ num_bvs(0)
+ {
+ }
+
+ /// @brief copy from another BVH
+ BVHModel(const BVHModel& other);
+
+ /// @brief deconstruction, delete mesh data related.
+ ~BVHModel()
+ {
+ delete [] bvs;
+ delete [] primitive_indices;
+ }
+
+ /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here
+
+ /// @brief Access the bv giving the its index
+ const BVNode<BV>& getBV(int id) const
+ {
+ assert (id < num_bvs);
+ return bvs[id];
+ }
+
+ /// @brief Access the bv giving the its index
+ BVNode<BV>& getBV(int id)
+ {
+ assert (id < num_bvs);
+ return bvs[id];
+ }
+
+ /// @brief Get the number of bv in the BVH
+ int getNumBVs() const
+ {
+ return num_bvs;
+ }
+
+ /// @brief Get the BV type: default is unknown
+ NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
+
+ /// @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
+ /// BV node. When traversing the BVH, this can save one matrix transformation.
+ void makeParentRelative()
+ {
+ Matrix3f I (Matrix3f::Identity());
+ makeParentRelativeRecurse(0, I, Vec3f());
+ }
+
+private:
+ void deleteBVs();
+ bool allocateBVs();
+
+ int num_bvs_allocated;
unsigned int* primitive_indices;
/// @brief Bounding volume hierarchy
diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh
index 7c5a3016..e2fa5302 100644
--- a/include/hpp/fcl/fwd.hh
+++ b/include/hpp/fcl/fwd.hh
@@ -51,6 +51,9 @@ namespace fcl {
class Transform3f;
class AABB;
+
+ class BVHModelBase;
+ typedef boost::shared_ptr<BVHModelBase> BVHModelPtr_t;
}
} // namespace hpp
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 0920041e..328030de 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -46,15 +46,13 @@ namespace hpp
namespace fcl
{
-template<typename BV>
-BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other),
- num_tris(other.num_tris),
- num_vertices(other.num_vertices),
- build_state(other.build_state),
- bv_splitter(other.bv_splitter),
- bv_fitter(other.bv_fitter),
- num_tris_allocated(other.num_tris),
- num_vertices_allocated(other.num_vertices)
+BVHModelBase::BVHModelBase(const BVHModelBase& other) :
+ CollisionGeometry(other),
+ num_tris(other.num_tris),
+ num_vertices(other.num_vertices),
+ build_state(other.build_state),
+ num_tris_allocated(other.num_tris),
+ num_vertices_allocated(other.num_vertices)
{
if(other.vertices)
{
@@ -79,7 +77,20 @@ BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other),
}
else
prev_vertices = NULL;
+}
+
+void BVHModelBase::buildConvexRepresentation(bool share_memory)
+{
+ if (!convex) {
+ convex.reset(new Convex<Triangle>(!share_memory, vertices, num_vertices, tri_indices, num_vertices));
+ }
+}
+template<typename BV>
+BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : BVHModelBase(other),
+ bv_splitter(other.bv_splitter),
+ bv_fitter(other.bv_fitter)
+{
if(other.primitive_indices)
{
int num_primitives = 0;
@@ -112,18 +123,16 @@ BVHModel<BV>::BVHModel(const BVHModel<BV>& other) : CollisionGeometry(other),
}
-template<typename BV>
-int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
+int BVHModelBase::beginModel(int num_tris_, int num_vertices_)
{
if(build_state != BVH_BUILD_STATE_EMPTY)
{
delete [] vertices; vertices = NULL;
delete [] tri_indices; tri_indices = NULL;
- delete [] bvs; bvs = NULL;
delete [] prev_vertices; prev_vertices = NULL;
- delete [] primitive_indices; primitive_indices = NULL;
- num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0;
+ num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = 0;
+ deleteBVs();
}
if(num_tris_ <= 0) num_tris_ = 8;
@@ -158,9 +167,7 @@ int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
return BVH_OK;
}
-
-template<typename BV>
-int BVHModel<BV>::addVertex(const Vec3f& p)
+int BVHModelBase::addVertex(const Vec3f& p)
{
if(build_state != BVH_BUILD_STATE_BEGUN)
{
@@ -189,8 +196,7 @@ int BVHModel<BV>::addVertex(const Vec3f& p)
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
+int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
{
if(build_state == BVH_BUILD_STATE_PROCESSED)
{
@@ -243,8 +249,7 @@ int BVHModel<BV>::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps)
+int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps)
{
if(build_state == BVH_BUILD_STATE_PROCESSED)
{
@@ -278,8 +283,7 @@ int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps)
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts)
+int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts)
{
if(build_state == BVH_BUILD_STATE_PROCESSED)
{
@@ -340,8 +344,7 @@ int BVHModel<BV>::addSubModel(const std::vector<Vec3f>& ps, const std::vector<Tr
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::endModel()
+int BVHModelBase::endModel()
{
if(build_state != BVH_BUILD_STATE_BEGUN)
{
@@ -383,24 +386,9 @@ int BVHModel<BV>::endModel()
num_vertices_allocated = num_vertices;
}
-
// construct BVH tree
- int num_bvs_to_be_allocated = 0;
- if(num_tris == 0)
- num_bvs_to_be_allocated = 2 * num_vertices - 1;
- else
- num_bvs_to_be_allocated = 2 * num_tris - 1;
-
-
- bvs = new BVNode<BV> [num_bvs_to_be_allocated];
- primitive_indices = new unsigned int [num_bvs_to_be_allocated];
- if(!bvs || !primitive_indices)
- {
- std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl;
+ if (!allocateBVs ())
return BVH_ERR_MODEL_OUT_OF_MEMORY;
- }
- num_bvs_allocated = num_bvs_to_be_allocated;
- num_bvs = 0;
buildTree();
@@ -412,8 +400,7 @@ int BVHModel<BV>::endModel()
-template<typename BV>
-int BVHModel<BV>::beginReplaceModel()
+int BVHModelBase::beginReplaceModel()
{
if(build_state != BVH_BUILD_STATE_PROCESSED)
{
@@ -430,8 +417,7 @@ int BVHModel<BV>::beginReplaceModel()
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::replaceVertex(const Vec3f& p)
+int BVHModelBase::replaceVertex(const Vec3f& p)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
@@ -445,8 +431,7 @@ int BVHModel<BV>::replaceVertex(const Vec3f& p)
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
+int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
@@ -460,8 +445,7 @@ int BVHModel<BV>::replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f&
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::replaceSubModel(const std::vector<Vec3f>& ps)
+int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
@@ -477,8 +461,7 @@ int BVHModel<BV>::replaceSubModel(const std::vector<Vec3f>& ps)
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::endReplaceModel(bool refit, bool bottomup)
+int BVHModelBase::endReplaceModel(bool refit, bool bottomup)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
@@ -510,8 +493,7 @@ int BVHModel<BV>::endReplaceModel(bool refit, bool bottomup)
-template<typename BV>
-int BVHModel<BV>::beginUpdateModel()
+int BVHModelBase::beginUpdateModel()
{
if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED)
{
@@ -538,8 +520,7 @@ int BVHModel<BV>::beginUpdateModel()
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::updateVertex(const Vec3f& p)
+int BVHModelBase::updateVertex(const Vec3f& p)
{
if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
{
@@ -553,8 +534,7 @@ int BVHModel<BV>::updateVertex(const Vec3f& p)
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
+int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
{
if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
{
@@ -568,8 +548,7 @@ int BVHModel<BV>::updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f&
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::updateSubModel(const std::vector<Vec3f>& ps)
+int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps)
{
if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
{
@@ -585,8 +564,7 @@ int BVHModel<BV>::updateSubModel(const std::vector<Vec3f>& ps)
return BVH_OK;
}
-template<typename BV>
-int BVHModel<BV>::endUpdateModel(bool refit, bool bottomup)
+int BVHModelBase::endUpdateModel(bool refit, bool bottomup)
{
if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
{
@@ -621,6 +599,58 @@ int BVHModel<BV>::endUpdateModel(bool refit, bool bottomup)
+void BVHModelBase::computeLocalAABB()
+{
+ AABB aabb_;
+ for(int i = 0; i < num_vertices; ++i)
+ {
+ aabb_ += vertices[i];
+ }
+
+ aabb_center = aabb_.center();
+
+ aabb_radius = 0;
+ for(int i = 0; i < num_vertices; ++i)
+ {
+ FCL_REAL r = (aabb_center - vertices[i]).squaredNorm();
+ if(r > aabb_radius) aabb_radius = r;
+ }
+
+ aabb_radius = sqrt(aabb_radius);
+
+ aabb_local = aabb_;
+}
+
+template<typename BV>
+void BVHModel<BV>::deleteBVs()
+{
+ delete [] bvs; bvs = NULL;
+ delete [] primitive_indices; primitive_indices = NULL;
+ num_bvs_allocated = num_bvs = 0;
+}
+
+template<typename BV>
+bool BVHModel<BV>::allocateBVs()
+{
+ // construct BVH tree
+ int num_bvs_to_be_allocated = 0;
+ if(num_tris == 0)
+ num_bvs_to_be_allocated = 2 * num_vertices - 1;
+ else
+ num_bvs_to_be_allocated = 2 * num_tris - 1;
+
+
+ bvs = new BVNode<BV> [num_bvs_to_be_allocated];
+ primitive_indices = new unsigned int [num_bvs_to_be_allocated];
+ if(!bvs || !primitive_indices)
+ {
+ std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl;
+ return false;
+ }
+ num_bvs_allocated = num_bvs_to_be_allocated;
+ num_bvs = 0;
+ return true;
+}
template<typename BV>
int BVHModel<BV>::memUsage(int msg) const
@@ -642,14 +672,6 @@ 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()
{
@@ -874,29 +896,6 @@ int BVHModel<BV>::refitTree_topdown()
return BVH_OK;
}
-template<typename BV>
-void BVHModel<BV>::computeLocalAABB()
-{
- AABB aabb_;
- for(int i = 0; i < num_vertices; ++i)
- {
- aabb_ += vertices[i];
- }
-
- aabb_center = aabb_.center();
-
- aabb_radius = 0;
- for(int i = 0; i < num_vertices; ++i)
- {
- FCL_REAL r = (aabb_center - vertices[i]).squaredNorm();
- if(r > aabb_radius) aabb_radius = r;
- }
-
- aabb_radius = sqrt(aabb_radius);
-
- aabb_local = aabb_;
-}
-
template<>
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)
--
GitLab