GCC Code Coverage Report

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