Directory: | ./ |
---|---|
File: | include/coal/internal/shape_shape_func.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 56 | 60 | 93.3% |
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 | 1581934 | 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 | 1581934 | const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1); | |
79 | 1581934 | const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2); | |
80 | 1581934 | return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2, | |
81 | 1581934 | 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 | 1552076 | 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 | 1552076 | return ::coal::ShapeShapeDistancer<ShapeType1, ShapeType2>::run( | |
121 | 1552076 | 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 | 2478986 | 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 1239493 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1239493 times.
|
2478986 | if (request.isSatisfied(result)) return result.numContacts(); |
142 | |||
143 | 2478986 | const bool compute_penetration = | |
144 |
3/4✓ Branch 0 taken 411 times.
✓ Branch 1 taken 1239082 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 411 times.
|
2478986 | request.enable_contact || (request.security_margin < 0); |
145 |
3/6✓ Branch 1 taken 1239493 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1239493 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1239493 times.
✗ Branch 8 not taken.
|
2478986 | Vec3s p1, p2, normal; |
146 |
1/2✓ Branch 1 taken 1239493 times.
✗ Branch 2 not taken.
|
2478986 | Scalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>( |
147 | o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal); | ||
148 | |||
149 | 2478986 | size_t num_contacts = 0; | |
150 | 2478986 | const Scalar distToCollision = distance - request.security_margin; | |
151 | |||
152 |
1/2✓ Branch 1 taken 1239493 times.
✗ Branch 2 not taken.
|
2478986 | internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, |
153 | p1, p2, normal); | ||
154 |
4/4✓ Branch 0 taken 13198 times.
✓ Branch 1 taken 1226295 times.
✓ Branch 2 taken 13198 times.
✓ Branch 3 taken 1226295 times.
|
2505382 | 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 | 2478986 | return num_contacts; | |
165 | } | ||
166 | }; | ||
167 | |||
168 | template <typename ShapeType1, typename ShapeType2> | ||
169 | 1239493 | 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 | 1239493 | return ShapeShapeCollider<ShapeType1, ShapeType2>::run( | |
177 | 1239493 | 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 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Plane) | |
286 | 13628 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Box, Sphere) | |
287 | 405 | SHAPE_SELF_DISTANCE_SPECIALIZATION(Capsule) | |
288 | 800 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Halfspace) | |
289 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Capsule, Plane) | |
290 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Halfspace) | |
291 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cone, Plane) | |
292 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Halfspace) | |
293 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Cylinder, Plane) | |
294 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace) | |
295 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane) | |
296 | 1661 | SHAPE_SELF_DISTANCE_SPECIALIZATION(Sphere) | |
297 | 5776 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder) | |
298 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule) | |
299 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Halfspace) | |
300 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellipsoid, Plane) | |
301 | 400 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace) | |
302 | ✗ | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Plane) | |
303 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace) | ||
304 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Plane) | ||
305 | 25664 | SHAPE_SELF_DISTANCE_SPECIALIZATION(TriangleP) | |
306 | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Sphere) | ||
307 | ✗ | SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Plane, Halfspace) | |
308 | ✗ | SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane) | |
309 | ✗ | SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace) | |
310 | |||
311 | #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION | ||
312 | #undef SHAPE_SELF_DISTANCE_SPECIALIZATION | ||
313 | |||
314 | } // namespace coal | ||
315 | |||
316 | /// @endcond | ||
317 | |||
318 | #endif | ||
319 |