GCC Code Coverage Report


Directory: ./
File: include/coal/BVH/BVH_model.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 26 113 23.0%
Branches: 20 178 11.2%

Line Branch Exec Source
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
37 /** \author Jia Pan */
38
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
53 /// @addtogroup Construction_Of_BVH
54 /// @{
55
56 class ConvexBase;
57
58 template <typename BV>
59 class BVFitter;
60 template <typename BV>
61 class BVSplitter;
62
63 /// @brief A base class describing the bounding hierarchy of a mesh model or a
64 /// point cloud model (which is viewed as a degraded version of mesh)
65 class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
66 public:
67 /// @brief Geometry point data
68 std::shared_ptr<std::vector<Vec3s>> vertices;
69
70 /// @brief Geometry triangle index data, will be NULL for point clouds
71 std::shared_ptr<std::vector<Triangle>> tri_indices;
72
73 /// @brief Geometry point data in previous frame
74 std::shared_ptr<std::vector<Vec3s>> prev_vertices;
75
76 /// @brief Number of triangles
77 unsigned int num_tris;
78
79 /// @brief Number of points
80 unsigned int num_vertices;
81
82 /// @brief The state of BVH building process
83 BVHBuildState build_state;
84
85 /// @brief Convex<Triangle> representation of this object
86 shared_ptr<ConvexBase> convex;
87
88 /// @brief Model type described by the instance
89 2347558 BVHModelType getModelType() const {
90
3/4
✓ Branch 0 taken 2347414 times.
✓ Branch 1 taken 144 times.
✓ Branch 2 taken 2347414 times.
✗ Branch 3 not taken.
2347558 if (num_tris && num_vertices)
91 2347414 return BVH_MODEL_TRIANGLES;
92
1/2
✓ Branch 0 taken 144 times.
✗ Branch 1 not taken.
144 else if (num_vertices)
93 144 return BVH_MODEL_POINTCLOUD;
94 else
95 return BVH_MODEL_UNKNOWN;
96 }
97
98 /// @brief Constructing an empty BVH
99 BVHModelBase();
100
101 /// @brief copy from another BVH
102 BVHModelBase(const BVHModelBase& other);
103
104 /// @brief deconstruction, delete mesh data related.
105 6176 virtual ~BVHModelBase() {}
106
107 /// @brief Get the object type: it is a BVH
108 35022 OBJECT_TYPE getObjectType() const { return OT_BVH; }
109
110 /// @brief Compute the AABB for the BVH, used for broad-phase collision
111 void computeLocalAABB();
112
113 /// @brief Begin a new BVH model
114 int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
115
116 /// @brief Add one point in the new BVH model
117 int addVertex(const Vec3s& p);
118
119 /// @brief Add points in the new BVH model
120 int addVertices(const MatrixX3s& points);
121
122 /// @brief Add triangles in the new BVH model
123 int addTriangles(const Matrixx3i& triangles);
124
125 /// @brief Add one triangle in the new BVH model
126 int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
127
128 /// @brief Add a set of triangles in the new BVH model
129 int addSubModel(const std::vector<Vec3s>& ps,
130 const std::vector<Triangle>& ts);
131
132 /// @brief Add a set of points in the new BVH model
133 int addSubModel(const std::vector<Vec3s>& ps);
134
135 /// @brief End BVH model construction, will build the bounding volume
136 /// hierarchy
137 int endModel();
138
139 /// @brief Replace the geometry information of current frame (i.e. should have
140 /// the same mesh topology with the previous frame)
141 int beginReplaceModel();
142
143 /// @brief Replace one point in the old BVH model
144 int replaceVertex(const Vec3s& p);
145
146 /// @brief Replace one triangle in the old BVH model
147 int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
148
149 /// @brief Replace a set of points in the old BVH model
150 int replaceSubModel(const std::vector<Vec3s>& ps);
151
152 /// @brief End BVH model replacement, will also refit or rebuild the bounding
153 /// volume hierarchy
154 int endReplaceModel(bool refit = true, bool bottomup = true);
155
156 /// @brief Replace the geometry information of current frame (i.e. should have
157 /// the same mesh topology with the previous frame). The current frame will be
158 /// saved as the previous frame in prev_vertices.
159 int beginUpdateModel();
160
161 /// @brief Update one point in the old BVH model
162 int updateVertex(const Vec3s& p);
163
164 /// @brief Update one triangle in the old BVH model
165 int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
166
167 /// @brief Update a set of points in the old BVH model
168 int updateSubModel(const std::vector<Vec3s>& ps);
169
170 /// @brief End BVH model update, will also refit or rebuild the bounding
171 /// volume hierarchy
172 int endUpdateModel(bool refit = true, bool bottomup = true);
173
174 /// @brief Build this \ref Convex "Convex<Triangle>" representation of this
175 /// model. The result is stored in attribute \ref convex. \note this only
176 /// takes the points of this model. It does not check that the
177 /// object is convex. It does not compute a convex hull.
178 void buildConvexRepresentation(bool share_memory);
179
180 /// @brief Build a convex hull
181 /// and store it in attribute \ref convex.
182 /// \param keepTriangle whether the convex should be triangulated.
183 /// \param qhullCommand see \ref ConvexBase::convexHull.
184 /// \return \c true if this object is convex, hence the convex hull represents
185 /// the same object.
186 /// \sa ConvexBase::convexHull
187 /// \warning At the moment, the return value only checks whether there are as
188 /// many points in the convex hull as in the original object. This is
189 /// neither necessary (duplicated vertices get merged) nor sufficient
190 /// (think of a U with 4 vertices and 3 edges).
191 bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
192
193 virtual int memUsage(const bool msg = false) const = 0;
194
195 /// @brief This is a special acceleration: BVH_model default stores the BV's
196 /// transform in world coordinate. However, we can also store each BV's
197 /// transform related to its parent BV node. When traversing the BVH, this can
198 /// save one matrix transformation.
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
231 Scalar computeVolume() const {
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
257 Matrix3s computeMomentofInertia() const {
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
302 /// @brief Build the bounding volume hierarchy
303 virtual int buildTree() = 0;
304
305 /// @brief Refit the bounding volume hierarchy
306 virtual int refitTree(bool bottomup) = 0;
307
308 unsigned int num_tris_allocated;
309 unsigned int num_vertices_allocated;
310 unsigned int num_vertex_updated; /// for ccd vertex update
311
312 protected:
313 /// \brief Comparison operators
314 virtual bool isEqual(const CollisionGeometry& other) const;
315 };
316
317 /// @brief A class describing the bounding hierarchy of a mesh model or a point
318 /// cloud model (which is viewed as a degraded version of mesh) \tparam BV one
319 /// of the bounding volume class in \ref Bounding_Volume.
320 template <typename BV>
321 class COAL_DLLAPI BVHModel : public BVHModelBase {
322 typedef BVHModelBase Base;
323
324 public:
325 using bv_node_vector_t =
326 std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
327
328 /// @brief Split rule to split one BV node into two children
329 shared_ptr<BVSplitter<BV>> bv_splitter;
330
331 /// @brief Fitting rule to fit a BV node to a set of geometry primitives
332 shared_ptr<BVFitter<BV>> bv_fitter;
333
334 /// @brief Default constructor to build an empty BVH
335 BVHModel();
336
337 /// @brief Copy constructor from another BVH
338 ///
339 /// \param[in] other BVHModel to copy.
340 ///
341 BVHModel(const BVHModel& other);
342
343 /// @brief Clone *this into a new BVHModel
344 virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
345
346 /// @brief deconstruction, delete mesh data related.
347 11770 ~BVHModel() {}
348
349 /// @brief We provide getBV() and getNumBVs() because BVH may be compressed
350 /// (in future), so we must provide some flexibility here
351
352 /// @brief Access the bv giving the its index
353 640482814 const BVNode<BV>& getBV(unsigned int i) const {
354
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 287578582 times.
640482814 assert(i < num_bvs);
355 640482814 return (*bvs)[i];
356 }
357
358 /// @brief Access the bv giving the its index
359 BVNode<BV>& getBV(unsigned int i) {
360 assert(i < num_bvs);
361 return (*bvs)[i];
362 }
363
364 /// @brief Get the number of bv in the BVH
365 32 unsigned int getNumBVs() const { return num_bvs; }
366
367 /// @brief Get the BV type: default is unknown
368 NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
369
370 /// @brief Check the number of memory used
371 int memUsage(const bool msg) const;
372
373 /// @brief This is a special acceleration: BVH_model default stores the BV's
374 /// transform in world coordinate. However, we can also store each BV's
375 /// transform related to its parent BV node. When traversing the BVH, this can
376 /// save one matrix transformation.
377 void makeParentRelative() {
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
389 /// @brief Bounding volume hierarchy
390 std::shared_ptr<bv_node_vector_t> bvs;
391
392 /// @brief Number of BV nodes in bounding volume hierarchy
393 unsigned int num_bvs;
394
395 /// @brief Build the bounding volume hierarchy
396 int buildTree();
397
398 /// @brief Refit the bounding volume hierarchy
399 int refitTree(bool bottomup);
400
401 /// @brief Refit the bounding volume hierarchy in a top-down way (slow but
402 /// more compact)
403 int refitTree_topdown();
404
405 /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but
406 /// less compact)
407 int refitTree_bottomup();
408
409 /// @brief Recursive kernel for hierarchy construction
410 int recursiveBuildTree(int bv_id, unsigned int first_primitive,
411 unsigned int num_primitives);
412
413 /// @brief Recursive kernel for bottomup refitting
414 int recursiveRefitTree_bottomup(int bv_id);
415
416 /// @ recursively compute each bv's transform related to its parent. For
417 /// default BV, only the translation works. For oriented BV (OBB, RSS,
418 /// OBBRSS), special implementation is provided.
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 21 virtual bool isEqual(const CollisionGeometry& _other) const {
438
1/2
✓ Branch 0 taken 21 times.
✗ Branch 1 not taken.
21 const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
439
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
21 if (other_ptr == nullptr) return false;
440 21 const BVHModel& other = *other_ptr;
441
442 21 bool res = Base::isEqual(other);
443
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 20 times.
21 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
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
20 if (num_bvs != other.num_bvs) return false;
489
490
4/10
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 20 times.
20 if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
491 return false;
492
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
20 if (bvs.get() && other.bvs.get()) {
493 20 const bv_node_vector_t& bvs_ = *bvs;
494 20 const bv_node_vector_t& other_bvs_ = *(other.bvs);
495
2/2
✓ Branch 0 taken 83252 times.
✓ Branch 1 taken 20 times.
83272 for (unsigned int k = 0; k < num_bvs; ++k) {
496
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 83252 times.
83252 if (bvs_[k] != other_bvs_[k]) return false;
497 }
498 }
499
500 20 return true;
501 }
502 };
503
504 /// @}
505
506 template <>
507 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
508 const Vec3s& parent_c);
509
510 template <>
511 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
512 const Vec3s& parent_c);
513
514 template <>
515 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
516 Matrix3s& parent_axes,
517 const Vec3s& parent_c);
518
519 /// @brief Specialization of getNodeType() for BVHModel with different BV types
520 template <>
521 NODE_TYPE BVHModel<AABB>::getNodeType() const;
522
523 template <>
524 NODE_TYPE BVHModel<OBB>::getNodeType() const;
525
526 template <>
527 NODE_TYPE BVHModel<RSS>::getNodeType() const;
528
529 template <>
530 NODE_TYPE BVHModel<kIOS>::getNodeType() const;
531
532 template <>
533 NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
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
547