hpp-fcl  2.3.5
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-2022, 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 namespace fcl {
50 
53 
54 class ConvexBase;
55 
56 template <typename BV>
57 class BVFitter;
58 template <typename BV>
59 class BVSplitter;
60 
64  public:
67 
70 
73 
75  unsigned int num_tris;
76 
78  unsigned int num_vertices;
79 
82 
84  shared_ptr<ConvexBase> convex;
85 
88  if (num_tris && num_vertices)
89  return BVH_MODEL_TRIANGLES;
90  else if (num_vertices)
91  return BVH_MODEL_POINTCLOUD;
92  else
93  return BVH_MODEL_UNKNOWN;
94  }
95 
98 
100  BVHModelBase(const BVHModelBase& other);
101 
103  virtual ~BVHModelBase() {
104  delete[] vertices;
105  delete[] tri_indices;
106  delete[] prev_vertices;
107  }
108 
110  OBJECT_TYPE getObjectType() const { return OT_BVH; }
111 
114 
116  int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
117 
119  int addVertex(const Vec3f& p);
120 
122  int addVertices(const Matrixx3f& points);
123 
125  int addTriangles(const Matrixx3i& triangles);
126 
128  int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
129 
131  int addSubModel(const std::vector<Vec3f>& ps,
132  const std::vector<Triangle>& ts);
133 
135  int addSubModel(const std::vector<Vec3f>& ps);
136 
139  int endModel();
140 
144 
146  int replaceVertex(const Vec3f& p);
147 
149  int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
150 
152  int replaceSubModel(const std::vector<Vec3f>& ps);
153 
156  int endReplaceModel(bool refit = true, bool bottomup = true);
157 
162 
164  int updateVertex(const Vec3f& p);
165 
167  int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
168 
170  int updateSubModel(const std::vector<Vec3f>& ps);
171 
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(const bool msg = false) const = 0;
196 
201  virtual void makeParentRelative() = 0;
202 
203  Vec3f computeCOM() const {
204  FCL_REAL vol = 0;
205  Vec3f com(0, 0, 0);
206  for (unsigned int i = 0; i < num_tris; ++i) {
207  const Triangle& tri = tri_indices[i];
208  FCL_REAL d_six_vol =
209  (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
210  vol += d_six_vol;
211  com +=
212  (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
213  }
214 
215  return com / (vol * 4);
216  }
217 
219  FCL_REAL vol = 0;
220  for (unsigned int i = 0; i < num_tris; ++i) {
221  const Triangle& tri = tri_indices[i];
222  FCL_REAL d_six_vol =
223  (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
224  vol += d_six_vol;
225  }
226 
227  return vol / 6;
228  }
229 
231  Matrix3f C = Matrix3f::Zero();
232 
233  Matrix3f C_canonical;
234  C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0,
235  1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0;
236 
237  for (unsigned int i = 0; i < num_tris; ++i) {
238  const Triangle& tri = tri_indices[i];
239  const Vec3f& v1 = vertices[tri[0]];
240  const Vec3f& v2 = vertices[tri[1]];
241  const Vec3f& v3 = vertices[tri[2]];
242  Matrix3f A;
243  A << v1.transpose(), v2.transpose(), v3.transpose();
244  C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
245  }
246 
247  return C.trace() * Matrix3f::Identity() - C;
248  }
249 
250  protected:
251  virtual void deleteBVs() = 0;
252  virtual bool allocateBVs() = 0;
253 
255  virtual int buildTree() = 0;
256 
258  virtual int refitTree(bool bottomup) = 0;
259 
260  unsigned int num_tris_allocated;
262  unsigned int num_vertex_updated;
263 
264  protected:
266  virtual bool isEqual(const CollisionGeometry& other) const;
267 };
268 
272 template <typename BV>
274  typedef BVHModelBase Base;
275 
276  public:
278  shared_ptr<BVSplitter<BV> > bv_splitter;
279 
281  shared_ptr<BVFitter<BV> > bv_fitter;
282 
285 
290  BVHModel(const BVHModel& other);
291 
293  virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
294 
297  delete[] bvs;
298  delete[] primitive_indices;
299  }
300 
303 
305  const BVNode<BV>& getBV(unsigned int i) const {
306  assert(i < num_bvs);
307  return bvs[i];
308  }
309 
311  BVNode<BV>& getBV(unsigned int i) {
312  assert(i < num_bvs);
313  return bvs[i];
314  }
315 
317  unsigned int getNumBVs() const { return num_bvs; }
318 
320  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
321 
323  int memUsage(const bool msg) const;
324 
330  Matrix3f I(Matrix3f::Identity());
331  makeParentRelativeRecurse(0, I, Vec3f());
332  }
333 
334  protected:
335  void deleteBVs();
336  bool allocateBVs();
337 
338  unsigned int num_bvs_allocated;
339  unsigned int* primitive_indices;
340 
343 
345  unsigned int num_bvs;
346 
348  int buildTree();
349 
351  int refitTree(bool bottomup);
352 
356 
360 
362  int recursiveBuildTree(int bv_id, unsigned int first_primitive,
363  unsigned int num_primitives);
364 
367 
371  void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
372  const Vec3f& parent_c) {
373  if (!bvs[bv_id].isLeaf()) {
374  makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes,
375  bvs[bv_id].getCenter());
376 
377  makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes,
378  bvs[bv_id].getCenter());
379  }
380 
381  bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
382  }
383 
384  private:
385  virtual bool isEqual(const CollisionGeometry& _other) const {
386  const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
387  if (other_ptr == nullptr) return false;
388  const BVHModel& other = *other_ptr;
389 
390  bool res = Base::isEqual(other);
391  if (!res) return false;
392 
393  // unsigned int other_num_primitives = 0;
394  // if(other.primitive_indices)
395  // {
396 
397  // switch(other.getModelType())
398  // {
399  // case BVH_MODEL_TRIANGLES:
400  // other_num_primitives = num_tris;
401  // break;
402  // case BVH_MODEL_POINTCLOUD:
403  // other_num_primitives = num_vertices;
404  // break;
405  // default:
406  // ;
407  // }
408  // }
409 
410  // unsigned int num_primitives = 0;
411  // if(primitive_indices)
412  // {
413  //
414  // switch(other.getModelType())
415  // {
416  // case BVH_MODEL_TRIANGLES:
417  // num_primitives = num_tris;
418  // break;
419  // case BVH_MODEL_POINTCLOUD:
420  // num_primitives = num_vertices;
421  // break;
422  // default:
423  // ;
424  // }
425  // }
426  //
427  // if(num_primitives != other_num_primitives)
428  // return false;
429  //
430  // for(int k = 0; k < num_primitives; ++k)
431  // {
432  // if(primitive_indices[k] != other.primitive_indices[k])
433  // return false;
434  // }
435 
436  if (num_bvs != other.num_bvs) return false;
437 
438  for (unsigned int k = 0; k < num_bvs; ++k) {
439  if (bvs[k] != other.bvs[k]) return false;
440  }
441 
442  return true;
443  }
444 };
445 
447 
448 template <>
450  const Vec3f& parent_c);
451 
452 template <>
454  const Vec3f& parent_c);
455 
456 template <>
458  Matrix3f& parent_axes,
459  const Vec3f& parent_c);
460 
462 template <>
464 
465 template <>
467 
468 template <>
470 
471 template <>
473 
474 template <>
476 
477 template <>
478 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
479 
480 template <>
481 NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
482 
483 template <>
484 NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
485 
486 } // namespace fcl
487 
488 } // namespace hpp
489 
490 #endif
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH_model.h:63
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:72
unsigned int num_tris
Number of triangles.
Definition: BVH_model.h:75
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.
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:69
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:261
int updateVertex(const Vec3f &p)
Update one point in the old BVH model.
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...
virtual bool isEqual(const CollisionGeometry &other) const
for ccd vertex update
BVHBuildState build_state
The state of BVH building process.
Definition: BVH_model.h:81
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:262
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model.h:87
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:103
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:66
unsigned int num_vertices
Number of points.
Definition: BVH_model.h:78
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH_model.h:230
unsigned int num_tris_allocated
Definition: BVH_model.h:260
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.
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH_model.h:84
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:218
OBJECT_TYPE getObjectType() const
Get the object type: it is a BVH.
Definition: BVH_model.h:110
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:203
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:273
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model.h:296
int refitTree_bottomup()
Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)
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:317
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
Definition: BVH_model.h:329
unsigned int * primitive_indices
Definition: BVH_model.h:339
BVNode< BV > * bvs
Bounding volume hierarchy.
Definition: BVH_model.h:342
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:281
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:278
void makeParentRelativeRecurse(int bv_id, Matrix3f &parent_axes, const Vec3f &parent_c)
Definition: BVH_model.h:371
unsigned int num_bvs_allocated
Definition: BVH_model.h:338
BVHModel()
Default constructor to build an empty BVH.
unsigned int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:345
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH_model.h:293
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:320
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:311
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:305
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
Triangle with 3 indices for points.
Definition: data_types.h:96
#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:113
@ OT_BVH
Definition: collision_object.h:55
@ BV_UNKNOWN
Definition: collision_object.h:66
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:50
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
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:53
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
BVHModelType
BVH model type.
Definition: BVH_internal.h:80
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:83
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:81
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:82
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)
Definition: tools.h:209
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:109