38#ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H
39# define HPP_FCL_SRC_NARROWPHASE_DETAILS_H
53 static inline void lineSegmentPointClosestToPoint (
const Vec3f &p,
const Vec3f &s1,
const Vec3f &s2,
Vec3f &sp) {
62 }
else if (c2 <= c1) {
66 Vec3f Pb = s1 + v * b;
82 lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
83 Vec3f diff = s_c - segment_point;
94 *normal_ = -diff / diffN;
97 *contact_points = segment_point + diff * s2.
radius;
114 lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
115 normal = segment_point - s_c;
119 static const FCL_REAL eps (std::numeric_limits<FCL_REAL>::epsilon());
125 p1 = s_c + normal * s1.
radius;
126 p2 = segment_point - normal * s2.
radius;
129 p1 = p2 = .5 * (p1+p2);
140 static const FCL_REAL eps (sqrt (std::numeric_limits <FCL_REAL>::epsilon ()));
152 assert ((B - A - (s2.
halfLength * 2) * u).norm () < eps);
169 dist = -s - r1; p1 = S + r1 * u; p2 = A + dPS * v; normal = u;
176 normal = (1/dSp2) * Sp2;
177 p1 = S + r1 * normal;
179 assert (fabs (dist) - (p1-p2).norm () < eps);
182 normal = .5 * (A + B) - p2; normal.normalize ();
190 dist = dPS - r1 - r2;
195 p2 = P + r2*v; p1 = S - r1*v;
201 dist = s - (s2.
halfLength * 2) - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u;
208 normal = (1/dSp2) * Sp2;
209 p1 = S + r1 * normal;
211 assert (fabs (dist) - (p1-p2).norm () < eps);
214 normal = .5 * (A + B) - p2; normal.normalize ();
221 p1 = p2 = .5 * (p1 + p2);
242 *normal = diff / len;
261 Vec3f diff = o1 - o2;
266 p1.noalias() = o1 + normal * s1.
radius;
267 p2.noalias() = o2 - normal * s2.
radius;
276 Vec3f diff = p - from;
297 nearest.noalias() = from + v * t;
298 return diff.squaredNorm();
306 Vec3f edge1(p2 - p1);
307 Vec3f edge2(p3 - p2);
308 Vec3f edge3(p1 - p3);
310 Vec3f p1_to_p(p - p1);
311 Vec3f p2_to_p(p - p2);
312 Vec3f p3_to_p(p - p3);
314 Vec3f edge1_normal(edge1.cross(normal));
315 Vec3f edge2_normal(edge2.cross(normal));
316 Vec3f edge3_normal(edge3.cross(normal));
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 ) )
336 Vec3f normal = (P2 - P1).cross(P3 - P1);
340 assert (radius >= 0);
341 Vec3f p1_to_center = center - P1;
342 FCL_REAL distance_from_plane = p1_to_center.dot(normal);
344 (Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
345 FCL_REAL min_distance_sqr, distance_sqr;
347 if(distance_from_plane < 0) {
348 distance_from_plane *= -1;
353 closest_point = center - normal * distance_from_plane;
354 min_distance_sqr = distance_from_plane;
357 Vec3f nearest_on_edge;
361 if (distance_sqr < min_distance_sqr) {
362 min_distance_sqr = distance_sqr;
363 closest_point = nearest_on_edge;
366 if (distance_sqr < min_distance_sqr) {
367 min_distance_sqr = distance_sqr;
368 closest_point = nearest_on_edge;
372 if (min_distance_sqr < radius * radius) {
374 normal_ = (closest_point - center).normalized ();
375 p1 = p2 = closest_point;
376 distance = sqrt (min_distance_sqr) - radius;
380 normal_ = (closest_point - center).normalized ();
381 p1 = center + normal_ * radius;
383 distance = sqrt (min_distance_sqr) - radius;
397 Vec3f diff = P1 - center;
398 Vec3f edge0 = P2 - P1;
399 Vec3f edge1 = P3 - P1;
406 FCL_REAL det = fabs(a00*a11 - a01*a01);
424 sqr_dist = a00 + 2*b0 + c;
443 sqr_dist = a11 + 2*b1 + c;
463 sqr_dist = a11 + 2*b1 + c;
483 sqr_dist = a00 + 2*b0 + c;
497 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
511 denom = a00 - 2*a01 + a11;
516 sqr_dist = a00 + 2*b0 + c;
522 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
531 sqr_dist = a11 + 2*b1 + c;
552 denom = a00 - 2*a01 + a11;
557 sqr_dist = a11 + 2*b1 + c;
563 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
572 sqr_dist = a00 + 2*b0 + c;
588 numer = a11 + b1 - a01 - b0;
593 sqr_dist = a11 + 2*b1 + c;
597 denom = a00 - 2*a01 + a11;
602 sqr_dist = a00 + 2*b0 + c;
608 sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
618 if(sqr_dist > radius * radius)
620 if(dist) *dist = std::sqrt(sqr_dist) - radius;
638 Project::ProjectResult result;
639 result = Project::projectTriangle(P1, P2, P3, o);
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;
646 if(p1) { *p1 = o - dir * sp.
radius; }
647 if(p2) *p2 = project_p;
680 static inline void lineClosestApproach(
const Vec3f& pa,
const Vec3f& ua,
697 *alpha = (q1 + uaub * q2) * d;
698 *beta = (uaub * q1 + q2) * d;
713 unsigned int nq = 4, nr = 0;
717 for(
int dir = 0; dir <= 1; ++dir)
720 for(
int sign = -1; sign <= 1; sign += 2)
726 for(
unsigned int i = nq; i > 0; --i)
729 if(sign * pq[dir] < h[dir])
742 FCL_REAL* nextq = (i > 1) ? pq+2 : q;
743 if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
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];
760 r = (q == ret) ? buffer : ret;
766 if(q != ret) memcpy(ret, q, nr*2*
sizeof(
FCL_REAL));
777 static inline void cullPoints2(
unsigned int n,
FCL_REAL p[],
unsigned int m,
unsigned int i0,
unsigned int iret[])
788 cx = 0.5 * (p[0] + p[2]);
789 cy = 0.5 * (p[1] + p[3]);
795 assert(n > 0 &&
"n should be positive");
796 for(
int i = 0; i < (int)n-1; ++i)
798 q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
800 cx += q*(p[i*2]+p[i*2+2]);
801 cy += q*(p[i*2+1]+p[i*2+3]);
803 q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
804 if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
809 cx = a*(cx + q*(p[n*2-2]+p[0]));
810 cy = a*(cy + q*(p[n*2-1]+p[1]));
816 for(
unsigned int i = 0; i < n; ++i)
817 A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
820 int avail[] = { 1, 1, 1, 1, 1, 1, 1, 1 };
824 const double pi = boost::math::constants::pi<FCL_REAL>();
825 for(
unsigned int j = 1; j < m; ++j)
827 a = j*(2*pi/m) + A[i0];
828 if (a > pi) a -= 2*pi;
832 for(
unsigned int i = 0; i < n; ++i)
836 diff = std::abs(A[i]-a);
837 if(diff > pi) diff = 2*pi - diff;
855 unsigned int maxc, std::vector<ContactPoint>& contacts)
860 int invert_normal, code;
863 Vec3f pp (R1.transpose() * p);
866 const Vec3f& A (halfSide1);
867 const Vec3f& B (halfSide2);
884 int best_col_id = -1;
888 s = - (std::numeric_limits<FCL_REAL>::max)();
894 s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
895 if(s2 > 0) { *return_code = 0;
return 0; }
901 invert_normal = (tmp < 0);
906 s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
907 if(s2 > 0) { *return_code = 0;
return 0; }
913 invert_normal = (tmp < 0);
918 s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
919 if(s2 > 0) { *return_code = 0;
return 0; }
925 invert_normal = (tmp < 0);
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; }
938 invert_normal = (tmp < 0);
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; }
950 invert_normal = (tmp < 0);
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; }
962 invert_normal = (tmp < 0);
971 static const FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
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));
982 if(s2 * fudge_factor > s)
987 invert_normal = (tmp < 0);
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));
1000 if(s2 * fudge_factor > s)
1005 invert_normal = (tmp < 0);
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));
1018 if(s2 * fudge_factor > s)
1023 invert_normal = (tmp < 0);
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));
1037 if(s2 * fudge_factor > s)
1042 invert_normal = (tmp < 0);
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));
1055 if(s2 * fudge_factor > s)
1060 invert_normal = (tmp < 0);
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));
1073 if(s2 * fudge_factor > s)
1078 invert_normal = (tmp < 0);
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);
1092 if(s2 * fudge_factor > s)
1097 invert_normal = (tmp < 0);
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);
1110 if(s2 * fudge_factor > s)
1115 invert_normal = (tmp < 0);
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);
1128 if(s2 * fudge_factor > s)
1133 invert_normal = (tmp < 0);
1140 if (!code) { *return_code = code;
return 0; }
1144 if(best_col_id != -1)
1145 normal = normalR->col(best_col_id);
1147 normal.noalias() = R1 * normalC;
1163 for(
int j = 0; j < 3; ++j)
1165 sign = (R1.col(j).dot(normal) > 0) ? 1 : -1;
1166 pa += R1.col(j) * (A[j] * sign);
1172 for(
int j = 0; j < 3; ++j)
1174 sign = (R2.col(j).dot(normal) > 0) ? -1 : 1;
1175 pb += R2.col(j) * (B[j] * sign);
1179 Vec3f ua(R1.col((code-7)/3));
1180 Vec3f ub(R2.col((code-7)%3));
1182 lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
1190 *return_code = code;
1201 const Vec3f *pa, *pb, *Sa, *Sb;
1224 Vec3f normal2, nr, anr;
1230 nr.noalias() = Rb->transpose() * normal2;
1231 anr = nr.cwiseAbs();
1271 center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
1273 center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
1276 int codeN, code1, code2;
1279 else codeN = code-4;
1299 FCL_REAL c1, c2, m11, m12, m21, m22;
1300 c1 = Ra->col(code1).dot(center);
1301 c2 = Ra->col(code2).dot(center);
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);
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;
1327 rect[0] = (*Sa)[code1];
1328 rect[1] = (*Sa)[code2];
1332 const unsigned int n_intersect = intersectRectQuad2(rect, quad, ret);
1333 if(n_intersect < 1) { *return_code = code;
return 0; }
1341 FCL_REAL det1 = 1.f/(m11*m22 - m12*m21);
1346 unsigned int cnum = 0;
1347 for(
unsigned int j = 0; j < n_intersect; ++j)
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]);
1355 ret[cnum*2] = ret[j*2];
1356 ret[cnum*2+1] = ret[j*2+1];
1360 if(cnum < 1) { *return_code = code;
return 0; }
1363 if(maxc > cnum) maxc = cnum;
1364 if(maxc < 1) maxc = 1;
1371 for(
unsigned int j = 0; j < cnum; ++j)
1373 Vec3f pointInWorld = points[j] + (*pa);
1374 contacts.push_back(
ContactPoint(-normal, pointInWorld, -dep[j]));
1380 for(
unsigned int j = 0; j < cnum; ++j)
1382 Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j];
1383 contacts.push_back(
ContactPoint(-normal, pointInWorld, -dep[j]));
1391 unsigned int i1 = 0;
1393 for(
unsigned int i = 1; i < cnum; ++i)
1395 if(dep[i] > maxdepth)
1402 unsigned int iret[8];
1403 cullPoints2(cnum, ret, maxc, i1, iret);
1405 for(
unsigned int j = 0; j < maxc; ++j)
1407 Vec3f posInWorld = points[iret[j]] + (*pa);
1409 contacts.push_back(
ContactPoint(-normal, posInWorld, -dep[iret[j]]));
1411 contacts.push_back(
ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
1416 *return_code = code;
1428 Vec3f* contact_points,
1431 std::vector<ContactPoint> contacts;
1437 normal, &depth, &return_code,
1440 if(normal_) *normal_ = normal;
1441 if(penetration_depth_) *penetration_depth_ = depth;
1445 if(contacts.size() > 0)
1448 *contact_points = contacts[0].point;
1449 if(penetration_depth_) *penetration_depth_ = -contacts[0].depth;
1453 return return_code != 0;
1456 template<
typename T>
1490 p1 = center - s1.
radius * new_s2.
n;
1510 Vec3f Q (R.transpose() * new_s2.
n);
1513 return (depth >= 0);
1528 const Vec3f Q (R.transpose() * new_s2.
n);
1534 p1.noalias() = T + R * (A.array() > 0).select (s1.
halfSide, - s1.
halfSide);
1535 p2.noalias() = p1 -
distance * new_s2.
n;
1543 if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
1545 sign = (A[0] > 0) ? -1 : 1;
1546 p += R.col(0) * (s1.
halfSide[0] * sign);
1548 else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
1550 sign = (A[1] > 0) ? -1 : 1;
1551 p += R.col(1) * (s1.
halfSide[1] * sign);
1553 else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
1555 sign = (A[2] > 0) ? -1 : 1;
1556 p += R.col(2) * (s1.
halfSide[2] * sign);
1565 p1 = p2 = p - new_s2.
n * (
distance * 0.5);
1581 Vec3f dir_z = R.col(2);
1584 if(std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>())
1590 p1 = T - s1.
radius * new_s2.
n;
1601 int sign = (cosa > 0) ? -1 : 1;
1609 p1 = T - s1.
radius * new_s2.
n;
1616 p1 = p2 = c - (0.5 *
distance) * new_s2.
n;
1633 Vec3f dir_z = R.col(2);
1636 if(cosa < halfspaceIntersectTolerance<FCL_REAL>())
1642 p1 = p2 =
Vec3f(0, 0, 0);
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>())
1663 int sign = (cosa > 0) ? -1 : 1;
1669 p1 = p2 =
Vec3f(0, 0, 0);
1675 p1 = p2 = p - (0.5 *
distance) * new_s2.
n;
1693 Vec3f dir_z = R.col(2);
1696 if(cosa < halfspaceIntersectTolerance<FCL_REAL>())
1701 p1 = p2 =
Vec3f (0, 0, 0);
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>())
1731 if(d1 > 0 && d2 > 0)
return false;
1736 p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 *
distance) * new_s2.
n;
1750 FCL_REAL depth = (std::numeric_limits<FCL_REAL>::max)();
1752 for(
unsigned int i = 0; i < s1.
num_points; ++i)
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;
1812 p1 = p2 = v - (0.5 * depth) * new_s1.
n;
1817 p1 = v - depth * new_s1.
n;
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())
1846 if((new_s1.
n).dot(new_s2.
n) > 0)
1848 penetration_depth = new_s2.
d - new_s1.
d;
1849 if(penetration_depth < 0)
1860 penetration_depth = -(new_s1.
d + new_s2.
d);
1861 if(penetration_depth < 0)
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);
1895 FCL_REAL& penetration_depth,
int& ret)
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())
1907 if((new_s1.
n).dot(new_s2.
n) > 0)
1909 if(new_s1.
d < new_s2.
d)
1923 penetration_depth = -(new_s1.
d + new_s2.
d);
1924 if(penetration_depth < 0)
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);
1961 p1 = p2 - dist * n_w;
1967 template<
typename T>
1999 if (signed_dist > 0)
2003 p1 = p2 = center - new_s2.
n * signed_dist;
2008 if (signed_dist > 0)
2010 p1 = center - s1.
radius * new_s2.
n;
2011 p2 = center - signed_dist * new_s2.
n;
2015 p1 = center + s1.
radius * new_s2.
n;
2016 p2 = center + signed_dist * new_s2.
n;
2036 static const FCL_REAL eps (sqrt (std::numeric_limits<FCL_REAL>::epsilon()));
2042 const Vec3f Q (R.transpose() * new_s2.
n);
2046 distance = std::abs(signed_dist) - A.lpNorm<1>();
2049 const bool positive = signed_dist > 0;
2052 for (Vec3f::Index i=0; i<3; ++i) {
2054 FCL_REAL alpha ((positive ? 1 : -1 ) * R.col (i).dot (new_s2.
n));
2057 }
else if (alpha < -eps) {
2062 assert (new_s2.
distance (p2) < 3 *eps);
2072 int sign = (signed_dist > 0) ? 1 : -1;
2074 if(std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() ||
2075 std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
2077 int sign2 = (A[0] > 0) ? -sign : sign;
2078 p.noalias() += R.col(0) * (s1.
halfSide[0] * sign2);
2080 else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
2081 std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
2083 int sign2 = (A[1] > 0) ? -sign : sign;
2084 p.noalias() += R.col(1) * (s1.
halfSide[1] * sign2);
2086 else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
2087 std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
2089 int sign2 = (A[2] > 0) ? -sign : sign;
2090 p.noalias() += R.col(2) * (s1.
halfSide[2] * sign2);
2095 p.noalias() += (A.array() > 0).select (- tmp, tmp);
2099 if (signed_dist > 0) normal = -new_s2.
n;
else normal = new_s2.
n;
2122 bool outside =
false;
2123 const Vec3f os_in_b_frame (Rb.transpose() * (os - ob));
2125 FCL_REAL min_d = (std::numeric_limits<FCL_REAL>::max)();
2126 for (
int i = 0; i < 3; ++i) {
2128 if (os_in_b_frame(i) < - b.
halfSide(i)) {
2129 pb.noalias() -= b.
halfSide(i) * Rb.col(i);
2131 }
else if (os_in_b_frame(i) > b.
halfSide(i)) {
2132 pb.noalias() += b.
halfSide(i) * Rb.col(i);
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) {
2142 normal.noalias() = pb - os;
2148 if (os_in_b_frame(axis) >= 0 ) normal = Rb.col(axis);
2149 else normal = -Rb.col(axis);
2150 dist = - min_d - s.
radius;
2152 if (!outside || dist <= 0) {
2156 ps = os - s.
radius * normal;
2173 Vec3f dir_z = R1.col(2);
2189 if(d1 * d2 < -planeIntersectTolerance<FCL_REAL>())
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;
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;
2205 assert (!p1.hasNaN () && !p2.hasNaN ());
2211 if (d1 > 0) normal = new_s2.
n;
else normal = -new_s2.
n;
2212 if (abs_d1 < abs_d2) {
2214 p1 = a1 - s1.
radius * normal;
2218 p1 = a2 - s1.
radius * normal;
2221 assert (!p1.hasNaN () && !p2.hasNaN ());
2232 Vec3f c1 = a1 - new_s2.
n * d1;
2233 Vec3f c2 = a2 - new_s2.
n * d2;
2234 p1 = p2 = (c1 + c2) * 0.5;
2236 else if(abs_d1 <= s1.
radius)
2238 Vec3f c = a1 - new_s2.
n * d1;
2241 else if(abs_d2 <= s1.
radius)
2243 Vec3f c = a2 - new_s2.
n * d2;
2249 if (d1 < 0) normal = new_s2.
n;
else normal = -new_s2.
n;
2250 assert (!p1.hasNaN () && !p2.hasNaN ());
2272 Vec3f dir_z = R.col(2);
2275 if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
2282 if (d < 0) normal = new_s2.
n;
2283 else normal = -new_s2.
n;
2284 p1 = p2 = T - new_s2.
n * d;
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>())
2327 p1 = p2 = c2 - new_s2.
n * d2;
2328 if (d2 < 0) normal = -new_s2.
n;
else normal = new_s2.
n;
2333 p1 = p2 = c1 - new_s2.
n * d1;
2334 if (d1 < 0) normal = -new_s2.
n;
2335 else normal = new_s2.
n;
2355 Vec3f dir_z = R.col(2);
2358 if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
2363 p1 = p2 =
Vec3f (0,0,0);
2368 if (d < 0) normal = new_s2.
n;
else normal = -new_s2.
n;
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>())
2397 if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) ||
2398 (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
2403 for(std::size_t i = 0; i < 3; ++i)
2404 positive[i] = (d[i] >= 0);
2407 FCL_REAL d_positive = 0, d_negative = 0;
2408 for(std::size_t i = 0; i < 3; ++i)
2413 if(d_positive <= d[i]) d_positive = d[i];
2417 if(d_negative <= -d[i]) d_negative = -d[i];
2421 distance = -std::min(d_positive, d_negative);
2422 if (d_positive > d_negative) normal = -new_s2.
n;
2423 else normal = new_s2.
n;
2432 for(std::size_t i = 0, j = 0; i < 3; ++i)
2434 if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2435 else { q = c[i]; q_d = d[i]; }
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;
2444 for(std::size_t i = 0, j = 0; i < 3; ++i)
2446 if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2447 else { q = c[i]; q_d = d[i]; }
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;
2467 FCL_REAL d_min = (std::numeric_limits<FCL_REAL>::max)(), d_max = -(std::numeric_limits<FCL_REAL>::max)();
2469 for(
unsigned int i = 0; i < s1.
num_points; ++i)
2475 if(d < d_min) { d_min = d; v_min = p; }
2476 if(d > d_max) { d_max = d; v_max = p; }
2479 if(d_min * d_max > 0)
return false;
2482 if(d_min + d_max > 0)
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;
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;
2517 if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0)
2535 p1 = c[imin] - d[imin] * new_s1.
n;
2538 if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)
2556 p1 = c[imin] - d[imin] * new_s1.
n;
2560 for(std::size_t i = 0; i < 3; ++i)
2561 positive[i] = (d[i] > 0);
2564 FCL_REAL d_positive = 0, d_negative = 0;
2565 for(std::size_t i = 0; i < 3; ++i)
2570 if(d_positive <= d[i]) d_positive = d[i];
2574 if(d_negative <= -d[i]) d_negative = -d[i];
2578 distance = -std::min(d_positive, d_negative);
2579 if (d_positive > d_negative)
2594 for(std::size_t i = 0, j = 0; i < 3; ++i)
2596 if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2597 else { q = c[i]; q_d = d[i]; }
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;
2606 for(std::size_t i = 0, j = 0; i < 3; ++i)
2608 if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
2609 else { q = c[i]; q_d = d[i]; }
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;
2636 if(a == 1 && new_s1.
d != new_s2.
d)
2638 if(a == -1 && new_s1.
d != -new_s2.
d)
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));
2677 globalQ1, globalQ2, globalQ3, normal);
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:103
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:193
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:250
Base for convex polytope.
Definition: geometric_shapes.h:356
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:306
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:461
Infinite plane.
Definition: geometric_shapes.h:531
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:55
Center at zero point sphere.
Definition: geometric_shapes.h:150
#define HPP_FCL_LOCAL
Definition: config.hh:66
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:214
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:398
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:327
Vec3f n
Plane normal.
Definition: geometric_shapes.h:583
FCL_REAL radius
Radius of capsule.
Definition: geometric_shapes.h:211
Vec3f n
Plane normal.
Definition: geometric_shapes.h:514
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:271
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:497
unsigned int num_points
Definition: geometric_shapes.h:399
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:571
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:586
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:166
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:566
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:268
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:324
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:517
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
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
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
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
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
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
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
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
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
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
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
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
bool planeIntersect(const Plane &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Vec3f *, FCL_REAL *, Vec3f *)
Definition: details.h:2628
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
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:852
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:2646
bool compareContactPoints(const ContactPoint &c1, const ContactPoint &c2)
Definition: details.h:1421
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
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
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
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
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
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
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
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
double planeIntersectTolerance< double >()
Definition: details.h:1974
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
bool sphereTriangleDistance(const Sphere &sp, const Transform3f &tf, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, FCL_REAL *dist)
Definition: details.h:390
T planeIntersectTolerance()
Definition: details.h:1968
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
T halfspaceIntersectTolerance()
Definition: details.h:1457
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
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
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
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
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
float planeIntersectTolerance< float >()
Definition: details.h:1980
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Halfspace transform(const Halfspace &a, const Transform3f &tf)
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44