hpp-fcl: include/hpp/fcl/BVH/BVH_model.h Source File

hpp-fcl 1.6.0 HPP fork of FCL -- The Flexible Collision Library

BVH_model.h
Go to the documentation of this file.
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
38 #ifndef HPP_FCL_BVH_MODEL_H
39 #define HPP_FCL_BVH_MODEL_H
40
41 #include <hpp/fcl/collision_object.h>
42 #include <hpp/fcl/BVH/BVH_internal.h>
43 #include <hpp/fcl/BV/BV_node.h>
44 #include <vector>
45 #include <boost/shared_ptr.hpp>
46 #include <boost/noncopyable.hpp>
47
48 namespace hpp
49 {
50 namespace fcl
51 {
52
55
56 class ConvexBase;
57
58 template <typename BV> class BVFitter;
59 template <typename BV> class BVSplitter;
60
62 class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry
63 {
64 public:
66 Vec3f* vertices;
67
69 Triangle* tri_indices;
70
72 Vec3f* prev_vertices;
73
75 int num_tris;
76
78 int num_vertices;
79
81 BVHBuildState build_state;
82
84 boost::shared_ptr< ConvexBase > convex;
85
87 BVHModelType getModelType() const
88 {
89 if(num_tris && num_vertices)
90 return BVH_MODEL_TRIANGLES;
91 else if(num_vertices)
92 return BVH_MODEL_POINTCLOUD;
93 else
94 return BVH_MODEL_UNKNOWN;
95 }
96
98 BVHModelBase() : vertices(NULL),
99 tri_indices(NULL),
100 prev_vertices(NULL),
101 num_tris(0),
102 num_vertices(0),
103 build_state(BVH_BUILD_STATE_EMPTY),
104 num_tris_allocated(0),
105 num_vertices_allocated(0),
106 num_vertex_updated(0)
107 {
108 }
109
111 BVHModelBase(const BVHModelBase& other);
112
114 virtual ~BVHModelBase ()
115 {
116 delete [] vertices;
117 delete [] tri_indices;
118 delete [] prev_vertices;
119 }
120
122 OBJECT_TYPE getObjectType() const { return OT_BVH; }
123
125 void computeLocalAABB();
126
128 int beginModel(int num_tris = 0, int num_vertices = 0);
129
131 int addVertex(const Vec3f& p);
132
134 int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
135
137 int addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts);
138
140 int addSubModel(const std::vector<Vec3f>& ps);
141
143 int endModel();
144
146 int beginReplaceModel();
147
149 int replaceVertex(const Vec3f& p);
150
152 int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
153
155 int replaceSubModel(const std::vector<Vec3f>& ps);
156
158 int endReplaceModel(bool refit = true, bool bottomup = true);
159
162 int beginUpdateModel();
163
165 int updateVertex(const Vec3f& p);
166
168 int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
169
171 int updateSubModel(const std::vector<Vec3f>& ps);
172
174 int endUpdateModel(bool refit = true, bool bottomup = true);
175
180 void buildConvexRepresentation(bool share_memory);
181
193 bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
194
195 virtual int memUsage(int msg) const = 0;
196
199 virtual void makeParentRelative() = 0;
200
201 Vec3f computeCOM() const
202 {
203 FCL_REAL vol = 0;
204 Vec3f com(0,0,0);
205 for(int i = 0; i < num_tris; ++i)
206 {
207 const Triangle& tri = tri_indices[i];
208 FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
209 vol += d_six_vol;
210 com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
211 }
212
213 return com / (vol * 4);
214 }
215
216 FCL_REAL computeVolume() const
217 {
218 FCL_REAL vol = 0;
219 for(int i = 0; i < num_tris; ++i)
220 {
221 const Triangle& tri = tri_indices[i];
222 FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
223 vol += d_six_vol;
224 }
225
226 return vol / 6;
227 }
228
229 Matrix3f computeMomentofInertia() const
230 {
231 Matrix3f C = Matrix3f::Zero();
232
233 Matrix3f C_canonical;
234 C_canonical << 1/60.0, 1/120.0, 1/120.0,
235 1/120.0, 1/60.0, 1/120.0,
236 1/120.0, 1/120.0, 1/60.0;
237
238 for(int i = 0; i < num_tris; ++i)
239 {
240 const Triangle& tri = tri_indices[i];
241 const Vec3f& v1 = vertices[tri[0]];
242 const Vec3f& v2 = vertices[tri[1]];
243 const Vec3f& v3 = vertices[tri[2]];
244 Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
245 C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
246 }
247
248 return C.trace() * Matrix3f::Identity() - C;
249 }
250
251 protected:
252 virtual void deleteBVs() = 0;
253 virtual bool allocateBVs() = 0;
254
256 virtual int buildTree() = 0;
257
259 virtual int refitTree(bool bottomup) = 0;
260
261 int num_tris_allocated;
262 int num_vertices_allocated;
263 int num_vertex_updated;
264 };
265
268 template<typename BV>
269 class HPP_FCL_DLLAPI BVHModel : public BVHModelBase
270 {
271
272 public:
274 boost::shared_ptr<BVSplitter<BV> > bv_splitter;
275
277 boost::shared_ptr<BVFitter<BV> > bv_fitter;
278
280 BVHModel();
281
283 BVHModel(const BVHModel& other);
284
286 ~BVHModel()
287 {
288 delete [] bvs;
289 delete [] primitive_indices;
290 }
291
293
295 const BVNode<BV>& getBV(int id) const
296 {
297 assert (id < num_bvs);
298 return bvs[id];
299 }
300
302 BVNode<BV>& getBV(int id)
303 {
304 assert (id < num_bvs);
305 return bvs[id];
306 }
307
309 int getNumBVs() const
310 {
311 return num_bvs;
312 }
313
315 NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
316
318 int memUsage(int msg) const;
319
322 void makeParentRelative()
323 {
324 Matrix3f I (Matrix3f::Identity());
325 makeParentRelativeRecurse(0, I, Vec3f());
326 }
327
328 private:
329 void deleteBVs();
330 bool allocateBVs();
331
332 int num_bvs_allocated;
333 unsigned int* primitive_indices;
334
336 BVNode<BV>* bvs;
337
339 int num_bvs;
340
342 int buildTree();
343
345 int refitTree(bool bottomup);
346
348 int refitTree_topdown();
349
351 int refitTree_bottomup();
352
354 int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);
355
357 int recursiveRefitTree_bottomup(int bv_id);
358
361 void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)
362 {
363 if(!bvs[bv_id].isLeaf())
364 {
365 makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter());
366
367 makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter());
368 }
369
370 bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
371 }
372 };
373
375
376 template<>
377 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
378
379 template<>
380 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
381
382 template<>
383 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
384
385
387 template<>
388 NODE_TYPE BVHModel<AABB>::getNodeType() const;
389
390 template<>
391 NODE_TYPE BVHModel<OBB>::getNodeType() const;
392
393 template<>
394 NODE_TYPE BVHModel<RSS>::getNodeType() const;
395
396 template<>
397 NODE_TYPE BVHModel<kIOS>::getNodeType() const;
398
399 template<>
400 NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
401
402 template<>
403 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
404
405 template<>
406 NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
407
408 template<>
409 NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
410
411 }
412
413 } // namespace hpp
414
415 #endif
hpp::fcl::BVHModelBase::endUpdateModel
int endUpdateModel(bool refit=true, bool bottomup=true)
End BVH model update, will also refit or rebuild the bounding volume hierarchy.
hpp::fcl::BVHModelBase::refitTree
virtual int refitTree(bool bottomup)=0
Refit the bounding volume hierarchy.
hpp::fcl::BVHModelBase::buildTree
virtual int buildTree()=0
Build the bounding volume hierarchy.
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::BVHModelBase::memUsage
virtual int memUsage(int msg) const =0
hpp::fcl::BVHModel::BVHModel
BVHModel()
Constructing an empty BVH.
collision_object.h
hpp::fcl::BVHModelBase::BVHModelBase
BVHModelBase(const BVHModelBase &other)
copy from another BVH
hpp::fcl::BVHModelBase::replaceTriangle
int replaceTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Replace one triangle in the old BVH model.
hpp::fcl::BVHModel::bv_fitter
boost::shared_ptr< BVFitter< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Definition: BVH_model.h:277
hpp::fcl::BVHModelBase::updateVertex
int updateVertex(const Vec3f &p)
Update one point in the old BVH model.
hpp::fcl::BVHModel::getNumBVs
int getNumBVs() const
Get the number of bv in the BVH.
Definition: BVH_model.h:309
hpp::fcl::BVHModelBase::computeMomentofInertia
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH_model.h:229
hpp::fcl::BVHModelBase::build_state
BVHBuildState build_state
The state of BVH building process.
Definition: BVH_model.h:81
hpp::fcl::BVHModel::memUsage
int memUsage(int msg) const
Check the number of memory used.
hpp::fcl::BVHModelBase::computeLocalAABB
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
hpp::fcl::BVH_MODEL_UNKNOWN
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:79
hpp::fcl::BVHModelBase::getObjectType
OBJECT_TYPE getObjectType() const
Get the object type: it is a BVH.
Definition: BVH_model.h:122
hpp::fcl::BVHModelBase::endReplaceModel
int endReplaceModel(bool refit=true, bool bottomup=true)
End BVH model replacement, will also refit or rebuild the bounding volume hierarchy.
hpp::fcl::BVHModelBase::prev_vertices
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:72
hpp::fcl::BVHModelBase::num_vertices
int num_vertices
Number of points.
Definition: BVH_model.h:78
hpp::fcl::BVHModelBase::updateTriangle
int updateTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Update one triangle in the old BVH model.
hpp::fcl::BVHModelBase::deleteBVs
virtual void deleteBVs()=0
hpp::fcl::BVHModelBase::replaceVertex
int replaceVertex(const Vec3f &p)
Replace one point in the old BVH model.
hpp::fcl::BVHModelBase::addSubModel
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:270
hpp::fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:56
hpp::fcl::BVHModelBase::num_vertex_updated
int num_vertex_updated
Definition: BVH_model.h:263
hpp::fcl::BVHModelBase::allocateBVs
virtual bool allocateBVs()=0
hpp::fcl::BVHModelBase::buildConvexRepresentation
void buildConvexRepresentation(bool share_memory)
Build this Convex<Triangle> representation of this model. The result is stored in attribute convex.
hpp::fcl::BVHModel::BVHModel
BVHModel(const BVHModel &other)
copy from another BVH
hpp::fcl::BVHModelBase::beginUpdateModel
int beginUpdateModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp::fcl::BVH_BUILD_STATE_EMPTY
@ BVH_BUILD_STATE_EMPTY
Definition: BVH_internal.h:54
hpp::fcl::BVHModelBase::buildConvexHull
bool buildConvexHull(bool keepTriangle, const char *qhullCommand=NULL)
Build a convex hull and store it in attribute convex.
hpp::fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
hpp::fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:80
hpp::fcl::BVHModel::getNodeType
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: BVH_model.h:315
hpp::fcl::BVHModelBase::addVertex
int addVertex(const Vec3f &p)
Add one point in the new BVH model.
hpp::fcl::BVHModelBase::makeParentRelative
virtual void makeParentRelative()=0
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
hpp::fcl::BVHModelBase::num_tris_allocated
int num_tris_allocated
Definition: BVH_model.h:261
hpp::fcl::BVHModelBase::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: BVH_model.h:216
hpp::fcl::BVHModel::makeParentRelative
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
Definition: BVH_model.h:322
hpp::fcl::BVHBuildState
BVHBuildState
States for BVH construction empty->begun->processed ->replace_begun->processed -> ....
Definition: BVH_internal.h:53
hpp::fcl::BVHModelBase::replaceSubModel
int replaceSubModel(const std::vector< Vec3f > &ps)
Replace a set of points in the old BVH model.
hpp::fcl::BVHModelType
BVHModelType
BVH model type.
Definition: BVH_internal.h:78
hpp::fcl::BVHModelBase::addTriangle
int addTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Add one triangle in the new BVH model.
BVH_internal.h
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::BVHModel::~BVHModel
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model.h:286
hpp::fcl::BVNode::bv
BV bv
bounding volume storing the geometry
Definition: BV_node.h:90
hpp::fcl::BVHModelBase::getModelType
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model.h:87
hpp::fcl::BV_UNKNOWN
@ BV_UNKNOWN
Definition: collision_object.h:56
hpp::fcl::BVHModelBase::convex
boost::shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH_model.h:84
hpp::fcl::BVHModelBase::BVHModelBase
BVHModelBase()
Constructing an empty BVH.
Definition: BVH_model.h:98
hpp::fcl::BVHModelBase::updateSubModel
int updateSubModel(const std::vector< Vec3f > &ps)
Update a set of points in the old BVH model.
BV_node.h
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::BVHModel::getBV
const BVNode< BV > & getBV(int id) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model.h:295
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:64
hpp::fcl::BVHModelBase::endModel
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
hpp::fcl::BVHModelBase
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH_model.h:63
hpp::fcl::BVHModelBase::vertices
Vec3f * vertices
Geometry point data.
Definition: BVH_model.h:66
hpp::fcl::BVHModelBase::beginReplaceModel
int beginReplaceModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
hpp::fcl::BVHModelBase::tri_indices
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH_model.h:69
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:73
hpp::fcl::BVHModel::getBV
BVNode< BV > & getBV(int id)
Access the bv giving the its index.
Definition: BVH_model.h:302
hpp::fcl::OT_BVH
@ OT_BVH
Definition: collision_object.h:53
hpp::fcl::BVHModelBase::computeCOM
Vec3f computeCOM() const
compute center of mass
Definition: BVH_model.h:201
hpp::fcl::BVHModelBase::addSubModel
int addSubModel(const std::vector< Vec3f > &ps)
Add a set of points in the new BVH model.
hpp::fcl::BVNode
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:88
hpp::fcl::BVHModelBase::num_vertices_allocated
int num_vertices_allocated
Definition: BVH_model.h:262
hpp::fcl::BVH_MODEL_POINTCLOUD
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:81
hpp::fcl::BVHModelBase::num_tris
int num_tris
Number of triangles.
Definition: BVH_model.h:75
hpp::fcl::BVHModelBase::~BVHModelBase
virtual ~BVHModelBase()
deconstruction, delete mesh data related.
Definition: BVH_model.h:114
hpp::fcl::BVHModel::bv_splitter
boost::shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:274
hpp::fcl::BVHModelBase::beginModel
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.