GCC Code Coverage Report

Directory: ./
File: include/coal/internal/shape_shape_func.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 56 62 90.3%
Functions: 246 440 55.9%
Branches: 25 44 56.8%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, CNRS-LAAS
5 * Copyright (c) 2024, INRIA
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 Willow Garage, Inc. 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 Florent Lamiraux */
37
38 #ifndef COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
39 #define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H
40
41 /// @cond INTERNAL
42
43 #include "coal/collision_data.h"
44 #include "coal/collision_utility.h"
45 #include "coal/narrowphase/narrowphase.h"
46 #include "coal/shape/geometric_shapes_traits.h"
47
48 #include "coal/tracy.hh"
49
50 namespace coal {
51
52 template <typename ShapeType1, typename ShapeType2>
53 struct ShapeShapeDistancer {
54 29378 static Scalar run(const CollisionGeometry* o1, const Transform3s& tf1,
55 const CollisionGeometry* o2, const Transform3s& tf2,
56 const GJKSolver* nsolver, const DistanceRequest& request,
57 DistanceResult& result) {
58
2/4
✓ Branch 1 taken 14929 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 14929 times.
 29378 if (request.isSatisfied(result)) return result.min_distance;
59
60 // Witness points on shape1 and shape2, normal pointing from shape1 to
61 // shape2.
62
3/6
✓ Branch 1 taken 14929 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14929 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14929 times.
✗ Branch 8 not taken.
 29378 Vec3s p1, p2, normal;
63 58756 const Scalar distance = ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
64
1/2
✓ Branch 1 taken 14929 times.
✗ Branch 2 not taken.
 29378 o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
65 normal);
66
67
1/2
✓ Branch 1 taken 14929 times.
✗ Branch 2 not taken.
 29378 result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE,
68 p1, p2, normal);
69
70 29378 return distance;
71 }
72
73 1581938 static Scalar run(const CollisionGeometry* o1, const Transform3s& tf1,
74 const CollisionGeometry* o2, const Transform3s& tf2,
75 const GJKSolver* nsolver,
76 const bool compute_signed_distance, Vec3s& p1, Vec3s& p2,
77 Vec3s& normal) {
78 1581938 const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1);
79 1581938 const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2);
80 1581938 return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2,
81 1581938 compute_signed_distance, p1, p2, normal);
82 }
83 };
84
85 /// @brief Shape-shape distance computation.
86 /// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
87 /// a `CollisionRequest`.
88 ///
89 /// @note This function is typically used for collision pairs containing two
90 /// primitive shapes.
91 /// @note This function might be specialized for some pairs of shapes.
92 template <typename ShapeType1, typename ShapeType2>
93 29378 Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,
94 const CollisionGeometry* o2, const Transform3s& tf2,
95 const GJKSolver* nsolver,
96 const DistanceRequest& request,
97 DistanceResult& result) {
98 COAL_TRACY_ZONE_SCOPED_N("coal::ShapeShapeDistance");
99 29378 return ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
100 29378 o1, tf1, o2, tf2, nsolver, request, result);
101 }
102
103 namespace internal {
104 /// @brief Shape-shape distance computation.
105 /// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
106 /// a `CollisionRequest`.
107 ///
108 /// @note This function is typically used for collision pairs complex structures
109 /// like BVHs, HeightFields or Octrees. These structures contain sets of
110 /// primitive shapes.
111 /// This function is meant to be called on the pairs of primitive shapes of
112 /// these structures.
113 /// @note This function might be specialized for some pairs of shapes.
114 template <typename ShapeType1, typename ShapeType2>
115 1552080 Scalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,
116 const CollisionGeometry* o2, const Transform3s& tf2,
117 const GJKSolver* nsolver,
118 const bool compute_signed_distance, Vec3s& p1,
119 Vec3s& p2, Vec3s& normal) {
120 1552080 return ::coal::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
121 1552080 o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
122 }
123 } // namespace internal
124
125 /// @brief Shape-shape collision detection.
126 /// Assumes that `nsolver` has already been set up by a `DistanceRequest` or
127 /// a `CollisionRequest`.
128 ///
129 /// @note This function is typically used for collision pairs containing two
130 /// primitive shapes.
131 /// Complex structures like BVHs, HeightFields or Octrees contain sets of
132 /// primitive shapes should use the `ShapeShapeDistance` function to do their
133 /// internal collision detection checks.
134 template <typename ShapeType1, typename ShapeType2>
135 struct ShapeShapeCollider {
136 2479016 static std::size_t run(const CollisionGeometry* o1, const Transform3s& tf1,
137 const CollisionGeometry* o2, const Transform3s& tf2,
138 const GJKSolver* nsolver,
139 const CollisionRequest& request,
140 CollisionResult& result) {
141
2/4
✓ Branch 1 taken 1239508 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1239508 times.
 2479016 if (request.isSatisfied(result)) return result.numContacts();
142
143 2479016 const bool compute_penetration =
144
3/4
✓ Branch 0 taken 411 times.
✓ Branch 1 taken 1239097 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 411 times.
 2479016 request.enable_contact || (request.security_margin < 0);
145
3/6
✓ Branch 1 taken 1239508 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1239508 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1239508 times.
✗ Branch 8 not taken.
 2479016 Vec3s p1, p2, normal;
146
1/2
✓ Branch 1 taken 1239508 times.
✗ Branch 2 not taken.
 2479016 Scalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
147 o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
148
149 2479016 size_t num_contacts = 0;
150 2479016 const Scalar distToCollision = distance - request.security_margin;
151
152
1/2
✓ Branch 1 taken 1239508 times.
✗ Branch 2 not taken.
 2479016 internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
153 p1, p2, normal);
154
4/4
✓ Branch 0 taken 13198 times.
✓ Branch 1 taken 1226310 times.
✓ Branch 2 taken 13198 times.
✓ Branch 3 taken 1226310 times.
 2505412 if (distToCollision <= request.collision_distance_threshold &&
155
1/2
✓ Branch 1 taken 13198 times.
✗ Branch 2 not taken.
 26396 result.numContacts() < request.num_max_contacts) {
156
1/2
✓ Branch 1 taken 13198 times.
✗ Branch 2 not taken.
 26396 if (result.numContacts() < request.num_max_contacts) {
157
1/2
✓ Branch 1 taken 13198 times.
✗ Branch 2 not taken.
 26396 Contact contact(o1, o2, Contact::NONE, Contact::NONE, p1, p2, normal,
158 distance);
159
1/2
✓ Branch 1 taken 13198 times.
✗ Branch 2 not taken.
 26396 result.addContact(contact);
160 }
161 26396 num_contacts = result.numContacts();
162 }
163
164 2479016 return num_contacts;
165 }
166 };
167
168 template <typename ShapeType1, typename ShapeType2>
169 1239508 std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
170 const Transform3s& tf1,
171 const CollisionGeometry* o2,
172 const Transform3s& tf2, const GJKSolver* nsolver,
173 const CollisionRequest& request,
174 CollisionResult& result) {
175 COAL_TRACY_ZONE_SCOPED_N("coal::ShapeShapeCollide");
176 1239508 return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
177 1239508 o1, tf1, o2, tf2, nsolver, request, result);
178 }
179
180 // clang-format off
181 // ==============================================================================================================
182 // ==============================================================================================================
183 // ==============================================================================================================
184 // Shape distance algorithms based on:
185 // - built-in function: 0
186 // - GJK: 1
187 //
188 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
189 // | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle | ellipsoid | convex |
190 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
191 // | box | 1 | 0 | 1 | 1 | 1 | 0 | 0 | 1 | 1 | 1 |
192 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
193 // | sphere |/////| 0 | 0 | 1 | 0 | 0 | 0 | 0 | 1 | 1 |
194 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
195 // | capsule |/////|////////| 0 | 1 | 1 | 0 | 0 | 1 | 1 | 1 |
196 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
197 // | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1 | 1 | 1 |
198 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
199 // | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1 | 1 | 1 |
200 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
201 // | plane |/////|////////|/////////|//////|//////////| ? | ? | 0 | 0 | 0 |
202 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
203 // | half-space |/////|////////|/////////|//////|//////////|///////| ? | 0 | 0 | 0 |
204 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
205 // | triangle |/////|////////|/////////|//////|//////////|///////|////////////| 0 | 1 | 1 |
206 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
207 // | ellipsoid |/////|////////|/////////|//////|//////////|///////|////////////|//////////| 1 | 1 |
208 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
209 // | convex |/////|////////|/////////|//////|//////////|///////|////////////|//////////|///////////| 1 |
210 // +------------+-----+--------+---------+------+----------+-------+------------+----------+-----------+--------+
211 //
212 // Number of pairs: 55
213 // - Specialized: 26
214 // - GJK: 29
215 // clang-format on
216
217 #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \
218 template <> \
219 COAL_DLLAPI Scalar internal::ShapeShapeDistance<T1, T2>( \
220 const CollisionGeometry* o1, const Transform3s& tf1, \
221 const CollisionGeometry* o2, const Transform3s& tf2, \
222 const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
223 Vec3s& p2, Vec3s& normal); \
224 template <> \
225 COAL_DLLAPI Scalar internal::ShapeShapeDistance<T2, T1>( \
226 const CollisionGeometry* o1, const Transform3s& tf1, \
227 const CollisionGeometry* o2, const Transform3s& tf2, \
228 const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
229 Vec3s& p2, Vec3s& normal); \
230 template <> \
231 inline COAL_DLLAPI Scalar ShapeShapeDistance<T1, T2>( \
232 const CollisionGeometry* o1, const Transform3s& tf1, \
233 const CollisionGeometry* o2, const Transform3s& tf2, \
234 const GJKSolver* nsolver, const DistanceRequest& request, \
235 DistanceResult& result) { \
236 result.o1 = o1; \
237 result.o2 = o2; \
238 result.b1 = DistanceResult::NONE; \
239 result.b2 = DistanceResult::NONE; \
240 result.min_distance = internal::ShapeShapeDistance<T1, T2>( \
241 o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
242 result.nearest_points[0], result.nearest_points[1], result.normal); \
243 return result.min_distance; \
244 } \
245 template <> \
246 inline COAL_DLLAPI Scalar ShapeShapeDistance<T2, T1>( \
247 const CollisionGeometry* o1, const Transform3s& tf1, \
248 const CollisionGeometry* o2, const Transform3s& tf2, \
249 const GJKSolver* nsolver, const DistanceRequest& request, \
250 DistanceResult& result) { \
251 result.o1 = o1; \
252 result.o2 = o2; \
253 result.b1 = DistanceResult::NONE; \
254 result.b2 = DistanceResult::NONE; \
255 result.min_distance = internal::ShapeShapeDistance<T2, T1>( \
256 o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
257 result.nearest_points[0], result.nearest_points[1], result.normal); \
258 return result.min_distance; \
259 }
260
261 #define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \
262 template <> \
263 COAL_DLLAPI Scalar internal::ShapeShapeDistance<T, T>( \
264 const CollisionGeometry* o1, const Transform3s& tf1, \
265 const CollisionGeometry* o2, const Transform3s& tf2, \
266 const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
267 Vec3s& p2, Vec3s& normal); \
268 template <> \
269 inline COAL_DLLAPI Scalar ShapeShapeDistance<T, T>( \
270 const CollisionGeometry* o1, const Transform3s& tf1, \
271 const CollisionGeometry* o2, const Transform3s& tf2, \
272 const GJKSolver* nsolver, const DistanceRequest& request, \
273 DistanceResult& result) { \
274 result.o1 = o1; \
275 result.o2 = o2; \
276 result.b1 = DistanceResult::NONE; \
277 result.b2 = DistanceResult::NONE; \
278 result.min_distance = internal::ShapeShapeDistance<T, T>( \
279 o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, \
280 result.nearest_points[0], result.nearest_points[1], result.normal); \
281 return result.min_distance; \
282 }
283
284 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Halfspace)
285 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane)
286 27260 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere)
287 405 SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule)
288 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace)
289 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane)
290 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace)
291 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane)
292 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace)
293 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane)
294 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace)
295 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane)
296 1661 SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere)
297 5776 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder)
298 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule)
299 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace)
300 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane)
301 ✗ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase16, Halfspace)
302 800 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase32, Halfspace)
303 ✗ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase16, Plane)
304 ✗ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase32, Plane)
305 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace)
306 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane)
307 25664 SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP)
308 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere)
309 ✗ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace)
310 ✗ SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane)
311 ✗ SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace)
312
313 #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
314 #undef SHAPE_SELF_DISTANCE_SPECIALIZATION
315
316 } // namespace coal
317
318 /// @endcond
319
320 #endif
321