coal  3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, 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 COAL_BVH_MODEL_H
40 #define COAL_BVH_MODEL_H
41 
42 #include "coal/fwd.hh"
43 #include "coal/collision_object.h"
44 #include "coal/BVH/BVH_internal.h"
45 #include "coal/BV/BV_node.h"
46 
47 #include <vector>
48 #include <memory>
49 #include <iostream>
50 
51 namespace coal {
52 
55 
56 class ConvexBase;
57 
58 template <typename BV>
59 class BVFitter;
60 template <typename BV>
61 class BVSplitter;
62 
66  public:
68  std::shared_ptr<std::vector<Vec3s>> vertices;
69 
71  std::shared_ptr<std::vector<Triangle>> tri_indices;
72 
74  std::shared_ptr<std::vector<Vec3s>> prev_vertices;
75 
77  unsigned int num_tris;
78 
80  unsigned int num_vertices;
81 
84 
86  shared_ptr<ConvexBase> convex;
87 
90  if (num_tris && num_vertices)
91  return BVH_MODEL_TRIANGLES;
92  else if (num_vertices)
93  return BVH_MODEL_POINTCLOUD;
94  else
95  return BVH_MODEL_UNKNOWN;
96  }
97 
100 
102  BVHModelBase(const BVHModelBase& other);
103 
105  virtual ~BVHModelBase() {}
106 
108  OBJECT_TYPE getObjectType() const { return OT_BVH; }
109 
112 
114  int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
115 
117  int addVertex(const Vec3s& p);
118 
120  int addVertices(const MatrixX3s& points);
121 
123  int addTriangles(const Matrixx3i& triangles);
124 
126  int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
127 
129  int addSubModel(const std::vector<Vec3s>& ps,
130  const std::vector<Triangle>& ts);
131 
133  int addSubModel(const std::vector<Vec3s>& ps);
134 
137  int endModel();
138 
142 
144  int replaceVertex(const Vec3s& p);
145 
147  int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
148 
150  int replaceSubModel(const std::vector<Vec3s>& ps);
151 
154  int endReplaceModel(bool refit = true, bool bottomup = true);
155 
160 
162  int updateVertex(const Vec3s& p);
163 
165  int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
166 
168  int updateSubModel(const std::vector<Vec3s>& ps);
169 
172  int endUpdateModel(bool refit = true, bool bottomup = true);
173 
178  void buildConvexRepresentation(bool share_memory);
179 
191  bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
192 
193  virtual int memUsage(const bool msg = false) const = 0;
194 
199  virtual void makeParentRelative() = 0;
200 
201  Vec3s computeCOM() const {
202  Scalar vol = 0;
203  Vec3s com(0, 0, 0);
204  if (!(vertices.get())) {
205  std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
206  "vertices."
207  << std::endl;
208  return com;
209  }
210  const std::vector<Vec3s>& vertices_ = *vertices;
211  if (!(tri_indices.get())) {
212  std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
213  "triangles."
214  << std::endl;
215  return com;
216  }
217  const std::vector<Triangle>& tri_indices_ = *tri_indices;
218 
219  for (unsigned int i = 0; i < num_tris; ++i) {
220  const Triangle& tri = tri_indices_[i];
221  Scalar d_six_vol =
222  (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
223  vol += d_six_vol;
224  com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) *
225  d_six_vol;
226  }
227 
228  return com / (vol * 4);
229  }
230 
232  Scalar vol = 0;
233  if (!(vertices.get())) {
234  std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
235  "vertices."
236  << std::endl;
237  return vol;
238  }
239  const std::vector<Vec3s>& vertices_ = *vertices;
240  if (!(tri_indices.get())) {
241  std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
242  "triangles."
243  << std::endl;
244  return vol;
245  }
246  const std::vector<Triangle>& tri_indices_ = *tri_indices;
247  for (unsigned int i = 0; i < num_tris; ++i) {
248  const Triangle& tri = tri_indices_[i];
249  Scalar d_six_vol =
250  (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
251  vol += d_six_vol;
252  }
253 
254  return vol / 6;
255  }
256 
258  Matrix3s C = Matrix3s::Zero();
259 
260  Matrix3s C_canonical;
261  C_canonical << Scalar(1 / 60.0), //
262  Scalar(1 / 120.0), //
263  Scalar(1 / 120.0), //
264  Scalar(1 / 120.0), //
265  Scalar(1 / 60.0), //
266  Scalar(1 / 120.0), //
267  Scalar(1 / 120.0), //
268  Scalar(1 / 120.0), //
269  Scalar(1 / 60.0);
270 
271  if (!(vertices.get())) {
272  std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
273  "not contain vertices."
274  << std::endl;
275  return C;
276  }
277  const std::vector<Vec3s>& vertices_ = *vertices;
278  if (!(vertices.get())) {
279  std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
280  "not contain vertices."
281  << std::endl;
282  return C;
283  }
284  const std::vector<Triangle>& tri_indices_ = *tri_indices;
285  for (unsigned int i = 0; i < num_tris; ++i) {
286  const Triangle& tri = tri_indices_[i];
287  const Vec3s& v1 = vertices_[tri[0]];
288  const Vec3s& v2 = vertices_[tri[1]];
289  const Vec3s& v3 = vertices_[tri[2]];
290  Matrix3s A;
291  A << v1.transpose(), v2.transpose(), v3.transpose();
292  C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
293  }
294 
295  return C.trace() * Matrix3s::Identity() - C;
296  }
297 
298  protected:
299  virtual void deleteBVs() = 0;
300  virtual bool allocateBVs() = 0;
301 
303  virtual int buildTree() = 0;
304 
306  virtual int refitTree(bool bottomup) = 0;
307 
308  unsigned int num_tris_allocated;
310  unsigned int num_vertex_updated;
311 
312  protected:
314  virtual bool isEqual(const CollisionGeometry& other) const;
315 };
316 
320 template <typename BV>
322  typedef BVHModelBase Base;
323 
324  public:
326  std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
327 
329  shared_ptr<BVSplitter<BV>> bv_splitter;
330 
332  shared_ptr<BVFitter<BV>> bv_fitter;
333 
336 
341  BVHModel(const BVHModel& other);
342 
344  virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
345 
348 
351 
353  const BVNode<BV>& getBV(unsigned int i) const {
354  assert(i < num_bvs);
355  return (*bvs)[i];
356  }
357 
359  BVNode<BV>& getBV(unsigned int i) {
360  assert(i < num_bvs);
361  return (*bvs)[i];
362  }
363 
365  unsigned int getNumBVs() const { return num_bvs; }
366 
368  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
369 
371  int memUsage(const bool msg) const;
372 
378  Matrix3s I(Matrix3s::Identity());
379  makeParentRelativeRecurse(0, I, Vec3s::Zero());
380  }
381 
382  protected:
383  void deleteBVs();
384  bool allocateBVs();
385 
386  unsigned int num_bvs_allocated;
387  std::shared_ptr<std::vector<unsigned int>> primitive_indices;
388 
390  std::shared_ptr<bv_node_vector_t> bvs;
391 
393  unsigned int num_bvs;
394 
396  int buildTree();
397 
399  int refitTree(bool bottomup);
400 
404 
408 
410  int recursiveBuildTree(int bv_id, unsigned int first_primitive,
411  unsigned int num_primitives);
412 
415 
419  void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
420  const Vec3s& parent_c) {
421  bv_node_vector_t& bvs_ = *bvs;
422  if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
423  makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
424  parent_axes,
425  bvs_[static_cast<size_t>(bv_id)].getCenter());
426 
427  makeParentRelativeRecurse(
428  bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
429  bvs_[static_cast<size_t>(bv_id)].getCenter());
430  }
431 
432  bvs_[static_cast<size_t>(bv_id)].bv =
433  translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
434  }
435 
436  private:
437  virtual bool isEqual(const CollisionGeometry& _other) const {
438  const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
439  if (other_ptr == nullptr) return false;
440  const BVHModel& other = *other_ptr;
441 
442  bool res = Base::isEqual(other);
443  if (!res) return false;
444 
445  // unsigned int other_num_primitives = 0;
446  // if(other.primitive_indices)
447  // {
448 
449  // switch(other.getModelType())
450  // {
451  // case BVH_MODEL_TRIANGLES:
452  // other_num_primitives = num_tris;
453  // break;
454  // case BVH_MODEL_POINTCLOUD:
455  // other_num_primitives = num_vertices;
456  // break;
457  // default:
458  // ;
459  // }
460  // }
461 
462  // unsigned int num_primitives = 0;
463  // if(primitive_indices)
464  // {
465  //
466  // switch(other.getModelType())
467  // {
468  // case BVH_MODEL_TRIANGLES:
469  // num_primitives = num_tris;
470  // break;
471  // case BVH_MODEL_POINTCLOUD:
472  // num_primitives = num_vertices;
473  // break;
474  // default:
475  // ;
476  // }
477  // }
478  //
479  // if(num_primitives != other_num_primitives)
480  // return false;
481  //
482  // for(int k = 0; k < num_primitives; ++k)
483  // {
484  // if(primitive_indices[k] != other.primitive_indices[k])
485  // return false;
486  // }
487 
488  if (num_bvs != other.num_bvs) return false;
489 
490  if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
491  return false;
492  if (bvs.get() && other.bvs.get()) {
493  const bv_node_vector_t& bvs_ = *bvs;
494  const bv_node_vector_t& other_bvs_ = *(other.bvs);
495  for (unsigned int k = 0; k < num_bvs; ++k) {
496  if (bvs_[k] != other_bvs_[k]) return false;
497  }
498  }
499 
500  return true;
501  }
502 };
503 
505 
506 template <>
508  const Vec3s& parent_c);
509 
510 template <>
512  const Vec3s& parent_c);
513 
514 template <>
516  Matrix3s& parent_axes,
517  const Vec3s& parent_c);
518 
520 template <>
522 
523 template <>
525 
526 template <>
528 
529 template <>
531 
532 template <>
534 
535 template <>
536 NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const;
537 
538 template <>
539 NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const;
540 
541 template <>
542 NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const;
543 
544 } // namespace coal
545 
546 #endif
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH_model.h:65
virtual void deleteBVs()=0
int endUpdateModel(bool refit=true, bool bottomup=true)
End BVH model update, will also refit or rebuild the bounding volume hierarchy.
OBJECT_TYPE getObjectType() const
Get the object type: it is a BVH.
Definition: BVH_model.h:108
unsigned int num_vertices
Number of points.
Definition: BVH_model.h:80
virtual int refitTree(bool bottomup)=0
Refit the bounding volume hierarchy.
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
int replaceTriangle(const Vec3s &p1, const Vec3s &p2, const Vec3s &p3)
Replace one triangle in the old BVH model.
std::shared_ptr< std::vector< Triangle > > tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH_model.h:71
virtual ~BVHModelBase()
deconstruction, delete mesh data related.
Definition: BVH_model.h:105
Scalar computeVolume() const
compute the volume
Definition: BVH_model.h:231
int addSubModel(const std::vector< Vec3s > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
int updateSubModel(const std::vector< Vec3s > &ps)
Update a set of points in the old BVH model.
BVHModelBase(const BVHModelBase &other)
copy from another BVH
virtual int memUsage(const bool msg=false) const =0
std::shared_ptr< std::vector< Vec3s > > prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:74
int addSubModel(const std::vector< Vec3s > &ps)
Add a set of points in the new BVH model.
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
unsigned int num_tris_allocated
Definition: BVH_model.h:308
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Matrix3s computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH_model.h:257
int replaceVertex(const Vec3s &p)
Replace one point in the old BVH model.
int updateTriangle(const Vec3s &p1, const Vec3s &p2, const Vec3s &p3)
Update one triangle in the old BVH model.
virtual bool allocateBVs()=0
int beginReplaceModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
int addTriangle(const Vec3s &p1, const Vec3s &p2, const Vec3s &p3)
Add one triangle in the new BVH model.
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....
int addTriangles(const Matrixx3i &triangles)
Add triangles in the new BVH model.
void buildConvexRepresentation(bool share_memory)
Build this Convex<Triangle> representation of this model. The result is stored in attribute convex.
virtual bool isEqual(const CollisionGeometry &other) const
for ccd vertex update
bool buildConvexHull(bool keepTriangle, const char *qhullCommand=NULL)
Build a convex hull and store it in attribute convex.
virtual int buildTree()=0
Build the bounding volume hierarchy.
int addVertex(const Vec3s &p)
Add one point 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...
unsigned int num_tris
Number of triangles.
Definition: BVH_model.h:77
BVHModelBase()
Constructing an empty BVH.
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:309
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model.h:89
std::shared_ptr< std::vector< Vec3s > > vertices
Geometry point data.
Definition: BVH_model.h:68
int replaceSubModel(const std::vector< Vec3s > &ps)
Replace a set of points in the old BVH model.
Vec3s computeCOM() const
compute center of mass
Definition: BVH_model.h:201
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH_model.h:86
unsigned int num_vertex_updated
Definition: BVH_model.h:310
int addVertices(const MatrixX3s &points)
Add points in the new BVH model.
int updateVertex(const Vec3s &p)
Update one point in the old BVH model.
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:321
std::shared_ptr< bv_node_vector_t > bvs
Bounding volume hierarchy.
Definition: BVH_model.h:390
unsigned int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:393
std::shared_ptr< std::vector< unsigned int > > primitive_indices
Definition: BVH_model.h:387
int refitTree_bottomup()
Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model.h:347
std::vector< BVNode< BV >, Eigen::aligned_allocator< BVNode< BV > >> bv_node_vector_t
Definition: BVH_model.h:326
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:353
int refitTree_topdown()
Refit the bounding volume hierarchy in a top-down way (slow but more compact)
void makeParentRelativeRecurse(int bv_id, Matrix3s &parent_axes, const Vec3s &parent_c)
Definition: BVH_model.h:419
BVNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: BVH_model.h:359
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:368
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
Definition: BVH_model.h:377
BVHModel()
Default constructor to build an empty BVH.
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:329
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH_model.h:344
int recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives)
Recursive kernel for hierarchy construction.
bool allocateBVs()
int buildTree()
Build the bounding volume hierarchy.
int recursiveRefitTree_bottomup(int bv_id)
Recursive kernel for bottomup refitting.
shared_ptr< BVFitter< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Definition: BVH_model.h:332
unsigned int num_bvs_allocated
Definition: BVH_model.h:386
unsigned int getNumBVs() const
Get the number of bv in the BVH.
Definition: BVH_model.h:365
BVHModel(const BVHModel &other)
Copy constructor from another BVH.
The geometry for the object for collision or distance computation.
Definition: collision_object.h:94
Triangle with 3 indices for points.
Definition: data_types.h:121
#define COAL_DLLAPI
Definition: config.hh:88
KDOP< N > translate(const KDOP< N > &bv, const Vec3s &t)
translate the KDOP BV
@ BV_UNKNOWN
Definition: collision_object.h:65
@ OT_BVH
Definition: collision_object.h:54
Main namespace.
Definition: broadphase_bruteforce.h:44
BVHModelType
BVH model type.
Definition: BVH_internal.h:79
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:82
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:81
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:80
BVHBuildState
States for BVH construction empty->begun->processed ->replace_begun->processed -> ....
Definition: BVH_internal.h:49
Eigen::Matrix< Scalar, Eigen::Dynamic, 3, Eigen::RowMajor > MatrixX3s
Definition: data_types.h:75
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:64
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition: data_types.h:70
double Scalar
Definition: data_types.h:68
Eigen::Matrix< Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor > Matrixx3i
Definition: data_types.h:78
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:52
Eigen::Matrix< Scalar, 3, 3 > Matrix3s
Definition: data_types.h:74
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const Scalar tol=std::numeric_limits< Scalar >::epsilon() *100)
Definition: tools.h:204
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:106