hpp-fcl: src/narrowphase/details.h Source File

hpp-fcl 1.7.5 HPP fork of FCL -- The Flexible Collision Library

details.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 *
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 */
38 #ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H
39 # define HPP_FCL_SRC_NARROWPHASE_DETAILS_H
40
41 #include <hpp/fcl/internal/traversal_node_setup.h>
42 #include <hpp/fcl/narrowphase/narrowphase.h>
43
44 namespace hpp
45 {
46 namespace fcl {
47 namespace details
48 {
49 // Compute the point on a line segment that is the closest point on the
50 // segment to to another point. The code is inspired by the explanation
51 // given by Dan Sunday's page:
52 // http://geomalgorithms.com/a02-_lines.html
53 static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp) {
54 Vec3f v = s2 - s1;
55 Vec3f w = p - s1;
56
57 FCL_REAL c1 = w.dot(v);
58 FCL_REAL c2 = v.dot(v);
59
60 if (c1 <= 0) {
61 sp = s1;
62 } else if (c2 <= c1) {
63 sp = s2;
64 } else {
65 FCL_REAL b = c1/c2;
66 Vec3f Pb = s1 + v * b;
67 sp = Pb;
68 }
69 }
70
71 inline bool sphereCapsuleIntersect
72 (const Sphere& s1, const Transform3f& tf1,
73 const Capsule& s2, const Transform3f& tf2,
74 FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal_)
75 {
76 Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength))); // from distance function
77 Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
78 Vec3f s_c = tf1.getTranslation ();
79
80 Vec3f segment_point;
81
82 lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
83 Vec3f diff = s_c - segment_point;
84
85 FCL_REAL diffN = diff.norm();
86 distance = diffN - s1.radius - s2.radius;
87
88 if (distance > 0)
89 return false;
90
91 // Vec3f normal (-diff.normalized());
92
93 if (normal_)
94 *normal_ = -diff / diffN;
95
96 if (contact_points) {
97 *contact_points = segment_point + diff * s2.radius;
98 }
99
100 return true;
101 }
102
103 inline bool sphereCapsuleDistance
104 (const Sphere& s1, const Transform3f& tf1,
105 const Capsule& s2, const Transform3f& tf2,
106 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
107 {
108 Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength)));
109 Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
110 Vec3f s_c = tf1.getTranslation ();
111
112 Vec3f segment_point;
113
114 lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
115 normal = segment_point - s_c;
116 FCL_REAL norm (normal.norm());
117 dist = norm - s1.radius - s2.radius;
118
119 static const FCL_REAL eps (std::numeric_limits<FCL_REAL>::epsilon());
120 if (norm > eps) {
121 normal.normalize();
122 } else {
123 normal << 1,0,0;
124 }
125 p1 = s_c + normal * s1.radius;
126 p2 = segment_point - normal * s2.radius;
127
128 if(dist <= 0) {
129 p1 = p2 = .5 * (p1+p2);
130 return false;
131 }
132 return true;
133 }
134
135 inline bool sphereCylinderDistance
136 (const Sphere& s1, const Transform3f& tf1,
137 const Cylinder& s2, const Transform3f& tf2,
138 FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
139 {
140 static const FCL_REAL eps (sqrt (std::numeric_limits <FCL_REAL>::epsilon ()));
141 FCL_REAL r1 (s1.radius);
142 FCL_REAL r2 (s2.radius);
143 FCL_REAL lz2 (s2.halfLength);
144 // boundaries of the cylinder axis
145 Vec3f A (tf2.transform (Vec3f (0, 0, -lz2)));
146 Vec3f B (tf2.transform (Vec3f (0, 0, lz2)));
147 // Position of the center of the sphere
148 Vec3f S (tf1.getTranslation ());
149 // axis of the cylinder
150 Vec3f u (tf2.getRotation ().col (2));
152 assert ((B - A - (s2.halfLength * 2) * u).norm () < eps);
153 Vec3f AS (S - A);
154 // abscissa of S on cylinder axis with A as the origin
155 FCL_REAL s (u.dot (AS));
156 Vec3f P (A + s*u);
157 Vec3f PS (S - P);
158 FCL_REAL dPS = PS.norm ();
159 // Normal to cylinder axis such that plane (A, u, v) contains sphere
160 // center
161 Vec3f v (0, 0, 0);
162 if (dPS > eps) {
163 // S is not on cylinder axis
164 v = (1/dPS) * PS;
165 }
166 if (s <= 0) {
167 if (dPS <= r2) {
168 // closest point on cylinder is on cylinder disc basis
169 dist = -s - r1; p1 = S + r1 * u; p2 = A + dPS * v; normal = u;
170 } else {
171 // closest point on cylinder is on cylinder circle basis
172 p2 = A + r2 * v;
173 Vec3f Sp2 (p2 - S);
174 FCL_REAL dSp2 = Sp2.norm ();
175 if (dSp2 > eps) {
176 normal = (1/dSp2) * Sp2;
177 p1 = S + r1 * normal;
178 dist = dSp2 - r1;
179 assert (fabs (dist) - (p1-p2).norm () < eps);
180 } else {
181 // Center of sphere is on cylinder boundary
182 normal = .5 * (A + B) - p2; normal.normalize ();
183 p1 = p2;
184 dist = -r1;
185 }
186 }
187 } else if (s <= (s2.halfLength * 2)) {
188 // 0 < s <= s2.lz
189 normal = -v;
190 dist = dPS - r1 - r2;
191 if (dPS <= r2) {
192 // Sphere center is inside cylinder
193 p1 = p2 = S;
194 } else {
195 p2 = P + r2*v; p1 = S - r1*v;
196 }
197 } else {
198 // lz < s
199 if (dPS <= r2) {
200 // closest point on cylinder is on cylinder disc basis
201 dist = s - (s2.halfLength * 2) - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u;
202 } else {
203 // closest point on cylinder is on cylinder circle basis
204 p2 = B + r2 * v;
205 Vec3f Sp2 (p2 - S);
206 FCL_REAL dSp2 = Sp2.norm ();
207 if (dSp2 > eps) {
208 normal = (1/dSp2) * Sp2;
209 p1 = S + r1 * normal;
210 dist = dSp2 - r1;
211 assert (fabs (dist) - (p1-p2).norm () < eps);
212 } else {
213 // Center of sphere is on cylinder boundary
214 normal = .5 * (A + B) - p2; normal.normalize ();
215 p1 = p2;
216 dist = -r1;
217 }
218 }
219 }
220 if (dist < 0) {
221 p1 = p2 = .5 * (p1 + p2);
222 }
223 return (dist > 0);
224 }
225
226 inline bool sphereSphereIntersect
227 (const Sphere& s1, const Transform3f& tf1,
228 const Sphere& s2, const Transform3f& tf2,
229 FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal)
230 {
231 const Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
232 FCL_REAL len = diff.norm();
233 distance = len - s1.radius - s2.radius;
234 if(distance > 0)
235 return false;
236
237 // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
238 // Otherwise, normal is pointing from center of object 1 to center of object 2
239 if(normal)
240 {
241 if(len > 0)
242 *normal = diff / len;
243 else
244 *normal = diff;
245 }
246
247 if(contact_points)
248 *contact_points = tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius);
249
250 return true;
251 }
252
253
254 inline bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
255 const Sphere& s2, const Transform3f& tf2,
256 FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
257 Vec3f& normal)
258 {
259 const Vec3f & o1 = tf1.getTranslation();
260 const Vec3f & o2 = tf2.getTranslation();
261 Vec3f diff = o1 - o2;
262 FCL_REAL len = diff.norm();
263 normal = -diff/len;
264 dist = len - s1.radius - s2.radius;
265
266 p1.noalias() = o1 + normal * s1.radius;
267 p2.noalias() = o2 - normal * s2.radius;
268
269 return (dist >=0);
270 }
271
273 inline FCL_REAL segmentSqrDistance
274 (const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest)
275 {
276 Vec3f diff = p - from;
277 Vec3f v = to - from;
278 FCL_REAL t = v.dot(diff);
279
280 if(t > 0)
281 {
282 FCL_REAL dotVV = v.squaredNorm();
283 if(t < dotVV)
284 {
285 t /= dotVV;
286 diff -= v * t;
287 }
288 else
289 {
290 t = 1;
291 diff -= v;
292 }
293 }
294 else
295 t = 0;
296
297 nearest.noalias() = from + v * t;
298 return diff.squaredNorm();
299 }
300
302 inline bool projectInTriangle (const Vec3f& p1, const Vec3f& p2,
303 const Vec3f& p3, const Vec3f& normal,
304 const Vec3f& p)
305 {
306 Vec3f edge1(p2 - p1);
307 Vec3f edge2(p3 - p2);
308 Vec3f edge3(p1 - p3);
309
310 Vec3f p1_to_p(p - p1);
311 Vec3f p2_to_p(p - p2);
312 Vec3f p3_to_p(p - p3);
313
314 Vec3f edge1_normal(edge1.cross(normal));
315 Vec3f edge2_normal(edge2.cross(normal));
316 Vec3f edge3_normal(edge3.cross(normal));
317
318 FCL_REAL r1, r2, r3;
319 r1 = edge1_normal.dot(p1_to_p);
320 r2 = edge2_normal.dot(p2_to_p);
321 r3 = edge3_normal.dot(p3_to_p);
322 if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
323 ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
324 return true;
325 return false;
326 }
327
328 // Intersection between sphere and triangle
329 // Sphere is in position tf1, Triangle is expressed in global frame
330 inline bool sphereTriangleIntersect
331 (const Sphere& s, const Transform3f& tf1,
332 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
333 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
334 Vec3f& normal_)
335 {
336 Vec3f normal = (P2 - P1).cross(P3 - P1);
337 normal.normalize();
338 const Vec3f& center = tf1.getTranslation();
339 const FCL_REAL& radius = s.radius;
340 assert (radius >= 0);
341 Vec3f p1_to_center = center - P1;
342 FCL_REAL distance_from_plane = p1_to_center.dot(normal);
343 Vec3f closest_point
344 (Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
345 FCL_REAL min_distance_sqr, distance_sqr;
346
347 if(distance_from_plane < 0) {
348 distance_from_plane *= -1;
349 normal *= -1;
350 }
351
352 if (projectInTriangle (P1, P2, P3, normal, center)) {
353 closest_point = center - normal * distance_from_plane;
354 min_distance_sqr = distance_from_plane;
355 } else {
356 // Compute distance to each each and take minimal distance
357 Vec3f nearest_on_edge;
358 min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point);
359
360 distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
361 if (distance_sqr < min_distance_sqr) {
362 min_distance_sqr = distance_sqr;
363 closest_point = nearest_on_edge;
364 }
365 distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
366 if (distance_sqr < min_distance_sqr) {
367 min_distance_sqr = distance_sqr;
368 closest_point = nearest_on_edge;
369 }
370 }
371
372 if (min_distance_sqr < radius * radius) {
373 // Collision
374 normal_ = (closest_point - center).normalized ();
375 p1 = p2 = closest_point;
376 distance = sqrt (min_distance_sqr) - radius;
377 assert (distance < 0);
378 return true;
379 } else {
380 normal_ = (closest_point - center).normalized ();
381 p1 = center + normal_ * radius;
382 p2 = closest_point;
383 distance = sqrt (min_distance_sqr) - radius;
384 assert (distance >= 0);
385 return false;
386 }
387 }
388
389 inline bool sphereTriangleDistance
390 (const Sphere& sp, const Transform3f& tf,
391 const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist)
392 {
393 // from geometric tools, very different from the collision code.
394
395 const Vec3f& center = tf.getTranslation();
396 FCL_REAL radius = sp.radius;
397 Vec3f diff = P1 - center;
398 Vec3f edge0 = P2 - P1;
399 Vec3f edge1 = P3 - P1;
400 FCL_REAL a00 = edge0.squaredNorm();
401 FCL_REAL a01 = edge0.dot(edge1);
402 FCL_REAL a11 = edge1.squaredNorm();
403 FCL_REAL b0 = diff.dot(edge0);
404 FCL_REAL b1 = diff.dot(edge1);
405 FCL_REAL c = diff.squaredNorm();
406 FCL_REAL det = fabs(a00*a11 - a01*a01);
407 FCL_REAL s = a01*b1 - a11*b0;
408 FCL_REAL t = a01*b0 - a00*b1;
409
410 FCL_REAL sqr_dist;
411
412 if(s + t <= det)
413 {
414 if(s < 0)
415 {
416 if(t < 0) // region 4
417 {
418 if(b0 < 0)
419 {
420 t = 0;
421 if(-b0 >= a00)
422 {
423 s = 1;
424 sqr_dist = a00 + 2*b0 + c;
425 }
426 else
427 {
428 s = -b0/a00;
429 sqr_dist = b0*s + c;
430 }
431 }
432 else
433 {
434 s = 0;
435 if(b1 >= 0)
436 {
437 t = 0;
438 sqr_dist = c;
439 }
440 else if(-b1 >= a11)
441 {
442 t = 1;
443 sqr_dist = a11 + 2*b1 + c;
444 }
445 else
446 {
447 t = -b1/a11;
448 sqr_dist = b1*t + c;
449 }
450 }
451 }
452 else // region 3
453 {
454 s = 0;
455 if(b1 >= 0)
456 {
457 t = 0;
458 sqr_dist = c;
459 }
460 else if(-b1 >= a11)
461 {
462 t = 1;
463 sqr_dist = a11 + 2*b1 + c;
464 }
465 else
466 {
467 t = -b1/a11;
468 sqr_dist = b1*t + c;
469 }
470 }
471 }
472 else if(t < 0) // region 5
473 {
474 t = 0;
475 if(b0 >= 0)
476 {
477 s = 0;
478 sqr_dist = c;
479 }
480 else if(-b0 >= a00)
481 {
482 s = 1;
483 sqr_dist = a00 + 2*b0 + c;
484 }
485 else
486 {
487 s = -b0/a00;
488 sqr_dist = b0*s + c;
489 }
490 }
491 else // region 0
492 {
493 // minimum at interior point
494 FCL_REAL inv_det = (1)/det;
495 s *= inv_det;
496 t *= inv_det;
497 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
498 }
499 }
500 else
501 {
502 FCL_REAL tmp0, tmp1, numer, denom;
503
504 if(s < 0) // region 2
505 {
506 tmp0 = a01 + b0;
507 tmp1 = a11 + b1;
508 if(tmp1 > tmp0)
509 {
510 numer = tmp1 - tmp0;
511 denom = a00 - 2*a01 + a11;
512 if(numer >= denom)
513 {
514 s = 1;
515 t = 0;
516 sqr_dist = a00 + 2*b0 + c;
517 }
518 else
519 {
520 s = numer/denom;
521 t = 1 - s;
522 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
523 }
524 }
525 else
526 {
527 s = 0;
528 if(tmp1 <= 0)
529 {
530 t = 1;
531 sqr_dist = a11 + 2*b1 + c;
532 }
533 else if(b1 >= 0)
534 {
535 t = 0;
536 sqr_dist = c;
537 }
538 else
539 {
540 t = -b1/a11;
541 sqr_dist = b1*t + c;
542 }
543 }
544 }
545 else if(t < 0) // region 6
546 {
547 tmp0 = a01 + b1;
548 tmp1 = a00 + b0;
549 if(tmp1 > tmp0)
550 {
551 numer = tmp1 - tmp0;
552 denom = a00 - 2*a01 + a11;
553 if(numer >= denom)
554 {
555 t = 1;
556 s = 0;
557 sqr_dist = a11 + 2*b1 + c;
558 }
559 else
560 {
561 t = numer/denom;
562 s = 1 - t;
563 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
564 }
565 }
566 else
567 {
568 t = 0;
569 if(tmp1 <= 0)
570 {
571 s = 1;
572 sqr_dist = a00 + 2*b0 + c;
573 }
574 else if(b0 >= 0)
575 {
576 s = 0;
577 sqr_dist = c;
578 }
579 else
580 {
581 s = -b0/a00;
582 sqr_dist = b0*s + c;
583 }
584 }
585 }
586 else // region 1
587 {
588 numer = a11 + b1 - a01 - b0;
589 if(numer <= 0)
590 {
591 s = 0;
592 t = 1;
593 sqr_dist = a11 + 2*b1 + c;
594 }
595 else
596 {
597 denom = a00 - 2*a01 + a11;
598 if(numer >= denom)
599 {
600 s = 1;
601 t = 0;
602 sqr_dist = a00 + 2*b0 + c;
603 }
604 else
605 {
606 s = numer/denom;
607 t = 1 - s;
608 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
609 }
610 }
611 }
612 }
613
614 // Account for numerical round-off error.
615 if(sqr_dist < 0)
616 sqr_dist = 0;
617
618 if(sqr_dist > radius * radius)
619 {
620 if(dist) *dist = std::sqrt(sqr_dist) - radius;
621 return true;
622 }
623 else
624 {
625 if(dist) *dist = -1;
626 return false;
627 }
628 }
629
630
631 inline bool sphereTriangleDistance
632 (const Sphere& sp, const Transform3f& tf, const Vec3f& P1,
633 const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
634 {
635 if(p1 || p2)
636 {
637 const Vec3f & o = tf.getTranslation();
638 Project::ProjectResult result;
639 result = Project::projectTriangle(P1, P2, P3, o);
640 if(result.sqr_distance > sp.radius * sp.radius)
641 {
642 if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius;
643 Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
644 Vec3f dir = o - project_p;
645 dir.normalize();
646 if(p1) { *p1 = o - dir * sp.radius; }
647 if(p2) *p2 = project_p;
648 return true;
649 }
650 else
651 return false;
652 }
653 else
654 {
655 return sphereTriangleDistance(sp, tf, P1, P2, P3, dist);
656 }
657 }
658
659
660 inline bool sphereTriangleDistance
661 (const Sphere& sp, const Transform3f& tf1, const Vec3f& P1,
662 const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
663 FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
664 {
665 bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2);
666 return res;
667 }
668
669
670
671 struct HPP_FCL_LOCAL ContactPoint
672 {
673 Vec3f normal;
674 Vec3f point;
675 FCL_REAL depth;
676 ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {}
677 };
678
679
680 static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
681 const Vec3f& pb, const Vec3f& ub,
682 FCL_REAL* alpha, FCL_REAL* beta)
683 {
684 Vec3f p = pb - pa;
685 FCL_REAL uaub = ua.dot(ub);
686 FCL_REAL q1 = ua.dot(p);
687 FCL_REAL q2 = -ub.dot(p);
688 FCL_REAL d = 1 - uaub * uaub;
689 if(d <= (FCL_REAL)(0.0001f))
690 {
691 *alpha = 0;
692 *beta = 0;
693 }
694 else
695 {
696 d = 1 / d;
697 *alpha = (q1 + uaub * q2) * d;
698 *beta = (uaub * q1 + q2) * d;
699 }
700 }
701
702 // find all the intersection points between the 2D rectangle with vertices
703 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
704 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
705 //
706 // the intersection points are returned as x,y pairs in the 'ret' array.
707 // the number of intersection points is returned by the function (this will
708 // be in the range 0 to 8).
709 static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
710 {
711 // q (and r) contain nq (and nr) coordinate points for the current (and
712 // chopped) polygons
713 int nq = 4, nr = 0;
714 FCL_REAL buffer[16];
715 FCL_REAL* q = p;
716 FCL_REAL* r = ret;
717 for(int dir = 0; dir <= 1; ++dir)
718 {
719 // direction notation: xy[0] = x axis, xy[1] = y axis
720 for(int sign = -1; sign <= 1; sign += 2)
721 {
722 // chop q along the line xy[dir] = sign*h[dir]
723 FCL_REAL* pq = q;
724 FCL_REAL* pr = r;
725 nr = 0;
726 for(int i = nq; i > 0; --i)
727 {
728 // go through all points in q and all lines between adjacent points
729 if(sign * pq[dir] < h[dir])
730 {
731 // this point is inside the chopping line
732 pr[0] = pq[0];
733 pr[1] = pq[1];
734 pr += 2;
735 nr++;
736 if(nr & 8)
737 {
738 q = r;
739 goto done;
740 }
741 }
742 FCL_REAL* nextq = (i > 1) ? pq+2 : q;
743 if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
744 {
745 // this line crosses the chopping line
746 pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
747 (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
748 pr[dir] = sign*h[dir];
749 pr += 2;
750 nr++;
751 if(nr & 8)
752 {
753 q = r;
754 goto done;
755 }
756 }
757 pq += 2;
758 }
759 q = r;
760 r = (q == ret) ? buffer : ret;
761 nq = nr;
762 }
763 }
764
765 done:
766 if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL));
767 return nr;
768 }
769
770 // given n points in the plane (array p, of size 2*n), generate m points that
771 // best represent the whole set. the definition of 'best' here is not
772 // predetermined - the idea is to select points that give good box-box
773 // collision detection behavior. the chosen point indexes are returned in the
774 // array iret (of size m). 'i0' is always the first entry in the array.
775 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
776 // in the range [0..n-1].
777 static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
778 {
779 // compute the centroid of the polygon in cx,cy
780 FCL_REAL a, cx, cy, q;
781 switch(n)
782 {
783 case 1:
784 cx = p[0];
785 cy = p[1];
786 break;
787 case 2:
788 cx = 0.5 * (p[0] + p[2]);
789 cy = 0.5 * (p[1] + p[3]);
790 break;
791 default:
792 a = 0;
793 cx = 0;
794 cy = 0;
795 for(int i = 0; i < n-1; ++i)
796 {
797 q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
798 a += q;
799 cx += q*(p[i*2]+p[i*2+2]);
800 cy += q*(p[i*2+1]+p[i*2+3]);
801 }
802 q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
803 if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
804 a = 1/(3*(a+q));
805 else
806 a= 1e18f;
807
808 cx = a*(cx + q*(p[n*2-2]+p[0]));
809 cy = a*(cy + q*(p[n*2-1]+p[1]));
810 }
811
812
813 // compute the angle of each point w.r.t. the centroid
814 FCL_REAL A[8];
815 for(int i = 0; i < n; ++i)
816 A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
817
818 // search for points that have angles closest to A[i0] + i*(2*pi/m).
819 int avail[8];
820 for(int i = 0; i < n; ++i) avail[i] = 1;
821 avail[i0] = 0;
822 iret[0] = i0;
823 iret++;
824 const double pi = boost::math::constants::pi<FCL_REAL>();
825 for(int j = 1; j < m; ++j)
826 {
827 a = j*(2*pi/m) + A[i0];
828 if (a > pi) a -= 2*pi;
829 FCL_REAL maxdiff= 1e9, diff;
830
831 *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
832 for(int i = 0; i < n; ++i)
833 {
834 if(avail[i])
835 {
836 diff = std::abs(A[i]-a);
837 if(diff > pi) diff = 2*pi - diff;
838 if(diff < maxdiff)
839 {
840 maxdiff = diff;
841 *iret = i;
842 }
843 }
844 }
845 avail[*iret] = 0;
846 iret++;
847 }
848 }
849
850
851
852 inline int boxBox2(const Vec3f& halfSide1, const Matrix3f& R1, const Vec3f& T1,
853 const Vec3f& halfSide2, const Matrix3f& R2, const Vec3f& T2,
854 Vec3f& normal, FCL_REAL* depth, int* return_code,
855 int maxc, std::vector<ContactPoint>& contacts)
856 {
857 const FCL_REAL fudge_factor = FCL_REAL(1.05);
858 Vec3f normalC;
859 FCL_REAL s, s2, l;
860 int invert_normal, code;
861
862 Vec3f p (T2 - T1); // get vector from centers of box 1 to box 2, relative to box 1
863 Vec3f pp (R1.transpose() * p); // get pp = p relative to body 1
864
865 // get side lengths / 2
866 const Vec3f& A (halfSide1);
867 const Vec3f& B (halfSide2);
868
869 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
870 Matrix3f R (R1.transpose() * R2);
871 Matrix3f Q (R.cwiseAbs());
872
873
874 // for all 15 possible separating axes:
875 // * see if the axis separates the boxes. if so, return 0.
876 // * find the depth of the penetration along the separating axis (s2)
877 // * if this is the largest depth so far, record it.
878 // the normal vector will be set to the separating axis with the smallest
879 // depth. note: normalR is set to point to a column of R1 or R2 if that is
880 // the smallest depth normal so far. otherwise normalR is 0 and normalC is
881 // set to a vector relative to body 1. invert_normal is 1 if the sign of
882 // the normal should be flipped.
883
884 int best_col_id = -1;
885 const Matrix3f* normalR = 0;
886 FCL_REAL tmp = 0;
887
888 s = - (std::numeric_limits<FCL_REAL>::max)();
889 invert_normal = 0;
890 code = 0;
891
892 // separating axis = u1, u2, u3
893 tmp = pp[0];
894 s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
895 if(s2 > 0) { *return_code = 0; return 0; }
896 if(s2 > s)
897 {
898 s = s2;
899 best_col_id = 0;
900 normalR = &R1;
901 invert_normal = (tmp < 0);
902 code = 1;
903 }
904
905 tmp = pp[1];
906 s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
907 if(s2 > 0) { *return_code = 0; return 0; }
908 if(s2 > s)
909 {
910 s = s2;
911 best_col_id = 1;
912 normalR = &R1;
913 invert_normal = (tmp < 0);
914 code = 2;
915 }
916
917 tmp = pp[2];
918 s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
919 if(s2 > 0) { *return_code = 0; return 0; }
920 if(s2 > s)
921 {
922 s = s2;
923 best_col_id = 2;
924 normalR = &R1;
925 invert_normal = (tmp < 0);
926 code = 3;
927 }
928
929 // separating axis = v1, v2, v3
930 tmp = R2.col(0).dot(p);
931 s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]);
932 if(s2 > 0) { *return_code = 0; return 0; }
933 if(s2 > s)
934 {
935 s = s2;
936 best_col_id = 0;
937 normalR = &R2;
938 invert_normal = (tmp < 0);
939 code = 4;
940 }
941
942 tmp = R2.col(1).dot(p);
943 s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]);
944 if(s2 > 0) { *return_code = 0; return 0; }
945 if(s2 > s)
946 {
947 s = s2;
948 best_col_id = 1;
949 normalR = &R2;
950 invert_normal = (tmp < 0);
951 code = 5;
952 }
953
954 tmp = R2.col(2).dot(p);
955 s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]);
956 if(s2 > 0) { *return_code = 0; return 0; }
957 if(s2 > s)
958 {
959 s = s2;
960 best_col_id = 2;
961 normalR = &R2;
962 invert_normal = (tmp < 0);
963 code = 6;
964 }
965
966
967 FCL_REAL fudge2(1.0e-6);
968 Q.array() += fudge2;
969
970 Vec3f n;
971 static const FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
972
973 // separating axis = u1 x (v1,v2,v3)
974 tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
975 s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
976 if(s2 > 0) { *return_code = 0; return 0; }
977 n = Vec3f(0, -R(2, 0), R(1, 0));
978 l = n.norm();
979 if(l > eps)
980 {
981 s2 /= l;
982 if(s2 * fudge_factor > s)
983 {
984 s = s2;
985 best_col_id = -1;
986 normalC = n / l;
987 invert_normal = (tmp < 0);
988 code = 7;
989 }
990 }
991
992 tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
993 s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
994 if(s2 > 0) { *return_code = 0; return 0; }
995 n = Vec3f(0, -R(2, 1), R(1, 1));
996 l = n.norm();
997 if(l > eps)
998 {
999 s2 /= l;
1000 if(s2 * fudge_factor > s)
1001 {
1002 s = s2;
1003 best_col_id = -1;
1004 normalC = n / l;
1005 invert_normal = (tmp < 0);
1006 code = 8;
1007 }
1008 }
1009
1010 tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
1011 s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
1012 if(s2 > 0) { *return_code = 0; return 0; }
1013 n = Vec3f(0, -R(2, 2), R(1, 2));
1014 l = n.norm();
1015 if(l > eps)
1016 {
1017 s2 /= l;
1018 if(s2 * fudge_factor > s)
1019 {
1020 s = s2;
1021 best_col_id = -1;
1022 normalC = n / l;
1023 invert_normal = (tmp < 0);
1024 code = 9;
1025 }
1026 }
1027
1028 // separating axis = u2 x (v1,v2,v3)
1029 tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
1030 s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
1031 if(s2 > 0) { *return_code = 0; return 0; }
1032 n = Vec3f(R(2, 0), 0, -R(0, 0));
1033 l = n.norm();
1034 if(l > eps)
1035 {
1036 s2 /= l;
1037 if(s2 * fudge_factor > s)
1038 {
1039 s = s2;
1040 best_col_id = -1;
1041 normalC = n / l;
1042 invert_normal = (tmp < 0);
1043 code = 10;
1044 }
1045 }
1046
1047 tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
1048 s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
1049 if(s2 > 0) { *return_code = 0; return 0; }
1050 n = Vec3f(R(2, 1), 0, -R(0, 1));
1051 l = n.norm();
1052 if(l > eps)
1053 {
1054 s2 /= l;
1055 if(s2 * fudge_factor > s)
1056 {
1057 s = s2;
1058 best_col_id = -1;
1059 normalC = n / l;
1060 invert_normal = (tmp < 0);
1061 code = 11;
1062 }
1063 }
1064
1065 tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
1066 s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
1067 if(s2 > 0) { *return_code = 0; return 0; }
1068 n = Vec3f(R(2, 2), 0, -R(0, 2));
1069 l = n.norm();
1070 if(l > eps)
1071 {
1072 s2 /= l;
1073 if(s2 * fudge_factor > s)
1074 {
1075 s = s2;
1076 best_col_id = -1;
1077 normalC = n / l;
1078 invert_normal = (tmp < 0);
1079 code = 12;
1080 }
1081 }
1082
1083 // separating axis = u3 x (v1,v2,v3)
1084 tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
1085 s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
1086 if(s2 > 0) { *return_code = 0; return 0; }
1087 n = Vec3f(-R(1, 0), R(0, 0), 0);
1088 l = n.norm();
1089 if(l > eps)
1090 {
1091 s2 /= l;
1092 if(s2 * fudge_factor > s)
1093 {
1094 s = s2;
1095 best_col_id = -1;
1096 normalC = n / l;
1097 invert_normal = (tmp < 0);
1098 code = 13;
1099 }
1100 }
1101
1102 tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
1103 s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
1104 if(s2 > 0) { *return_code = 0; return 0; }
1105 n = Vec3f(-R(1, 1), R(0, 1), 0);
1106 l = n.norm();
1107 if(l > eps)
1108 {
1109 s2 /= l;
1110 if(s2 * fudge_factor > s)
1111 {
1112 s = s2;
1113 best_col_id = -1;
1114 normalC = n / l;
1115 invert_normal = (tmp < 0);
1116 code = 14;
1117 }
1118 }
1119
1120 tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
1121 s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
1122 if(s2 > 0) { *return_code = 0; return 0; }
1123 n = Vec3f(-R(1, 2), R(0, 2), 0);
1124 l = n.norm();
1125 if(l > eps)
1126 {
1127 s2 /= l;
1128 if(s2 * fudge_factor > s)
1129 {
1130 s = s2;
1131 best_col_id = -1;
1132 normalC = n / l;
1133 invert_normal = (tmp < 0);
1134 code = 15;
1135 }
1136 }
1137
1138
1139
1140 if (!code) { *return_code = code; return 0; }
1141
1142 // if we get to this point, the boxes interpenetrate. compute the normal
1143 // in global coordinates.
1144 if(best_col_id != -1)
1145 normal = normalR->col(best_col_id);
1146 else
1147 normal = R1 * normalC;
1148
1149 if(invert_normal)
1150 normal = -normal;
1151
1152 *depth = -s; // s is negative when the boxes are in collision
1153
1154 // compute contact point(s)
1155
1156 if(code > 6)
1157 {
1158 // an edge from box 1 touches an edge from box 2.
1159 // find a point pa on the intersecting edge of box 1
1160 Vec3f pa(T1);
1161 FCL_REAL sign;
1162
1163 for(int j = 0; j < 3; ++j)
1164 {
1165 sign = (R1.col(j).dot(normal) > 0) ? 1 : -1;
1166 pa += R1.col(j) * (A[j] * sign);
1167 }
1168
1169 // find a point pb on the intersecting edge of box 2
1170 Vec3f pb(T2);
1171
1172 for(int j = 0; j < 3; ++j)
1173 {
1174 sign = (R2.col(j).dot(normal) > 0) ? -1 : 1;
1175 pb += R2.col(j) * (B[j] * sign);
1176 }
1177
1178 FCL_REAL alpha, beta;
1179 Vec3f ua(R1.col((code-7)/3));
1180 Vec3f ub(R2.col((code-7)%3));
1181
1182 lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
1183 pa += ua * alpha;
1184 pb += ub * beta;
1185
1186
1187 // Vec3f pointInWorld((pa + pb) * 0.5);
1188 // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
1189 contacts.push_back(ContactPoint(-normal,pb,-*depth));
1190 *return_code = code;
1191
1192 return 1;
1193 }
1194
1195 // okay, we have a face-something intersection (because the separating
1196 // axis is perpendicular to a face). define face 'a' to be the reference
1197 // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
1198 // the incident face (the closest face of the other box).
1199
1200 const Matrix3f *Ra, *Rb;
1201 const Vec3f *pa, *pb, *Sa, *Sb;
1202
1203 if(code <= 3)
1204 {
1205 Ra = &R1;
1206 Rb = &R2;
1207 pa = &T1;
1208 pb = &T2;
1209 Sa = &A;
1210 Sb = &B;
1211 }
1212 else
1213 {
1214 Ra = &R2;
1215 Rb = &R1;
1216 pa = &T2;
1217 pb = &T1;
1218 Sa = &B;
1219 Sb = &A;
1220 }
1221
1222 // nr = normal vector of reference face dotted with axes of incident box.
1223 // anr = absolute values of nr.
1224 Vec3f normal2, nr, anr;
1225 if(code <= 3)
1226 normal2 = normal;
1227 else
1228 normal2 = -normal;
1229
1230 nr = Rb->transpose() * normal2;
1231 anr = nr.cwiseAbs();
1232
1233 // find the largest compontent of anr: this corresponds to the normal
1234 // for the indident face. the other axis numbers of the indicent face
1235 // are stored in a1,a2.
1236 int lanr, a1, a2;
1237 if(anr[1] > anr[0])
1238 {
1239 if(anr[1] > anr[2])
1240 {
1241 a1 = 0;
1242 lanr = 1;
1243 a2 = 2;
1244 }
1245 else
1246 {
1247 a1 = 0;
1248 a2 = 1;
1249 lanr = 2;
1250 }
1251 }
1252 else
1253 {
1254 if(anr[0] > anr[2])
1255 {
1256 lanr = 0;
1257 a1 = 1;
1258 a2 = 2;
1259 }
1260 else
1261 {
1262 a1 = 0;
1263 a2 = 1;
1264 lanr = 2;
1265 }
1266 }
1267
1268 // compute center point of incident face, in reference-face coordinates
1269 Vec3f center;
1270 if(nr[lanr] < 0)
1271 center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
1272 else
1273 center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
1274
1275 // find the normal and non-normal axis numbers of the reference box
1276 int codeN, code1, code2;
1277 if(code <= 3)
1278 codeN = code-1;
1279 else codeN = code-4;
1280
1281 if(codeN == 0)
1282 {
1283 code1 = 1;
1284 code2 = 2;
1285 }
1286 else if(codeN == 1)
1287 {
1288 code1 = 0;
1289 code2 = 2;
1290 }
1291 else
1292 {
1293 code1 = 0;
1294 code2 = 1;
1295 }
1296
1297 // find the four corners of the incident face, in reference-face coordinates
1298 FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
1299 FCL_REAL c1, c2, m11, m12, m21, m22;
1300 c1 = Ra->col(code1).dot(center);
1301 c2 = Ra->col(code2).dot(center);
1302 // optimize this? - we have already computed this data above, but it is not
1303 // stored in an easy-to-index format. for now it's quicker just to recompute
1304 // the four dot products.
1305 Vec3f tempRac = Ra->col(code1);
1306 m11 = Rb->col(a1).dot(tempRac);
1307 m12 = Rb->col(a2).dot(tempRac);
1308 tempRac = Ra->col(code2);
1309 m21 = Rb->col(a1).dot(tempRac);
1310 m22 = Rb->col(a2).dot(tempRac);
1311
1312 FCL_REAL k1 = m11 * (*Sb)[a1];
1313 FCL_REAL k2 = m21 * (*Sb)[a1];
1314 FCL_REAL k3 = m12 * (*Sb)[a2];
1315 FCL_REAL k4 = m22 * (*Sb)[a2];
1316 quad[0] = c1 - k1 - k3;
1317 quad[1] = c2 - k2 - k4;
1318 quad[2] = c1 - k1 + k3;
1319 quad[3] = c2 - k2 + k4;
1320 quad[4] = c1 + k1 + k3;
1321 quad[5] = c2 + k2 + k4;
1322 quad[6] = c1 + k1 - k3;
1323 quad[7] = c2 + k2 - k4;
1324
1325 // find the size of the reference face
1326 FCL_REAL rect[2];
1327 rect[0] = (*Sa)[code1];
1328 rect[1] = (*Sa)[code2];
1329
1330 // intersect the incident and reference faces
1331 FCL_REAL ret[16];
1332 int n_intersect = intersectRectQuad2(rect, quad, ret);
1333 if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen
1334
1335 // convert the intersection points into reference-face coordinates,
1336 // and compute the contact position and depth for each point. only keep
1337 // those points that have a positive (penetrating) depth. delete points in
1338 // the 'ret' array as necessary so that 'point' and 'ret' correspond.
1339 Vec3f points[8]; // penetrating contact points
1340 FCL_REAL dep[8]; // depths for those points
1341 FCL_REAL det1 = 1.f/(m11*m22 - m12*m21);
1342 m11 *= det1;
1343 m12 *= det1;
1344 m21 *= det1;
1345 m22 *= det1;
1346 int cnum = 0; // number of penetrating contact points found
1347 for(int j = 0; j < n_intersect; ++j)
1348 {
1349 FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
1350 FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
1351 points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2;
1352 dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
1353 if(dep[cnum] >= 0)
1354 {
1355 ret[cnum*2] = ret[j*2];
1356 ret[cnum*2+1] = ret[j*2+1];
1357 cnum++;
1358 }
1359 }
1360 if(cnum < 1) { *return_code = code; return 0; } // this should never happen
1361
1362 // we can't generate more contacts than we actually have
1363 if(maxc > cnum) maxc = cnum;
1364 if(maxc < 1) maxc = 1;
1365
1366 if(cnum <= maxc)
1367 {
1368 if(code<4)
1369 {
1370 // we have less contacts than we need, so we use them all
1371 for(int j = 0; j < cnum; ++j)
1372 {
1373 Vec3f pointInWorld = points[j] + (*pa);
1374 contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
1375 }
1376 }
1377 else
1378 {
1379 // we have less contacts than we need, so we use them all
1380 for(int j = 0; j < cnum; ++j)
1381 {
1382 Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j];
1383 contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
1384 }
1385 }
1386 }
1387 else
1388 {
1389 // we have more contacts than are wanted, some of them must be culled.
1390 // find the deepest point, it is always the first contact.
1391 int i1 = 0;
1392 FCL_REAL maxdepth = dep[0];
1393 for(int i = 1; i < cnum; ++i)
1394 {
1395 if(dep[i] > maxdepth)
1396 {
1397 maxdepth = dep[i];
1398 i1 = i;
1399 }
1400 }
1401
1402 int iret[8];
1403 cullPoints2(cnum, ret, maxc, i1, iret);
1404
1405 for(int j = 0; j < maxc; ++j)
1406 {
1407 Vec3f posInWorld = points[iret[j]] + (*pa);
1408 if(code < 4)
1409 contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]]));
1410 else
1411 contacts.push_back(ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
1412 }
1413 cnum = maxc;
1414 }
1415
1416 *return_code = code;
1417 return cnum;
1418 }
1419
1420 inline bool compareContactPoints
1421 (const ContactPoint& c1,const ContactPoint& c2)
1422 {
1423 return c1.depth < c2.depth;
1424 } // Accending order, assuming penetration depth is a negative number!
1425
1426 inline bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
1427 const Box& s2, const Transform3f& tf2,
1428 Vec3f* contact_points,
1429 FCL_REAL* penetration_depth_, Vec3f* normal_)
1430 {
1431 std::vector<ContactPoint> contacts;
1432 int return_code;
1433 Vec3f normal;
1434 FCL_REAL depth;
1435 /* int cnum = */ boxBox2(s1.halfSide, tf1.getRotation(), tf1.getTranslation(),
1436 s2.halfSide, tf2.getRotation(), tf2.getTranslation(),
1437 normal, &depth, &return_code,
1438 4, contacts);
1439
1440 if(normal_) *normal_ = normal;
1441 if(penetration_depth_) *penetration_depth_ = depth;
1442
1443 if(contact_points)
1444 {
1445 if(contacts.size() > 0)
1446 {
1447 std::sort(contacts.begin(), contacts.end(), compareContactPoints);
1448 *contact_points = contacts[0].point;
1449 if(penetration_depth_) *penetration_depth_ = -contacts[0].depth;
1450 }
1451 }
1452
1453 return return_code != 0;
1454 }
1455
1456 template<typename T>
1457 inline T halfspaceIntersectTolerance()
1458 {
1459 return 0;
1460 }
1461
1462 template<>
1463 inline float halfspaceIntersectTolerance()
1464 {
1465 return 0.0001f;
1466 }
1467
1468 template<>
1469 inline double halfspaceIntersectTolerance()
1470 {
1471 return 0.0000001;
1472 }
1473
1474 inline bool sphereHalfspaceIntersect
1475 (const Sphere& s1, const Transform3f& tf1,
1476 const Halfspace& s2, const Transform3f& tf2,
1477 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1478 Vec3f& normal)
1479 {
1480 Halfspace new_s2 = transform(s2, tf2);
1481 const Vec3f& center = tf1.getTranslation();
1482 distance = new_s2.signedDistance(center) - s1.radius;
1483 if(distance <= 0)
1484 {
1485 normal = -new_s2.n; // pointing from s1 to s2
1486 p1 = p2 = center - new_s2.n * s1.radius - (distance * 0.5) * new_s2.n;
1487 return true;
1488 }
1489 else {
1490 p1 = center - s1.radius * new_s2.n;
1491 p2 = p1 - distance * new_s2.n;
1492 return false;
1493 }
1494 }
1495
1501 inline bool boxHalfspaceIntersect
1502 (const Box& s1, const Transform3f& tf1,
1503 const Halfspace& s2, const Transform3f& tf2)
1504 {
1505 Halfspace new_s2 = transform(s2, tf2);
1506
1507 const Matrix3f& R = tf1.getRotation();
1508 const Vec3f& T = tf1.getTranslation();
1509
1510 Vec3f Q (R.transpose() * new_s2.n);
1511
1512 FCL_REAL depth = Q.cwiseProduct(s1.halfSide).lpNorm<1>() - new_s2.signedDistance(T);
1513 return (depth >= 0);
1514 }
1515
1516
1517 inline bool boxHalfspaceIntersect
1518 (const Box& s1, const Transform3f& tf1,
1519 const Halfspace& s2, const Transform3f& tf2,
1520 FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal)
1521 {
1522 Halfspace new_s2 = transform(s2, tf2);
1523
1524 const Matrix3f& R = tf1.getRotation();
1525 const Vec3f& T = tf1.getTranslation();
1526
1527 // Q: plane normal expressed in box frame
1528 const Vec3f Q (R.transpose() * new_s2.n);
1529 // A: scalar products of each side with normal
1530 const Vec3f A (Q.cwiseProduct(s1.halfSide));
1531
1532 distance = new_s2.signedDistance(T) - A.lpNorm<1>();
1533 if(distance > 0) {
1534 p1.noalias() = T + R * (A.array() > 0).select (s1.halfSide, - s1.halfSide);
1535 p2.noalias() = p1 - distance * new_s2.n;
1536 return false;
1537 }
1538
1540 Vec3f p(T);
1541 int sign = 0;
1542
1543 if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
1544 {
1545 sign = (A[0] > 0) ? -1 : 1;
1546 p += R.col(0) * (s1.halfSide[0] * sign);
1547 }
1548 else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
1549 {
1550 sign = (A[1] > 0) ? -1 : 1;
1551 p += R.col(1) * (s1.halfSide[1] * sign);
1552 }
1553 else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
1554 {
1555 sign = (A[2] > 0) ? -1 : 1;
1556 p += R.col(2) * (s1.halfSide[2] * sign);
1557 }
1558 else
1559 {
1560 p.noalias() += R * (A.array() > 0).select (-s1.halfSide, s1.halfSide);
1561 }
1562
1564 normal = -new_s2.n;
1565 p1 = p2 = p - new_s2.n * (distance * 0.5);
1566
1567 return true;
1568 }
1569
1570 inline bool capsuleHalfspaceIntersect
1571 (const Capsule& s1, const Transform3f& tf1,
1572 const Halfspace& s2, const Transform3f& tf2,
1573 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1574 Vec3f& normal)
1575 {
1576 Halfspace new_s2 = transform(s2, tf2);
1577
1578 const Matrix3f& R = tf1.getRotation();
1579 const Vec3f& T = tf1.getTranslation();
1580
1581 Vec3f dir_z = R.col(2);
1582
1583 FCL_REAL cosa = dir_z.dot(new_s2.n);
1584 if(std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>())
1585 {
1586 // Capsule parallel to plane
1587 FCL_REAL signed_dist = new_s2.signedDistance(T);
1588 distance = signed_dist - s1.radius;
1589 if(distance > 0) {
1590 p1 = T - s1.radius * new_s2.n;
1591 p2 = p1 - distance * new_s2.n;
1592 return false;
1593 }
1594
1595 normal = -new_s2.n;
1596 p1 = p2 = T + new_s2.n * (-0.5 * distance - s1.radius);
1597 return true;
1598 }
1599 else
1600 {
1601 int sign = (cosa > 0) ? -1 : 1;
1602 // closest capsule vertex to halfspace if no collision,
1603 // or deeper inside halspace if collision
1604 Vec3f p = T + dir_z * (s1.halfLength * sign);
1605
1606 FCL_REAL signed_dist = new_s2.signedDistance(p);
1607 distance = signed_dist - s1.radius;
1608 if(distance > 0) {
1609 p1 = T - s1.radius * new_s2.n;
1610 p2 = p1 - distance * new_s2.n;
1611 return false;
1612 }
1613 normal = -new_s2.n;
1614 // deepest point
1615 Vec3f c = p - new_s2.n * s1.radius;
1616 p1 = p2 = c - (0.5 * distance) * new_s2.n;
1617 return true;
1618 }
1619 }
1620
1621
1622 inline bool cylinderHalfspaceIntersect
1623 (const Cylinder& s1, const Transform3f& tf1,
1624 const Halfspace& s2, const Transform3f& tf2,
1625 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1626 Vec3f& normal)
1627 {
1628 Halfspace new_s2 = transform(s2, tf2);
1629
1630 const Matrix3f& R = tf1.getRotation();
1631 const Vec3f& T = tf1.getTranslation();
1632
1633 Vec3f dir_z = R.col(2);
1634 FCL_REAL cosa = dir_z.dot(new_s2.n);
1635
1636 if(cosa < halfspaceIntersectTolerance<FCL_REAL>())
1637 {
1638 FCL_REAL signed_dist = new_s2.signedDistance(T);
1639 distance = signed_dist - s1.radius;
1640 if(distance > 0) {
1641 // TODO: compute closest points
1642 p1 = p2 = Vec3f(0, 0, 0);
1643 return false;
1644 }
1645
1646 normal = -new_s2.n;
1647 p1 = p2 = T - (0.5 * distance + s1.radius) * new_s2.n;
1648 return true;
1649 }
1650 else
1651 {
1652 Vec3f C = dir_z * cosa - new_s2.n;
1653 if(std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
1654 std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>())
1655 C = Vec3f(0, 0, 0);
1656 else
1657 {
1658 FCL_REAL s = C.norm();
1659 s = s1.radius / s;
1660 C *= s;
1661 }
1662
1663 int sign = (cosa > 0) ? -1 : 1;
1664 // deepest point
1665 Vec3f p = T + dir_z * (s1.halfLength * sign) + C;
1666 distance = new_s2.signedDistance(p);
1667 if(distance > 0) {
1668 // TODO: compute closest points
1669 p1 = p2 = Vec3f(0, 0, 0);
1670 return false;
1671 }
1672 else
1673 {
1674 normal = -new_s2.n;
1675 p1 = p2 = p - (0.5 * distance) * new_s2.n;
1676 return true;
1677 }
1678 }
1679 }
1680
1681
1682 inline bool coneHalfspaceIntersect
1683 (const Cone& s1, const Transform3f& tf1,
1684 const Halfspace& s2, const Transform3f& tf2,
1685 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1686 Vec3f& normal)
1687 {
1688 Halfspace new_s2 = transform(s2, tf2);
1689
1690 const Matrix3f& R = tf1.getRotation();
1691 const Vec3f& T = tf1.getTranslation();
1692
1693 Vec3f dir_z = R.col(2);
1694 FCL_REAL cosa = dir_z.dot(new_s2.n);
1695
1696 if(cosa < halfspaceIntersectTolerance<FCL_REAL>())
1697 {
1698 FCL_REAL signed_dist = new_s2.signedDistance(T);
1699 distance = signed_dist - s1.radius;
1700 if(distance > 0) {
1701 p1 = p2 = Vec3f (0, 0, 0);
1702 return false;
1703 }
1704 else
1705 {
1706 normal = -new_s2.n;
1707 p1 = p2 = T - dir_z * (s1.halfLength) -
1708 new_s2.n * (0.5 * distance + s1.radius);
1709 return true;
1710 }
1711 }
1712 else
1713 {
1714 Vec3f C = dir_z * cosa - new_s2.n;
1715 if(std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
1716 std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>())
1717 C = Vec3f(0, 0, 0);
1718 else
1719 {
1720 FCL_REAL s = C.norm();
1721 s = s1.radius / s;
1722 C *= s;
1723 }
1724
1725 Vec3f a1 = T + dir_z * (s1.halfLength);
1726 Vec3f a2 = T - dir_z * (s1.halfLength) + C;
1727
1728 FCL_REAL d1 = new_s2.signedDistance(a1);
1729 FCL_REAL d2 = new_s2.signedDistance(a2);
1730
1731 if(d1 > 0 && d2 > 0) return false;
1732 else
1733 {
1734 distance = std::min(d1, d2);
1735 normal = -new_s2.n;
1736 p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n;
1737 return true;
1738 }
1739 }
1740 }
1741
1742 inline bool convexHalfspaceIntersect
1743 (const ConvexBase& s1, const Transform3f& tf1,
1744 const Halfspace& s2, const Transform3f& tf2,
1745 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
1746 {
1747 Halfspace new_s2 = transform(s2, tf2);
1748
1749 Vec3f v;
1750 FCL_REAL depth = (std::numeric_limits<FCL_REAL>::max)();
1751
1752 for(int i = 0; i < s1.num_points; ++i)
1753 {
1754 Vec3f p = tf1.transform(s1.points[i]);
1755
1756 FCL_REAL d = new_s2.signedDistance(p);
1757 if(d < depth)
1758 {
1759 depth = d;
1760 v = p;
1761 }
1762 }
1763
1764 if(depth <= 0)
1765 {
1766 if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth);
1767 if(penetration_depth) *penetration_depth = depth;
1768 if(normal) *normal = -new_s2.n;
1769 return true;
1770 }
1771 else
1772 return false;
1773 }
1774
1775 // Intersection between half-space and triangle
1776 // Half-space is in pose tf1,
1777 // Triangle is in pose tf2
1778 // Computes distance and closest points (p1, p2) if no collision,
1779 // contact point (p1 = p2), normal and penetration depth (-distance)
1780 // if collision
1781 inline bool halfspaceTriangleIntersect
1782 (const Halfspace& s1, const Transform3f& tf1, const Vec3f& P1,
1783 const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
1784 FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal)
1785 {
1786 Halfspace new_s1 = transform(s1, tf1);
1787
1788 Vec3f v = tf2.transform(P1);
1789 FCL_REAL depth = new_s1.signedDistance(v);
1790
1791 Vec3f p = tf2.transform(P2);
1792 FCL_REAL d = new_s1.signedDistance(p);
1793 if(d < depth)
1794 {
1795 depth = d;
1796 v = p;
1797 }
1798
1799 p = tf2.transform(P3);
1800 d = new_s1.signedDistance(p);
1801 if(d < depth)
1802 {
1803 depth = d;
1804 v = p;
1805 }
1806 // v is the vertex with minimal projection abscissa (depth) on normal to
1807 //plane,
1808 distance = depth;
1809 if(depth <= 0)
1810 {
1811 normal = new_s1.n;
1812 p1 = p2 = v - (0.5 * depth) * new_s1.n;
1813 return true;
1814 }
1815 else
1816 {
1817 p1 = v - depth * new_s1.n;
1818 p2 = v;
1819 return false;
1820 }
1821 }
1822
1823
1831 inline bool planeHalfspaceIntersect
1832 (const Plane& s1, const Transform3f& tf1,
1833 const Halfspace& s2, const Transform3f& tf2, Plane& pl, Vec3f& p,
1834 Vec3f& d, FCL_REAL& penetration_depth, int& ret)
1835 {
1836 Plane new_s1 = transform(s1, tf1);
1837 Halfspace new_s2 = transform(s2, tf2);
1838
1839 ret = 0;
1840
1841 penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
1842 Vec3f dir = (new_s1.n).cross(new_s2.n);
1843 FCL_REAL dir_norm = dir.squaredNorm();
1844 if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
1845 {
1846 if((new_s1.n).dot(new_s2.n) > 0)
1847 {
1848 penetration_depth = new_s2.d - new_s1.d;
1849 if(penetration_depth < 0)
1850 return false;
1851 else
1852 {
1853 ret = 1;
1854 pl = new_s1;
1855 return true;
1856 }
1857 }
1858 else
1859 {
1860 penetration_depth = -(new_s1.d + new_s2.d);
1861 if(penetration_depth < 0)
1862 return false;
1863 else
1864 {
1865 ret = 2;
1866 pl = new_s1;
1867 return true;
1868 }
1869 }
1870 }
1871
1872 Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
1873 Vec3f origin = n.cross(dir);
1874 origin *= (1.0 / dir_norm);
1875
1876 p = origin;
1877 d = dir;
1878 ret = 3;
1879
1880 return true;
1881 }
1882
1891 inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1,
1892 const Halfspace& s2, const Transform3f& tf2,
1893 Vec3f& p, Vec3f& d,
1894 Halfspace& s,
1895 FCL_REAL& penetration_depth, int& ret)
1896 {
1897 Halfspace new_s1 = transform(s1, tf1);
1898 Halfspace new_s2 = transform(s2, tf2);
1899
1900 ret = 0;
1901
1902 penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
1903 Vec3f dir = (new_s1.n).cross(new_s2.n);
1904 FCL_REAL dir_norm = dir.squaredNorm();
1905 if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
1906 {
1907 if((new_s1.n).dot(new_s2.n) > 0)
1908 {
1909 if(new_s1.d < new_s2.d) // s1 is inside s2
1910 {
1911 ret = 1;
1912 s = new_s1;
1913 }
1914 else // s2 is inside s1
1915 {
1916 ret = 2;
1917 s = new_s2;
1918 }
1919 return true;
1920 }
1921 else
1922 {
1923 penetration_depth = -(new_s1.d + new_s2.d);
1924 if(penetration_depth < 0) // not collision
1925 return false;
1926 else // in each other
1927 {
1928 ret = 3;
1929 return true;
1930 }
1931 }
1932 }
1933
1934 Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
1935 Vec3f origin = n.cross(dir);
1936 origin *= (1.0 / dir_norm);
1937
1938 p = origin;
1939 d = dir;
1940 ret = 4;
1941
1942 return true;
1943 }
1944
1949 inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
1950 const ShapeBase& s, const Transform3f& tf2,
1951 FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
1952 Vec3f& normal)
1953 {
1954 Vec3f n_w = tf1.getRotation() * h.n;
1955 Vec3f n_2 (tf2.getRotation().transpose() * n_w);
1956 int hint = 0;
1957 p2 = getSupport(&s, -n_2, true, hint);
1958 p2 = tf2.transform(p2);
1959
1960 dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d;
1961 p1 = p2 - dist * n_w;
1962 normal = n_w;
1963
1964 return dist <= 0;
1965 }
1966
1967 template<typename T>
1968 inline T planeIntersectTolerance()
1969 {
1970 return 0;
1971 }
1972
1973 template<>
1974 inline double planeIntersectTolerance<double>()
1975 {
1976 return 0.0000001;
1977 }
1978
1979 template<>
1980 float inline planeIntersectTolerance<float>()
1981 {
1982 return 0.0001f;
1983 }
1984
1985 inline bool spherePlaneIntersect
1986 (const Sphere& s1, const Transform3f& tf1,
1987 const Plane& s2, const Transform3f& tf2,
1988 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
1989 Vec3f& normal)
1990 // Vec3f& contact_points, FCL_REAL& penetration_depth, Vec3f* normal)
1991 {
1992 Plane new_s2 = transform(s2, tf2);
1993
1994 const Vec3f& center = tf1.getTranslation();
1995 FCL_REAL signed_dist = new_s2.signedDistance(center);
1996 distance = std::abs(signed_dist) - s1.radius;
1997 if(distance <= 0)
1998 {
1999 if (signed_dist > 0)
2000 normal = -new_s2.n;
2001 else
2002 normal = new_s2.n;
2003 p1 = p2 = center - new_s2.n * signed_dist;
2004 return true;
2005 }
2006 else
2007 {
2008 if (signed_dist > 0)
2009 {
2010 p1 = center - s1.radius * new_s2.n;
2011 p2 = center - signed_dist * new_s2.n;
2012 }
2013 else
2014 {
2015 p1 = center + s1.radius * new_s2.n;
2016 p2 = center + signed_dist * new_s2.n;
2017 }
2018 return false;
2019 }
2020 }
2021
2029 inline bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
2030 const Plane& s2, const Transform3f& tf2,
2031 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2032 Vec3f& normal)
2033 // Vec3f* contact_points,
2034 // FCL_REAL* penetration_depth, Vec3f* normal)
2035 {
2036 static const FCL_REAL eps (sqrt (std::numeric_limits<FCL_REAL>::epsilon()));
2037 const Plane new_s2 = transform(s2, tf2);
2038
2039 const Matrix3f& R = tf1.getRotation();
2040 const Vec3f& T = tf1.getTranslation();
2041
2042 const Vec3f Q (R.transpose() * new_s2.n);
2043 const Vec3f A (Q.cwiseProduct(s1.halfSide));
2044
2045 const FCL_REAL signed_dist = new_s2.signedDistance(T);
2046 distance = std::abs(signed_dist) - A.lpNorm<1>();
2047 if(distance > 0) {
2048 // Is the box above or below the plane
2049 const bool positive = signed_dist > 0;
2050 // Set p1 at the center of the box
2051 p1 = T;
2052 for (Vec3f::Index i=0; i<3; ++i) {
2053 // scalar product between box axis and plane normal
2054 FCL_REAL alpha ((positive ? 1 : -1 ) * R.col (i).dot (new_s2.n));
2055 if (alpha > eps) {
2056 p1 -= R.col (i) * s1.halfSide [i];
2057 } else if (alpha < -eps) {
2058 p1 += R.col (i) * s1.halfSide [i];
2059 }
2060 }
2061 p2 = p1 - ( positive ? distance : -distance) * new_s2.n;
2062 assert (new_s2.distance (p2) < 3 *eps);
2063 return false;
2064 }
2065
2066 // find the deepest point
2067 Vec3f p = T;
2068
2069 // when center is on the positive side of the plane, use a, b, c
2070 // make (R^T n) (a v1 + b v2 + c v3) the minimum
2071 // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum
2072 int sign = (signed_dist > 0) ? 1 : -1;
2073
2074 if(std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() ||
2075 std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
2076 {
2077 int sign2 = (A[0] > 0) ? -sign : sign;
2078 p.noalias() += R.col(0) * (s1.halfSide[0] * sign2);
2079 }
2080 else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
2081 std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
2082 {
2083 int sign2 = (A[1] > 0) ? -sign : sign;
2084 p.noalias() += R.col(1) * (s1.halfSide[1] * sign2);
2085 }
2086 else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
2087 std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
2088 {
2089 int sign2 = (A[2] > 0) ? -sign : sign;
2090 p.noalias() += R.col(2) * (s1.halfSide[2] * sign2);
2091 }
2092 else
2093 {
2094 Vec3f tmp(sign * R * s1.halfSide);
2095 p.noalias() += (A.array() > 0).select (- tmp, tmp);
2096 }
2097
2098 // compute the contact point by project the deepest point onto the plane
2099 if (signed_dist > 0) normal = -new_s2.n; else normal = new_s2.n;
2100 p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p);
2101
2102 return true;
2103 }
2104
2111 inline bool boxSphereDistance(const Box & b, const Transform3f& tfb,
2112 const Sphere& s, const Transform3f& tfs,
2113 FCL_REAL& dist, Vec3f& pb, Vec3f& ps,
2114 Vec3f& normal)
2115 {
2116 const Vec3f& os = tfs.getTranslation();
2117 const Vec3f& ob = tfb.getTranslation();
2118 const Matrix3f& Rb = tfb.getRotation();
2119
2120 pb = ob;
2121
2122 bool outside = false;
2123 const Vec3f os_in_b_frame (Rb.transpose() * (os - ob));
2124 int axis = -1;
2125 FCL_REAL min_d = (std::numeric_limits<FCL_REAL>::max)();
2126 for (int i = 0; i < 3; ++i) {
2127 FCL_REAL facedist;
2128 if (os_in_b_frame(i) < - b.halfSide(i)) { // outside
2129 pb.noalias() -= b.halfSide(i) * Rb.col(i);
2130 outside = true;
2131 } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside
2132 pb.noalias() += b.halfSide(i) * Rb.col(i);
2133 outside = true;
2134 } else {
2135 pb.noalias() += os_in_b_frame(i) * Rb.col(i);
2136 if (!outside && (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) {
2137 axis = i;
2138 min_d = facedist;
2139 }
2140 }
2141 }
2142 normal.noalias() = pb - os;
2143 FCL_REAL pdist = normal.norm();
2144 if (outside) { // pb is on the box
2145 dist = pdist - s.radius;
2146 normal /= - pdist;
2147 } else { // pb is inside the box
2148 if (os_in_b_frame(axis) >= 0 ) normal = Rb.col(axis);
2149 else normal = -Rb.col(axis);
2150 dist = - min_d - s.radius;
2151 }
2152 if (!outside || dist <= 0) {
2153 ps = pb;
2154 return true;
2155 } else {
2156 ps = os - s.radius * normal;
2157 return false;
2158 }
2159 }
2160
2161 inline bool capsulePlaneIntersect
2162 (const Capsule& s1, const Transform3f& tf1,
2163 const Plane& s2, const Transform3f& tf2,
2164 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2165 Vec3f& normal)
2166 {
2167 Plane new_s2 = transform(s2, tf2);
2168
2169 // position orientation of capsule
2170 const Matrix3f& R1 = tf1.getRotation();
2171 const Vec3f& T1 = tf1.getTranslation();
2172
2173 Vec3f dir_z = R1.col(2);
2174
2175 // ends of capsule inner segment
2176 Vec3f a1 = T1 + dir_z * s1.halfLength;
2177 Vec3f a2 = T1 - dir_z * s1.halfLength;
2178
2179 FCL_REAL d1 = new_s2.signedDistance(a1);
2180 FCL_REAL d2 = new_s2.signedDistance(a2);
2181
2182 FCL_REAL abs_d1 = std::abs(d1);
2183 FCL_REAL abs_d2 = std::abs(d2);
2184
2185 // two end points on different side of the plane
2186 // the contact point is the intersect of axis with the plane
2187 // the normal is the direction to avoid intersection
2188 // the depth is the minimum distance to resolve the collision
2189 if(d1 * d2 < -planeIntersectTolerance<FCL_REAL>())
2190 {
2191 if(abs_d1 < abs_d2)
2192 {
2193 distance = -abs_d1 - s1.radius;
2194 p1 = p2 = a1 * (abs_d2 / (abs_d1 + abs_d2)) +
2195 a2 * (abs_d1 / (abs_d1 + abs_d2));
2196 if (d1 < 0) normal = -new_s2.n; else normal = new_s2.n;
2197 }
2198 else
2199 {
2200 distance = -abs_d2 - s1.radius;
2201 p1 = p2 = a1 * (abs_d2 / (abs_d1 + abs_d2)) +
2202 a2 * (abs_d1 / (abs_d1 + abs_d2));
2203 if (d2 < 0) normal = -new_s2.n; else normal = new_s2.n;
2204 }
2205 assert (!p1.hasNaN () && !p2.hasNaN ());
2206 return true;
2207 }
2208
2209 if(abs_d1 > s1.radius && abs_d2 > s1.radius) {
2210 // Here both capsule ends are on the same side of the plane
2211 if (d1 > 0) normal = new_s2.n; else normal = -new_s2.n;
2212 if (abs_d1 < abs_d2) {
2213 distance = abs_d1 - s1.radius;
2214 p1 = a1 - s1.radius * normal;
2215 p2 = p1 - distance * normal;
2216 } else {
2217 distance = abs_d2 - s1.radius;
2218 p1 = a2 - s1.radius * normal;
2219 p2 = p1 - distance * normal;
2220 }
2221 assert (!p1.hasNaN () && !p2.hasNaN ());
2222 return false;
2223 }
2224 else
2225 {
2226 // Both capsule ends are on the same side of the plane, but one
2227 // is closer than the capsule radius, hence collision
2228 distance = std::min(abs_d1, abs_d2) - s1.radius;
2229
2230 if(abs_d1 <= s1.radius && abs_d2 <= s1.radius)
2231 {
2232 Vec3f c1 = a1 - new_s2.n * d1;
2233 Vec3f c2 = a2 - new_s2.n * d2;
2234 p1 = p2 = (c1 + c2) * 0.5;
2235 }
2236 else if(abs_d1 <= s1.radius)
2237 {
2238 Vec3f c = a1 - new_s2.n * d1;
2239 p1 = p2 = c;
2240 }
2241 else if(abs_d2 <= s1.radius)
2242 {
2243 Vec3f c = a2 - new_s2.n * d2;
2244 p1 = p2 = c;
2245 } else {
2246 assert (false);
2247 }
2248
2249 if (d1 < 0) normal = new_s2.n; else normal = -new_s2.n;
2250 assert (!p1.hasNaN () && !p2.hasNaN ());
2251 return true;
2252 }
2253 assert (false);
2254 }
2255
2261 inline bool cylinderPlaneIntersect
2262 (const Cylinder& s1, const Transform3f& tf1,
2263 const Plane& s2, const Transform3f& tf2,
2264 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2265 Vec3f& normal)
2266 {
2267 Plane new_s2 = transform(s2, tf2);
2268
2269 const Matrix3f& R = tf1.getRotation();
2270 const Vec3f& T = tf1.getTranslation();
2271
2272 Vec3f dir_z = R.col(2);
2273 FCL_REAL cosa = dir_z.dot(new_s2.n);
2274
2275 if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
2276 {
2277 FCL_REAL d = new_s2.signedDistance(T);
2278 distance = std::abs(d) - s1.radius;
2279 if(distance > 0) return false;
2280 else
2281 {
2282 if (d < 0) normal = new_s2.n;
2283 else normal = -new_s2.n;
2284 p1 = p2 = T - new_s2.n * d;
2285 return true;
2286 }
2287 }
2288 else
2289 {
2290 Vec3f C = dir_z * cosa - new_s2.n;
2291 if(std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() ||
2292 std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
2293 C = Vec3f(0, 0, 0);
2294 else
2295 {
2296 FCL_REAL s = C.norm();
2297 s = s1.radius / s;
2298 C *= s;
2299 }
2300
2301 Vec3f a1 = T + dir_z * (s1.halfLength);
2302 Vec3f a2 = T - dir_z * (s1.halfLength);
2303
2304 Vec3f c1, c2;
2305 if(cosa > 0)
2306 {
2307 c1 = a1 - C;
2308 c2 = a2 + C;
2309 }
2310 else
2311 {
2312 c1 = a1 + C;
2313 c2 = a2 - C;
2314 }
2315
2316 FCL_REAL d1 = new_s2.signedDistance(c1);
2317 FCL_REAL d2 = new_s2.signedDistance(c2);
2318
2319 if(d1 * d2 <= 0)
2320 {
2321 FCL_REAL abs_d1 = std::abs(d1);
2322 FCL_REAL abs_d2 = std::abs(d2);
2323
2324 if(abs_d1 > abs_d2)
2325 {
2326 distance = -abs_d2;
2327 p1 = p2 = c2 - new_s2.n * d2;
2328 if (d2 < 0) normal = -new_s2.n; else normal = new_s2.n;
2329 }
2330 else
2331 {
2332 distance = -abs_d1;
2333 p1 = p2 = c1 - new_s2.n * d1;
2334 if (d1 < 0) normal = -new_s2.n;
2335 else normal = new_s2.n;
2336 }
2337 return true;
2338 }
2339 else
2340 return false;
2341 }
2342 }
2343
2344 inline bool conePlaneIntersect
2345 (const Cone& s1, const Transform3f& tf1,
2346 const Plane& s2, const Transform3f& tf2,
2347 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
2348 Vec3f& normal)
2349 {
2350 Plane new_s2 = transform(s2, tf2);
2351
2352 const Matrix3f& R = tf1.getRotation();
2353 const Vec3f& T = tf1.getTranslation();
2354
2355 Vec3f dir_z = R.col(2);
2356 FCL_REAL cosa = dir_z.dot(new_s2.n);
2357
2358 if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
2359 {
2360 FCL_REAL d = new_s2.signedDistance(T);
2361 distance = std::abs(d) - s1.radius;
2362 if(distance > 0) {
2363 p1 = p2 = Vec3f (0,0,0);
2364 return false;
2365 }
2366 else
2367 {
2368 if (d < 0) normal = new_s2.n; else normal = -new_s2.n;
2369 p1 = p2 = T - dir_z * (s1.halfLength) +
2370 dir_z * (- distance / s1.radius * s1.halfLength) - new_s2.n * d;
2371 return true;
2372 }
2373 }
2374 else
2375 {
2376 Vec3f C = dir_z * cosa - new_s2.n;
2377 if(std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() ||
2378 std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
2379 C = Vec3f(0, 0, 0);
2380 else
2381 {
2382 FCL_REAL s = C.norm();
2383 s = s1.radius / s;
2384 C *= s;
2385 }
2386
2387 Vec3f c[3];
2388 c[0] = T + dir_z * (s1.halfLength);
2389 c[1] = T - dir_z * (s1.halfLength) + C;
2390 c[2] = T - dir_z * (s1.halfLength) - C;
2391
2392 FCL_REAL d[3];
2393 d[0] = new_s2.signedDistance(c[0]);
2394 d[1] = new_s2.signedDistance(c[1]);
2395 d[2] = new_s2.signedDistance(c[2]);
2396
2397 if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) ||
2398 (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
2399 return false;
2400 else
2401 {
2402 bool positive[3];
2403 for(std::size_t i = 0; i < 3; ++i)
2404 positive[i] = (d[i] >= 0);
2405
2406 int n_positive = 0;
2407 FCL_REAL d_positive = 0, d_negative = 0;
2408 for(std::size_t i = 0; i < 3; ++i)
2409 {
2410 if(positive[i])
2411 {
2412 n_positive++;
2413 if(d_positive <= d[i]) d_positive = d[i];
2414 }
2415 else
2416 {
2417 if(d_negative <= -d[i]) d_negative = -d[i];
2418 }
2419 }
2420
2421 distance = -std::min(d_positive, d_negative);
2422 if (d_positive > d_negative) normal = -new_s2.n;
2423 else normal = new_s2.n;
2424 Vec3f p[2];
2425 Vec3f q;
2426
2427 FCL_REAL p_d[2];
2428 FCL_REAL q_d(0);
2429
2430 if(n_positive == 2)
2431 {
2432 for(std::size_t i = 0, j = 0; i < 3; ++i)
2433 {
2434 if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2435 else { q = c[i]; q_d = d[i]; }
2436 }
2437
2438 Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
2439 Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
2440 p1 = p2 = (t1 + t2) * 0.5;
2441 }
2442 else
2443 {
2444 for(std::size_t i = 0, j = 0; i < 3; ++i)
2445 {
2446 if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2447 else { q = c[i]; q_d = d[i]; }
2448 }
2449
2450 Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
2451 Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
2452 p1 = p2 = (t1 + t2) * 0.5;
2453 }
2454 }
2455 return true;
2456 }
2457 }
2458
2459 inline bool convexPlaneIntersect
2460 (const ConvexBase& s1, const Transform3f& tf1,
2461 const Plane& s2, const Transform3f& tf2,
2462 Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
2463 {
2464 Plane new_s2 = transform(s2, tf2);
2465
2466 Vec3f v_min, v_max;
2467 FCL_REAL d_min = (std::numeric_limits<FCL_REAL>::max)(), d_max = -(std::numeric_limits<FCL_REAL>::max)();
2468
2469 for(int i = 0; i < s1.num_points; ++i)
2470 {
2471 Vec3f p = tf1.transform(s1.points[i]);
2472
2473 FCL_REAL d = new_s2.signedDistance(p);
2474
2475 if(d < d_min) { d_min = d; v_min = p; }
2476 if(d > d_max) { d_max = d; v_max = p; }
2477 }
2478
2479 if(d_min * d_max > 0) return false;
2480 else
2481 {
2482 if(d_min + d_max > 0)
2483 {
2484 if(penetration_depth) *penetration_depth = -d_min;
2485 if(normal) *normal = -new_s2.n;
2486 if(contact_points) *contact_points = v_min - new_s2.n * d_min;
2487 }
2488 else
2489 {
2490 if(penetration_depth) *penetration_depth = d_max;
2491 if(normal) *normal = new_s2.n;
2492 if(contact_points) *contact_points = v_max - new_s2.n * d_max;
2493 }
2494 return true;
2495 }
2496 }
2497
2498
2499
2500 inline bool planeTriangleIntersect
2501 (const Plane& s1, const Transform3f& tf1, const Vec3f& P1,
2502 const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
2503 FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal)
2504 {
2505 Plane new_s1 = transform(s1, tf1);
2506
2507 Vec3f c[3];
2508 c[0] = tf2.transform(P1);
2509 c[1] = tf2.transform(P2);
2510 c[2] = tf2.transform(P3);
2511
2512 FCL_REAL d[3];
2513 d[0] = new_s1.signedDistance(c[0]);
2514 d[1] = new_s1.signedDistance(c[1]);
2515 d[2] = new_s1.signedDistance(c[2]);
2516 int imin;
2517 if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0)
2518 {
2519 if (d[0] < d[1])
2520 if (d[0] < d[2]) {
2521 imin = 0;
2522 }
2523 else { // d [2] <= d[0] < d [1]
2524 imin = 2;
2525 }
2526 else { // d[1] <= d[0]
2527 if (d[2] < d[1]) {
2528 imin = 2;
2529 } else { // d[1] <= d[2]
2530 imin = 1;
2531 }
2532 }
2533 distance = d[imin];
2534 p2 = c[imin];
2535 p1 = c[imin] - d[imin] * new_s1.n;
2536 return false;
2537 }
2538 if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)
2539 {
2540 if (d[0] > d[1])
2541 if (d[0] > d[2]) {
2542 imin = 0;
2543 }
2544 else { // d [2] >= d[0] > d [1]
2545 imin = 2;
2546 }
2547 else { // d[1] >= d[0]
2548 if (d[2] > d[1]) {
2549 imin = 2;
2550 } else { // d[1] >= d[2]
2551 imin = 1;
2552 }
2553 }
2554 distance = -d[imin];
2555 p2 = c[imin];
2556 p1 = c[imin] - d[imin] * new_s1.n;
2557 return false;
2558 }
2559 bool positive[3];
2560 for(std::size_t i = 0; i < 3; ++i)
2561 positive[i] = (d[i] > 0);
2562
2563 int n_positive = 0;
2564 FCL_REAL d_positive = 0, d_negative = 0;
2565 for(std::size_t i = 0; i < 3; ++i)
2566 {
2567 if(positive[i])
2568 {
2569 n_positive++;
2570 if(d_positive <= d[i]) d_positive = d[i];
2571 }
2572 else
2573 {
2574 if(d_negative <= -d[i]) d_negative = -d[i];
2575 }
2576 }
2577
2578 distance = -std::min(d_positive, d_negative);
2579 if (d_positive > d_negative)
2580 {
2581 normal = new_s1.n;
2582 } else
2583 {
2584 normal = -new_s1.n;
2585 }
2586 Vec3f p[2];
2587 Vec3f q;
2588
2589 FCL_REAL p_d[2];
2590 FCL_REAL q_d(0);
2591
2592 if(n_positive == 2)
2593 {
2594 for(std::size_t i = 0, j = 0; i < 3; ++i)
2595 {
2596 if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2597 else { q = c[i]; q_d = d[i]; }
2598 }
2599
2600 Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
2601 Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
2602 p1 = p2 = (t1 + t2) * 0.5;
2603 }
2604 else
2605 {
2606 for(std::size_t i = 0, j = 0; i < 3; ++i)
2607 {
2608 if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2609 else { q = c[i]; q_d = d[i]; }
2610 }
2611
2612 Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
2613 Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
2614 p1 = p2 = (t1 + t2) * 0.5;
2615 }
2616 return true;
2617 }
2618
2619 inline bool halfspacePlaneIntersect
2620 (const Halfspace& s1, const Transform3f& tf1,
2621 const Plane& s2, const Transform3f& tf2,
2622 Plane& pl, Vec3f& p, Vec3f& d, FCL_REAL& penetration_depth, int& ret)
2623 {
2624 return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret);
2625 }
2626
2627 inline bool planeIntersect
2628 (const Plane& s1, const Transform3f& tf1,
2629 const Plane& s2, const Transform3f& tf2,
2630 Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/)
2631 {
2632 Plane new_s1 = transform(s1, tf1);
2633 Plane new_s2 = transform(s2, tf2);
2634
2635 FCL_REAL a = (new_s1.n).dot(new_s2.n);
2636 if(a == 1 && new_s1.d != new_s2.d)
2637 return false;
2638 if(a == -1 && new_s1.d != -new_s2.d)
2639 return false;
2640
2641 return true;
2642 }
2643
2645 inline FCL_REAL computePenetration
2646 (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
2647 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
2648 Vec3f& normal)
2649 {
2650 Vec3f u ((P2-P1).cross (P3-P1));
2651 normal = u.normalized ();
2652 FCL_REAL depth1 ((P1-Q1).dot (normal));
2653 FCL_REAL depth2 ((P1-Q2).dot (normal));
2654 FCL_REAL depth3 ((P1-Q3).dot (normal));
2655 return std::max (depth1, std::max (depth2, depth3));
2656 }
2657
2658 // Compute penetration distance and normal of two triangles in collision
2659 // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
2660 // minimal distance (Q1, Q2, Q3) should be translated along the normal so
2661 // that the triangles are collision free.
2662 //
2663 // Note that we compute here an upper bound of the penetration distance,
2664 // not the exact value.
2665 inline FCL_REAL computePenetration
2666 (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
2667 const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
2668 const Transform3f& tf1, const Transform3f& tf2, Vec3f& normal)
2669 {
2670 Vec3f globalP1 (tf1.transform (P1));
2671 Vec3f globalP2 (tf1.transform (P2));
2672 Vec3f globalP3 (tf1.transform (P3));
2673 Vec3f globalQ1 (tf2.transform (Q1));
2674 Vec3f globalQ2 (tf2.transform (Q2));
2675 Vec3f globalQ3 (tf2.transform (Q3));
2676 return computePenetration (globalP1, globalP2, globalP3,
2677 globalQ1, globalQ2, globalQ3, normal);
2678 }
2679 } // details
2680 } // namespace fcl
2681 } // namespace hpp
2682
2683 #endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:103
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:193
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:250
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: geometric_shapes.h:356
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:306
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:454
hpp::fcl::Plane
Infinite plane.
Definition: geometric_shapes.h:524
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:55
hpp::fcl::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:150
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
hpp::fcl::Transform3f::getTranslation
const Vec3f & getTranslation() const
get translation
Definition: transform.h:118
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:124
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:173
hpp::fcl::distance
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.
hpp::fcl::Capsule::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:214
hpp::fcl::ConvexBase::points
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:398
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:327
hpp::fcl::Plane::n
Vec3f n
Plane normal.
Definition: geometric_shapes.h:576
hpp::fcl::Capsule::radius
FCL_REAL radius
Radius of capsule.
Definition: geometric_shapes.h:208
hpp::fcl::Halfspace::n
Vec3f n
Plane normal.
Definition: geometric_shapes.h:507
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:271
hpp::fcl::Halfspace::signedDistance
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:490
hpp::fcl::Plane::distance
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:564
hpp::fcl::Plane::d
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:579
hpp::fcl::Sphere::radius
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:163
hpp::fcl::Plane::signedDistance
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:559
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
hpp::fcl::ConvexBase::num_points
int num_points
Definition: geometric_shapes.h:399
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:265
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:321
hpp::fcl::Halfspace::d
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:510
details
Definition: traversal_node_setup.h:785
hpp::fcl::details::boxPlaneIntersect
bool boxPlaneIntersect(const Box &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + ...
Definition: details.h:2029
hpp::fcl::details::boxBoxIntersect
bool boxBoxIntersect(const Box &s1, const Transform3f &tf1, const Box &s2, const Transform3f &tf2, Vec3f *contact_points, FCL_REAL *penetration_depth_, Vec3f *normal_)
Definition: details.h:1426
hpp::fcl::details::boxBox2
int boxBox2(const Vec3f &halfSide1, const Matrix3f &R1, const Vec3f &T1, const Vec3f &halfSide2, const Matrix3f &R2, const Vec3f &T2, Vec3f &normal, FCL_REAL *depth, int *return_code, int maxc, std::vector< ContactPoint > &contacts)
Definition: details.h:852
hpp::fcl::details::coneHalfspaceIntersect
bool coneHalfspaceIntersect(const Cone &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1683
hpp::fcl::details::planeTriangleIntersect
bool planeTriangleIntersect(const Plane &s1, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2501
hpp::fcl::details::planeHalfspaceIntersect
bool planeHalfspaceIntersect(const Plane &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Plane &pl, Vec3f &p, Vec3f &d, FCL_REAL &penetration_depth, int &ret)
return whether plane collides with halfspace if the separation plane of the halfspace is parallel wit...
Definition: details.h:1832
hpp::fcl::details::conePlaneIntersect
bool conePlaneIntersect(const Cone &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2345
hpp::fcl::details::sphereCapsuleIntersect
bool sphereCapsuleIntersect(const Sphere &s1, const Transform3f &tf1, const Capsule &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f *contact_points, Vec3f *normal_)
Definition: details.h:72
hpp::fcl::details::halfspaceIntersect
bool halfspaceIntersect(const Halfspace &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Vec3f &p, Vec3f &d, Halfspace &s, FCL_REAL &penetration_depth, int &ret)
Definition: details.h:1891
hpp::fcl::details::halfspacePlaneIntersect
bool halfspacePlaneIntersect(const Halfspace &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Plane &pl, Vec3f &p, Vec3f &d, FCL_REAL &penetration_depth, int &ret)
Definition: details.h:2620
hpp::fcl::details::sphereCylinderDistance
bool sphereCylinderDistance(const Sphere &s1, const Transform3f &tf1, const Cylinder &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:136
hpp::fcl::details::halfspaceDistance
bool halfspaceDistance(const Halfspace &h, const Transform3f &tf1, const ShapeBase &s, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1949
hpp::fcl::details::capsulePlaneIntersect
bool capsulePlaneIntersect(const Capsule &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2162
hpp::fcl::details::planeIntersect
bool planeIntersect(const Plane &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Vec3f *, FCL_REAL *, Vec3f *)
Definition: details.h:2628
hpp::fcl::details::sphereSphereDistance
bool sphereSphereDistance(const Sphere &s1, const Transform3f &tf1, const Sphere &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:254
hpp::fcl::details::getSupport
Vec3f getSupport(const ShapeBase *shape, const Vec3f &dir, bool dirIsNormalized, int &hint)
the support function for shape
hpp::fcl::details::computePenetration
FCL_REAL computePenetration(const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Vec3f &Q1, const Vec3f &Q2, const Vec3f &Q3, Vec3f &normal)
See the prototype below.
Definition: details.h:2646
hpp::fcl::details::compareContactPoints
bool compareContactPoints(const ContactPoint &c1, const ContactPoint &c2)
Definition: details.h:1421
hpp::fcl::details::sphereTriangleIntersect
bool sphereTriangleIntersect(const Sphere &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal_)
Definition: details.h:331
hpp::fcl::details::convexPlaneIntersect
bool convexPlaneIntersect(const ConvexBase &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Vec3f *contact_points, FCL_REAL *penetration_depth, Vec3f *normal)
Definition: details.h:2460
hpp::fcl::details::capsuleHalfspaceIntersect
bool capsuleHalfspaceIntersect(const Capsule &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1571
hpp::fcl::details::spherePlaneIntersect
bool spherePlaneIntersect(const Sphere &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1986
hpp::fcl::details::cylinderPlaneIntersect
bool cylinderPlaneIntersect(const Cylinder &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
cylinder-plane intersect n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d need one point to b...
Definition: details.h:2262
hpp::fcl::details::boxHalfspaceIntersect
bool boxHalfspaceIntersect(const Box &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2)
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) <= d so (R^T n) (a v1 +...
Definition: details.h:1502
hpp::fcl::details::sphereCapsuleDistance
bool sphereCapsuleDistance(const Sphere &s1, const Transform3f &tf1, const Capsule &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:104
hpp::fcl::details::sphereSphereIntersect
bool sphereSphereIntersect(const Sphere &s1, const Transform3f &tf1, const Sphere &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f *contact_points, Vec3f *normal)
Definition: details.h:227
hpp::fcl::details::planeIntersectTolerance< double >
double planeIntersectTolerance< double >()
Definition: details.h:1974
hpp::fcl::details::halfspaceTriangleIntersect
bool halfspaceTriangleIntersect(const Halfspace &s1, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1782
hpp::fcl::details::sphereTriangleDistance
bool sphereTriangleDistance(const Sphere &sp, const Transform3f &tf, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, FCL_REAL *dist)
Definition: details.h:390
hpp::fcl::details::planeIntersectTolerance
T planeIntersectTolerance()
Definition: details.h:1968
hpp::fcl::details::cylinderHalfspaceIntersect
bool cylinderHalfspaceIntersect(const Cylinder &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1623
hpp::fcl::details::halfspaceIntersectTolerance
T halfspaceIntersectTolerance()
Definition: details.h:1457
hpp::fcl::details::boxSphereDistance
bool boxSphereDistance(const Box &b, const Transform3f &tfb, const Sphere &s, const Transform3f &tfs, FCL_REAL &dist, Vec3f &pb, Vec3f &ps, Vec3f &normal)
Definition: details.h:2111
hpp::fcl::details::projectInTriangle
bool projectInTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &normal, const Vec3f &p)
Whether a point's projection is in a triangle.
Definition: details.h:302
hpp::fcl::details::segmentSqrDistance
FCL_REAL segmentSqrDistance(const Vec3f &from, const Vec3f &to, const Vec3f &p, Vec3f &nearest)
the minimum distance from a point to a line
Definition: details.h:274
hpp::fcl::details::sphereHalfspaceIntersect
bool sphereHalfspaceIntersect(const Sphere &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1475
hpp::fcl::details::convexHalfspaceIntersect
bool convexHalfspaceIntersect(const ConvexBase &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Vec3f *contact_points, FCL_REAL *penetration_depth, Vec3f *normal)
Definition: details.h:1743
hpp::fcl::details::planeIntersectTolerance< float >
float planeIntersectTolerance< float >()
Definition: details.h:1980
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::transform
Halfspace transform(const Halfspace &a, const Transform3f &tf)
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
narrowphase.h
hpp::fcl::details::ContactPoint
Definition: details.h:672
hpp::fcl::details::ContactPoint::depth
FCL_REAL depth
Definition: details.h:675
hpp::fcl::details::ContactPoint::normal
Vec3f normal
Definition: details.h:673
hpp::fcl::details::ContactPoint::ContactPoint
ContactPoint(const Vec3f &n, const Vec3f &p, FCL_REAL d)
Definition: details.h:676
hpp::fcl::details::ContactPoint::point
Vec3f point
Definition: details.h:674
traversal_node_setup.h