48 bool Intersect::buildTrianglePlane(
const Vec3f& v1,
const Vec3f& v2,
50 Vec3f n_ = (v2 - v1).cross(v3 - v1);
53 *n = n_ / sqrt(norm2);
60 void TriangleDistance::segPoints(
const Vec3f& P,
const Vec3f& A,
const Vec3f& Q,
64 FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
82 FCL_REAL denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B;
84 t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom;
88 if ((t < 0) || std::isnan(t))
95 u = (
t * A_dot_B - B_dot_T) / B_dot_B;
101 if ((u <= 0) || std::isnan(u)) {
104 t = A_dot_T / A_dot_A;
106 if ((t <= 0) || std::isnan(t)) {
120 t = (A_dot_B + A_dot_T) / A_dot_A;
122 if ((t <= 0) || std::isnan(t)) {
137 if ((t <= 0) || std::isnan(t)) {
149 if (VEC.dot(T) < 0) {
182 int shown_disjoint = 0;
184 mindd = (S[0] - T[0]).squaredNorm() + 1;
186 for (
int i = 0; i < 3; ++i) {
187 for (
int j = 0; j < 3; ++j) {
190 segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
203 Z = S[(i + 2) % 3] - P;
205 Z = T[(j + 2) % 3] - Q;
208 if ((a <= 0) && (
b >= 0))
return dd;
214 if ((p - a +
b) > 0) shown_disjoint = 1;
238 Sn = Sv[0].cross(Sv[1]);
261 if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) {
266 if (Tp[2] < Tp[point]) point = 2;
267 }
else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) {
272 if (Tp[2] > Tp[point]) point = 2;
294 P = T[point] + Sn * (Tp[point] / Snl);
296 return (P - Q).squaredNorm();
306 Tn = Tv[0].cross(Tv[1]);
322 if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) {
327 if (Sp[2] < Sp[point]) point = 2;
328 }
else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) {
333 if (Sp[2] > Sp[point]) point = 2;
349 Q = S[point] + Tn * (Sp[point] / Tnl);
350 return (P - Q).squaredNorm();
362 if (shown_disjoint) {
383 return sqrTriDistance(S, T, P, Q);
389 Vec3f T_transformed[3];
390 T_transformed[0] =
R * T[0] + Tl;
391 T_transformed[1] =
R * T[1] + Tl;
392 T_transformed[2] =
R * T[2] + Tl;
394 return sqrTriDistance(S, T_transformed, P, Q);
398 const Transform3f& tf,
Vec3f& P,
400 Vec3f T_transformed[3];
401 T_transformed[0] = tf.transform(T[0]);
402 T_transformed[1] = tf.transform(T[1]);
403 T_transformed[2] = tf.transform(T[2]);
405 return sqrTriDistance(S, T_transformed, P, Q);
413 Vec3f T1_transformed =
R * T1 + Tl;
414 Vec3f T2_transformed =
R * T2 + Tl;
415 Vec3f T3_transformed =
R * T3 + Tl;
416 return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
417 T3_transformed, P, Q);
423 const Transform3f& tf,
Vec3f& P,
425 Vec3f T1_transformed = tf.transform(T1);
426 Vec3f T2_transformed = tf.transform(T2);
427 Vec3f T3_transformed = tf.transform(T3);
428 return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
429 T3_transformed, P, Q);
432 Project::ProjectResult Project::projectLine(
const Vec3f& a,
const Vec3f&
b,
441 res.parameterization[1] = (
t >= l) ? 1 : ((t <= 0) ? 0 : (
t / l));
442 res.parameterization[0] = 1 -
res.parameterization[1];
444 res.sqr_distance = (p -
b).squaredNorm();
447 res.sqr_distance = (p -
a).squaredNorm();
450 res.sqr_distance = (
a + d *
res.parameterization[1] - p).squaredNorm();
458 Project::ProjectResult Project::projectTriangle(
const Vec3f& a,
const Vec3f&
b,
463 static const size_t nexti[3] = {1, 2, 0};
466 const Vec3f& n = dl[0].cross(dl[1]);
471 for (
size_t i = 0; i < 3; ++i) {
472 if ((*vt[i] - p).dot(dl[i].cross(n)) >
477 ProjectResult res_line = projectLine(*vt[i], *vt[j], p);
479 if (mindist < 0 || res_line.sqr_distance < mindist) {
480 mindist = res_line.sqr_distance;
482 static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) +
483 ((res_line.encode & 2) ? 1 << j : 0));
484 res.parameterization[i] = res_line.parameterization[0];
485 res.parameterization[j] = res_line.parameterization[1];
486 res.parameterization[nexti[j]] = 0;
495 Vec3f p_to_project = n * (d / l);
496 mindist = p_to_project.squaredNorm();
498 res.parameterization[0] = dl[1].cross(
b - p - p_to_project).norm() / s;
499 res.parameterization[1] = dl[2].cross(c - p - p_to_project).norm() / s;
500 res.parameterization[2] =
501 1 -
res.parameterization[0] -
res.parameterization[1];
504 res.sqr_distance = mindist;
510 Project::ProjectResult Project::projectTetrahedra(
const Vec3f& a,
517 static const size_t nexti[] = {1, 2, 0};
518 const Vec3f* vt[] = {&
a, &
b, &
c, &d};
519 const Vec3f dl[3] = {
a - d,
b - d,
c - d};
521 bool ng = (vl * (
a - p).dot((
b - c).cross(a -
b))) <= 0;
530 for (
size_t i = 0; i < 3; ++i) {
532 FCL_REAL s = vl * (d - p).dot(dl[i].cross(dl[j]));
536 ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p);
537 if (mindist < 0 || res_triangle.sqr_distance < mindist) {
538 mindist = res_triangle.sqr_distance;
540 static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) +
541 (res_triangle.encode & 2 ? 1 << j : 0) +
542 (res_triangle.encode & 4 ? 8 : 0));
543 res.parameterization[i] = res_triangle.parameterization[0];
544 res.parameterization[j] = res_triangle.parameterization[1];
545 res.parameterization[nexti[j]] = 0;
546 res.parameterization[3] = res_triangle.parameterization[2];
554 res.parameterization[0] =
triple(c - p,
b - p, d - p) / vl;
555 res.parameterization[1] =
triple(a - p, c - p, d - p) / vl;
556 res.parameterization[2] =
triple(
b - p, a - p, d - p) / vl;
557 res.parameterization[3] =
558 1 - (
res.parameterization[0] +
res.parameterization[1] +
559 res.parameterization[2]);
562 res.sqr_distance = mindist;
564 res = projectTriangle(a,
b, c, p);
565 res.parameterization[3] = 0;
570 Project::ProjectResult Project::projectLineOrigin(
const Vec3f& a,
579 res.parameterization[1] = (
t >= l) ? 1 : ((t <= 0) ? 0 : (
t / l));
580 res.parameterization[0] = 1 -
res.parameterization[1];
582 res.sqr_distance =
b.squaredNorm();
585 res.sqr_distance =
a.squaredNorm();
588 res.sqr_distance = (
a + d *
res.parameterization[1]).squaredNorm();
596 Project::ProjectResult Project::projectTriangleOrigin(
const Vec3f& a,
601 static const size_t nexti[3] = {1, 2, 0};
604 const Vec3f& n = dl[0].cross(dl[1]);
609 for (
size_t i = 0; i < 3; ++i) {
610 if (vt[i]->dot(dl[i].cross(n)) >
615 ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]);
617 if (mindist < 0 || res_line.sqr_distance < mindist) {
618 mindist = res_line.sqr_distance;
620 static_cast<unsigned int>(((res_line.encode & 1) ? 1 << i : 0) +
621 ((res_line.encode & 2) ? 1 << j : 0));
622 res.parameterization[i] = res_line.parameterization[0];
623 res.parameterization[j] = res_line.parameterization[1];
624 res.parameterization[nexti[j]] = 0;
633 Vec3f o_to_project = n * (d / l);
634 mindist = o_to_project.squaredNorm();
636 res.parameterization[0] = dl[1].cross(
b - o_to_project).norm() / s;
637 res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s;
638 res.parameterization[2] =
639 1 -
res.parameterization[0] -
res.parameterization[1];
642 res.sqr_distance = mindist;
648 Project::ProjectResult Project::projectTetrahedraOrigin(
const Vec3f& a,
654 static const size_t nexti[] = {1, 2, 0};
655 const Vec3f* vt[] = {&
a, &
b, &
c, &d};
656 const Vec3f dl[3] = {
a - d,
b - d,
c - d};
658 bool ng = (vl *
a.dot((
b - c).cross(a -
b))) <= 0;
667 for (
size_t i = 0; i < 3; ++i) {
669 FCL_REAL s = vl * d.dot(dl[i].cross(dl[j]));
673 ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d);
674 if (mindist < 0 || res_triangle.sqr_distance < mindist) {
675 mindist = res_triangle.sqr_distance;
677 static_cast<unsigned int>((res_triangle.encode & 1 ? 1 << i : 0) +
678 (res_triangle.encode & 2 ? 1 << j : 0) +
679 (res_triangle.encode & 4 ? 8 : 0));
680 res.parameterization[i] = res_triangle.parameterization[0];
681 res.parameterization[j] = res_triangle.parameterization[1];
682 res.parameterization[nexti[j]] = 0;
683 res.parameterization[3] = res_triangle.parameterization[2];
691 res.parameterization[0] =
triple(c,
b, d) / vl;
692 res.parameterization[1] =
triple(a, c, d) / vl;
693 res.parameterization[2] =
triple(
b, a, d) / vl;
694 res.parameterization[3] =
695 1 - (
res.parameterization[0] +
res.parameterization[1] +
696 res.parameterization[2]);
699 res.sqr_distance = mindist;
701 res = projectTriangleOrigin(a,
b, c);
702 res.parameterization[3] = 0;