BVH/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 
63 class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
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 
97  BVHModelBase();
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 
113  void computeLocalAABB();
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 
143  int beginReplaceModel();
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 
161  int beginUpdateModel();
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>
273 class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
275 
276  public:
278  shared_ptr<BVSplitter<BV> > bv_splitter;
279 
281  shared_ptr<BVFitter<BV> > bv_fitter;
282 
284  BVHModel();
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::Zero());
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 
355  int refitTree_topdown();
356 
359  int refitTree_bottomup();
360 
362  int recursiveBuildTree(int bv_id, unsigned int first_primitive,
363  unsigned int num_primitives);
364 
366  int recursiveRefitTree_bottomup(int bv_id);
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 <>
449 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
450  const Vec3f& parent_c);
451 
452 template <>
453 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
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
hpp::fcl::BVHModel::num_bvs_allocated
unsigned int num_bvs_allocated
Definition: BVH/BVH_model.h:338
hpp::fcl::isEqual
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
BVH_internal.h
hpp::fcl::BVHModel::isEqual
virtual bool isEqual(const CollisionGeometry &_other) const
for ccd vertex update
Definition: BVH/BVH_model.h:385
hpp::fcl::BVHModel::getBV
BVNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: BVH/BVH_model.h:311
hpp::fcl::BVHModelBase::build_state
BVHBuildState build_state
The state of BVH building process.
Definition: BVH/BVH_model.h:81
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
BV_node.h
collision_object.h
hpp::fcl::BVHModelBase::computeCOM
Vec3f computeCOM() const
compute center of mass
Definition: BVH/BVH_model.h:203
hpp::fcl::BVHModel::bv_fitter
shared_ptr< BVFitter< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Definition: BVH/BVH_model.h:281
hpp::fcl::Matrixx3i
Eigen::Matrix< Eigen::DenseIndex, Eigen::Dynamic, 3 > Matrixx3i
Definition: data_types.h:70
hpp::fcl::BVHModelBase::~BVHModelBase
virtual ~BVHModelBase()
deconstruction, delete mesh data related.
Definition: BVH/BVH_model.h:103
hpp::fcl::BVHModel::num_bvs
unsigned int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH/BVH_model.h:345
hpp::fcl::BVHModelBase::num_vertices_allocated
unsigned int num_vertices_allocated
Definition: BVH/BVH_model.h:261
hpp::fcl::BVHModelBase::convex
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH/BVH_model.h:84
hpp::fcl::BVH_MODEL_UNKNOWN
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:81
hpp::fcl::BVHModelBase::num_vertices
unsigned int num_vertices
Number of points.
Definition: BVH/BVH_model.h:78
hpp::fcl::BVHModel::bvs
BVNode< BV > * bvs
Bounding volume hierarchy.
Definition: BVH/BVH_model.h:342
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
hpp::fcl::BVHModelBase::tri_indices
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH/BVH_model.h:69
hpp::fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
hpp::fcl::OT_BVH
@ OT_BVH
Definition: collision_object.h:55
hpp::fcl::BVHModel::bv_splitter
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH/BVH_model.h:278
res
res
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
hpp::fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:82
hpp::fcl::BVNode::bv
BV bv
bounding volume storing the geometry
Definition: BV/BV_node.h:113
hpp::fcl::BVHModelBase::prev_vertices
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH/BVH_model.h:72
hpp::fcl::BVHModel::getNodeType
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: BVH/BVH_model.h:320
hpp::fcl::BVNode
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV/BV_node.h:109
hpp::fcl::BVHBuildState
BVHBuildState
States for BVH construction empty->begun->processed ->replace_begun->processed -> ....
Definition: BVH_internal.h:50
hpp::fcl::BVHModelBase::getModelType
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH/BVH_model.h:87
hpp::fcl::BVHModelType
BVHModelType
BVH model type.
Definition: BVH_internal.h:80
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::BVHModelBase::num_vertex_updated
unsigned int num_vertex_updated
Definition: BVH/BVH_model.h:262
hpp::fcl::BVFitter
The class for the default algorithm fitting a bounding volume to a set of points.
Definition: BVH/BVH_model.h:57
hpp::fcl::BVHModelBase::computeMomentofInertia
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH/BVH_model.h:230
hpp::fcl::BVHModel::makeParentRelativeRecurse
void makeParentRelativeRecurse(int bv_id, Matrix3f &parent_axes, const Vec3f &parent_c)
Definition: BVH/BVH_model.h:371
hpp::fcl::BVHModel::primitive_indices
unsigned int * primitive_indices
Definition: BVH/BVH_model.h:339
octree.p1
tuple p1
Definition: octree.py:54
A
A
hpp::fcl::BVHModel::getNumBVs
unsigned int getNumBVs() const
Get the number of bv in the BVH.
Definition: BVH/BVH_model.h:317
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::BVHModelBase::num_tris_allocated
unsigned int num_tris_allocated
Definition: BVH/BVH_model.h:260
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BVH/BVH_model.h:59
hpp::fcl::BVHModel::makeParentRelative
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV's transform in world coordinate....
Definition: BVH/BVH_model.h:329
hpp::fcl::BVHModel::~BVHModel
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH/BVH_model.h:296
hpp::fcl::Matrixx3f
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > Matrixx3f
Definition: data_types.h:69
fwd.hh
hpp::fcl::BVHModelBase::getObjectType
OBJECT_TYPE getObjectType() const
Get the object type: it is a BVH.
Definition: BVH/BVH_model.h:110
hpp::fcl::BVHModelBase
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH/BVH_model.h:63
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:96
hpp::fcl::BVHModel::clone
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH/BVH_model.h:293
hpp::fcl::BVHModelBase::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: BVH/BVH_model.h:218
hpp::fcl::BVH_MODEL_POINTCLOUD
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:83
hpp::fcl::translate
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
Definition: BV/AABB.h:226
hpp::fcl::BVHModelBase::vertices
Vec3f * vertices
Geometry point data.
Definition: BVH/BVH_model.h:66
hpp::fcl::BVHModelBase::num_tris
unsigned int num_tris
Number of triangles.
Definition: BVH/BVH_model.h:75
hpp::fcl::BVHModel::getBV
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/BVH_model.h:305
hpp::fcl::BV_UNKNOWN
@ BV_UNKNOWN
Definition: collision_object.h:66
hpp::fcl::BVHModel::Base
BVHModelBase Base
Definition: BVH/BVH_model.h:274


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13