GCC Code Coverage Report

Directory: ./
File: include/coal/collision_object.h
Date: 2025-05-02 10:16:21
Exec Total Coverage
Lines: 71 89 79.8%
Branches: 101 208 48.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 * 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
36 /** \author Jia Pan */
37
38 #ifndef COAL_COLLISION_OBJECT_BASE_H
39 #define COAL_COLLISION_OBJECT_BASE_H
40
41 #include <limits>
42 #include <typeinfo>
43
44 #include "coal/deprecated.hh"
45 #include "coal/fwd.hh"
46 #include "coal/BV/AABB.h"
47 #include "coal/math/transform.h"
48
49 namespace coal {
50
51 /// @brief object type: BVH (mesh, points), basic geometry, octree
52 enum OBJECT_TYPE {
53 OT_UNKNOWN,
54 OT_BVH,
55 OT_GEOM,
56 OT_OCTREE,
57 OT_HFIELD,
58 OT_COUNT
59 };
60
61 /// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS,
62 /// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone,
63 /// cylinder, convex, plane, triangle), and octree
64 enum NODE_TYPE {
65 BV_UNKNOWN,
66 BV_AABB,
67 BV_OBB,
68 BV_RSS,
69 BV_kIOS,
70 BV_OBBRSS,
71 BV_KDOP16,
72 BV_KDOP18,
73 BV_KDOP24,
74 GEOM_BOX,
75 GEOM_SPHERE,
76 GEOM_CAPSULE,
77 GEOM_CONE,
78 GEOM_CYLINDER,
79 GEOM_CONVEX16,
80 GEOM_CONVEX32,
81 GEOM_CONVEX = GEOM_CONVEX32,
82 GEOM_PLANE,
83 GEOM_HALFSPACE,
84 GEOM_TRIANGLE,
85 GEOM_OCTREE,
86 GEOM_ELLIPSOID,
87 HF_AABB,
88 HF_OBBRSS,
89 NODE_COUNT
90 };
91
92 /// @addtogroup Construction_Of_BVH
93 /// @{
94
95 /// @brief The geometry for the object for collision or distance computation
96 class COAL_DLLAPI CollisionGeometry {
97 public:
98 116926345 CollisionGeometry()
99
2/4
✓ Branch 1 taken 116926345 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116926345 times.
✗ Branch 5 not taken.
 116926345 : aabb_center(Vec3s::Constant((std::numeric_limits<Scalar>::max)())),
100 116926345 aabb_radius(-1),
101 116926345 cost_density(1),
102 116926345 threshold_occupied(1),
103 233852690 threshold_free(0) {}
104
105 /// \brief Copy constructor
106 235 CollisionGeometry(const CollisionGeometry& other) = default;
107
108 233852888 virtual ~CollisionGeometry() {}
109
110 /// @brief Clone *this into a new CollisionGeometry
111 virtual CollisionGeometry* clone() const = 0;
112
113 /// \brief Equality operator
114 198 bool operator==(const CollisionGeometry& other) const {
115 396 return cost_density == other.cost_density &&
116
1/2
✓ Branch 0 taken 198 times.
✗ Branch 1 not taken.
 198 threshold_occupied == other.threshold_occupied &&
117
1/2
✓ Branch 0 taken 198 times.
✗ Branch 1 not taken.
 198 threshold_free == other.threshold_free &&
118
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 198 aabb_center == other.aabb_center &&
119
3/6
✓ Branch 0 taken 198 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 198 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 198 times.
✗ Branch 6 not taken.
 594 aabb_radius == other.aabb_radius && aabb_local == other.aabb_local &&
120
2/2
✓ Branch 1 taken 197 times.
✓ Branch 2 taken 1 times.
 396 isEqual(other);
121 }
122
123 /// \brief Difference operator
124 1 bool operator!=(const CollisionGeometry& other) const {
125 1 return isNotEqual(other);
126 }
127
128 /// @brief get the type of the object
129 ✗ virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
130
131 /// @brief get the node type
132 ✗ virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
133
134 /// @brief compute the AABB for object in local coordinate
135 virtual void computeLocalAABB() = 0;
136
137 /// @brief get user data in geometry
138 void* getUserData() const { return user_data; }
139
140 /// @brief set user data in geometry
141 void setUserData(void* data) { user_data = data; }
142
143 /// @brief whether the object is completely occupied
144 113592 inline bool isOccupied() const { return cost_density >= threshold_occupied; }
145
146 /// @brief whether the object is completely free
147 ✗ inline bool isFree() const { return cost_density <= threshold_free; }
148
149 /// @brief whether the object has some uncertainty
150 bool isUncertain() const;
151
152 /// @brief AABB center in local coordinate
153 Vec3s aabb_center;
154
155 /// @brief AABB radius
156 Scalar aabb_radius;
157
158 /// @brief AABB in local coordinate, used for tight AABB when only translation
159 /// transform
160 AABB aabb_local;
161
162 /// @brief pointer to user defined data specific to this object
163 void* user_data;
164
165 /// @brief collision cost for unit volume
166 Scalar cost_density;
167
168 /// @brief threshold for occupied ( >= is occupied)
169 Scalar threshold_occupied;
170
171 /// @brief threshold for free (<= is free)
172 Scalar threshold_free;
173
174 /// @brief compute center of mass
175
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 virtual Vec3s computeCOM() const { return Vec3s::Zero(); }
176
177 /// @brief compute the inertia matrix, related to the origin
178 ✗ virtual Matrix3s computeMomentofInertia() const {
179 ✗ return Matrix3s::Constant(NAN);
180 }
181
182 /// @brief compute the volume
183 ✗ virtual Scalar computeVolume() const { return 0; }
184
185 /// @brief compute the inertia matrix, related to the com
186 5 virtual Matrix3s computeMomentofInertiaRelatedToCOM() const {
187
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 Matrix3s C = computeMomentofInertia();
188
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 Vec3s com = computeCOM();
189
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 Scalar V = computeVolume();
190
191
7/14
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
 10 return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
192
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
 5 C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2],
193
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 C(1, 0) + V * com[1] * com[0],
194
6/12
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
 5 C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
195
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
 5 C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0],
196
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 C(2, 1) + V * com[2] * com[1],
197
6/12
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
 10 C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]))
198
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 10 .finished();
199 }
200
201 private:
202 /// @brief equal operator with another object of derived type.
203 virtual bool isEqual(const CollisionGeometry& other) const = 0;
204
205 /// @brief not equal operator with another object of derived type.
206 1 virtual bool isNotEqual(const CollisionGeometry& other) const {
207 1 return !(*this == other);
208 }
209
210 public:
211 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
212 };
213
214 /// @brief the object for collision or distance computation, contains the
215 /// geometry and the transform information
216 class COAL_DLLAPI CollisionObject {
217 public:
218 6966 CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
219 bool compute_local_aabb = true)
220
2/4
✓ Branch 2 taken 6966 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6966 times.
✗ Branch 6 not taken.
 6966 : cgeom(cgeom_), user_data(nullptr) {
221
1/2
✓ Branch 1 taken 6966 times.
✗ Branch 2 not taken.
 6966 init(compute_local_aabb);
222 6966 }
223
224 38485 CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
225 const Transform3s& tf, bool compute_local_aabb = true)
226
2/4
✓ Branch 2 taken 38485 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38485 times.
✗ Branch 6 not taken.
 38485 : cgeom(cgeom_), t(tf), user_data(nullptr) {
227
1/2
✓ Branch 1 taken 38485 times.
✗ Branch 2 not taken.
 38485 init(compute_local_aabb);
228 38485 }
229
230 ✗ CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
231 const Matrix3s& R, const Vec3s& T,
232 bool compute_local_aabb = true)
233 ✗ : cgeom(cgeom_), t(R, T), user_data(nullptr) {
234 ✗ init(compute_local_aabb);
235 }
236
237 bool operator==(const CollisionObject& other) const {
238 return cgeom == other.cgeom && t == other.t && user_data == other.user_data;
239 }
240
241 bool operator!=(const CollisionObject& other) const {
242 return !(*this == other);
243 }
244
245 45315 ~CollisionObject() {}
246
247 /// @brief get the type of the object
248 ✗ OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); }
249
250 /// @brief get the node type
251 162532 NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
252
253 /// @brief get the AABB in world space
254 1230338 const AABB& getAABB() const { return aabb; }
255
256 /// @brief get the AABB in world space
257 6806483 AABB& getAABB() { return aabb; }
258
259 /// @brief compute the AABB in world space
260 39839 void computeAABB() {
261
3/4
✓ Branch 3 taken 39839 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 7525 times.
✓ Branch 6 taken 32314 times.
 39839 if (t.getRotation().isIdentity()) {
262
1/2
✓ Branch 4 taken 7525 times.
✗ Branch 5 not taken.
 7525 aabb = translate(cgeom->aabb_local, t.getTranslation());
263 } else {
264
2/4
✓ Branch 2 taken 32314 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32314 times.
✗ Branch 6 not taken.
 32314 aabb.min_ = aabb.max_ = t.getTranslation();
265
266
2/4
✓ Branch 1 taken 32314 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32314 times.
✗ Branch 5 not taken.
 32314 Vec3s min_world, max_world;
267
2/2
✓ Branch 0 taken 96942 times.
✓ Branch 1 taken 32314 times.
 129256 for (int k = 0; k < 3; ++k) {
268
4/8
✓ Branch 2 taken 96942 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96942 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 96942 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 96942 times.
✗ Branch 12 not taken.
 96942 min_world.array() = t.getRotation().row(k).array() *
269
3/6
✓ Branch 2 taken 96942 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96942 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 96942 times.
✗ Branch 9 not taken.
 193884 cgeom->aabb_local.min_.transpose().array();
270
4/8
✓ Branch 2 taken 96942 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96942 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 96942 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 96942 times.
✗ Branch 12 not taken.
 96942 max_world.array() = t.getRotation().row(k).array() *
271
3/6
✓ Branch 2 taken 96942 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96942 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 96942 times.
✗ Branch 9 not taken.
 193884 cgeom->aabb_local.max_.transpose().array();
272
273
5/10
✓ Branch 1 taken 96942 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96942 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 96942 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 96942 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 96942 times.
✗ Branch 14 not taken.
 96942 aabb.min_[k] += (min_world.array().min)(max_world.array()).sum();
274
5/10
✓ Branch 1 taken 96942 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96942 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 96942 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 96942 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 96942 times.
✗ Branch 14 not taken.
 96942 aabb.max_[k] += (min_world.array().max)(max_world.array()).sum();
275 }
276 }
277 39839 }
278
279 /// @brief get user data in object
280 void* getUserData() const { return user_data; }
281
282 /// @brief set user data in object
283 void setUserData(void* data) { user_data = data; }
284
285 /// @brief get translation of the object
286 1344 inline const Vec3s& getTranslation() const { return t.getTranslation(); }
287
288 /// @brief get matrix rotation of the object
289 1344 inline const Matrix3s& getRotation() const { return t.getRotation(); }
290
291 /// @brief get object's transform
292 2531572 inline const Transform3s& getTransform() const { return t; }
293
294 /// @brief set object's rotation matrix
295 ✗ void setRotation(const Matrix3s& R) { t.setRotation(R); }
296
297 /// @brief set object's translation
298 2 void setTranslation(const Vec3s& T) { t.setTranslation(T); }
299
300 /// @brief set object's transform
301 1344 void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); }
302
303 /// @brief set object's transform
304 4 void setTransform(const Transform3s& tf) { t = tf; }
305
306 /// @brief whether the object is in local coordinate
307 ✗ bool isIdentityTransform() const { return t.isIdentity(); }
308
309 /// @brief set the object in local coordinate
310 ✗ void setIdentityTransform() { t.setIdentity(); }
311
312 /// @brief get shared pointer to collision geometry of the object instance
313 const shared_ptr<const CollisionGeometry> collisionGeometry() const {
314 return cgeom;
315 }
316
317 /// @brief get shared pointer to collision geometry of the object instance
318 32822 const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
319
320 /// @brief get raw pointer to collision geometry of the object instance
321 2531572 const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); }
322
323 /// @brief get raw pointer to collision geometry of the object instance
324 ✗ CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); }
325
326 /// @brief Associate a new CollisionGeometry
327 ///
328 /// @param[in] collision_geometry The new CollisionGeometry
329 /// @param[in] compute_local_aabb Whether the local aabb of the input new has
330 /// to be computed.
331 ///
332 ✗ void setCollisionGeometry(
333 const shared_ptr<CollisionGeometry>& collision_geometry,
334 bool compute_local_aabb = true) {
335 ✗ if (collision_geometry.get() != cgeom.get()) {
336 ✗ cgeom = collision_geometry;
337 ✗ init(compute_local_aabb);
338 }
339 }
340
341 protected:
342 45451 void init(bool compute_local_aabb = true) {
343
2/2
✓ Branch 1 taken 38493 times.
✓ Branch 2 taken 6958 times.
 45451 if (cgeom) {
344
1/2
✓ Branch 0 taken 38493 times.
✗ Branch 1 not taken.
 38493 if (compute_local_aabb) cgeom->computeLocalAABB();
345 38493 computeAABB();
346 }
347 45451 }
348
349 shared_ptr<CollisionGeometry> cgeom;
350
351 Transform3s t;
352
353 /// @brief AABB in global coordinate
354 mutable AABB aabb;
355
356 /// @brief pointer to user defined data specific to this object
357 void* user_data;
358 };
359
360 } // namespace coal
361
362 #endif
363