GCC Code Coverage Report

Directory: ./
File: include/coal/BVH/BVH_model.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 26 115 22.6%
Functions: 28 61 45.9%
Branches: 20 188 10.6%
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 template <typename _IndexType>
57 class ConvexBaseTpl;
58
59 template <typename BV>
60 class BVFitter;
61 template <typename BV>
62 class BVSplitter;
63
64 /// @brief A base class describing the bounding hierarchy of a mesh model or a
65 /// point cloud model (which is viewed as a degraded version of mesh)
66 class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
67 public:
68 typedef typename Triangle32::IndexType IndexType;
69 typedef ConvexBaseTpl<IndexType> ConvexType;
70
71 /// @brief Geometry point data
72 std::shared_ptr<std::vector<Vec3s>> vertices;
73
74 /// @brief Geometry triangle index data, will be NULL for point clouds
75 std::shared_ptr<std::vector<Triangle32>> tri_indices;
76
77 /// @brief Geometry point data in previous frame
78 std::shared_ptr<std::vector<Vec3s>> prev_vertices;
79
80 /// @brief Number of triangles
81 unsigned int num_tris;
82
83 /// @brief Number of points
84 unsigned int num_vertices;
85
86 /// @brief The state of BVH building process
87 BVHBuildState build_state;
88
89 /// @brief Convex<Triangle> representation of this object
90 // TODO: deprecated
91 shared_ptr<ConvexType> convex;
92
93 /// @brief Model type described by the instance
94 2347548 BVHModelType getModelType() const {
95
3/4
✓ Branch 0 taken 2347404 times.
✓ Branch 1 taken 144 times.
✓ Branch 2 taken 2347404 times.
✗ Branch 3 not taken.
 2347548 if (num_tris && num_vertices)
96 2347404 return BVH_MODEL_TRIANGLES;
97
1/2
✓ Branch 0 taken 144 times.
✗ Branch 1 not taken.
 144 else if (num_vertices)
98 144 return BVH_MODEL_POINTCLOUD;
99 else
100 ✗ return BVH_MODEL_UNKNOWN;
101 }
102
103 /// @brief Constructing an empty BVH
104 BVHModelBase();
105
106 /// @brief copy from another BVH
107 BVHModelBase(const BVHModelBase& other);
108
109 /// @brief deconstruction, delete mesh data related.
110 6176 virtual ~BVHModelBase() {}
111
112 /// @brief Get the object type: it is a BVH
113 35012 OBJECT_TYPE getObjectType() const { return OT_BVH; }
114
115 /// @brief Compute the AABB for the BVH, used for broad-phase collision
116 void computeLocalAABB();
117
118 /// @brief Begin a new BVH model
119 int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
120
121 /// @brief Add one point in the new BVH model
122 int addVertex(const Vec3s& p);
123
124 /// @brief Add points in the new BVH model
125 int addVertices(const MatrixX3s& points);
126
127 /// @brief Add triangles in the new BVH model
128 int addTriangles(const Matrixx3i& triangles);
129
130 /// @brief Add one triangle in the new BVH model
131 int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
132
133 /// @brief Add a set of triangles in the new BVH model
134 int addSubModel(const std::vector<Vec3s>& ps,
135 const std::vector<Triangle32>& ts);
136
137 /// @brief Add a set of points in the new BVH model
138 int addSubModel(const std::vector<Vec3s>& ps);
139
140 /// @brief End BVH model construction, will build the bounding volume
141 /// hierarchy
142 int endModel();
143
144 /// @brief Replace the geometry information of current frame (i.e. should have
145 /// the same mesh topology with the previous frame)
146 int beginReplaceModel();
147
148 /// @brief Replace one point in the old BVH model
149 int replaceVertex(const Vec3s& p);
150
151 /// @brief Replace one triangle in the old BVH model
152 int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
153
154 /// @brief Replace a set of points in the old BVH model
155 int replaceSubModel(const std::vector<Vec3s>& ps);
156
157 /// @brief End BVH model replacement, will also refit or rebuild the bounding
158 /// volume hierarchy
159 int endReplaceModel(bool refit = true, bool bottomup = true);
160
161 /// @brief Replace the geometry information of current frame (i.e. should have
162 /// the same mesh topology with the previous frame). The current frame will be
163 /// saved as the previous frame in prev_vertices.
164 int beginUpdateModel();
165
166 /// @brief Update one point in the old BVH model
167 int updateVertex(const Vec3s& p);
168
169 /// @brief Update one triangle in the old BVH model
170 int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
171
172 /// @brief Update a set of points in the old BVH model
173 int updateSubModel(const std::vector<Vec3s>& ps);
174
175 /// @brief End BVH model update, will also refit or rebuild the bounding
176 /// volume hierarchy
177 int endUpdateModel(bool refit = true, bool bottomup = true);
178
179 /// @brief Build this \ref Convex "Convex<Triangle>" representation of this
180 /// model. The result is stored in attribute \ref convex. \note this only
181 /// takes the points of this model. It does not check that the
182 /// object is convex. It does not compute a convex hull.
183 void buildConvexRepresentation(bool share_memory);
184
185 /// @brief Build a convex hull
186 /// and store it in attribute \ref convex.
187 /// \param keepTriangle whether the convex should be triangulated.
188 /// \param qhullCommand see \ref ConvexBase::convexHull.
189 /// \return \c true if this object is convex, hence the convex hull represents
190 /// the same object.
191 /// \sa ConvexBase::convexHull
192 /// \warning At the moment, the return value only checks whether there are as
193 /// many points in the convex hull as in the original object. This is
194 /// neither necessary (duplicated vertices get merged) nor sufficient
195 /// (think of a U with 4 vertices and 3 edges).
196 bool buildConvexHull(bool keepTriangle, const char* qhullCommand = NULL);
197
198 virtual int memUsage(const bool msg = false) const = 0;
199
200 /// @brief This is a special acceleration: BVH_model default stores the BV's
201 /// transform in world coordinate. However, we can also store each BV's
202 /// transform related to its parent BV node. When traversing the BVH, this can
203 /// save one matrix transformation.
204 virtual void makeParentRelative() = 0;
205
206 ✗ Vec3s computeCOM() const {
207 ✗ Scalar vol = 0;
208 ✗ Vec3s com(0, 0, 0);
209 ✗ if (!(vertices.get())) {
210 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
211 ✗ "vertices."
212 ✗ << std::endl;
213 ✗ return com;
214 }
215 ✗ const std::vector<Vec3s>& vertices_ = *vertices;
216 ✗ if (!(tri_indices.get())) {
217 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
218 ✗ "triangles."
219 ✗ << std::endl;
220 ✗ return com;
221 }
222 ✗ const std::vector<Triangle32>& tri_indices_ = *tri_indices;
223
224 ✗ for (unsigned int i = 0; i < num_tris; ++i) {
225 ✗ const Triangle32& tri = tri_indices_[i];
226 Scalar d_six_vol =
227 ✗ (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
228 ✗ vol += d_six_vol;
229 ✗ com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) *
230 ✗ d_six_vol;
231 }
232
233 ✗ return com / (vol * 4);
234 }
235
236 ✗ Scalar computeVolume() const {
237 ✗ Scalar vol = 0;
238 ✗ if (!(vertices.get())) {
239 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
240 ✗ "vertices."
241 ✗ << std::endl;
242 ✗ return vol;
243 }
244 ✗ const std::vector<Vec3s>& vertices_ = *vertices;
245 ✗ if (!(tri_indices.get())) {
246 std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
247 ✗ "triangles."
248 ✗ << std::endl;
249 ✗ return vol;
250 }
251 ✗ const std::vector<Triangle32>& tri_indices_ = *tri_indices;
252 ✗ for (unsigned int i = 0; i < num_tris; ++i) {
253 ✗ const Triangle32& tri = tri_indices_[i];
254 Scalar d_six_vol =
255 ✗ (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]);
256 ✗ vol += d_six_vol;
257 }
258
259 ✗ return vol / 6;
260 }
261
262 ✗ Matrix3s computeMomentofInertia() const {
263 ✗ Matrix3s C = Matrix3s::Zero();
264
265 ✗ Matrix3s C_canonical;
266 ✗ C_canonical << Scalar(1 / 60.0), //
267 ✗ Scalar(1 / 120.0), //
268 ✗ Scalar(1 / 120.0), //
269 ✗ Scalar(1 / 120.0), //
270 ✗ Scalar(1 / 60.0), //
271 ✗ Scalar(1 / 120.0), //
272 ✗ Scalar(1 / 120.0), //
273 ✗ Scalar(1 / 120.0), //
274 ✗ Scalar(1 / 60.0);
275
276 ✗ if (!(vertices.get())) {
277 std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
278 ✗ "not contain vertices."
279 ✗ << std::endl;
280 ✗ return C;
281 }
282 ✗ const std::vector<Vec3s>& vertices_ = *vertices;
283 ✗ if (!(vertices.get())) {
284 std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
285 ✗ "not contain vertices."
286 ✗ << std::endl;
287 ✗ return C;
288 }
289 ✗ const std::vector<Triangle32>& tri_indices_ = *tri_indices;
290 ✗ for (unsigned int i = 0; i < num_tris; ++i) {
291 ✗ const Triangle32& tri = tri_indices_[i];
292 ✗ const Vec3s& v1 = vertices_[tri[0]];
293 ✗ const Vec3s& v2 = vertices_[tri[1]];
294 ✗ const Vec3s& v3 = vertices_[tri[2]];
295 ✗ Matrix3s A;
296 ✗ A << v1.transpose(), v2.transpose(), v3.transpose();
297 ✗ C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
298 }
299
300 ✗ return C.trace() * Matrix3s::Identity() - C;
301 }
302
303 protected:
304 virtual void deleteBVs() = 0;
305 virtual bool allocateBVs() = 0;
306
307 /// @brief Build the bounding volume hierarchy
308 virtual int buildTree() = 0;
309
310 /// @brief Refit the bounding volume hierarchy
311 virtual int refitTree(bool bottomup) = 0;
312
313 unsigned int num_tris_allocated;
314 unsigned int num_vertices_allocated;
315 unsigned int num_vertex_updated; /// for ccd vertex update
316
317 protected:
318 /// \brief Comparison operators
319 virtual bool isEqual(const CollisionGeometry& other) const;
320 };
321
322 /// @brief A class describing the bounding hierarchy of a mesh model or a point
323 /// cloud model (which is viewed as a degraded version of mesh) \tparam BV one
324 /// of the bounding volume class in \ref Bounding_Volume.
325 template <typename BV>
326 class COAL_DLLAPI BVHModel : public BVHModelBase {
327 typedef BVHModelBase Base;
328
329 public:
330 using bv_node_vector_t =
331 std::vector<BVNode<BV>, Eigen::aligned_allocator<BVNode<BV>>>;
332
333 /// @brief Split rule to split one BV node into two children
334 shared_ptr<BVSplitter<BV>> bv_splitter;
335
336 /// @brief Fitting rule to fit a BV node to a set of geometry primitives
337 shared_ptr<BVFitter<BV>> bv_fitter;
338
339 /// @brief Default constructor to build an empty BVH
340 BVHModel();
341
342 /// @brief Copy constructor from another BVH
343 ///
344 /// \param[in] other BVHModel to copy.
345 ///
346 BVHModel(const BVHModel& other);
347
348 /// @brief Clone *this into a new BVHModel
349 ✗ virtual BVHModel<BV>* clone() const { return new BVHModel(*this); }
350
351 /// @brief deconstruction, delete mesh data related.
352 11770 ~BVHModel() {}
353
354 /// @brief We provide getBV() and getNumBVs() because BVH may be compressed
355 /// (in future), so we must provide some flexibility here
356
357 /// @brief Access the bv giving the its index
358 640426442 const BVNode<BV>& getBV(unsigned int i) const {
359
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 287578582 times.
 640426442 assert(i < num_bvs);
360 640426442 return (*bvs)[i];
361 }
362
363 /// @brief Access the bv giving the its index
364 ✗ BVNode<BV>& getBV(unsigned int i) {
365 ✗ assert(i < num_bvs);
366 ✗ return (*bvs)[i];
367 }
368
369 /// @brief Get the number of bv in the BVH
370 32 unsigned int getNumBVs() const { return num_bvs; }
371
372 /// @brief Get the BV type: default is unknown
373 NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
374
375 /// @brief Check the number of memory used
376 int memUsage(const bool msg) const;
377
378 /// @brief This is a special acceleration: BVH_model default stores the BV's
379 /// transform in world coordinate. However, we can also store each BV's
380 /// transform related to its parent BV node. When traversing the BVH, this can
381 /// save one matrix transformation.
382 ✗ void makeParentRelative() {
383 ✗ Matrix3s I(Matrix3s::Identity());
384 ✗ makeParentRelativeRecurse(0, I, Vec3s::Zero());
385 ✗ }
386
387 protected:
388 void deleteBVs();
389 bool allocateBVs();
390
391 unsigned int num_bvs_allocated;
392 std::shared_ptr<std::vector<unsigned int>> primitive_indices;
393
394 /// @brief Bounding volume hierarchy
395 std::shared_ptr<bv_node_vector_t> bvs;
396
397 /// @brief Number of BV nodes in bounding volume hierarchy
398 unsigned int num_bvs;
399
400 /// @brief Build the bounding volume hierarchy
401 int buildTree();
402
403 /// @brief Refit the bounding volume hierarchy
404 int refitTree(bool bottomup);
405
406 /// @brief Refit the bounding volume hierarchy in a top-down way (slow but
407 /// more compact)
408 int refitTree_topdown();
409
410 /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but
411 /// less compact)
412 int refitTree_bottomup();
413
414 /// @brief Recursive kernel for hierarchy construction
415 int recursiveBuildTree(int bv_id, unsigned int first_primitive,
416 unsigned int num_primitives);
417
418 /// @brief Recursive kernel for bottomup refitting
419 int recursiveRefitTree_bottomup(int bv_id);
420
421 /// @ recursively compute each bv's transform related to its parent. For
422 /// default BV, only the translation works. For oriented BV (OBB, RSS,
423 /// OBBRSS), special implementation is provided.
424 ✗ void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
425 const Vec3s& parent_c) {
426 ✗ bv_node_vector_t& bvs_ = *bvs;
427 ✗ if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
428 ✗ makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
429 parent_axes,
430 ✗ bvs_[static_cast<size_t>(bv_id)].getCenter());
431
432 ✗ makeParentRelativeRecurse(
433 ✗ bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
434 ✗ bvs_[static_cast<size_t>(bv_id)].getCenter());
435 }
436
437 ✗ bvs_[static_cast<size_t>(bv_id)].bv =
438 ✗ translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
439 ✗ }
440
441 private:
442 21 virtual bool isEqual(const CollisionGeometry& _other) const {
443
1/2
✓ Branch 0 taken 21 times.
✗ Branch 1 not taken.
 21 const BVHModel* other_ptr = dynamic_cast<const BVHModel*>(&_other);
444
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
 21 if (other_ptr == nullptr) return false;
445 21 const BVHModel& other = *other_ptr;
446
447 21 bool res = Base::isEqual(other);
448
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 20 times.
 21 if (!res) return false;
449
450 // unsigned int other_num_primitives = 0;
451 // if(other.primitive_indices)
452 // {
453
454 // switch(other.getModelType())
455 // {
456 // case BVH_MODEL_TRIANGLES:
457 // other_num_primitives = num_tris;
458 // break;
459 // case BVH_MODEL_POINTCLOUD:
460 // other_num_primitives = num_vertices;
461 // break;
462 // default:
463 // ;
464 // }
465 // }
466
467 // unsigned int num_primitives = 0;
468 // if(primitive_indices)
469 // {
470 //
471 // switch(other.getModelType())
472 // {
473 // case BVH_MODEL_TRIANGLES:
474 // num_primitives = num_tris;
475 // break;
476 // case BVH_MODEL_POINTCLOUD:
477 // num_primitives = num_vertices;
478 // break;
479 // default:
480 // ;
481 // }
482 // }
483 //
484 // if(num_primitives != other_num_primitives)
485 // return false;
486 //
487 // for(int k = 0; k < num_primitives; ++k)
488 // {
489 // if(primitive_indices[k] != other.primitive_indices[k])
490 // return false;
491 // }
492
493
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
 20 if (num_bvs != other.num_bvs) return false;
494
495
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())))
496 ✗ return false;
497
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()) {
498 20 const bv_node_vector_t& bvs_ = *bvs;
499 20 const bv_node_vector_t& other_bvs_ = *(other.bvs);
500
2/2
✓ Branch 0 taken 83252 times.
✓ Branch 1 taken 20 times.
 83272 for (unsigned int k = 0; k < num_bvs; ++k) {
501
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 83252 times.
 83252 if (bvs_[k] != other_bvs_[k]) return false;
502 }
503 }
504
505 20 return true;
506 }
507 };
508
509 /// @}
510
511 template <>
512 void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
513 const Vec3s& parent_c);
514
515 template <>
516 void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
517 const Vec3s& parent_c);
518
519 template <>
520 void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
521 Matrix3s& parent_axes,
522 const Vec3s& parent_c);
523
524 /// @brief Specialization of getNodeType() for BVHModel with different BV types
525 template <>
526 NODE_TYPE BVHModel<AABB>::getNodeType() const;
527
528 template <>
529 NODE_TYPE BVHModel<OBB>::getNodeType() const;
530
531 template <>
532 NODE_TYPE BVHModel<RSS>::getNodeType() const;
533
534 template <>
535 NODE_TYPE BVHModel<kIOS>::getNodeType() const;
536
537 template <>
538 NODE_TYPE BVHModel<OBBRSS>::getNodeType() const;
539
540 template <>
541 NODE_TYPE BVHModel<KDOP<16>>::getNodeType() const;
542
543 template <>
544 NODE_TYPE BVHModel<KDOP<18>>::getNodeType() const;
545
546 template <>
547 NODE_TYPE BVHModel<KDOP<24>>::getNodeType() const;
548
549 } // namespace coal
550
551 #endif
552