38 #ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H
39 #define HPP_FCL_SRC_NARROWPHASE_DETAILS_H
62 }
else if (
c2 <= c1) {
83 Vec3f diff = s_c - segment_point;
92 if (normal_) *normal_ = -diff / diffN;
95 *contact_points = segment_point + diff * s2.
radius;
112 normal = segment_point - s_c;
123 p2 = segment_point - normal * s2.
radius;
126 p1 = p2 = .5 * (
p1 + p2);
176 normal = (1 / dSp2) * Sp2;
177 p1 = S + r1 * normal;
179 assert(fabs(dist) - (
p1 - p2).norm() <
eps);
182 normal = .5 * (
A +
B) - p2;
191 dist = dPS - r1 -
r2;
213 normal = (1 / dSp2) * Sp2;
214 p1 = S + r1 * normal;
216 assert(fabs(dist) - (
p1 - p2).norm() <
eps);
219 normal = .5 * (
A +
B) - p2;
227 p1 = p2 = .5 * (
p1 + p2);
236 const Vec3f diff =
tf2.getTranslation() -
tf1.getTranslation();
246 *normal = diff / len;
262 const Vec3f& o1 =
tf1.getTranslation();
263 const Vec3f& o2 =
tf2.getTranslation();
264 Vec3f diff = o1 - o2;
266 normal = -diff / len;
269 p1.noalias() = o1 + normal * s1.
radius;
270 p2.noalias() = o2 - normal * s2.
radius;
278 Vec3f diff = p - from;
294 nearest.noalias() = from +
v *
t;
295 return diff.squaredNorm();
302 Vec3f edge2(p3 - p2);
306 Vec3f p2_to_p(p - p2);
307 Vec3f p3_to_p(p - p3);
309 Vec3f edge1_normal(edge1.cross(normal));
310 Vec3f edge2_normal(edge2.cross(normal));
311 Vec3f edge3_normal(edge3.cross(normal));
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))
330 const Vec3f& center =
tf1.getTranslation();
333 Vec3f p1_to_center = center -
P1;
334 FCL_REAL distance_from_plane = p1_to_center.dot(normal);
336 Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
337 FCL_REAL min_distance_sqr, distance_sqr;
339 if (distance_from_plane < 0) {
340 distance_from_plane *= -1;
345 closest_point = center - normal * distance_from_plane;
346 min_distance_sqr = distance_from_plane;
349 Vec3f nearest_on_edge;
353 if (distance_sqr < min_distance_sqr) {
354 min_distance_sqr = distance_sqr;
355 closest_point = nearest_on_edge;
358 if (distance_sqr < min_distance_sqr) {
359 min_distance_sqr = distance_sqr;
360 closest_point = nearest_on_edge;
364 if (min_distance_sqr < radius * radius) {
366 normal_ = (closest_point - center).normalized();
367 p1 = p2 = closest_point;
368 distance = sqrt(min_distance_sqr) - radius;
372 normal_ = (closest_point - center).normalized();
373 p1 = center + normal_ * radius;
375 distance = sqrt(min_distance_sqr) - radius;
397 FCL_REAL det = fabs(a00 * a11 - a01 * a01);
411 sqr_dist = a00 + 2 * b0 +
c;
414 sqr_dist = b0 * s +
c;
421 }
else if (-b1 >= a11) {
423 sqr_dist = a11 + 2 * b1 +
c;
426 sqr_dist = b1 *
t +
c;
435 }
else if (-b1 >= a11) {
437 sqr_dist = a11 + 2 * b1 +
c;
440 sqr_dist = b1 *
t +
c;
449 }
else if (-b0 >= a00) {
451 sqr_dist = a00 + 2 * b0 +
c;
454 sqr_dist = b0 * s +
c;
462 sqr_dist = s * (a00 * s + a01 *
t + 2 * b0) +
463 t * (a01 * s + a11 *
t + 2 * b1) +
c;
474 denom = a00 - 2 * a01 + a11;
475 if (numer >= denom) {
478 sqr_dist = a00 + 2 * b0 +
c;
482 sqr_dist = s * (a00 * s + a01 *
t + 2 * b0) +
483 t * (a01 * s + a11 *
t + 2 * b1) +
c;
489 sqr_dist = a11 + 2 * b1 +
c;
490 }
else if (b1 >= 0) {
495 sqr_dist = b1 *
t +
c;
504 denom = a00 - 2 * a01 + a11;
505 if (numer >= denom) {
508 sqr_dist = a11 + 2 * b1 +
c;
512 sqr_dist = s * (a00 * s + a01 *
t + 2 * b0) +
513 t * (a01 * s + a11 *
t + 2 * b1) +
c;
519 sqr_dist = a00 + 2 * b0 +
c;
520 }
else if (b0 >= 0) {
525 sqr_dist = b0 * s +
c;
530 numer = a11 + b1 - a01 - b0;
534 sqr_dist = a11 + 2 * b1 +
c;
536 denom = a00 - 2 * a01 + a11;
537 if (numer >= denom) {
540 sqr_dist = a00 + 2 * b0 +
c;
544 sqr_dist = s * (a00 * s + a01 *
t + 2 * b0) +
545 t * (a01 * s + a11 *
t + 2 * b1) +
c;
552 if (sqr_dist < 0) sqr_dist = 0;
554 if (sqr_dist > radius * radius) {
555 if (dist) *dist = std::sqrt(sqr_dist) - radius;
558 if (dist) *dist = -1;
569 Project::ProjectResult result;
570 result = Project::projectTriangle(
P1,
P2,
P3, o);
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;
581 if (p2) *p2 = project_p;
596 tf2.transform(
P3), dist,
p1, p2);
605 : normal(n), point(p), depth(d) {}
621 *alpha = (
q1 + uaub *
q2) * d;
622 *beta = (uaub *
q1 +
q2) * d;
637 unsigned int nq = 4, nr = 0;
641 for (
int dir = 0; dir <= 1; ++dir) {
643 for (
int sign = -1; sign <= 1; sign += 2) {
648 for (
unsigned int i = nq; i > 0; --i) {
650 if (sign * pq[dir] < h[dir]) {
662 if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir])) {
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];
678 r = (
q == ret) ? buffer : ret;
684 if (
q != ret) memcpy(ret,
q, nr * 2 *
sizeof(
FCL_REAL));
696 unsigned int i0,
unsigned int iret[]) {
705 cx = 0.5 * (p[0] + p[2]);
706 cy = 0.5 * (p[1] + p[3]);
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];
716 cx +=
q * (p[i * 2] + p[i * 2 + 2]);
717 cy +=
q * (p[i * 2 + 1] + p[i * 2 + 3]);
719 q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
721 a = 1 / (3 * (
a +
q));
725 cx =
a * (cx +
q * (p[n * 2 - 2] + p[0]));
726 cy =
a * (cy +
q * (p[n * 2 - 1] + p[1]));
731 for (
unsigned int i = 0; i < n; ++i)
732 A[i] = atan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
735 int avail[] = {1, 1, 1, 1, 1, 1, 1, 1};
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;
747 for (
unsigned int i = 0; i < n; ++i) {
749 diff = std::abs(
A[i] -
a);
750 if (diff > pi) diff = 2 * pi - diff;
751 if (diff < maxdiff) {
767 std::vector<ContactPoint>& contacts) {
771 int invert_normal, code;
775 Vec3f pp(R1.transpose() * p);
778 const Vec3f&
A(halfSide1);
779 const Vec3f&
B(halfSide2);
795 int best_col_id = -1;
799 s = -(std::numeric_limits<FCL_REAL>::max)();
805 s2 = std::abs(tmp) - (Q.row(0).dot(
B) +
A[0]);
814 invert_normal = (tmp < 0);
819 s2 = std::abs(tmp) - (Q.row(1).dot(
B) +
A[1]);
828 invert_normal = (tmp < 0);
833 s2 = std::abs(tmp) - (Q.row(2).dot(
B) +
A[2]);
842 invert_normal = (tmp < 0);
847 tmp = R2.col(0).dot(p);
848 s2 = std::abs(tmp) - (Q.col(0).dot(
A) +
B[0]);
857 invert_normal = (tmp < 0);
861 tmp = R2.col(1).dot(p);
862 s2 = std::abs(tmp) - (Q.col(1).dot(
A) +
B[1]);
871 invert_normal = (tmp < 0);
875 tmp = R2.col(2).dot(p);
876 s2 = std::abs(tmp) - (Q.col(2).dot(
A) +
B[2]);
885 invert_normal = (tmp < 0);
896 tmp = pp[2] *
R(1, 0) - pp[1] *
R(2, 0);
898 (
A[1] * Q(2, 0) +
A[2] * Q(1, 0) +
B[1] * Q(0, 2) +
B[2] * Q(0, 1));
903 n =
Vec3f(0, -
R(2, 0),
R(1, 0));
907 if (s2 * fudge_factor > s) {
911 invert_normal = (tmp < 0);
916 tmp = pp[2] *
R(1, 1) - pp[1] *
R(2, 1);
918 (
A[1] * Q(2, 1) +
A[2] * Q(1, 1) +
B[0] * Q(0, 2) +
B[2] * Q(0, 0));
923 n =
Vec3f(0, -
R(2, 1),
R(1, 1));
927 if (s2 * fudge_factor > s) {
931 invert_normal = (tmp < 0);
936 tmp = pp[2] *
R(1, 2) - pp[1] *
R(2, 2);
938 (
A[1] * Q(2, 2) +
A[2] * Q(1, 2) +
B[0] * Q(0, 1) +
B[1] * Q(0, 0));
943 n =
Vec3f(0, -
R(2, 2),
R(1, 2));
947 if (s2 * fudge_factor > s) {
951 invert_normal = (tmp < 0);
957 tmp = pp[0] *
R(2, 0) - pp[2] *
R(0, 0);
959 (
A[0] * Q(2, 0) +
A[2] * Q(0, 0) +
B[1] * Q(1, 2) +
B[2] * Q(1, 1));
964 n =
Vec3f(
R(2, 0), 0, -
R(0, 0));
968 if (s2 * fudge_factor > s) {
972 invert_normal = (tmp < 0);
977 tmp = pp[0] *
R(2, 1) - pp[2] *
R(0, 1);
979 (
A[0] * Q(2, 1) +
A[2] * Q(0, 1) +
B[0] * Q(1, 2) +
B[2] * Q(1, 0));
984 n =
Vec3f(
R(2, 1), 0, -
R(0, 1));
988 if (s2 * fudge_factor > s) {
992 invert_normal = (tmp < 0);
997 tmp = pp[0] *
R(2, 2) - pp[2] *
R(0, 2);
999 (
A[0] * Q(2, 2) +
A[2] * Q(0, 2) +
B[0] * Q(1, 1) +
B[1] * Q(1, 0));
1004 n =
Vec3f(
R(2, 2), 0, -
R(0, 2));
1008 if (s2 * fudge_factor > s) {
1012 invert_normal = (tmp < 0);
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));
1025 n =
Vec3f(-
R(1, 0),
R(0, 0), 0);
1029 if (s2 * fudge_factor > s) {
1033 invert_normal = (tmp < 0);
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));
1045 n =
Vec3f(-
R(1, 1),
R(0, 1), 0);
1049 if (s2 * fudge_factor > s) {
1053 invert_normal = (tmp < 0);
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));
1065 n =
Vec3f(-
R(1, 2),
R(0, 2), 0);
1069 if (s2 * fudge_factor > s) {
1073 invert_normal = (tmp < 0);
1079 *return_code = code;
1085 if (best_col_id != -1)
1086 normal = normalR->col(best_col_id);
1088 normal.noalias() = R1 * normalC;
1090 if (invert_normal) normal = -normal;
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);
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);
1116 Vec3f ua(R1.col((code - 7) / 3));
1117 Vec3f ub(R2.col((code - 7) % 3));
1125 contacts.push_back(
ContactPoint(-normal, pb, -*depth));
1126 *return_code = code;
1137 const Vec3f *pa, *pb, *Sa, *Sb;
1157 Vec3f normal2, nr, anr;
1163 nr.noalias() = Rb->transpose() * normal2;
1164 anr = nr.cwiseAbs();
1170 if (anr[1] > anr[0]) {
1171 if (anr[1] > anr[2]) {
1181 if (anr[0] > anr[2]) {
1195 center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
1197 center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
1200 int codeN, code1, code2;
1209 }
else if (codeN == 1) {
1220 c1 = Ra->col(code1).dot(center);
1221 c2 = Ra->col(code2).dot(center);
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);
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;
1247 rect[0] = (*Sa)[code1];
1248 rect[1] = (*Sa)[code2];
1253 if (n_intersect < 1) {
1254 *return_code = code;
1264 FCL_REAL det1 = 1.f / (m11 * m22 - m12 * m21);
1269 unsigned int cnum = 0;
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];
1282 *return_code = code;
1287 if (maxc > cnum) maxc = cnum;
1288 if (maxc < 1) maxc = 1;
1293 for (
unsigned int j = 0; j < cnum; ++j) {
1294 Vec3f pointInWorld = points[j] + (*pa);
1295 contacts.push_back(
ContactPoint(-normal, pointInWorld, -dep[j]));
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]));
1307 unsigned int i1 = 0;
1309 for (
unsigned int i = 1; i < cnum; ++i) {
1310 if (dep[i] > maxdepth) {
1316 unsigned int iret[8];
1319 for (
unsigned int j = 0; j < maxc; ++j) {
1320 Vec3f posInWorld = points[iret[j]] + (*pa);
1322 contacts.push_back(
ContactPoint(-normal, posInWorld, -dep[iret[j]]));
1325 -normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
1330 *return_code = code;
1343 std::vector<ContactPoint> contacts;
1349 normal, &depth, &return_code, 4, contacts);
1351 if (normal_) *normal_ = normal;
1352 if (penetration_depth_) *penetration_depth_ = depth;
1354 if (contact_points) {
1355 if (contacts.size() > 0) {
1357 *contact_points = contacts[0].point;
1358 if (penetration_depth_) *penetration_depth_ = -contacts[0].depth;
1362 return return_code != 0;
1365 template <
typename T>
1385 const Vec3f& center =
tf1.getTranslation();
1409 const Vec3f& T =
tf1.getTranslation();
1411 Vec3f Q(
R.transpose() * new_s2.
n);
1415 return (depth >= 0);
1425 const Vec3f& T =
tf1.getTranslation();
1428 const Vec3f Q(
R.transpose() * new_s2.
n);
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);
1474 const Vec3f& T =
tf1.getTranslation();
1479 if (std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>()) {
1493 int sign = (cosa > 0) ? -1 : 1;
1522 const Vec3f& T =
tf1.getTranslation();
1527 if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) {
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>())
1550 int sign = (cosa > 0) ? -1 : 1;
1573 const Vec3f& T =
tf1.getTranslation();
1578 if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) {
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>())
1607 if (
d1 > 0 && d2 > 0)
1612 p1 = p2 = ((
d1 < d2) ? a1 : a2) - (0.5 *
distance) * new_s2.
n;
1625 FCL_REAL depth = (std::numeric_limits<FCL_REAL>::max)();
1627 for (
unsigned int i = 0; i < s1.
num_points; ++i) {
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;
1670 p =
tf2.transform(
P3);
1681 p1 = p2 =
v - (0.5 * depth) * new_s1.
n;
1684 p1 =
v - depth * new_s1.
n;
1703 FCL_REAL& penetration_depth,
int& ret) {
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();
1714 if ((new_s1.
n).dot(new_s2.
n) > 0) {
1715 penetration_depth = new_s2.
d - new_s1.
d;
1716 if (penetration_depth < 0)
1724 penetration_depth = -(new_s1.
d + new_s2.
d);
1725 if (penetration_depth < 0)
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);
1759 FCL_REAL& penetration_depth,
int& ret) {
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();
1770 if ((new_s1.
n).dot(new_s2.
n) > 0) {
1771 if (new_s1.
d < new_s2.
d)
1782 penetration_depth = -(new_s1.
d + new_s2.
d);
1783 if (penetration_depth < 0)
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);
1813 Vec3f n_2(
tf2.getRotation().transpose() * n_w);
1816 p2 =
tf2.transform(p2);
1818 dist = (p2 -
tf1.getTranslation()).dot(n_w) - h.
d;
1819 p1 = p2 - dist * n_w;
1825 template <
typename T>
1848 const Vec3f& center =
tf1.getTranslation();
1852 if (signed_dist > 0)
1856 p1 = p2 = center - new_s2.
n * signed_dist;
1859 if (signed_dist > 0) {
1861 p2 = center - signed_dist * new_s2.
n;
1864 p2 = center + signed_dist * new_s2.
n;
1890 const Vec3f& T =
tf1.getTranslation();
1892 const Vec3f Q(
R.transpose() * new_s2.
n);
1896 distance = std::abs(signed_dist) -
A.lpNorm<1>();
1899 const bool positive = signed_dist > 0;
1902 for (Vec3f::Index i = 0; i < 3; ++i) {
1904 FCL_REAL alpha((positive ? 1 : -1) *
R.col(i).dot(new_s2.
n));
1907 }
else if (alpha < -
eps) {
1922 int sign = (signed_dist > 0) ? 1 : -1;
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);
1938 p.noalias() += (
A.array() > 0).
select(-tmp, tmp);
1942 if (signed_dist > 0)
1967 bool outside =
false;
1968 const Vec3f os_in_b_frame(Rb.transpose() * (os - ob));
1970 FCL_REAL min_d = (std::numeric_limits<FCL_REAL>::max)();
1971 for (
int i = 0; i < 3; ++i) {
1973 if (os_in_b_frame(i) < -
b.halfSide(i)) {
1974 pb.noalias() -=
b.halfSide(i) * Rb.col(i);
1976 }
else if (os_in_b_frame(i) >
b.halfSide(i)) {
1977 pb.noalias() +=
b.halfSide(i) * Rb.col(i);
1980 pb.noalias() += os_in_b_frame(i) * Rb.col(i);
1982 (facedist =
b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) {
1994 if (os_in_b_frame(
axis) >= 0)
1995 normal = Rb.col(
axis);
1997 normal = -Rb.col(
axis);
1998 dist = -min_d - s.
radius;
2000 if (!outside || dist <= 0) {
2004 ps = os - s.
radius * normal;
2017 const Vec3f& T1 =
tf1.getTranslation();
2019 Vec3f dir_z = R1.col(2);
2035 if (
d1 * d2 < -planeIntersectTolerance<FCL_REAL>()) {
2036 if (abs_d1 < abs_d2) {
2039 a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
2047 a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
2053 assert(!
p1.hasNaN() && !p2.hasNaN());
2063 if (abs_d1 < abs_d2) {
2072 assert(!
p1.hasNaN() && !p2.hasNaN());
2082 p1 = p2 = (c1 +
c2) * 0.5;
2083 }
else if (abs_d1 <= s1.
radius) {
2086 }
else if (abs_d2 <= s1.
radius) {
2097 assert(!
p1.hasNaN() && !p2.hasNaN());
2116 const Vec3f& T =
tf1.getTranslation();
2121 if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) {
2131 p1 = p2 = T - new_s2.
n * d;
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>())
2164 if (abs_d1 > abs_d2) {
2166 p1 = p2 =
c2 - new_s2.
n * d2;
2173 p1 = p2 = c1 - new_s2.
n *
d1;
2192 const Vec3f& T =
tf1.getTranslation();
2197 if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) {
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>())
2233 if ((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) ||
2234 (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
2238 for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] >= 0);
2241 FCL_REAL d_positive = 0, d_negative = 0;
2242 for (std::size_t i = 0; i < 3; ++i) {
2245 if (d_positive <= d[i]) d_positive = d[i];
2247 if (d_negative <= -d[i]) d_negative = -d[i];
2251 distance = -std::min(d_positive, d_negative);
2252 if (d_positive > d_negative)
2262 if (n_positive == 2) {
2263 for (std::size_t i = 0, j = 0; i < 3; ++i) {
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;
2278 for (std::size_t i = 0, j = 0; i < 3; ++i) {
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;
2300 Vec3f* contact_points,
2305 FCL_REAL d_min = (std::numeric_limits<FCL_REAL>::max)(),
2306 d_max = -(std::numeric_limits<FCL_REAL>::max)();
2308 for (
unsigned int i = 0; i < s1.
num_points; ++i) {
2323 if (d_min * d_max > 0)
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;
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;
2347 c[0] =
tf2.transform(
P1);
2348 c[1] =
tf2.transform(
P2);
2349 c[2] =
tf2.transform(
P3);
2356 if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0) {
2372 p1 =
c[imin] - d[imin] * new_s1.
n;
2375 if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0) {
2391 p1 =
c[imin] - d[imin] * new_s1.
n;
2395 for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] > 0);
2398 FCL_REAL d_positive = 0, d_negative = 0;
2399 for (std::size_t i = 0; i < 3; ++i) {
2402 if (d_positive <= d[i]) d_positive = d[i];
2404 if (d_negative <= -d[i]) d_negative = -d[i];
2408 distance = -std::min(d_positive, d_negative);
2409 if (d_positive > d_negative) {
2420 if (n_positive == 2) {
2421 for (std::size_t i = 0, j = 0; i < 3; ++i) {
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;
2436 for (std::size_t i = 0, j = 0; i < 3; ++i) {
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;
2457 FCL_REAL& penetration_depth,
int& ret) {
2470 if (
a == 1 && new_s1.
d != new_s2.
d)
return false;
2471 if (
a == -1 && new_s1.
d != -new_s2.
d)
return false;
2482 normal = u.normalized();
2486 return std::max(depth1, std::max(depth2, depth3));
2514 #endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H