GCC Code Coverage Report

Directory: ./
File: include/coal/collision_object.h
Date: 2025-04-01 09:23:31
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_CONVEX,
80 GEOM_PLANE,
81 GEOM_HALFSPACE,
82 GEOM_TRIANGLE,
83 GEOM_OCTREE,
84 GEOM_ELLIPSOID,
85 HF_AABB,
86 HF_OBBRSS,
87 NODE_COUNT
88 };
89
90 /// @addtogroup Construction_Of_BVH
91 /// @{
92
93 /// @brief The geometry for the object for collision or distance computation
94 class COAL_DLLAPI CollisionGeometry {
95 public:
96 116926325 CollisionGeometry()
97
2/4
✓ Branch 1 taken 116926325 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116926325 times.
✗ Branch 5 not taken.
 116926325 : aabb_center(Vec3s::Constant((std::numeric_limits<Scalar>::max)())),
98 116926325 aabb_radius(-1),
99 116926325 cost_density(1),
100 116926325 threshold_occupied(1),
101 233852650 threshold_free(0) {}
102
103 /// \brief Copy constructor
104 254 CollisionGeometry(const CollisionGeometry& other) = default;
105
106 233852886 virtual ~CollisionGeometry() {}
107
108 /// @brief Clone *this into a new CollisionGeometry
109 virtual CollisionGeometry* clone() const = 0;
110
111 /// \brief Equality operator
112 198 bool operator==(const CollisionGeometry& other) const {
113 396 return cost_density == other.cost_density &&
114
1/2
✓ Branch 0 taken 198 times.
✗ Branch 1 not taken.
 198 threshold_occupied == other.threshold_occupied &&
115
1/2
✓ Branch 0 taken 198 times.
✗ Branch 1 not taken.
 198 threshold_free == other.threshold_free &&
116
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 198 aabb_center == other.aabb_center &&
117
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 &&
118
2/2
✓ Branch 1 taken 197 times.
✓ Branch 2 taken 1 times.
 396 isEqual(other);
119 }
120
121 /// \brief Difference operator
122 1 bool operator!=(const CollisionGeometry& other) const {
123 1 return isNotEqual(other);
124 }
125
126 /// @brief get the type of the object
127 ✗ virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
128
129 /// @brief get the node type
130 ✗ virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
131
132 /// @brief compute the AABB for object in local coordinate
133 virtual void computeLocalAABB() = 0;
134
135 /// @brief get user data in geometry
136 void* getUserData() const { return user_data; }
137
138 /// @brief set user data in geometry
139 void setUserData(void* data) { user_data = data; }
140
141 /// @brief whether the object is completely occupied
142 113592 inline bool isOccupied() const { return cost_density >= threshold_occupied; }
143
144 /// @brief whether the object is completely free
145 ✗ inline bool isFree() const { return cost_density <= threshold_free; }
146
147 /// @brief whether the object has some uncertainty
148 bool isUncertain() const;
149
150 /// @brief AABB center in local coordinate
151 Vec3s aabb_center;
152
153 /// @brief AABB radius
154 Scalar aabb_radius;
155
156 /// @brief AABB in local coordinate, used for tight AABB when only translation
157 /// transform
158 AABB aabb_local;
159
160 /// @brief pointer to user defined data specific to this object
161 void* user_data;
162
163 /// @brief collision cost for unit volume
164 Scalar cost_density;
165
166 /// @brief threshold for occupied ( >= is occupied)
167 Scalar threshold_occupied;
168
169 /// @brief threshold for free (<= is free)
170 Scalar threshold_free;
171
172 /// @brief compute center of mass
173
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 virtual Vec3s computeCOM() const { return Vec3s::Zero(); }
174
175 /// @brief compute the inertia matrix, related to the origin
176 ✗ virtual Matrix3s computeMomentofInertia() const {
177 ✗ return Matrix3s::Constant(NAN);
178 }
179
180 /// @brief compute the volume
181 ✗ virtual Scalar computeVolume() const { return 0; }
182
183 /// @brief compute the inertia matrix, related to the com
184 5 virtual Matrix3s computeMomentofInertiaRelatedToCOM() const {
185
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 Matrix3s C = computeMomentofInertia();
186
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 Vec3s com = computeCOM();
187
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 Scalar V = computeVolume();
188
189
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]),
190
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],
191
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],
192
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]),
193
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],
194
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],
195
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]))
196
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 10 .finished();
197 }
198
199 private:
200 /// @brief equal operator with another object of derived type.
201 virtual bool isEqual(const CollisionGeometry& other) const = 0;
202
203 /// @brief not equal operator with another object of derived type.
204 1 virtual bool isNotEqual(const CollisionGeometry& other) const {
205 1 return !(*this == other);
206 }
207
208 public:
209 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
210 };
211
212 /// @brief the object for collision or distance computation, contains the
213 /// geometry and the transform information
214 class COAL_DLLAPI CollisionObject {
215 public:
216 6966 CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
217 bool compute_local_aabb = true)
218
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) {
219
1/2
✓ Branch 1 taken 6966 times.
✗ Branch 2 not taken.
 6966 init(compute_local_aabb);
220 6966 }
221
222 38485 CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
223 const Transform3s& tf, bool compute_local_aabb = true)
224
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) {
225
1/2
✓ Branch 1 taken 38485 times.
✗ Branch 2 not taken.
 38485 init(compute_local_aabb);
226 38485 }
227
228 ✗ CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
229 const Matrix3s& R, const Vec3s& T,
230 bool compute_local_aabb = true)
231 ✗ : cgeom(cgeom_), t(R, T), user_data(nullptr) {
232 ✗ init(compute_local_aabb);
233 }
234
235 bool operator==(const CollisionObject& other) const {
236 return cgeom == other.cgeom && t == other.t && user_data == other.user_data;
237 }
238
239 bool operator!=(const CollisionObject& other) const {
240 return !(*this == other);
241 }
242
243 45315 ~CollisionObject() {}
244
245 /// @brief get the type of the object
246 ✗ OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); }
247
248 /// @brief get the node type
249 162532 NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
250
251 /// @brief get the AABB in world space
252 1230338 const AABB& getAABB() const { return aabb; }
253
254 /// @brief get the AABB in world space
255 6806631 AABB& getAABB() { return aabb; }
256
257 /// @brief compute the AABB in world space
258 39839 void computeAABB() {
259
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()) {
260
1/2
✓ Branch 4 taken 7525 times.
✗ Branch 5 not taken.
 7525 aabb = translate(cgeom->aabb_local, t.getTranslation());
261 } else {
262
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();
263
264
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;
265
2/2
✓ Branch 0 taken 96942 times.
✓ Branch 1 taken 32314 times.
 129256 for (int k = 0; k < 3; ++k) {
266
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() *
267
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();
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 max_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.max_.transpose().array();
270
271
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();
272
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();
273 }
274 }
275 39839 }
276
277 /// @brief get user data in object
278 void* getUserData() const { return user_data; }
279
280 /// @brief set user data in object
281 void setUserData(void* data) { user_data = data; }
282
283 /// @brief get translation of the object
284 1344 inline const Vec3s& getTranslation() const { return t.getTranslation(); }
285
286 /// @brief get matrix rotation of the object
287 1344 inline const Matrix3s& getRotation() const { return t.getRotation(); }
288
289 /// @brief get object's transform
290 2531624 inline const Transform3s& getTransform() const { return t; }
291
292 /// @brief set object's rotation matrix
293 ✗ void setRotation(const Matrix3s& R) { t.setRotation(R); }
294
295 /// @brief set object's translation
296 2 void setTranslation(const Vec3s& T) { t.setTranslation(T); }
297
298 /// @brief set object's transform
299 1344 void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); }
300
301 /// @brief set object's transform
302 4 void setTransform(const Transform3s& tf) { t = tf; }
303
304 /// @brief whether the object is in local coordinate
305 ✗ bool isIdentityTransform() const { return t.isIdentity(); }
306
307 /// @brief set the object in local coordinate
308 ✗ void setIdentityTransform() { t.setIdentity(); }
309
310 /// @brief get shared pointer to collision geometry of the object instance
311 const shared_ptr<const CollisionGeometry> collisionGeometry() const {
312 return cgeom;
313 }
314
315 /// @brief get shared pointer to collision geometry of the object instance
316 32822 const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
317
318 /// @brief get raw pointer to collision geometry of the object instance
319 2531624 const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); }
320
321 /// @brief get raw pointer to collision geometry of the object instance
322 ✗ CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); }
323
324 /// @brief Associate a new CollisionGeometry
325 ///
326 /// @param[in] collision_geometry The new CollisionGeometry
327 /// @param[in] compute_local_aabb Whether the local aabb of the input new has
328 /// to be computed.
329 ///
330 ✗ void setCollisionGeometry(
331 const shared_ptr<CollisionGeometry>& collision_geometry,
332 bool compute_local_aabb = true) {
333 ✗ if (collision_geometry.get() != cgeom.get()) {
334 ✗ cgeom = collision_geometry;
335 ✗ init(compute_local_aabb);
336 }
337 }
338
339 protected:
340 45451 void init(bool compute_local_aabb = true) {
341
2/2
✓ Branch 1 taken 38493 times.
✓ Branch 2 taken 6958 times.
 45451 if (cgeom) {
342
1/2
✓ Branch 0 taken 38493 times.
✗ Branch 1 not taken.
 38493 if (compute_local_aabb) cgeom->computeLocalAABB();
343 38493 computeAABB();
344 }
345 45451 }
346
347 shared_ptr<CollisionGeometry> cgeom;
348
349 Transform3s t;
350
351 /// @brief AABB in global coordinate
352 mutable AABB aabb;
353
354 /// @brief pointer to user defined data specific to this object
355 void* user_data;
356 };
357
358 } // namespace coal
359
360 #endif
361