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