hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
narrowphase.h
Go to the documentation of this file.
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) 2018-2019, Centre National de la Recherche Scientifique
7 * All rights reserved.
8 * Copyright (c) 2021-2022, INRIA
9 * All rights reserved.
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of Open Source Robotics Foundation nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 */
37
40#ifndef HPP_FCL_NARROWPHASE_H
41#define HPP_FCL_NARROWPHASE_H
42
43#include <limits>
44#include <iostream>
45
48
49namespace hpp {
50namespace fcl {
51
55 typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;
56
58 template <typename S1, typename S2>
60 const S1& s1, const S2& s2, Vec3f& guess,
61 support_func_guess_t& support_hint) const {
62 switch (gjk_initial_guess) {
63 case GJKInitialGuess::DefaultGuess:
64 guess = Vec3f(1, 0, 0);
65 support_hint.setZero();
66 break;
67 case GJKInitialGuess::CachedGuess:
68 guess = cached_guess;
69 support_hint = support_func_cached_guess;
70 break;
71 case GJKInitialGuess::BoundingVolumeGuess:
72 if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
74 "computeLocalAABB must have been called on the shapes before "
75 "using "
76 "GJKInitialGuess::BoundingVolumeGuess.",
77 std::logic_error);
78 }
79 guess.noalias() = s1.aabb_local.center() -
80 (shape.oR1 * s2.aabb_local.center() + shape.ot1);
81 support_hint.setZero();
82 break;
83 default:
84 HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
85 }
86 // TODO: use gjk_initial_guess instead
89 if (enable_cached_guess) {
90 guess = cached_guess;
91 support_hint = support_func_cached_guess;
92 }
94
95 gjk.setDistanceEarlyBreak(distance_upper_bound);
96
97 gjk.gjk_variant = gjk_variant;
98 gjk.convergence_criterion = gjk_convergence_criterion;
99 gjk.convergence_criterion_type = gjk_convergence_criterion_type;
100 }
101
103 template <typename S1, typename S2>
104 bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2,
105 const Transform3f& tf2, FCL_REAL& distance_lower_bound,
106 bool enable_penetration, Vec3f* contact_points,
107 Vec3f* normal) const {
109 shape.set(&s1, &s2, tf1, tf2);
110
111 Vec3f guess;
112 support_func_guess_t support_hint;
113 details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
114 initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
115
116 details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
119 if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
120 enable_cached_guess) {
121 cached_guess = gjk.getGuessFromSimplex();
122 support_func_cached_guess = gjk.support_hint;
123 }
125
126 Vec3f w0, w1;
127 switch (gjk_status) {
128 case details::GJK::Inside:
129 if (!enable_penetration && contact_points == NULL && normal == NULL)
130 return true;
131 if (gjk.hasPenetrationInformation(shape)) {
132 gjk.getClosestPoints(shape, w0, w1);
133 distance_lower_bound = gjk.distance;
134 if (normal)
135 (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized();
136 if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
137 return true;
138 } else {
139 details::EPA epa(epa_max_face_num, epa_max_vertex_num,
140 epa_max_iterations, epa_tolerance);
141 details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
142 if (epa_status & details::EPA::Valid ||
143 epa_status == details::EPA::OutOfFaces // Warnings
144 || epa_status == details::EPA::OutOfVertices // Warnings
145 ) {
146 epa.getClosestPoints(shape, w0, w1);
147 distance_lower_bound = -epa.depth;
148 if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal;
149 if (contact_points)
150 *contact_points =
151 tf1.transform(w0 - epa.normal * (epa.depth * 0.5));
152 return true;
153 } else if (epa_status == details::EPA::FallBack) {
154 epa.getClosestPoints(shape, w0, w1);
155 distance_lower_bound = -epa.depth; // Should be zero
156 if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal;
157 if (contact_points) *contact_points = tf1.transform(w0);
158 return true;
159 }
160 distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)();
161 // EPA failed but we know there is a collision so we should
162 return true;
163 }
164 break;
165 case details::GJK::Valid:
166 distance_lower_bound = gjk.distance;
167 break;
168 default:;
169 }
170
171 return false;
172 }
173
177 template <typename S>
178 bool shapeTriangleInteraction(const S& s, const Transform3f& tf1,
179 const Vec3f& P1, const Vec3f& P2,
180 const Vec3f& P3, const Transform3f& tf2,
181 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
182 Vec3f& normal) const {
183 bool col;
184 // Express everything in frame 1
185 const Transform3f tf_1M2(tf1.inverseTimes(tf2));
186 TriangleP tri(tf_1M2.transform(P1), tf_1M2.transform(P2),
187 tf_1M2.transform(P3));
188
190 shape.set(&s, &tri);
191
192 Vec3f guess;
193 support_func_guess_t support_hint;
194 details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
195 initialize_gjk(gjk, shape, s, tri, guess, support_hint);
196
197 details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
198
201 if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
202 enable_cached_guess) {
203 cached_guess = gjk.getGuessFromSimplex();
204 support_func_cached_guess = gjk.support_hint;
205 }
207
208 Vec3f w0, w1;
209 switch (gjk_status) {
210 case details::GJK::Inside:
211 col = true;
212 if (gjk.hasPenetrationInformation(shape)) {
213 gjk.getClosestPoints(shape, w0, w1);
214 distance = gjk.distance;
215 normal.noalias() = tf1.getRotation() * (w0 - w1).normalized();
216 p1 = p2 = tf1.transform((w0 + w1) / 2);
217 } else {
218 details::EPA epa(epa_max_face_num, epa_max_vertex_num,
219 epa_max_iterations, epa_tolerance);
220 details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
221 if (epa_status & details::EPA::Valid ||
222 epa_status == details::EPA::OutOfFaces // Warnings
223 || epa_status == details::EPA::OutOfVertices // Warnings
224 ) {
225 epa.getClosestPoints(shape, w0, w1);
226 distance = -epa.depth;
227 normal.noalias() = tf1.getRotation() * epa.normal;
228 p1 = p2 = tf1.transform(w0 - epa.normal * (epa.depth * 0.5));
229 assert(distance <= 1e-6);
230 } else {
231 distance = -(std::numeric_limits<FCL_REAL>::max)();
232 gjk.getClosestPoints(shape, w0, w1);
233 p1 = p2 = tf1.transform(w0);
234 }
235 }
236 break;
237 case details::GJK::Valid:
238 case details::GJK::EarlyStopped:
239 case details::GJK::Failed:
240 col = false;
241
242 gjk.getClosestPoints(shape, p1, p2);
243 // TODO On degenerated case, the closest point may be wrong
244 // (i.e. an object face normal is colinear to gjk.ray
245 // assert (distance == (w0 - w1).norm());
246 distance = gjk.distance;
247
248 p1 = tf1.transform(p1);
249 p2 = tf1.transform(p2);
250 assert(distance > 0);
251 break;
252 default:
253 assert(false && "should not reach type part.");
254 throw std::logic_error("GJKSolver: should not reach this part.");
255 }
256 return col;
257 }
258
260 template <typename S1, typename S2>
261 bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2,
262 const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
263 Vec3f& p2, Vec3f& normal) const {
264#ifndef NDEBUG
265 FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
266#endif
268 shape.set(&s1, &s2, tf1, tf2);
269
270 Vec3f guess;
271 support_func_guess_t support_hint;
272 details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
273 initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
274
275 details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
276 if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
277 enable_cached_guess) {
278 cached_guess = gjk.getGuessFromSimplex();
279 support_func_cached_guess = gjk.support_hint;
280 }
281
282 if (gjk_status == details::GJK::Failed) {
283 // TODO: understand why GJK fails between cylinder and box
284 assert(distance * distance < sqrt(eps));
285 Vec3f w0, w1;
286 gjk.getClosestPoints(shape, w0, w1);
287 distance = 0;
288 p1 = tf1.transform(w0);
289 p2 = tf1.transform(w1);
290 normal.setZero();
291 return false;
292 } else if (gjk_status == details::GJK::Valid) {
293 gjk.getClosestPoints(shape, p1, p2);
294 // TODO On degenerated case, the closest point may be wrong
295 // (i.e. an object face normal is colinear to gjk.ray
296 // assert (distance == (w0 - w1).norm());
297 distance = gjk.distance;
298
299 normal.noalias() = tf1.getRotation() * gjk.ray;
300 normal.normalize();
301 p1 = tf1.transform(p1);
302 p2 = tf1.transform(p2);
303 return true;
304 } else if (gjk_status == details::GJK::EarlyStopped) {
305 distance = gjk.distance;
306 p1 = p2 = normal =
307 Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
308 return true;
309 } else {
310 assert(gjk_status == details::GJK::Inside);
311 if (gjk.hasPenetrationInformation(shape)) {
312 gjk.getClosestPoints(shape, p1, p2);
313 distance = gjk.distance;
314 // Return contact points in case of collision
315 // p1 = tf1.transform (p1);
316 // p2 = tf1.transform (p2);
317 normal.noalias() = tf1.getRotation() * (p1 - p2);
318 normal.normalize();
319 p1 = tf1.transform(p1);
320 p2 = tf1.transform(p2);
321 } else {
322 details::EPA epa(epa_max_face_num, epa_max_vertex_num,
323 epa_max_iterations, epa_tolerance);
324 details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
325 if (epa_status & details::EPA::Valid ||
326 epa_status == details::EPA::OutOfFaces // Warnings
327 || epa_status == details::EPA::OutOfVertices // Warnings
328 || epa_status == details::EPA::FallBack) {
329 Vec3f w0, w1;
330 epa.getClosestPoints(shape, w0, w1);
331 assert(epa.depth >= -eps);
332 distance = (std::min)(0., -epa.depth);
333 normal.noalias() = tf1.getRotation() * epa.normal;
334 p1 = tf1.transform(w0);
335 p2 = tf1.transform(w1);
336 return false;
337 }
338 distance = -(std::numeric_limits<FCL_REAL>::max)();
339 gjk.getClosestPoints(shape, p1, p2);
340 p1 = tf1.transform(p1);
341 p2 = tf1.transform(p2);
342 }
343 return false;
344 }
345 }
346
351 gjk_max_iterations = 128;
352 gjk_tolerance = 1e-6;
353 epa_max_face_num = 128;
354 epa_max_vertex_num = 64;
355 epa_max_iterations = 255;
356 epa_tolerance = 1e-6;
357 enable_cached_guess = false; // TODO: use gjk_initial_guess instead
358 cached_guess = Vec3f(1, 0, 0);
359 support_func_cached_guess = support_func_guess_t::Zero();
360 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
361 gjk_initial_guess = GJKInitialGuess::DefaultGuess;
362 gjk_variant = GJKVariant::DefaultGJK;
363 gjk_convergence_criterion = GJKConvergenceCriterion::VDB;
364 gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative;
365 }
366
371 GJKSolver(const DistanceRequest& request) {
372 cached_guess = Vec3f(1, 0, 0);
373 support_func_cached_guess = support_func_guess_t::Zero();
374 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
375
376 // EPS settings
377 epa_max_face_num = 128;
378 epa_max_vertex_num = 64;
379 epa_max_iterations = 255;
380 epa_tolerance = 1e-6;
381
382 set(request);
383 }
384
389 void set(const DistanceRequest& request) {
390 gjk_initial_guess = request.gjk_initial_guess;
391 // TODO: use gjk_initial_guess instead
392 enable_cached_guess = request.enable_cached_gjk_guess;
393 gjk_variant = request.gjk_variant;
394 gjk_convergence_criterion = request.gjk_convergence_criterion;
395 gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
396 gjk_tolerance = request.gjk_tolerance;
397 gjk_max_iterations = request.gjk_max_iterations;
398 if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
399 enable_cached_guess) {
400 cached_guess = request.cached_gjk_guess;
401 support_func_cached_guess = request.cached_support_func_guess;
402 }
403 }
404
409 GJKSolver(const CollisionRequest& request) {
410 cached_guess = Vec3f(1, 0, 0);
411 support_func_cached_guess = support_func_guess_t::Zero();
412 distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
413
414 // EPS settings
415 epa_max_face_num = 128;
416 epa_max_vertex_num = 64;
417 epa_max_iterations = 255;
418 epa_tolerance = 1e-6;
419
420 set(request);
421 }
422
427 void set(const CollisionRequest& request) {
428 gjk_initial_guess = request.gjk_initial_guess;
429 // TODO: use gjk_initial_guess instead
430 enable_cached_guess = request.enable_cached_gjk_guess;
431 gjk_variant = request.gjk_variant;
432 gjk_convergence_criterion = request.gjk_convergence_criterion;
433 gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
434 gjk_tolerance = request.gjk_tolerance;
435 gjk_max_iterations = request.gjk_max_iterations;
436 if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
437 enable_cached_guess) {
438 cached_guess = request.cached_gjk_guess;
439 support_func_cached_guess = request.cached_support_func_guess;
440 }
441
442 // The distance upper bound should be at least greater to the requested
443 // security margin. Otherwise, we will likely miss some collisions.
444 distance_upper_bound = (std::max)(
445 0., (std::max)(request.distance_upper_bound, request.security_margin));
446 }
447
449 GJKSolver(const GJKSolver& other) = default;
450
453 bool operator==(const GJKSolver& other) const {
454 return epa_max_face_num == other.epa_max_face_num &&
455 epa_max_vertex_num == other.epa_max_vertex_num &&
456 epa_max_iterations == other.epa_max_iterations &&
457 epa_tolerance == other.epa_tolerance &&
458 gjk_max_iterations == other.gjk_max_iterations &&
459 enable_cached_guess ==
460 other.enable_cached_guess && // TODO: use gjk_initial_guess
461 // instead
462 cached_guess == other.cached_guess &&
463 support_func_cached_guess == other.support_func_cached_guess &&
464 distance_upper_bound == other.distance_upper_bound &&
465 gjk_initial_guess == other.gjk_initial_guess &&
466 gjk_variant == other.gjk_variant &&
467 gjk_convergence_criterion == other.gjk_convergence_criterion &&
468 gjk_convergence_criterion_type ==
470 }
472
473 bool operator!=(const GJKSolver& other) const { return !(*this == other); }
474
476 unsigned int epa_max_face_num;
477
479 unsigned int epa_max_vertex_num;
480
482 unsigned int epa_max_iterations;
483
486
489
491 mutable size_t gjk_max_iterations;
492
495 HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead")
496 mutable bool enable_cached_guess;
497
499 mutable Vec3f cached_guess;
500
502 mutable GJKInitialGuess gjk_initial_guess;
503
505 mutable GJKVariant gjk_variant;
506
508 mutable GJKConvergenceCriterion gjk_convergence_criterion;
509
511 mutable GJKConvergenceCriterionType gjk_convergence_criterion_type;
512
514 mutable support_func_guess_t support_func_cached_guess;
515
520 mutable FCL_REAL distance_upper_bound;
521
522 public:
523 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
524};
525
526template <>
527HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
528 const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
529 const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
530 Vec3f& p2, Vec3f& normal) const;
531
532template <>
533HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
534 const Halfspace& s, const Transform3f& tf1, const Vec3f& P1,
535 const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
536 FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
537
538template <>
539HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
540 const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
541 const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
542 Vec3f& p2, Vec3f& normal) const;
543
544#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2) \
545 template <> \
546 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2>( \
547 const S1& s1, const Transform3f& tf1, const S2& s2, \
548 const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \
549 Vec3f* contact_points, Vec3f* normal) const
550
551#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \
552 SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \
553 SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1)
554
560
566
571
572#undef SHAPE_INTERSECT_SPECIALIZATION
573#undef SHAPE_INTERSECT_SPECIALIZATION_BASE
574
575#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2) \
576 template <> \
577 HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2>( \
578 const S1& s1, const Transform3f& tf1, const S2& s2, \
579 const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
580 Vec3f& normal) const
581
582#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \
583 SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \
584 SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1)
585
592
593#undef SHAPE_DISTANCE_SPECIALIZATION
594#undef SHAPE_DISTANCE_SPECIALIZATION_BASE
595
596#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
597#pragma GCC diagnostic push
598#pragma GCC diagnostic ignored "-Wc99-extensions"
599#endif
602
603// param doc is the doxygen detailled description (should be enclosed in /** */
604// and contain no dot for some obscure reasons).
605#define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc) \
606 \
607 doc template <> \
608 HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2>( \
609 const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
610 const Transform3f& tf2, FCL_REAL& distance_lower_bound, \
611 bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const
612#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \
613 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc)
614#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \
615 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc); \
616 HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc)
617
622
623template <>
624HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Sphere>(
625 const Box& s1, const Transform3f& tf1, const Sphere& s2,
626 const Transform3f& tf2, FCL_REAL& distance_lower_bound,
627 bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const;
628
629#ifdef IS_DOXYGEN // for doxygen only
633template <>
634HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Box>(
635 const Box& s1, const Transform3f& tf1, const Box& s2,
636 const Transform3f& tf2, FCL_REAL& distance_lower_bound,
637 bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const;
638#endif
639// HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
642
645
648
651
653
656
657#undef HPP_FCL_DECLARE_SHAPE_INTERSECT
658#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
659#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR
660
662
665
666// param doc is the doxygen detailled description (should be enclosed in /** */
667// and contain no dot for some obscure reasons).
668#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc) \
669 \
670 doc template <> \
671 HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape>( \
672 const Shape& s, const Transform3f& tf1, const Vec3f& P1, \
673 const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, \
674 FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
675
679
680#undef HPP_FCL_DECLARE_SHAPE_TRIANGLE
681
683
686
687// param doc is the doxygen detailled description (should be enclosed in /** */
688// and contain no dot for some obscure reasons).
689#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc) \
690 \
691 doc template <> \
692 bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2>( \
693 const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
694 const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
695 Vec3f& normal) const
696#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \
697 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc)
698#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \
699 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc); \
700 HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc)
701
706
708 Capsule,
711
713 TriangleP,
717
718#undef HPP_FCL_DECLARE_SHAPE_DISTANCE
719#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
720#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
721
723#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
724#pragma GCC diagnostic pop
725#endif
726} // namespace fcl
727
728} // namespace hpp
729
730#endif
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:125
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:333
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:414
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:501
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:729
Infinite plane.
Definition: geometric_shapes.h:810
Center at zero point sphere.
Definition: geometric_shapes.h:196
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:170
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:71
#define HPP_FCL_DLLAPI
Definition: config.hh:88
#define HPP_FCL_DEPRECATED_MESSAGE(message)
Definition: deprecated.hh:38
#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: fwd.hh:84
#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
Definition: fwd.hh:82
#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
Definition: fwd.hh:83
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:93
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
Definition: data_types.h:89
GJKInitialGuess
Initial guess to use for the GJK algorithm DefaultGuess: Vec3f(1, 0, 0) CachedGuess: previous vector ...
Definition: data_types.h:80
double FCL_REAL
Definition: data_types.h:65
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
Main namespace.
Definition: broadphase_bruteforce.h:44
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)
Definition: narrowphase.h:668
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:551
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:698
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:575
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc)
Definition: narrowphase.h:614
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)
Definition: narrowphase.h:544
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc)
Definition: narrowphase.h:612
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc)
Definition: narrowphase.h:696
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2)
Definition: narrowphase.h:582
request to the collision algorithm
Definition: collision_data.h:235
FCL_REAL distance_upper_bound
Distance above which GJK solver makes an early stopping. GJK stops searching for the closest points w...
Definition: collision_data.h:263
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.
Definition: collision_data.h:251
request to the distance computation
Definition: collision_data.h:392
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
unsigned int epa_max_face_num
maximum number of simplex face used in EPA algorithm
Definition: narrowphase.h:476
GJKConvergenceCriterion gjk_convergence_criterion
Criterion used to stop GJK.
Definition: narrowphase.h:508
bool shapeIntersect(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance_lower_bound, bool enable_penetration, Vec3f *contact_points, Vec3f *normal) const
intersection checking between two shapes
Definition: narrowphase.h:104
unsigned int epa_max_vertex_num
maximum number of simplex vertex used in EPA algorithm
Definition: narrowphase.h:479
bool shapeDistance(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
distance computation between two shapes
Definition: narrowphase.h:261
void set(const DistanceRequest &request)
setter from a DistanceRequest
Definition: narrowphase.h:389
bool shapeTriangleInteraction(const S &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
intersection checking between one shape and a triangle with transformation
Definition: narrowphase.h:178
GJKVariant gjk_variant
Variant to use for the GJK algorithm.
Definition: narrowphase.h:505
GJKSolver(const GJKSolver &other)=default
Copy constructor.
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
Definition: narrowphase.h:502
unsigned int epa_max_iterations
maximum number of iterations used for EPA iterations
Definition: narrowphase.h:482
Eigen::Array< FCL_REAL, 1, 2 > Array2d
Definition: narrowphase.h:55
bool operator!=(const GJKSolver &other) const
Definition: narrowphase.h:473
bool operator==(const GJKSolver &other) const
Definition: narrowphase.h:453
FCL_REAL distance_upper_bound
Distance above which the GJK solver stoppes its computations and processes to an early stopping....
Definition: narrowphase.h:520
GJKSolver(const CollisionRequest &request)
Constructor from a CollisionRequest.
Definition: narrowphase.h:409
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:488
Vec3f cached_guess
smart guess
Definition: narrowphase.h:499
GJKConvergenceCriterionType gjk_convergence_criterion_type
Relative or absolute.
Definition: narrowphase.h:511
void initialize_gjk(details::GJK &gjk, const details::MinkowskiDiff &shape, const S1 &s1, const S2 &s2, Vec3f &guess, support_func_guess_t &support_hint) const
initialize GJK
Definition: narrowphase.h:59
bool enable_cached_guess
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.
Definition: narrowphase.h:496
GJKSolver(const DistanceRequest &request)
Constructor from a DistanceRequest.
Definition: narrowphase.h:371
FCL_REAL epa_tolerance
the threshold used in EPA to stop iteration
Definition: narrowphase.h:485
GJKSolver()
Default constructor for GJK algorithm.
Definition: narrowphase.h:350
void set(const CollisionRequest &request)
setter from a CollisionRequest
Definition: narrowphase.h:427
size_t gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:491
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:514
GJKVariant gjk_variant
whether to enable the Nesterov accleration of GJK
Definition: collision_data.h:129
Vec3f cached_gjk_guess
the gjk initial guess set by user
Definition: collision_data.h:144
GJKInitialGuess gjk_initial_guess
Definition: collision_data.h:121
GJKConvergenceCriterion gjk_convergence_criterion
convergence criterion used to stop GJK
Definition: collision_data.h:132
size_t gjk_max_iterations
maximum iteration for the GJK algorithm
Definition: collision_data.h:141
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Definition: collision_data.h:147
FCL_REAL gjk_tolerance
tolerance for the GJK algorithm
Definition: collision_data.h:138
bool enable_cached_gjk_guess
whether enable gjk initial guess @Deprecated Use gjk_initial_guess instead
Definition: collision_data.h:126
GJKConvergenceCriterionType gjk_convergence_criterion_type
convergence criterion used to stop GJK
Definition: collision_data.h:135
class for EPA algorithm
Definition: gjk.h:312
Vec3f normal
Definition: gjk.h:381
Status
Definition: gjk.h:367
Status evaluate(GJK &gjk, const Vec3f &guess)
FCL_REAL depth
Definition: gjk.h:382
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
class for GJK algorithm
Definition: gjk.h:141
void setDistanceEarlyBreak(const FCL_REAL &dup)
Distance threshold for early break. GJK stops when it proved the distance is more than this threshold...
Definition: gjk.h:245
bool hasPenetrationInformation(const MinkowskiDiff &shape)
Definition: gjk.h:230
support_func_guess_t support_hint
Definition: gjk.h:172
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision....
Definition: gjk.h:165
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
GJKConvergenceCriterion convergence_criterion
Definition: gjk.h:170
GJKConvergenceCriterionType convergence_criterion_type
Definition: gjk.h:171
FCL_REAL distance
Definition: gjk.h:186
Vec3f ray
Definition: gjk.h:168
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess, const support_func_guess_t &supportHint=support_func_guess_t::Zero())
GJK algorithm, given the initial value guess.
GJKVariant gjk_variant
Definition: gjk.h:169
Vec3f getGuessFromSimplex() const
get the guess from current simplex
Minkowski difference class of two shapes.
Definition: gjk.h:59
Vec3f ot1
translation from shape1 to shape0 such that .
Definition: gjk.h:79
Matrix3f oR1
rotation from shape1 to shape0 such that .
Definition: gjk.h:75
void set(const ShapeBase *shape0, const ShapeBase *shape1)