hpp-fcl  1.7.4
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  int num_tris;
78 
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(int num_tris = 0, 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(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(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(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 
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 
291  BVHModel(const BVHModel& other);
292 
294  virtual BVHModel<BV> * clone() const { return new BVHModel(*this); }
295 
298  {
299  delete [] bvs;
300  delete [] primitive_indices;
301  }
302 
304  bool operator==(const BVHModel & other) const
305  {
306  bool res = Base::operator==(other);
307  if(!res)
308  return false;
309 
310  int other_num_primitives = 0;
311  if(other.primitive_indices)
312  {
313 
314  switch(other.getModelType())
315  {
316  case BVH_MODEL_TRIANGLES:
317  other_num_primitives = num_tris;
318  break;
320  other_num_primitives = num_vertices;
321  break;
322  default:
323  ;
324  }
325  }
326 
327 // int num_primitives = 0;
328 // if(primitive_indices)
329 // {
330 //
331 // switch(other.getModelType())
332 // {
333 // case BVH_MODEL_TRIANGLES:
334 // num_primitives = num_tris;
335 // break;
336 // case BVH_MODEL_POINTCLOUD:
337 // num_primitives = num_vertices;
338 // break;
339 // default:
340 // ;
341 // }
342 // }
343 //
344 // if(num_primitives != other_num_primitives)
345 // return false;
346 //
347 // for(int k = 0; k < num_primitives; ++k)
348 // {
349 // if(primitive_indices[k] != other.primitive_indices[k])
350 // return false;
351 // }
352 
353  if(num_bvs != other.num_bvs)
354  return false;
355 
356  for(int k = 0; k < num_bvs; ++k)
357  {
358  if(bvs[k] != other.bvs[k])
359  return false;
360  }
361 
362  return true;
363  }
364 
366  bool operator!=(const BVHModel & other) const
367  {
368  return !(*this == other);
369  }
370 
372 
374  const BVNode<BV>& getBV(int id) const
375  {
376  assert (id < num_bvs);
377  return bvs[id];
378  }
379 
381  BVNode<BV>& getBV(int id)
382  {
383  assert (id < num_bvs);
384  return bvs[id];
385  }
386 
388  int getNumBVs() const
389  {
390  return num_bvs;
391  }
392 
394  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
395 
397  int memUsage(const bool msg) const;
398 
402  {
403  Matrix3f I (Matrix3f::Identity());
404  makeParentRelativeRecurse(0, I, Vec3f());
405  }
406 
407 protected:
408  void deleteBVs();
409  bool allocateBVs();
410 
412  unsigned int* primitive_indices;
413 
416 
418  int num_bvs;
419 
421  int buildTree();
422 
424  int refitTree(bool bottomup);
425 
428 
431 
433  int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);
434 
437 
440  void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)
441  {
442  if(!bvs[bv_id].isLeaf())
443  {
444  makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter());
445 
446  makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter());
447  }
448 
449  bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
450  }
451 };
452 
454 
455 template<>
456 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
457 
458 template<>
459 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
460 
461 template<>
462 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
463 
464 
466 template<>
468 
469 template<>
471 
472 template<>
474 
475 template<>
477 
478 template<>
480 
481 template<>
482 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
483 
484 template<>
485 NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
486 
487 template<>
488 NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
489 
490 }
491 
492 } // namespace hpp
493 
494 #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
int num_vertex_updated
Definition: BVH_model.h:270
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.
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 num_vertices_allocated
Definition: BVH_model.h:269
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
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
int num_tris
Number of triangles.
Definition: BVH_model.h:77
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
int num_tris_allocated
Definition: BVH_model.h:268
virtual int buildTree()=0
Build the bounding volume hierarchy.
Vec3f * vertices
Geometry point data.
Definition: BVH_model.h:68
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH_model.h:236
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 beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
int num_vertices
Number of points.
Definition: BVH_model.h:80
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
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:304
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model.h:297
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:366
int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:418
BVHModel(const BVHModel &other)
copy from another BVH
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
Definition: BVH_model.h:401
int getNumBVs() const
Get the number of bv in the BVH.
Definition: BVH_model.h:388
unsigned int * primitive_indices
Definition: BVH_model.h:412
BVNode< BV > * bvs
Bounding volume hierarchy.
Definition: BVH_model.h:415
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
int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives)
Recursive kernel for hierarchy construction.
void makeParentRelativeRecurse(int bv_id, Matrix3f &parent_axes, const Vec3f &parent_c)
Definition: BVH_model.h:440
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:374
BVHModel()
Constructing an empty BVH.
int num_bvs_allocated
Definition: BVH_model.h:411
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH_model.h:294
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:394
int refitTree_topdown()
Refit the bounding volume hierarchy in a top-down way (slow but more compact)
BVNode< BV > & getBV(int id)
Access the bv giving the its index.
Definition: BVH_model.h:381
int refitTree(bool bottomup)
Refit the bounding volume hierarchy.
The geometry for the object for collision or distance computation.
Definition: collision_object.h:66
Triangle with 3 indices for points.
Definition: data_types.h:75
#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:70
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
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:69
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