hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
48namespace hpp {
49namespace fcl {
50
53
54class ConvexBase;
55
56template <typename BV>
57class BVFitter;
58template <typename BV>
59class 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)
90 else if (num_vertices)
92 else
93 return BVH_MODEL_UNKNOWN;
94 }
95
98
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
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
272template <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::Zero());
332 }
333
334 protected:
335 void deleteBVs();
337
338 unsigned int num_bvs_allocated;
339 unsigned int* primitive_indices;
340
343
345 unsigned int num_bvs;
346
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
448template <>
450 const Vec3f& parent_c);
451
452template <>
454 const Vec3f& parent_c);
455
456template <>
458 Matrix3f& parent_axes,
459 const Vec3f& parent_c);
460
462template <>
464
465template <>
467
468template <>
470
471template <>
473
474template <>
476
477template <>
478NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
479
480template <>
481NODE_TYPE BVHModel<KDOP<18> >::getNodeType() const;
482
483template <>
484NODE_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)
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
BVHModel(const BVHModel &other)
Copy constructor from another BVH.
virtual BVHModel< BV > * clone() const
Clone *this into a new BVHModel.
Definition: BVH_model.h:293
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
BVNode< BV > & getBV(unsigned int i)
Access the bv giving the its index.
Definition: BVH_model.h:311
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
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)
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.
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:88
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