hpp-fcl  1.4.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  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef HPP_FCL_BVH_MODEL_H
39 #define HPP_FCL_BVH_MODEL_H
40 
43 #include <hpp/fcl/BV/BV_node.h>
44 #include <vector>
45 #include <boost/shared_ptr.hpp>
46 #include <boost/noncopyable.hpp>
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 {
64 public:
67 
70 
73 
75  int num_tris;
76 
79 
82 
84  boost::shared_ptr< ConvexBase > convex;
85 
88  {
89  if(num_tris && num_vertices)
90  return BVH_MODEL_TRIANGLES;
91  else if(num_vertices)
92  return BVH_MODEL_POINTCLOUD;
93  else
94  return BVH_MODEL_UNKNOWN;
95  }
96 
98  BVHModelBase() : vertices(NULL),
99  tri_indices(NULL),
100  prev_vertices(NULL),
101  num_tris(0),
102  num_vertices(0),
103  build_state(BVH_BUILD_STATE_EMPTY),
104  num_tris_allocated(0),
105  num_vertices_allocated(0),
106  num_vertex_updated(0)
107  {
108  }
109 
111  BVHModelBase(const BVHModelBase& other);
112 
114  virtual ~BVHModelBase ()
115  {
116  delete [] vertices;
117  delete [] tri_indices;
118  delete [] prev_vertices;
119  }
120 
122  OBJECT_TYPE getObjectType() const { return OT_BVH; }
123 
125  void computeLocalAABB();
126 
128  int beginModel(int num_tris = 0, int num_vertices = 0);
129 
131  int addVertex(const Vec3f& p);
132 
134  int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
135 
137  int addSubModel(const std::vector<Vec3f>& ps, const std::vector<Triangle>& ts);
138 
140  int addSubModel(const std::vector<Vec3f>& ps);
141 
143  int endModel();
144 
146  int beginReplaceModel();
147 
149  int replaceVertex(const Vec3f& p);
150 
152  int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
153 
155  int replaceSubModel(const std::vector<Vec3f>& ps);
156 
158  int endReplaceModel(bool refit = true, bool bottomup = true);
159 
162  int beginUpdateModel();
163 
165  int updateVertex(const Vec3f& p);
166 
168  int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
169 
171  int updateSubModel(const std::vector<Vec3f>& ps);
172 
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(int msg) const = 0;
196 
199  virtual void makeParentRelative() = 0;
200 
202  {
203  FCL_REAL vol = 0;
204  Vec3f com(0,0,0);
205  for(int i = 0; i < num_tris; ++i)
206  {
207  const Triangle& tri = tri_indices[i];
208  FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
209  vol += d_six_vol;
210  com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
211  }
212 
213  return com / (vol * 4);
214  }
215 
217  {
218  FCL_REAL vol = 0;
219  for(int i = 0; i < num_tris; ++i)
220  {
221  const Triangle& tri = tri_indices[i];
222  FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
223  vol += d_six_vol;
224  }
225 
226  return vol / 6;
227  }
228 
230  {
231  Matrix3f C = Matrix3f::Zero();
232 
233  Matrix3f C_canonical;
234  C_canonical << 1/60.0, 1/120.0, 1/120.0,
235  1/120.0, 1/60.0, 1/120.0,
236  1/120.0, 1/120.0, 1/60.0;
237 
238  for(int i = 0; i < num_tris; ++i)
239  {
240  const Triangle& tri = tri_indices[i];
241  const Vec3f& v1 = vertices[tri[0]];
242  const Vec3f& v2 = vertices[tri[1]];
243  const Vec3f& v3 = vertices[tri[2]];
244  Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
245  C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
246  }
247 
248  return C.trace() * Matrix3f::Identity() - C;
249  }
250 
251 protected:
252  virtual void deleteBVs() = 0;
253  virtual bool allocateBVs() = 0;
254 
256  virtual int buildTree() = 0;
257 
259  virtual int refitTree(bool bottomup) = 0;
260 
264 };
265 
268 template<typename BV>
270 {
271 
272 public:
274  boost::shared_ptr<BVSplitter<BV> > bv_splitter;
275 
277  boost::shared_ptr<BVFitter<BV> > bv_fitter;
278 
280  BVHModel();
281 
283  BVHModel(const BVHModel& other);
284 
287  {
288  delete [] bvs;
289  delete [] primitive_indices;
290  }
291 
293 
295  const BVNode<BV>& getBV(int id) const
296  {
297  assert (id < num_bvs);
298  return bvs[id];
299  }
300 
302  BVNode<BV>& getBV(int id)
303  {
304  assert (id < num_bvs);
305  return bvs[id];
306  }
307 
309  int getNumBVs() const
310  {
311  return num_bvs;
312  }
313 
315  NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
316 
318  int memUsage(int msg) const;
319 
323  {
324  Matrix3f I (Matrix3f::Identity());
325  makeParentRelativeRecurse(0, I, Vec3f());
326  }
327 
328 private:
329  void deleteBVs();
330  bool allocateBVs();
331 
332  int num_bvs_allocated;
333  unsigned int* primitive_indices;
334 
336  BVNode<BV>* bvs;
337 
339  int num_bvs;
340 
342  int buildTree();
343 
345  int refitTree(bool bottomup);
346 
348  int refitTree_topdown();
349 
351  int refitTree_bottomup();
352 
354  int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives);
355 
357  int recursiveRefitTree_bottomup(int bv_id);
358 
361  void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)
362  {
363  if(!bvs[bv_id].isLeaf())
364  {
365  makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter());
366 
367  makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter());
368  }
369 
370  bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
371  }
372 };
373 
375 
376 template<>
377 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
378 
379 template<>
380 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
381 
382 template<>
383 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
384 
385 
387 template<>
389 
390 template<>
392 
393 template<>
395 
396 template<>
398 
399 template<>
401 
402 template<>
403 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
404 
405 template<>
406 NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
407 
408 template<>
409 NODE_TYPE BVHModel<KDOP<24> >::getNodeType() const;
410 
411 }
412 
413 } // namespace hpp
414 
415 #endif
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:72
FCL_REAL computeVolume() const
compute the volume
Definition: BVH_model.h:216
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
Main namespace.
Definition: AABB.h:43
BVNode< BV > & getBV(int id)
Access the bv giving the its index.
Definition: BVH_model.h:302
int num_tris_allocated
Definition: BVH_model.h:261
Definition: collision_object.h:56
BV bv
bounding volume storing the geometry
Definition: BV_node.h:90
Definition: BVH_internal.h:79
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:75
int num_tris
Number of triangles.
Definition: BVH_model.h:75
boost::shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH_model.h:84
Definition: BVH_internal.h:54
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) HPP_FCL_DLLAPI
translate the KDOP BV
boost::shared_ptr< BVFitter< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Definition: BVH_model.h:277
Vec3f computeCOM() const
compute center of mass
Definition: BVH_model.h:201
unknown model type
Definition: BVH_internal.h:80
OBJECT_TYPE getObjectType() const
Get the object type: it is a BVH.
Definition: BVH_model.h:122
int getNumBVs() const
Get the number of bv in the BVH.
Definition: BVH_model.h:309
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH_model.h:69
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:66
double FCL_REAL
Definition: data_types.h:69
int num_vertex_updated
Definition: BVH_model.h:263
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model.h:87
BVHModelType
BVH model type.
Definition: BVH_internal.h:77
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:295
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:87
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:269
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV&#39;s transform in world coordinate...
Definition: BVH_model.h:322
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model.h:286
int num_vertices_allocated
Definition: BVH_model.h:262
Triangle with 3 indices for points.
Definition: data_types.h:79
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:56
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
Definition: BVH_model.h:315
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: BVH_model.h:229
Definition: collision_object.h:53
BVHBuildState build_state
The state of BVH building process.
Definition: BVH_model.h:81
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:74
boost::shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:274
The geometry for the object for collision or distance computation.
Definition: collision_object.h:63
virtual ~BVHModelBase()
deconstruction, delete mesh data related.
Definition: BVH_model.h:114
triangle model
Definition: BVH_internal.h:81
int num_vertices
Number of points.
Definition: BVH_model.h:78
BVHModelBase()
Constructing an empty BVH.
Definition: BVH_model.h:98
#define HPP_FCL_DLLAPI
Definition: config.hh:64