hpp-fcl  1.8.1
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  * 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 
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 
63 : public CollisionGeometry
64 {
65 public:
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)
92  return BVH_MODEL_TRIANGLES;
93  else if(num_vertices)
94  return BVH_MODEL_POINTCLOUD;
95  else
96  return BVH_MODEL_UNKNOWN;
97  }
98 
101 
103  BVHModelBase(const BVHModelBase& other);
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 
258 protected:
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 
275 template<typename BV>
277 : public BVHModelBase
278 {
279  typedef BVHModelBase Base;
280 public:
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 
410 protected:
411  void deleteBVs();
412  bool allocateBVs();
413 
414  unsigned int num_bvs_allocated;
415  unsigned int* primitive_indices;
416 
419 
421  unsigned int num_bvs;
422 
424  int buildTree();
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 
458 template<>
459 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
460 
461 template<>
462 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
463 
464 template<>
465 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
466 
467 
469 template<>
471 
472 template<>
474 
475 template<>
477 
478 template<>
480 
481 template<>
483 
484 template<>
485 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
486 
487 template<>
488 NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
489 
490 template<>
491 NODE_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)
bool operator!=(const BVHModel &other) const
Difference operator.
Definition: BVH_model.h:369
BVHModel(const BVHModel &other)
Copy constructor from another BVH.
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
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
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH_model.h:297
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)
BVNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: BVH_model.h:384
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.
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
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