hpp-fcl  1.7.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  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 
100  BVHModelBase();
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 
126  void computeLocalAABB();
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 
153  int beginReplaceModel();
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 
169  int beginUpdateModel();
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 
288  BVHModel();
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 
427  int refitTree_topdown();
428 
430  int refitTree_bottomup();
431 
433  int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);
434 
436  int recursiveRefitTree_bottomup(int bv_id);
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
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:74
bool operator!=(const BVHModelBase &other) const
Difference operator.
Definition: BVH_model.h:117
FCL_REAL computeVolume() const
compute the volume
Definition: BVH_model.h:223
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH_model.h:86
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
Main namespace.
Definition: AABB.h:43
BVNode< BV > & getBV(int id)
Access the bv giving the its index.
Definition: BVH_model.h:381
int num_tris_allocated
Definition: BVH_model.h:268
Definition: collision_object.h:58
BV bv
bounding volume storing the geometry
Definition: BV_node.h:114
Definition: BVH_internal.h:79
shared_ptr< BVFitter< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Definition: BVH_model.h:285
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
int num_tris
Number of triangles.
Definition: BVH_model.h:77
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH_model.h:294
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH_model.h:62
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
Vec3f computeCOM() const
compute center of mass
Definition: BVH_model.h:208
unknown model type
Definition: BVH_internal.h:80
OBJECT_TYPE getObjectType() const
Get the object type: it is a BVH.
Definition: BVH_model.h:123
void makeParentRelativeRecurse(int bv_id, Matrix3f &parent_axes, const Vec3f &parent_c)
Definition: BVH_model.h:440
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
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH_model.h:71
BVHBuildState
States for BVH construction empty->begun->processed ->replace_begun->processed -> ...
Definition: BVH_internal.h:52
Vec3f * vertices
Geometry point data.
Definition: BVH_model.h:68
double FCL_REAL
Definition: data_types.h:66
Eigen::Matrix< Eigen::DenseIndex, Eigen::Dynamic, 3 > Matrixx3i
Definition: data_types.h:70
int num_vertex_updated
Definition: BVH_model.h:270
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model.h:89
int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:418
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > Matrixx3f
Definition: data_types.h:69
BVHModelType
BVH model type.
Definition: BVH_internal.h:77
int num_bvs_allocated
Definition: BVH_model.h:411
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
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:109
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:276
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV&#39;s transform in world coordinate...
Definition: BVH_model.h:401
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model.h:297
int num_vertices_allocated
Definition: BVH_model.h:269
Triangle with 3 indices for points.
Definition: data_types.h:74
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
Definition: collision_object.h:58
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: BVH_model.h:394
bool operator!=(const BVHModel &other) const
Difference operator.
Definition: BVH_model.h:366
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH_model.h:236
Definition: collision_object.h:55
BVHBuildState build_state
The state of BVH building process.
Definition: BVH_model.h:83
BVNode< BV > * bvs
Bounding volume hierarchy.
Definition: BVH_model.h:415
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:282
The class for the default algorithm fitting a bounding volume to a set of points. ...
Definition: BVH_model.h:58
A class describing the split rule that splits each BV node.
Definition: BVH_model.h:59
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
bool operator==(const BVHModel &other) const
Equality operator.
Definition: BVH_model.h:304
The geometry for the object for collision or distance computation.
Definition: collision_object.h:65
virtual ~BVHModelBase()
deconstruction, delete mesh data related.
Definition: BVH_model.h:106
triangle model
Definition: BVH_internal.h:81
int num_vertices
Number of points.
Definition: BVH_model.h:80
#define HPP_FCL_DLLAPI
Definition: config.hh:64