hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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 * Copyright (c) 2020, INRIA
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Open Source Robotics Foundation nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36
39#ifndef HPP_FCL_BVH_MODEL_H
40#define HPP_FCL_BVH_MODEL_H
41
42#include <hpp/fcl/fwd.hh>
45#include <hpp/fcl/BV/BV_node.h>
46#include <vector>
47
48namespace hpp
49{
50namespace fcl
51{
52
55
56class ConvexBase;
57
58template <typename BV> class BVFitter;
59template <typename BV> class BVSplitter;
60
63: public CollisionGeometry
64{
65public:
66
69
72
75
77 unsigned int num_tris;
78
80 unsigned int num_vertices;
81
84
86 shared_ptr< ConvexBase > convex;
87
90 {
91 if(num_tris && num_vertices)
93 else if(num_vertices)
95 else
96 return BVH_MODEL_UNKNOWN;
97 }
98
101
104
106 virtual ~BVHModelBase ()
107 {
108 delete [] vertices;
109 delete [] tri_indices;
110 delete [] prev_vertices;
111 }
112
114 bool operator==(const BVHModelBase & other) const;
115
117 bool operator!=(const BVHModelBase & other) const
118 {
119 return !(*this == other);
120 }
121
123 OBJECT_TYPE getObjectType() const { return OT_BVH; }
124
127
129 int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
130
132 int addVertex(const Vec3f& p);
133
135 int addVertices(const Matrixx3f & points);
136
138 int addTriangles(const Matrixx3i & triangles);
139
141 int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
142
144 int addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts);
145
147 int addSubModel(const std::vector<Vec3f>& ps);
148
150 int endModel();
151
154
156 int replaceVertex(const Vec3f& p);
157
159 int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
160
162 int replaceSubModel(const std::vector<Vec3f>& ps);
163
165 int endReplaceModel(bool refit = true, bool bottomup = true);
166
170
172 int updateVertex(const Vec3f& p);
173
175 int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
176
178 int updateSubModel(const std::vector<Vec3f>& ps);
179
181 int endUpdateModel(bool refit = true, bool bottomup = true);
182
187 void buildConvexRepresentation(bool share_memory);
188
200 bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
201
202 virtual int memUsage(const bool msg = false) const = 0;
203
206 virtual void makeParentRelative() = 0;
207
209 {
210 FCL_REAL vol = 0;
211 Vec3f com(0,0,0);
212 for(unsigned int i = 0; i < num_tris; ++i)
213 {
214 const Triangle& tri = tri_indices[i];
215 FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
216 vol += d_six_vol;
217 com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
218 }
219
220 return com / (vol * 4);
221 }
222
224 {
225 FCL_REAL vol = 0;
226 for(unsigned int i = 0; i < num_tris; ++i)
227 {
228 const Triangle& tri = tri_indices[i];
229 FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
230 vol += d_six_vol;
231 }
232
233 return vol / 6;
234 }
235
237 {
238 Matrix3f C = Matrix3f::Zero();
239
240 Matrix3f C_canonical;
241 C_canonical << 1/60.0, 1/120.0, 1/120.0,
242 1/120.0, 1/60.0, 1/120.0,
243 1/120.0, 1/120.0, 1/60.0;
244
245 for(unsigned int i = 0; i < num_tris; ++i)
246 {
247 const Triangle& tri = tri_indices[i];
248 const Vec3f& v1 = vertices[tri[0]];
249 const Vec3f& v2 = vertices[tri[1]];
250 const Vec3f& v3 = vertices[tri[2]];
251 Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
252 C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
253 }
254
255 return C.trace() * Matrix3f::Identity() - C;
256 }
257
258protected:
259 virtual void deleteBVs() = 0;
260 virtual bool allocateBVs() = 0;
261
263 virtual int buildTree() = 0;
264
266 virtual int refitTree(bool bottomup) = 0;
267
268 unsigned int num_tris_allocated;
270 unsigned int num_vertex_updated;
271};
272
275template<typename BV>
277: public BVHModelBase
278{
279 typedef BVHModelBase Base;
280public:
282 shared_ptr<BVSplitter<BV> > bv_splitter;
283
285 shared_ptr<BVFitter<BV> > bv_fitter;
286
289
294 BVHModel(const BVHModel& other);
295
297 virtual BVHModel<BV> * clone() const { return new BVHModel(*this); }
298
301 {
302 delete [] bvs;
303 delete [] primitive_indices;
304 }
305
307 bool operator==(const BVHModel & other) const
308 {
309 bool res = Base::operator==(other);
310 if(!res)
311 return false;
312
313 // unsigned int other_num_primitives = 0;
314 // if(other.primitive_indices)
315 // {
316
317 // switch(other.getModelType())
318 // {
319 // case BVH_MODEL_TRIANGLES:
320 // other_num_primitives = num_tris;
321 // break;
322 // case BVH_MODEL_POINTCLOUD:
323 // other_num_primitives = num_vertices;
324 // break;
325 // default:
326 // ;
327 // }
328 // }
329
330// unsigned int num_primitives = 0;
331// if(primitive_indices)
332// {
333//
334// switch(other.getModelType())
335// {
336// case BVH_MODEL_TRIANGLES:
337// num_primitives = num_tris;
338// break;
339// case BVH_MODEL_POINTCLOUD:
340// num_primitives = num_vertices;
341// break;
342// default:
343// ;
344// }
345// }
346//
347// if(num_primitives != other_num_primitives)
348// return false;
349//
350// for(int k = 0; k < num_primitives; ++k)
351// {
352// if(primitive_indices[k] != other.primitive_indices[k])
353// return false;
354// }
355
356 if(num_bvs != other.num_bvs)
357 return false;
358
359 for(unsigned int k = 0; k < num_bvs; ++k)
360 {
361 if(bvs[k] != other.bvs[k])
362 return false;
363 }
364
365 return true;
366 }
367
369 bool operator!=(const BVHModel & other) const
370 {
371 return !(*this == other);
372 }
373
375
377 const BVNode<BV>& getBV(unsigned int i) const
378 {
379 assert (i < num_bvs);
380 return bvs[i];
381 }
382
384 BVNode<BV>& getBV(unsigned int i)
385 {
386 assert (i < num_bvs);
387 return bvs[i];
388 }
389
391 unsigned int getNumBVs() const
392 {
393 return num_bvs;
394 }
395
397 NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
398
400 int memUsage(const bool msg) const;
401
405 {
406 Matrix3f I (Matrix3f::Identity());
407 makeParentRelativeRecurse(0, I, Vec3f());
408 }
409
410protected:
411 void deleteBVs();
413
414 unsigned int num_bvs_allocated;
415 unsigned int* primitive_indices;
416
419
421 unsigned int num_bvs;
422
425
427 int refitTree(bool bottomup);
428
431
434
436 int recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives);
437
440
443 void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)
444 {
445 if(!bvs[bv_id].isLeaf())
446 {
447 makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter());
448
449 makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter());
450 }
451
452 bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
453 }
454};
455
457
458template<>
459void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
460
461template<>
462void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
463
464template<>
465void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
466
467
469template<>
471
472template<>
474
475template<>
477
478template<>
480
481template<>
483
484template<>
485NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
486
487template<>
488NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
489
490template<>
491NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
492
493}
494
495} // namespace hpp
496
497#endif
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH_model.h:64
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:74
unsigned int num_tris
Number of triangles.
Definition: BVH_model.h:77
int addTriangles(const Matrixx3i &triangles)
Add triangles in the new BVH model.
int replaceTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Replace one triangle in the old BVH model.
int replaceSubModel(const std::vector< Vec3f > &ps)
Replace a set of points in the old BVH model.
bool operator!=(const BVHModelBase &other) const
Difference operator.
Definition: BVH_model.h:117
int updateTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Update one triangle in the old BVH model.
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH_model.h:71
int endReplaceModel(bool refit=true, bool bottomup=true)
End BVH model replacement, will also refit or rebuild the bounding volume hierarchy.
unsigned int num_vertices_allocated
Definition: BVH_model.h:269
int updateVertex(const Vec3f &p)
Update one point in the old BVH model.
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH_model.h:86
bool operator==(const BVHModelBase &other) const
Comparison operators.
int addSubModel(const std::vector< Vec3f > &ps)
Add a set of points in the new BVH model.
int beginUpdateModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
BVHBuildState build_state
The state of BVH building process.
Definition: BVH_model.h:83
virtual void makeParentRelative()=0
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
virtual int refitTree(bool bottomup)=0
Refit the bounding volume hierarchy.
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
unsigned int num_vertex_updated
Definition: BVH_model.h:270
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model.h:89
int beginReplaceModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
int addTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Add one triangle in the new BVH model.
virtual ~BVHModelBase()
deconstruction, delete mesh data related.
Definition: BVH_model.h:106
virtual bool allocateBVs()=0
int replaceVertex(const Vec3f &p)
Replace one point in the old BVH model.
BVHModelBase(const BVHModelBase &other)
copy from another BVH
virtual int buildTree()=0
Build the bounding volume hierarchy.
Vec3f * vertices
Geometry point data.
Definition: BVH_model.h:68
unsigned int num_vertices
Number of points.
Definition: BVH_model.h:80
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH_model.h:236
unsigned int num_tris_allocated
Definition: BVH_model.h:268
int addVertices(const Matrixx3f &points)
Add points in the new BVH model.
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
BVHModelBase()
Constructing an empty BVH.
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
int addVertex(const Vec3f &p)
Add one point in the new BVH model.
int endUpdateModel(bool refit=true, bool bottomup=true)
End BVH model update, will also refit or rebuild the bounding volume hierarchy.
virtual void deleteBVs()=0
bool buildConvexHull(bool keepTriangle, const char *qhullCommand=NULL)
Build a convex hull and store it in attribute convex.
FCL_REAL computeVolume() const
compute the volume
Definition: BVH_model.h:223
OBJECT_TYPE getObjectType() const
Get the object type: it is a BVH.
Definition: BVH_model.h:123
int updateSubModel(const std::vector< Vec3f > &ps)
Update a set of points in the old BVH model.
virtual int memUsage(const bool msg=false) const =0
Vec3f computeCOM() const
compute center of mass
Definition: BVH_model.h:208
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
void buildConvexRepresentation(bool share_memory)
Build this Convex<Triangle> representation of this model. The result is stored in attribute convex.
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:278
bool operator==(const BVHModel &other) const
Equality operator.
Definition: BVH_model.h:307
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model.h:300
int refitTree_bottomup()
Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)
const BVNode< BV > & getBV(unsigned int i) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model.h:377
bool operator!=(const BVHModel &other) const
Difference operator.
Definition: BVH_model.h:369
BVHModel(const BVHModel &other)
Copy constructor from another BVH.
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH_model.h:297
unsigned int getNumBVs() const
Get the number of bv in the BVH.
Definition: BVH_model.h:391
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
Definition: BVH_model.h:404
unsigned int * primitive_indices
Definition: BVH_model.h:415
BVNode< BV > * bvs
Bounding volume hierarchy.
Definition: BVH_model.h:418
int buildTree()
Build the bounding volume hierarchy.
shared_ptr< BVFitter< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Definition: BVH_model.h:285
int recursiveRefitTree_bottomup(int bv_id)
Recursive kernel for bottomup refitting.
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:282
BVNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: BVH_model.h:384
void makeParentRelativeRecurse(int bv_id, Matrix3f &parent_axes, const Vec3f &parent_c)
Definition: BVH_model.h:443
unsigned int num_bvs_allocated
Definition: BVH_model.h:414
BVHModel()
Default constructor to build an empty BVH.
unsigned int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:421
int memUsage(const bool msg) const
Check the number of memory used.
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: BVH_model.h:397
int refitTree_topdown()
Refit the bounding volume hierarchy in a top-down way (slow but more compact)
int recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives)
Recursive kernel for hierarchy construction.
int refitTree(bool bottomup)
Refit the bounding volume hierarchy.
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
Triangle with 3 indices for points.
Definition: data_types.h:77
#define HPP_FCL_DLLAPI
Definition: config.hh:64
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
BV bv
bounding volume storing the geometry
Definition: BV_node.h:114
@ OT_BVH
Definition: collision_object.h:55
@ BV_UNKNOWN
Definition: collision_object.h:58
Eigen::Matrix< Eigen::DenseIndex, Eigen::Dynamic, 3 > Matrixx3i
Definition: data_types.h:71
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
BVHBuildState
States for BVH construction empty->begun->processed ->replace_begun->processed -> ....
Definition: BVH_internal.h:53
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > Matrixx3f
Definition: data_types.h:70
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:58
BVHModelType
BVH model type.
Definition: BVH_internal.h:78
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:81
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:79
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:80
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:110