GCC Code Coverage Report

Directory: ./
File: include/coal/collision_data.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 313 378 82.8%
Branches: 167 468 35.7%
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) 2024, 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_COLLISION_DATA_H
40 #define COAL_COLLISION_DATA_H
41
42 #include <vector>
43 #include <array>
44 #include <set>
45 #include <limits>
46 #include <numeric>
47
48 #include "coal/collision_object.h"
49 #include "coal/config.hh"
50 #include "coal/data_types.h"
51 #include "coal/timings.h"
52 #include "coal/narrowphase/narrowphase_defaults.h"
53 #include "coal/logging.h"
54
55 namespace coal {
56
57 /// @brief Contact information returned by collision
58 struct COAL_DLLAPI Contact {
59 /// @brief collision object 1
60 const CollisionGeometry* o1;
61
62 /// @brief collision object 2
63 const CollisionGeometry* o2;
64
65 /// @brief contact primitive in object 1
66 /// if object 1 is mesh or point cloud, it is the triangle or point id
67 /// if object 1 is geometry shape, it is NONE (-1),
68 /// if object 1 is octree, it is the id of the cell
69 int b1;
70
71 /// @brief contact primitive in object 2
72 /// if object 2 is mesh or point cloud, it is the triangle or point id
73 /// if object 2 is geometry shape, it is NONE (-1),
74 /// if object 2 is octree, it is the id of the cell
75 int b2;
76
77 /// @brief contact normal, pointing from o1 to o2.
78 /// The normal defined as the normalized separation vector:
79 /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0]
80 /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is
81 /// the **signed** distance between o1 and o2. The normal always points from
82 /// o1 to o2.
83 /// @note The separation vector is the smallest vector such that if o1 is
84 /// translated by it, o1 and o2 are in touching contact (they share at least
85 /// one contact point but have a zero intersection volume). If the shapes
86 /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) =
87 /// (p2-p1).norm().
88 Vec3s normal;
89
90 /// @brief nearest points associated to this contact.
91 /// @note Also referred as "witness points" in other collision libraries.
92 /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the
93 /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1
94 /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from
95 /// o2. See \ref DistanceResult::normal for the definition of the separation
96 /// vector. If o1 and o2 have multiple contacts, the nearest_points are
97 /// associated with the contact which has the greatest penetration depth.
98 /// TODO (louis): rename `nearest_points` to `witness_points`.
99 std::array<Vec3s, 2> nearest_points;
100
101 /// @brief contact position, in world space
102 Vec3s pos;
103
104 /// @brief penetration depth
105 Scalar penetration_depth;
106
107 /// @brief invalid contact primitive information
108 static const int NONE = -1;
109
110 /// @brief Default constructor
111 2480 Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
112 2480 penetration_depth = (std::numeric_limits<Scalar>::max)();
113 4960 nearest_points[0] = nearest_points[1] = normal = pos =
114
5/10
✓ Branch 2 taken 2480 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2480 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2480 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 2480 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 2480 times.
✗ Branch 17 not taken.
 2480 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
115 2480 }
116
117 ✗ Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
118 int b2_)
119 ✗ : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
120 ✗ penetration_depth = (std::numeric_limits<Scalar>::max)();
121 ✗ nearest_points[0] = nearest_points[1] = normal = pos =
122 ✗ Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
123 }
124
125 17 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
126 int b2_, const Vec3s& pos_, const Vec3s& normal_, Scalar depth_)
127 17 : o1(o1_),
128 17 o2(o2_),
129 17 b1(b1_),
130 17 b2(b2_),
131 17 normal(normal_),
132
5/10
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
 34 nearest_points{pos_ - (depth_ * normal_ / 2),
133
3/6
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
 17 pos_ + (depth_ * normal_ / 2)},
134 17 pos(pos_),
135 17 penetration_depth(depth_) {}
136
137 28914 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
138 int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_,
139 Scalar depth_)
140 28914 : o1(o1_),
141 28914 o2(o2_),
142 28914 b1(b1_),
143 28914 b2(b2_),
144 28914 normal(normal_),
145 28914 nearest_points{p1, p2},
146
3/6
✓ Branch 1 taken 28914 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28914 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28914 times.
✗ Branch 8 not taken.
 28914 pos((p1 + p2) / 2),
147 28914 penetration_depth(depth_) {}
148
149 ✗ bool operator<(const Contact& other) const {
150 ✗ if (b1 == other.b1) return b2 < other.b2;
151 ✗ return b1 < other.b1;
152 }
153
154 42 bool operator==(const Contact& other) const {
155
2/4
✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 42 return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
156
3/6
✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 42 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 42 times.
✗ Branch 7 not taken.
 42 b2 == other.b2 && normal == other.normal && pos == other.pos &&
157
1/2
✓ Branch 3 taken 42 times.
✗ Branch 4 not taken.
 42 nearest_points[0] == other.nearest_points[0] &&
158
2/4
✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
 126 nearest_points[1] == other.nearest_points[1] &&
159
1/2
✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
 84 penetration_depth == other.penetration_depth;
160 }
161
162 ✗ bool operator!=(const Contact& other) const { return !(*this == other); }
163
164 Scalar getDistanceToCollision(const CollisionRequest& request) const;
165 };
166
167 struct QueryResult;
168
169 /// @brief base class for all query requests
170 struct COAL_DLLAPI QueryRequest {
171 // @brief Initial guess to use for the GJK algorithm
172 GJKInitialGuess gjk_initial_guess;
173
174 /// @brief whether enable gjk initial guess
175 /// @Deprecated Use gjk_initial_guess instead
176 COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
177 bool enable_cached_gjk_guess;
178
179 /// @brief the gjk initial guess set by user
180 mutable Vec3s cached_gjk_guess;
181
182 /// @brief the support function initial guess set by user
183 mutable support_func_guess_t cached_support_func_guess;
184
185 /// @brief maximum iteration for the GJK algorithm
186 size_t gjk_max_iterations;
187
188 /// @brief tolerance for the GJK algorithm.
189 /// Note: This tolerance determines the precision on the estimated distance
190 /// between two geometries which are not in collision.
191 /// It is recommended to not set this tolerance to less than 1e-6.
192 Scalar gjk_tolerance;
193
194 /// @brief whether to enable the Nesterov accleration of GJK
195 GJKVariant gjk_variant;
196
197 /// @brief convergence criterion used to stop GJK
198 GJKConvergenceCriterion gjk_convergence_criterion;
199
200 /// @brief convergence criterion used to stop GJK
201 GJKConvergenceCriterionType gjk_convergence_criterion_type;
202
203 /// @brief max number of iterations for EPA
204 size_t epa_max_iterations;
205
206 /// @brief tolerance for EPA.
207 /// Note: This tolerance determines the precision on the estimated distance
208 /// between two geometries which are in collision.
209 /// It is recommended to not set this tolerance to less than 1e-6.
210 /// Also, setting EPA's tolerance to less than GJK's is not recommended.
211 Scalar epa_tolerance;
212
213 /// @brief enable timings when performing collision/distance request
214 bool enable_timings;
215
216 /// @brief threshold below which a collision is considered.
217 Scalar collision_distance_threshold;
218
219 COAL_COMPILER_DIAGNOSTIC_PUSH
220 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
221 /// @brief Default constructor.
222 99047 QueryRequest()
223 99047 : gjk_initial_guess(GJKInitialGuess::DefaultGuess),
224 99047 enable_cached_gjk_guess(false),
225
1/2
✓ Branch 1 taken 99047 times.
✗ Branch 2 not taken.
 99047 cached_gjk_guess(1, 0, 0),
226
1/2
✓ Branch 2 taken 99047 times.
✗ Branch 3 not taken.
 99047 cached_support_func_guess(support_func_guess_t::Zero()),
227 99047 gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
228 99047 gjk_tolerance(GJK_DEFAULT_TOLERANCE),
229 99047 gjk_variant(GJKVariant::DefaultGJK),
230 99047 gjk_convergence_criterion(GJKConvergenceCriterion::Default),
231 99047 gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
232 99047 epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
233 99047 epa_tolerance(EPA_DEFAULT_TOLERANCE),
234 99047 enable_timings(false),
235 99047 collision_distance_threshold(
236 99047 Eigen::NumTraits<Scalar>::dummy_precision()) {}
237
238 /// @brief Copy constructor.
239 103 QueryRequest(const QueryRequest& other) = default;
240
241 /// @brief Copy assignment operator.
242 7289 QueryRequest& operator=(const QueryRequest& other) = default;
243 COAL_COMPILER_DIAGNOSTIC_POP
244
245 /// @brief Updates the guess for the internal GJK algorithm in order to
246 /// warm-start it when reusing this collision request on the same collision
247 /// pair.
248 /// @note The option `gjk_initial_guess` must be set to
249 /// `GJKInitialGuess::CachedGuess` for this to work.
250 void updateGuess(const QueryResult& result) const;
251
252 /// @brief whether two QueryRequest are the same or not
253 28 inline bool operator==(const QueryRequest& other) const {
254 COAL_COMPILER_DIAGNOSTIC_PUSH
255 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
256 56 return gjk_initial_guess == other.gjk_initial_guess &&
257
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
258
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 gjk_variant == other.gjk_variant &&
259
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 gjk_convergence_criterion == other.gjk_convergence_criterion &&
260 28 gjk_convergence_criterion_type ==
261
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 other.gjk_convergence_criterion_type &&
262
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 gjk_tolerance == other.gjk_tolerance &&
263
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 gjk_max_iterations == other.gjk_max_iterations &&
264
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 cached_gjk_guess == other.cached_gjk_guess &&
265
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 cached_support_func_guess == other.cached_support_func_guess &&
266
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 epa_max_iterations == other.epa_max_iterations &&
267
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 28 epa_tolerance == other.epa_tolerance &&
268
2/4
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 84 enable_timings == other.enable_timings &&
269
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 56 collision_distance_threshold == other.collision_distance_threshold;
270 COAL_COMPILER_DIAGNOSTIC_POP
271 }
272 };
273
274 /// @brief base class for all query results
275 struct COAL_DLLAPI QueryResult {
276 /// @brief stores the last GJK ray when relevant.
277 Vec3s cached_gjk_guess;
278
279 /// @brief stores the last support function vertex index, when relevant.
280 support_func_guess_t cached_support_func_guess;
281
282 /// @brief timings for the given request
283 CPUTimes timings;
284
285 29280606 QueryResult()
286
1/2
✓ Branch 2 taken 29280606 times.
✗ Branch 3 not taken.
 29280606 : cached_gjk_guess(Vec3s::Zero()),
287
2/4
✓ Branch 1 taken 29280606 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29280606 times.
✗ Branch 5 not taken.
 29280606 cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
288 };
289
290 1294296 inline void QueryRequest::updateGuess(const QueryResult& result) const {
291 COAL_COMPILER_DIAGNOSTIC_PUSH
292 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
293
1/2
✓ Branch 0 taken 1294296 times.
✗ Branch 1 not taken.
 1294296 if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
294
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1294292 times.
 1294296 enable_cached_gjk_guess) {
295 4 cached_gjk_guess = result.cached_gjk_guess;
296 4 cached_support_func_guess = result.cached_support_func_guess;
297 }
298 COAL_COMPILER_DIAGNOSTIC_POP
299 1294296 }
300
301 struct CollisionResult;
302
303 /// @brief flag declaration for specifying required params in CollisionResult
304 enum CollisionRequestFlag {
305 CONTACT = 0x00001,
306 DISTANCE_LOWER_BOUND = 0x00002,
307 NO_REQUEST = 0x01000
308 };
309
310 /// @brief request to the collision algorithm
311 struct COAL_DLLAPI CollisionRequest : QueryRequest {
312 /// @brief The maximum number of contacts that can be returned
313 size_t num_max_contacts;
314
315 /// @brief whether the contact information (normal, penetration depth and
316 /// contact position) will return.
317 bool enable_contact;
318
319 /// Whether a lower bound on distance is returned when objects are disjoint
320 COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.)
321 bool enable_distance_lower_bound;
322
323 /// @brief Distance below which objects are considered in collision.
324 /// See \ref coal_collision_and_distance_lower_bound_computation
325 /// @note If set to -inf, the objects tested for collision are considered
326 /// as collision free and no test is actually performed by functions
327 /// coal::collide of class coal::ComputeCollision.
328 Scalar security_margin;
329
330 /// @brief Distance below which bounding volumes are broken down.
331 /// See \ref coal_collision_and_distance_lower_bound_computation
332 Scalar break_distance;
333
334 /// @brief Distance above which GJK solver makes an early stopping.
335 /// GJK stops searching for the closest points when it proves that the
336 /// distance between two geometries is above this threshold.
337 ///
338 /// @remarks Consequently, the closest points might be incorrect, but allows
339 /// to save computational resources.
340 Scalar distance_upper_bound;
341
342 /// @brief Constructor from a flag and a maximal number of contacts.
343 ///
344 /// @param[in] flag Collision request flag
345 /// @param[in] num_max_contacts Maximal number of allowed contacts
346 ///
347 COAL_COMPILER_DIAGNOSTIC_PUSH
348 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
349 16809 CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
350 33618 : num_max_contacts(num_max_contacts_),
351 16809 enable_contact(flag & CONTACT),
352 16809 enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND),
353 16809 security_margin(0),
354 16809 break_distance(Scalar(1e-3)),
355 16809 distance_upper_bound((std::numeric_limits<Scalar>::max)()) {}
356
357 /// @brief Default constructor.
358 50000 CollisionRequest()
359 100000 : num_max_contacts(1),
360 50000 enable_contact(true),
361 50000 enable_distance_lower_bound(false),
362 50000 security_margin(0),
363 50000 break_distance(Scalar(1e-3)),
364 50000 distance_upper_bound((std::numeric_limits<Scalar>::max)()) {}
365 COAL_COMPILER_DIAGNOSTIC_POP
366
367 bool isSatisfied(const CollisionResult& result) const;
368
369 /// @brief whether two CollisionRequest are the same or not
370 14 inline bool operator==(const CollisionRequest& other) const {
371 COAL_COMPILER_DIAGNOSTIC_PUSH
372 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
373 14 return QueryRequest::operator==(other) &&
374
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 num_max_contacts == other.num_max_contacts &&
375
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 enable_contact == other.enable_contact &&
376
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 enable_distance_lower_bound == other.enable_distance_lower_bound &&
377
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 security_margin == other.security_margin &&
378
2/4
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 42 break_distance == other.break_distance &&
379
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 28 distance_upper_bound == other.distance_upper_bound;
380 COAL_COMPILER_DIAGNOSTIC_POP
381 }
382 };
383
384 inline Scalar Contact::getDistanceToCollision(
385 const CollisionRequest& request) const {
386 return penetration_depth - request.security_margin;
387 }
388
389 /// @brief collision result
390 struct COAL_DLLAPI CollisionResult : QueryResult {
391 private:
392 /// @brief contact information
393 std::vector<Contact> contacts;
394
395 public:
396 /// Lower bound on distance between objects if they are disjoint.
397 /// See \ref coal_collision_and_distance_lower_bound_computation
398 /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is
399 /// set to infinity, distance_lower_bound is the actual distance between the
400 /// shapes.
401 Scalar distance_lower_bound;
402
403 /// @brief normal associated to nearest_points.
404 /// Same as `CollisionResult::nearest_points` but for the normal.
405 Vec3s normal;
406
407 /// @brief nearest points.
408 /// A `CollisionResult` can have multiple contacts.
409 /// The nearest points in `CollisionResults` correspond to the witness points
410 /// associated with the smallest distance i.e the `distance_lower_bound`.
411 /// For bounding volumes and BVHs, these nearest points are available
412 /// only when distance_lower_bound is inferior to
413 /// CollisionRequest::break_distance.
414 std::array<Vec3s, 2> nearest_points;
415
416 public:
417 66507 CollisionResult()
418
2/4
✓ Branch 4 taken 66507 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 66507 times.
✗ Branch 8 not taken.
 66507 : distance_lower_bound((std::numeric_limits<Scalar>::max)()) {
419 133014 nearest_points[0] = nearest_points[1] = normal =
420
4/8
✓ Branch 2 taken 66507 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66507 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 66507 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 66507 times.
✗ Branch 14 not taken.
 66507 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
421 66507 }
422
423 /// @brief Update the lower bound only if the distance is inferior.
424 8460475 inline void updateDistanceLowerBound(const Scalar& distance_lower_bound_) {
425
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8460475 times.
 8460475 if (distance_lower_bound_ < distance_lower_bound)
426 ✗ distance_lower_bound = distance_lower_bound_;
427 8460475 }
428
429 /// @brief add one contact into result structure
430 28930 inline void addContact(const Contact& c) { contacts.push_back(c); }
431
432 /// @brief whether two CollisionResult are the same or not
433 14 inline bool operator==(const CollisionResult& other) const {
434 14 return contacts == other.contacts &&
435
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 distance_lower_bound == other.distance_lower_bound &&
436
1/2
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
 14 nearest_points[0] == other.nearest_points[0] &&
437
2/4
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 42 nearest_points[1] == other.nearest_points[1] &&
438
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 normal == other.normal;
439 }
440
441 /// @brief return binary collision result
442 23354892 bool isCollision() const { return contacts.size() > 0; }
443
444 /// @brief number of contacts found
445 8670748 size_t numContacts() const { return contacts.size(); }
446
447 /// @brief get the i-th contact calculated
448 7178 const Contact& getContact(size_t i) const {
449
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7178 times.
 7178 if (contacts.size() == 0)
450 ✗ COAL_THROW_PRETTY(
451 "The number of contacts is zero. No Contact can be returned.",
452 std::invalid_argument);
453
454
1/2
✓ Branch 1 taken 7178 times.
✗ Branch 2 not taken.
 7178 if (i < contacts.size())
455 7178 return contacts[i];
456 else
457 ✗ return contacts.back();
458 }
459
460 /// @brief set the i-th contact calculated
461 16 void setContact(size_t i, const Contact& c) {
462
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 16 if (contacts.size() == 0)
463 ✗ COAL_THROW_PRETTY(
464 "The number of contacts is zero. No Contact can be returned.",
465 std::invalid_argument);
466
467
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 if (i < contacts.size())
468 16 contacts[i] = c;
469 else
470 ✗ contacts.back() = c;
471 16 }
472
473 /// @brief get all the contacts
474 ✗ void getContacts(std::vector<Contact>& contacts_) const {
475 ✗ contacts_.resize(contacts.size());
476 ✗ std::copy(contacts.begin(), contacts.end(), contacts_.begin());
477 }
478
479 7 const std::vector<Contact>& getContacts() const { return contacts; }
480
481 /// @brief clear the results obtained
482 32586 void clear() {
483 32586 distance_lower_bound = (std::numeric_limits<Scalar>::max)();
484 32586 contacts.clear();
485 32586 timings.clear();
486 65172 nearest_points[0] = nearest_points[1] = normal =
487
4/8
✓ Branch 2 taken 32586 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32586 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 32586 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 32586 times.
✗ Branch 14 not taken.
 32586 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
488 32586 }
489
490 /// @brief reposition Contact objects when fcl inverts them
491 /// during their construction.
492 void swapObjects();
493 };
494
495 /// @brief This structure allows to encode contact patches.
496 /// A contact patch is defined by a set of points belonging to a subset of a
497 /// plane passing by `p` and supported by `n`, where `n = Contact::normal` and
498 /// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first
499 /// and second shape of a collision pair, a contact patch is represented as a
500 /// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the
501 /// set-intersection. Since a contact patch is a subset of a plane supported by
502 /// `n`, it has a preferred direction. In Coal, the `Contact::normal` points
503 /// from S1 to S2. In the same way, a contact patch points by default from S1
504 /// to S2.
505 ///
506 /// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope),
507 /// so the points of the set, forming the convex-hull of the polytope, are
508 /// stored in a counter-clockwise fashion.
509 /// @note If needed, the internal algorithms of Coal can easily be extended
510 /// to compute a contact volume instead of a contact patch.
511 struct COAL_DLLAPI ContactPatch {
512 public:
513 using Polygon = std::vector<Vec2s>;
514
515 /// @brief Frame of the set, expressed in the world coordinates.
516 /// The z-axis of the frame's rotation is the contact patch normal.
517 Transform3s tf;
518
519 /// @brief Direction of ContactPatch.
520 /// When doing collision detection, the convention of Coal is that the
521 /// normal always points from the first to the second shape of the collision
522 /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`.
523 /// The PatchDirection enum allows to identify if the patch points from
524 /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted
525 /// type). The Inverted type should only be used for internal Coal
526 /// computations (it allows to properly define two separate contact patches in
527 /// the same frame).
528 enum PatchDirection { DEFAULT = 0, INVERTED = 1 };
529
530 /// @brief Direction of this contact patch.
531 PatchDirection direction;
532
533 /// @brief Penetration depth of the contact patch. This value corresponds to
534 /// the signed distance `d` between the shapes.
535 /// @note For each contact point `p` in the patch of normal `n`, `p1 = p -
536 /// 0.5*d*n` and `p2 = p + 0.5*d*n` define a pair of witness points. `p1`
537 /// belongs to the surface of the first shape and `p2` belongs to the surface
538 /// of the second shape. For any pair of witness points, we always have `p2 -
539 /// p1 = d * n`. The vector `d * n` is called a minimum separation vector:
540 /// if S1 is translated by it, S1 and S2 are not in collision anymore.
541 /// @note Although there may exist multiple minimum separation vectors between
542 /// two shapes, the term "minimum" comes from the fact that it's impossible to
543 /// find a different separation vector which has a smaller norm than `d * n`.
544 Scalar penetration_depth;
545
546 /// @brief Default maximum size of the polygon representing the contact patch.
547 /// Used to pre-allocate memory for the patch.
548 static constexpr size_t default_preallocated_size = 12;
549
550 protected:
551 /// @brief Container for the vertices of the set.
552 Polygon m_points;
553
554 public:
555 /// @brief Default constructor.
556 /// Note: the preallocated size does not determine the maximum number of
557 /// points in the patch, it only serves as preallocation if the maximum size
558 /// of the patch is known in advance. Coal will automatically expand/shrink
559 /// the contact patch if needed.
560 124 explicit ContactPatch(size_t preallocated_size = default_preallocated_size)
561 124 : tf(Transform3s::Identity()),
562 124 direction(PatchDirection::DEFAULT),
563 124 penetration_depth(0) {
564
1/2
✓ Branch 1 taken 124 times.
✗ Branch 2 not taken.
 124 this->m_points.reserve(preallocated_size);
565 124 }
566
567 /// @brief Normal of the contact patch, expressed in the WORLD frame.
568 84 Vec3s getNormal() const {
569
2/2
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 61 times.
 84 if (this->direction == PatchDirection::INVERTED) {
570
2/4
✓ Branch 3 taken 23 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 23 times.
✗ Branch 7 not taken.
 46 return -this->tf.rotation().col(2);
571 }
572
1/2
✓ Branch 3 taken 61 times.
✗ Branch 4 not taken.
 122 return this->tf.rotation().col(2);
573 }
574
575 /// @brief Returns the number of points in the contact patch.
576 890 size_t size() const { return this->m_points.size(); }
577
578 /// @brief Add a 3D point to the set, expressed in the world frame.
579 /// @note This function takes a 3D point and expresses it in the local frame
580 /// of the set. It then takes only the x and y components of the vector,
581 /// effectively doing a projection onto the plane to which the set belongs.
582 /// TODO(louis): if necessary, we can store the offset to the plane (x, y).
583 119 void addPoint(const Vec3s& point_3d) {
584
1/2
✓ Branch 1 taken 119 times.
✗ Branch 2 not taken.
 119 const Vec3s point = this->tf.inverseTransform(point_3d);
585
2/4
✓ Branch 1 taken 119 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 119 times.
✗ Branch 5 not taken.
 119 this->m_points.emplace_back(point.template head<2>());
586 119 }
587
588 /// @brief Get the i-th point of the set, expressed in the 3D world frame.
589 644 Vec3s getPoint(const size_t i) const {
590
1/2
✓ Branch 1 taken 644 times.
✗ Branch 2 not taken.
 644 Vec3s point(0, 0, 0);
591
1/2
✓ Branch 3 taken 644 times.
✗ Branch 4 not taken.
 644 point.head<2>() = this->point(i);
592 644 point = tf.transform(point);
593 644 return point;
594 }
595
596 /// @brief Get the i-th point of the contact patch, projected back onto the
597 /// first shape of the collision pair. This point is expressed in the 3D
598 /// world frame.
599 ✗ Vec3s getPointShape1(const size_t i) const {
600 ✗ Vec3s point = this->getPoint(i);
601 ✗ point -= (this->penetration_depth / 2) * this->getNormal();
602 ✗ return point;
603 }
604
605 /// @brief Get the i-th point of the contact patch, projected back onto the
606 /// first shape of the collision pair. This 3D point is expressed in the world
607 /// frame.
608 ✗ Vec3s getPointShape2(const size_t i) const {
609 ✗ Vec3s point = this->getPoint(i);
610 ✗ point += (this->penetration_depth / 2) * this->getNormal();
611 ✗ return point;
612 }
613
614 /// @brief Getter for the 2D points in the set.
615 269 Polygon& points() { return this->m_points; }
616
617 /// @brief Const getter for the 2D points in the set.
618 80 const Polygon& points() const { return this->m_points; }
619
620 /// @brief Getter for the i-th 2D point in the set.
621 ✗ Vec2s& point(const size_t i) {
622 ✗ COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
623 ✗ if (i < this->m_points.size()) {
624 ✗ return this->m_points[i];
625 }
626 ✗ return this->m_points.back();
627 }
628
629 /// @brief Const getter for the i-th 2D point in the set.
630 644 const Vec2s& point(const size_t i) const {
631
1/2
✓ Branch 3 taken 644 times.
✗ Branch 4 not taken.
 644 COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
632
1/2
✓ Branch 1 taken 644 times.
✗ Branch 2 not taken.
 644 if (i < this->m_points.size()) {
633 644 return this->m_points[i];
634 }
635 ✗ return this->m_points.back();
636 }
637
638 /// @brief Clear the set.
639 125 void clear() {
640 125 this->m_points.clear();
641 125 this->tf.setIdentity();
642 125 this->penetration_depth = 0;
643 125 }
644
645 /// @brief Whether two contact patches are the same or not.
646 /// @note This compares the two sets terms by terms.
647 /// However, two contact patches can be identical, but have a different
648 /// order for their points. Use `isEqual` in this case.
649 28 bool operator==(const ContactPatch& other) const {
650
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 return this->tf == other.tf && this->direction == other.direction &&
651
2/4
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 84 this->penetration_depth == other.penetration_depth &&
652
1/2
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 56 this->points() == other.points();
653 }
654
655 /// @brief Whether two contact patches are the same or not.
656 /// Checks for different order of the points.
657 22 bool isSame(
658 const ContactPatch& other,
659 const Scalar tol = Eigen::NumTraits<Scalar>::dummy_precision()) const {
660 // The x and y axis of the set are arbitrary, but the z axis is
661 // always the normal. The position of the origin of the frame is also
662 // arbitrary. So we only check if the normals are the same.
663
3/6
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 22 times.
 22 if (!this->getNormal().isApprox(other.getNormal(), tol)) {
664 ✗ return false;
665 }
666
667
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
 22 if (std::abs(this->penetration_depth - other.penetration_depth) > tol) {
668 ✗ return false;
669 }
670
671
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
 22 if (this->direction != other.direction) {
672 ✗ return false;
673 }
674
675
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
 22 if (this->size() != other.size()) {
676 ✗ return false;
677 }
678
679 // Check all points of the contact patch.
680
2/2
✓ Branch 1 taken 79 times.
✓ Branch 2 taken 22 times.
 101 for (size_t i = 0; i < this->size(); ++i) {
681 79 bool found = false;
682
1/2
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
 79 const Vec3s pi = this->getPoint(i);
683
2/2
✓ Branch 1 taken 559 times.
✓ Branch 2 taken 79 times.
 638 for (size_t j = 0; j < other.size(); ++j) {
684
1/2
✓ Branch 1 taken 559 times.
✗ Branch 2 not taken.
 559 const Vec3s other_pj = other.getPoint(j);
685
3/4
✓ Branch 1 taken 559 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 79 times.
✓ Branch 4 taken 480 times.
 559 if (pi.isApprox(other_pj, tol)) {
686 79 found = true;
687 }
688 }
689
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 79 times.
 79 if (!found) {
690 ✗ return false;
691 }
692 }
693
694 22 return true;
695 }
696 };
697
698 /// @brief Construct a frame from a `Contact`'s position and normal.
699 /// Because both `Contact`'s position and normal are expressed in the world
700 /// frame, this frame is also expressed w.r.t the world frame.
701 /// The origin of the frame is `contact.pos` and the z-axis of the frame is
702 /// `contact.normal`.
703 24 inline void constructContactPatchFrameFromContact(const Contact& contact,
704 ContactPatch& contact_patch) {
705 24 contact_patch.penetration_depth = contact.penetration_depth;
706 24 contact_patch.tf.translation() = contact.pos;
707 24 contact_patch.tf.rotation() =
708 48 constructOrthonormalBasisFromVector(contact.normal);
709 24 contact_patch.direction = ContactPatch::PatchDirection::DEFAULT;
710 24 }
711
712 /// @brief Structure used for internal computations. A support set and a
713 /// contact patch can be represented by the same structure. In fact, a contact
714 /// patch is the intersection of two support sets, one with
715 /// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`.
716 /// @note A support set with `DEFAULT` direction is the support set of a shape
717 /// in the direction given by `+n`, where `n` is the z-axis of the frame's
718 /// patch rotation. An `INVERTED` support set is the support set of a shape in
719 /// the direction `-n`.
720 using SupportSet = ContactPatch;
721
722 /// @brief Request for a contact patch computation.
723 struct COAL_DLLAPI ContactPatchRequest {
724 /// @brief Maximum number of contact patches that will be computed.
725 size_t max_num_patch;
726
727 protected:
728 /// @brief Maximum samples to compute the support sets of curved shapes,
729 /// i.e. when the normal is perpendicular to the base of a cylinder. For
730 /// now, only relevant for Cone and Cylinder. In the future this might be
731 /// extended to Sphere and Ellipsoid.
732 size_t m_num_samples_curved_shapes;
733
734 /// @brief Tolerance below which points are added to a contact patch.
735 /// In details, given two shapes S1 and S2, a contact patch is the triple
736 /// intersection between the separating plane (P) (passing by `Contact::pos`
737 /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is
738 /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point
739 /// p1 of S1 is at a distance below `patch_tolerance` from the separating
740 /// plane, it is taken into account in the computation of the contact patch.
741 /// Otherwise, it is not used for the computation.
742 /// @note Needs to be positive.
743 Scalar m_patch_tolerance;
744
745 public:
746 /// @brief Default constructor.
747 /// @param max_num_patch maximum number of contact patches per collision pair.
748 /// @param max_sub_patch_size maximum size of each sub contact patch. Each
749 /// contact patch contains an internal representation for an inscribed sub
750 /// contact patch. This allows physics simulation to always work with a
751 /// predetermined maximum size for each contact patch. A sub contact patch is
752 /// simply a subset of the vertices of a contact patch.
753 /// @param num_samples_curved_shapes for shapes like cones and cylinders,
754 /// which have smooth basis (circles in this case), we need to sample a
755 /// certain amount of point of this basis.
756 /// @param patch_tolerance the tolerance below which a point of a shape is
757 /// considered to belong to the support set of this shape in the direction of
758 /// the normal. Said otherwise, `patch_tolerance` determines the "thickness"
759 /// of the separating plane between shapes of a collision pair.
760 24 explicit ContactPatchRequest(size_t max_num_patch = 1,
761 size_t num_samples_curved_shapes =
762 ContactPatch::default_preallocated_size,
763 Scalar patch_tolerance = Scalar(1e-3))
764 24 : max_num_patch(max_num_patch) {
765 24 this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
766 24 this->setPatchTolerance(patch_tolerance);
767 24 }
768
769 /// @brief Construct a contact patch request from a collision request.
770 ✗ explicit ContactPatchRequest(const CollisionRequest& collision_request,
771 size_t num_samples_curved_shapes =
772 ContactPatch::default_preallocated_size,
773 Scalar patch_tolerance = Scalar(1e-3))
774 ✗ : max_num_patch(collision_request.num_max_contacts) {
775 ✗ this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
776 ✗ this->setPatchTolerance(patch_tolerance);
777 }
778
779 /// @copydoc m_num_samples_curved_shapes
780 31 void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) {
781
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 31 times.
 31 if (num_samples_curved_shapes < 3) {
782 COAL_LOG_WARNING(
783 "`num_samples_curved_shapes` cannot be lower than 3. Setting it to "
784 "3 to prevent bugs.");
785 ✗ this->m_num_samples_curved_shapes = 3;
786 } else {
787 31 this->m_num_samples_curved_shapes = num_samples_curved_shapes;
788 }
789 31 }
790
791 /// @copydoc m_num_samples_curved_shapes
792 190 size_t getNumSamplesCurvedShapes() const {
793 190 return this->m_num_samples_curved_shapes;
794 }
795
796 /// @copydoc m_patch_tolerance
797 31 void setPatchTolerance(const Scalar patch_tolerance) {
798
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 31 times.
 31 if (patch_tolerance < 0) {
799 COAL_LOG_WARNING(
800 "`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
801 "bugs.");
802 ✗ this->m_patch_tolerance = Eigen::NumTraits<Scalar>::dummy_precision();
803 } else {
804 31 this->m_patch_tolerance = patch_tolerance;
805 }
806 31 }
807
808 /// @copydoc m_patch_tolerance
809 66 Scalar getPatchTolerance() const { return this->m_patch_tolerance; }
810
811 /// @brief Whether two ContactPatchRequest are identical or not.
812 14 bool operator==(const ContactPatchRequest& other) const {
813
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 return this->max_num_patch == other.max_num_patch &&
814 14 this->getNumSamplesCurvedShapes() ==
815
2/4
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
 42 other.getNumSamplesCurvedShapes() &&
816 28 this->getPatchTolerance() == other.getPatchTolerance();
817 }
818 };
819
820 /// @brief Result for a contact patch computation.
821 struct COAL_DLLAPI ContactPatchResult {
822 using ContactPatchVector = std::vector<ContactPatch>;
823 using ContactPatchRef = std::reference_wrapper<ContactPatch>;
824 using ContactPatchRefVector = std::vector<ContactPatchRef>;
825
826 protected:
827 /// @brief Data container for the vector of contact patches.
828 /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a
829 /// very small memory footprint, contact patches can contain relatively
830 /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding
831 /// successive mallocs, we have a data container and a vector which points
832 /// to the currently active patches in this data container.
833 ContactPatchVector m_contact_patches_data;
834
835 /// @brief Contact patches in `m_contact_patches_data` can have two
836 /// statuses: used or unused. This index tracks the first unused patch in
837 /// the `m_contact_patches_data` vector.
838 size_t m_id_available_patch;
839
840 /// @brief Vector of contact patches of the result.
841 ContactPatchRefVector m_contact_patches;
842
843 public:
844 /// @brief Default constructor.
845 2 ContactPatchResult() : m_id_available_patch(0) {
846 2 const size_t max_num_patch = 1;
847
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const ContactPatchRequest request(max_num_patch);
848
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 this->set(request);
849 2 }
850
851 /// @brief Constructor using a `ContactPatchRequest`.
852 16 explicit ContactPatchResult(const ContactPatchRequest& request)
853 16 : m_id_available_patch(0) {
854
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 this->set(request);
855 16 };
856
857 /// @brief Number of contact patches in the result.
858 119 size_t numContactPatches() const { return this->m_contact_patches.size(); }
859
860 /// @brief Returns a new unused contact patch from the internal data vector.
861 31 ContactPatchRef getUnusedContactPatch() {
862
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 31 times.
 31 if (this->m_id_available_patch >= this->m_contact_patches_data.size()) {
863 COAL_LOG_WARNING(
864 "Trying to get an unused contact patch but all contact patches are "
865 "used. Increasing size of contact patches vector, at the cost of a "
866 "copy. You should increase `max_num_patch` in the "
867 "`ContactPatchRequest`.");
868 ✗ this->m_contact_patches_data.emplace_back(
869 ✗ this->m_contact_patches_data.back());
870 ✗ this->m_contact_patches_data.back().clear();
871 }
872 ContactPatch& contact_patch =
873 31 this->m_contact_patches_data[this->m_id_available_patch];
874 31 contact_patch.clear();
875 31 this->m_contact_patches.emplace_back(contact_patch);
876 31 ++(this->m_id_available_patch);
877 31 return this->m_contact_patches.back();
878 }
879
880 /// @brief Const getter for the i-th contact patch of the result.
881 59 const ContactPatch& getContactPatch(const size_t i) const {
882
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59 times.
 59 if (this->m_contact_patches.empty()) {
883 ✗ COAL_THROW_PRETTY(
884 "The number of contact patches is zero. No ContactPatch can be "
885 "returned.",
886 std::invalid_argument);
887 }
888
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 if (i < this->m_contact_patches.size()) {
889 59 return this->m_contact_patches[i];
890 }
891 ✗ return this->m_contact_patches.back();
892 }
893
894 /// @brief Getter for the i-th contact patch of the result.
895 ✗ ContactPatch& contactPatch(const size_t i) {
896 ✗ if (this->m_contact_patches.empty()) {
897 ✗ COAL_THROW_PRETTY(
898 "The number of contact patches is zero. No ContactPatch can be "
899 "returned.",
900 std::invalid_argument);
901 }
902 ✗ if (i < this->m_contact_patches.size()) {
903 ✗ return this->m_contact_patches[i];
904 }
905 ✗ return this->m_contact_patches.back();
906 }
907
908 /// @brief Clears the contact patch result.
909 61 void clear() {
910 61 this->m_contact_patches.clear();
911 61 this->m_id_available_patch = 0;
912
2/2
✓ Branch 5 taken 61 times.
✓ Branch 6 taken 61 times.
 122 for (ContactPatch& patch : this->m_contact_patches_data) {
913
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 61 patch.clear();
914 }
915 61 }
916
917 /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest`
918 49 void set(const ContactPatchRequest& request) {
919
2/2
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 31 times.
 49 if (this->m_contact_patches_data.size() < request.max_num_patch) {
920 18 this->m_contact_patches_data.resize(request.max_num_patch);
921 }
922
2/2
✓ Branch 5 taken 49 times.
✓ Branch 6 taken 49 times.
 98 for (ContactPatch& patch : this->m_contact_patches_data) {
923
1/2
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
 49 patch.points().reserve(request.getNumSamplesCurvedShapes());
924 }
925 49 this->clear();
926 49 }
927
928 /// @brief Return true if this `ContactPatchResult` is aligned with the
929 /// `ContactPatchRequest` given as input.
930 24 bool check(const ContactPatchRequest& request) const {
931
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 24 assert(this->m_contact_patches_data.size() >= request.max_num_patch);
932
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 24 if (this->m_contact_patches_data.size() < request.max_num_patch) {
933 ✗ return false;
934 }
935
936
2/2
✓ Branch 5 taken 24 times.
✓ Branch 6 taken 24 times.
 48 for (const ContactPatch& patch : this->m_contact_patches_data) {
937
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 24 times.
 24 if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) {
938 ✗ assert(patch.points().capacity() >=
939 request.getNumSamplesCurvedShapes());
940 ✗ return false;
941 }
942 }
943 24 return true;
944 }
945
946 /// @brief Whether two ContactPatchResult are identical or not.
947 14 bool operator==(const ContactPatchResult& other) const {
948
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 14 if (this->numContactPatches() != other.numContactPatches()) {
949 ✗ return false;
950 }
951
952
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 14 times.
 28 for (size_t i = 0; i < this->numContactPatches(); ++i) {
953 14 const ContactPatch& patch = this->getContactPatch(i);
954 14 const ContactPatch& other_patch = other.getContactPatch(i);
955
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
 14 if (!(patch == other_patch)) {
956 ✗ return false;
957 }
958 }
959
960 14 return true;
961 }
962
963 /// @brief Repositions the ContactPatch when they get inverted during their
964 /// construction.
965 ✗ void swapObjects() {
966 // Create new transform: it's the reflection of `tf` along the z-axis.
967 // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
968 // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
969 ✗ for (size_t i = 0; i < this->numContactPatches(); ++i) {
970 ✗ ContactPatch& patch = this->contactPatch(i);
971 ✗ patch.tf.rotation().col(0) *= -1.0;
972 ✗ patch.tf.rotation().col(2) *= -1.0;
973
974 ✗ for (size_t j = 0; j < patch.size(); ++j) {
975 ✗ patch.point(i)(0) *= Scalar(-1); // only invert the x-axis
976 }
977 }
978 }
979 };
980
981 struct DistanceResult;
982
983 /// @brief request to the distance computation
984 struct COAL_DLLAPI DistanceRequest : QueryRequest {
985 /// @brief whether to return the nearest points.
986 /// Nearest points are always computed and are the points of the shapes that
987 /// achieve a distance of `DistanceResult::min_distance`.
988 COAL_DEPRECATED_MESSAGE(
989 Nearest points are always computed : they are the points of the shapes
990 that achieve a distance of
991 `DistanceResult::min_distance`
992 .\n Use `enable_signed_distance` if you want to compute a signed
993 minimum distance(and thus its corresponding nearest points)
994 .)
995 bool enable_nearest_points;
996
997 /// @brief whether to compute the penetration depth when objects are in
998 /// collision.
999 /// Turning this off can save computation time if only the distance
1000 /// when objects are disjoint is needed.
1001 /// @note The minimum distance between the shapes is stored in
1002 /// `DistanceResult::min_distance`.
1003 /// If `enable_signed_distance` is off, `DistanceResult::min_distance`
1004 /// is always positive.
1005 /// If `enable_signed_distance` is on, `DistanceResult::min_distance`
1006 /// can be positive or negative.
1007 /// The nearest points are the points of the shapes that achieve
1008 /// a distance of `DistanceResult::min_distance`.
1009 bool enable_signed_distance;
1010
1011 /// @brief error threshold for approximate distance
1012 Scalar rel_err; // relative error, between 0 and 1
1013 Scalar abs_err; // absolute error
1014
1015 /// \param enable_nearest_points_ enables the nearest points computation.
1016 /// \param enable_signed_distance_ allows to compute the penetration depth
1017 /// \param rel_err_
1018 /// \param abs_err_
1019 COAL_COMPILER_DIAGNOSTIC_PUSH
1020 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
1021 32238 DistanceRequest(bool enable_nearest_points_ = true,
1022 bool enable_signed_distance_ = true, Scalar rel_err_ = 0.0,
1023 Scalar abs_err_ = 0.0)
1024 64476 : enable_nearest_points(enable_nearest_points_),
1025 32238 enable_signed_distance(enable_signed_distance_),
1026 32238 rel_err(rel_err_),
1027 32238 abs_err(abs_err_) {}
1028 COAL_COMPILER_DIAGNOSTIC_POP
1029
1030 bool isSatisfied(const DistanceResult& result) const;
1031
1032 COAL_COMPILER_DIAGNOSTIC_PUSH
1033 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
1034 7289 DistanceRequest& operator=(const DistanceRequest& other) = default;
1035 COAL_COMPILER_DIAGNOSTIC_POP
1036
1037 /// @brief whether two DistanceRequest are the same or not
1038 14 inline bool operator==(const DistanceRequest& other) const {
1039 COAL_COMPILER_DIAGNOSTIC_PUSH
1040 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
1041 14 return QueryRequest::operator==(other) &&
1042
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 enable_nearest_points == other.enable_nearest_points &&
1043
1/2
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
 14 enable_signed_distance == other.enable_signed_distance &&
1044
3/6
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 rel_err == other.rel_err && abs_err == other.abs_err;
1045 COAL_COMPILER_DIAGNOSTIC_POP
1046 }
1047 };
1048
1049 /// @brief distance result
1050 struct COAL_DLLAPI DistanceResult : QueryResult {
1051 public:
1052 /// @brief minimum distance between two objects.
1053 /// If two objects are in collision and
1054 /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0.
1055 /// @note The nearest points are the points of the shapes that achieve a
1056 /// distance of `DistanceResult::min_distance`.
1057 Scalar min_distance;
1058
1059 /// @brief normal.
1060 Vec3s normal;
1061
1062 /// @brief nearest points.
1063 /// See CollisionResult::nearest_points.
1064 std::array<Vec3s, 2> nearest_points;
1065
1066 /// @brief collision object 1
1067 const CollisionGeometry* o1;
1068
1069 /// @brief collision object 2
1070 const CollisionGeometry* o2;
1071
1072 /// @brief information about the nearest point in object 1
1073 /// if object 1 is mesh or point cloud, it is the triangle or point id
1074 /// if object 1 is geometry shape, it is NONE (-1),
1075 /// if object 1 is octree, it is the id of the cell
1076 int b1;
1077
1078 /// @brief information about the nearest point in object 2
1079 /// if object 2 is mesh or point cloud, it is the triangle or point id
1080 /// if object 2 is geometry shape, it is NONE (-1),
1081 /// if object 2 is octree, it is the id of the cell
1082 int b2;
1083
1084 /// @brief invalid contact primitive information
1085 static const int NONE = -1;
1086
1087 29214099 DistanceResult(Scalar min_distance_ = (std::numeric_limits<Scalar>::max)())
1088 29214099 : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
1089
2/4
✓ Branch 2 taken 29214099 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29214099 times.
✗ Branch 6 not taken.
 29214099 const Vec3s nan(Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
1090
3/6
✓ Branch 1 taken 29214099 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 29214099 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 29214099 times.
✗ Branch 10 not taken.
 29214099 nearest_points[0] = nearest_points[1] = normal = nan;
1091 29214099 }
1092
1093 /// @brief add distance information into the result
1094 void update(Scalar distance, const CollisionGeometry* o1_,
1095 const CollisionGeometry* o2_, int b1_, int b2_) {
1096 if (min_distance > distance) {
1097 min_distance = distance;
1098 o1 = o1_;
1099 o2 = o2_;
1100 b1 = b1_;
1101 b2 = b2_;
1102 }
1103 }
1104
1105 /// @brief add distance information into the result
1106 356548 void update(Scalar distance, const CollisionGeometry* o1_,
1107 const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1,
1108 const Vec3s& p2, const Vec3s& normal_) {
1109
2/2
✓ Branch 0 taken 14656 times.
✓ Branch 1 taken 341892 times.
 356548 if (min_distance > distance) {
1110 14656 min_distance = distance;
1111 14656 o1 = o1_;
1112 14656 o2 = o2_;
1113 14656 b1 = b1_;
1114 14656 b2 = b2_;
1115 14656 nearest_points[0] = p1;
1116 14656 nearest_points[1] = p2;
1117 14656 normal = normal_;
1118 }
1119 356548 }
1120
1121 /// @brief add distance information into the result
1122 void update(const DistanceResult& other_result) {
1123 if (min_distance > other_result.min_distance) {
1124 min_distance = other_result.min_distance;
1125 o1 = other_result.o1;
1126 o2 = other_result.o2;
1127 b1 = other_result.b1;
1128 b2 = other_result.b2;
1129 nearest_points[0] = other_result.nearest_points[0];
1130 nearest_points[1] = other_result.nearest_points[1];
1131 normal = other_result.normal;
1132 }
1133 }
1134
1135 /// @brief clear the result
1136 6361 void clear() {
1137
2/4
✓ Branch 2 taken 6361 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6361 times.
✗ Branch 6 not taken.
 6361 const Vec3s nan(Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
1138 6361 min_distance = (std::numeric_limits<Scalar>::max)();
1139 6361 o1 = NULL;
1140 6361 o2 = NULL;
1141 6361 b1 = NONE;
1142 6361 b2 = NONE;
1143
3/6
✓ Branch 1 taken 6361 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6361 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6361 times.
✗ Branch 10 not taken.
 6361 nearest_points[0] = nearest_points[1] = normal = nan;
1144 6361 timings.clear();
1145 6361 }
1146
1147 /// @brief whether two DistanceResult are the same or not
1148 14 inline bool operator==(const DistanceResult& other) const {
1149 28 bool is_same = min_distance == other.min_distance &&
1150
1/2
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
 14 nearest_points[0] == other.nearest_points[0] &&
1151
1/2
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
 14 nearest_points[1] == other.nearest_points[1] &&
1152
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 normal == other.normal && o1 == other.o1 && o2 == other.o2 &&
1153
3/6
✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 b1 == other.b1 && b2 == other.b2;
1154
1155 // TODO: check also that two GeometryObject are indeed equal.
1156
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 14 if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
1157 14 is_same &= (o1 == other.o1);
1158 // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 ==
1159 // *other.o1;
1160
1161
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
 14 if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
1162 14 is_same &= (o2 == other.o2);
1163 // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 ==
1164 // *other.o2;
1165
1166 14 return is_same;
1167 }
1168 };
1169
1170 namespace internal {
1171 154595 inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
1172 CollisionResult& res,
1173 const Scalar sqrDistLowerBound) {
1174 // BV cannot find negative distance.
1175
2/2
✓ Branch 0 taken 51323 times.
✓ Branch 1 taken 103272 times.
 154595 if (res.distance_lower_bound <= 0) return;
1176 103272 Scalar new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin;
1177
2/2
✓ Branch 0 taken 11157 times.
✓ Branch 1 taken 92115 times.
 103272 if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
1178 }
1179
1180 30450971 inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
1181 CollisionResult& res,
1182 const Scalar& distance,
1183 const Vec3s& p0, const Vec3s& p1,
1184 const Vec3s& normal) {
1185
2/2
✓ Branch 0 taken 55516 times.
✓ Branch 1 taken 30395455 times.
 30450971 if (distance < res.distance_lower_bound) {
1186 55516 res.distance_lower_bound = distance;
1187 55516 res.nearest_points[0] = p0;
1188 55516 res.nearest_points[1] = p1;
1189 55516 res.normal = normal;
1190 }
1191 30450971 }
1192 } // namespace internal
1193
1194 inline CollisionRequestFlag operator~(CollisionRequestFlag a) {
1195 return static_cast<CollisionRequestFlag>(~static_cast<int>(a));
1196 }
1197
1198 1007 inline CollisionRequestFlag operator|(CollisionRequestFlag a,
1199 CollisionRequestFlag b) {
1200 1007 return static_cast<CollisionRequestFlag>(static_cast<int>(a) |
1201 1007 static_cast<int>(b));
1202 }
1203
1204 inline CollisionRequestFlag operator&(CollisionRequestFlag a,
1205 CollisionRequestFlag b) {
1206 return static_cast<CollisionRequestFlag>(static_cast<int>(a) &
1207 static_cast<int>(b));
1208 }
1209
1210 inline CollisionRequestFlag operator^(CollisionRequestFlag a,
1211 CollisionRequestFlag b) {
1212 return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^
1213 static_cast<int>(b));
1214 }
1215
1216 inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a,
1217 CollisionRequestFlag b) {
1218 return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b));
1219 }
1220
1221 inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a,
1222 CollisionRequestFlag b) {
1223 return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b));
1224 }
1225
1226 inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a,
1227 CollisionRequestFlag b) {
1228 return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b));
1229 }
1230
1231 } // namespace coal
1232
1233 #endif
1234