38 #ifndef FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H
39 #define FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H
45 #include <unordered_map>
46 #include <unordered_set>
61 class FCL_EXPORT GJKInitializer<double, Cylinder<double>>;
65 class FCL_EXPORT GJKInitializer<double, Sphere<double>>;
69 class FCL_EXPORT GJKInitializer<double, Ellipsoid<double>>;
73 class FCL_EXPORT GJKInitializer<double, Box<double>>;
77 class FCL_EXPORT GJKInitializer<double, Capsule<double>>;
81 class FCL_EXPORT GJKInitializer<double, Cone<double>>;
85 class FCL_EXPORT GJKInitializer<double, Convex<double>>;
104 ccd_support_fn supp1,
107 ccd_support_fn supp2,
109 unsigned int max_iterations,
112 double* penetration_depth,
119 ccd_support_fn supp1,
121 ccd_support_fn supp2,
122 unsigned int max_iterations,
131 ccd_support_fn supp1,
133 ccd_support_fn supp2,
134 unsigned int max_iterations,
176 template <
typename S>
188 namespace libccd_extension
203 const ccd_vec3_t* c) {
216 return ccdVec3PointTriDist2(P, a, b, c, &unused);
228 return ccdVec3PointTriDist2(P, a, b, c, w);
233 ccd_vec3_t *best_witness)
241 for (i = 0; i < 3; i++){
246 newdist = CCD_SQRT(newdist);
252 ccdVec3Copy(best_witness, &witness);
264 _ccd_inline
void tripleCross(
const ccd_vec3_t *a,
const ccd_vec3_t *b,
265 const ccd_vec3_t *c, ccd_vec3_t *d)
268 ccdVec3Cross(&e, a, b);
269 ccdVec3Cross(d, &e, c);
376 assert(p_OA.dot(p_OB) <= p_OB.squaredNorm() * eps &&
"A is not in region 1");
396 if (plane_normal.squaredNorm() <
397 p_AB.squaredNorm() * p_OB.squaredNorm() * eps * eps) {
410 ccdVec3Set(dir, new_dir(0), new_dir(1), new_dir(2));
432 ccd_vec3_t AO, AB, AC, ABC, tmp;
443 ccd_vec3_origin, &A->
v, &B->
v, &C->
v);
454 if (ccdVec3Eq(&A->
v, &B->
v) || ccdVec3Eq(&A->
v, &C->
v)){
460 ccdVec3Copy(&AO, &A->
v);
461 ccdVec3Scale(&AO, -CCD_ONE);
464 ccdVec3Sub2(&AB, &B->
v, &A->
v);
465 ccdVec3Sub2(&AC, &C->
v, &A->
v);
466 ccdVec3Cross(&ABC, &AB, &AC);
468 ccdVec3Cross(&tmp, &ABC, &AC);
469 dot = ccdVec3Dot(&tmp, &AO);
470 if (ccdIsZero(dot) || dot > CCD_ZERO){
471 dot = ccdVec3Dot(&AC, &AO);
472 if (ccdIsZero(dot) || dot > CCD_ZERO){
479 dot = ccdVec3Dot(&AB, &AO);
480 if (ccdIsZero(dot) || dot > CCD_ZERO){
488 ccdVec3Copy(dir, &AO);
492 ccdVec3Cross(&tmp, &AB, &ABC);
493 dot = ccdVec3Dot(&tmp, &AO);
494 if (ccdIsZero(dot) || dot > CCD_ZERO){
495 goto ccd_do_simplex3_45;
497 dot = ccdVec3Dot(&ABC, &AO);
498 if (ccdIsZero(dot) || dot > CCD_ZERO){
499 ccdVec3Copy(dir, &ABC);
506 ccdVec3Copy(dir, &ABC);
507 ccdVec3Scale(dir, -CCD_ONE);
518 ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
519 int B_on_ACD, C_on_ADB, D_on_ABC;
520 int AB_O, AC_O, AD_O;
533 &A->
v, &B->
v, &C->
v, &D->
v);
554 ccdVec3Copy(&AO, &A->
v);
555 ccdVec3Scale(&AO, -CCD_ONE);
556 ccdVec3Sub2(&AB, &B->
v, &A->
v);
557 ccdVec3Sub2(&AC, &C->
v, &A->
v);
558 ccdVec3Sub2(&AD, &D->
v, &A->
v);
559 ccdVec3Cross(&ABC, &AB, &AC);
560 ccdVec3Cross(&ACD, &AC, &AD);
561 ccdVec3Cross(&ADB, &AD, &AB);
565 B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
566 C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
567 D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
571 AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
572 AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
573 AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
575 if (AB_O && AC_O && AD_O){
626 ccd_vec3_t ab, ac, dir;
645 for (i = 0; i < ccd_points_on_sphere_len; i++){
647 if (!ccdVec3Eq(&a->
v, &supp[0].
v) && !ccdVec3Eq(&b->
v, &supp[0].
v)){
653 goto simplexToPolytope2_touching_contact;
656 ccdVec3Copy(&dir, &supp[0].v);
657 ccdVec3Scale(&dir, -CCD_ONE);
659 if (ccdVec3Eq(&a->
v, &supp[1].
v) || ccdVec3Eq(&b->
v, &supp[1].
v))
660 goto simplexToPolytope2_touching_contact;
663 ccdVec3Sub2(&ab, &supp[0].v, &a->
v);
664 ccdVec3Sub2(&ac, &supp[1].v, &a->
v);
665 ccdVec3Cross(&dir, &ab, &ac);
667 if (ccdVec3Eq(&a->
v, &supp[2].
v) || ccdVec3Eq(&b->
v, &supp[2].
v))
668 goto simplexToPolytope2_touching_contact;
671 ccdVec3Scale(&dir, -CCD_ONE);
673 if (ccdVec3Eq(&a->
v, &supp[3].
v) || ccdVec3Eq(&b->
v, &supp[3].
v))
674 goto simplexToPolytope2_touching_contact;
676 goto simplexToPolytope2_not_touching_contact;
677 simplexToPolytope2_touching_contact:
681 if (*nearest == NULL)
686 simplexToPolytope2_not_touching_contact:
771 assert(simplex->
last == 2);
774 ccd_vec3_t ab, ac, dir;
787 ccdVec3Sub2(&ab, &b->
v, &a->
v);
788 ccdVec3Sub2(&ac, &c->
v, &a->
v);
789 ccdVec3Cross(&dir, &ab, &ac);
791 const ccd_real_t dist_squared =
795 ccdVec3Scale(&dir, -CCD_ONE);
797 const ccd_real_t dist_squared_opposite =
802 if (ccdIsZero(dist_squared) || ccdIsZero(dist_squared_opposite)) {
810 if (*nearest == NULL)
return -2;
818 auto FormTetrahedron = [polytope, a, b, c, &v,
848 if (std::abs(dist_squared) > std::abs(dist_squared_opposite)) {
849 return FormTetrahedron(d);
851 return FormTetrahedron(d2);
861 bool use_polytope3{
false};
878 ccd_real_t dist_squared =
881 use_polytope3 =
true;
883 if (!use_polytope3) {
887 use_polytope3 =
true;
892 if (!use_polytope3) {
896 use_polytope3 =
true;
900 if (!use_polytope3) {
904 use_polytope3 =
true;
917 for (i = 0; i < 4; i++) {
959 for (
int i = 0; i < 3; ++i) {
961 max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps;
962 const ccd_real_t delta = abs(p.v[i] - q.v[i]);
975 const ccd_vec3_t& c) {
988 ccd_vec3_t AB, AC, n;
989 ccdVec3Sub2(&AB, &b, &a);
990 ccdVec3Sub2(&AC, &c, &a);
991 ccdVec3Normalize(&AB);
992 ccdVec3Normalize(&AC);
993 ccdVec3Cross(&n, &AB, &AC);
996 if (ccdVec3Len2(&n) < eps * eps)
return true;
1021 const ccd_vec3_t& test_v = face->
edge[1]->
vertex[0]->
v.
v;
1026 "Cannot compute face normal for a degenerate (zero-area) triangle");
1040 ccdVec3Cross(&dir, &e1, &e2);
1041 const ccd_real_t dir_norm = std::sqrt(ccdVec3Len2(&dir));
1042 ccd_vec3_t unit_dir = dir;
1043 ccdVec3Scale(&unit_dir, 1.0 / dir_norm);
1062 const ccd_real_t dist_tol = 0.01;
1066 const ccd_real_t origin_distance_to_plane =
1067 ccdVec3Dot(&unit_dir, &(face->
edge[0]->
vertex[0]->
v.
v));
1068 if (origin_distance_to_plane < -dist_tol) {
1072 ccdVec3Scale(&dir, ccd_real_t(-1));
1073 }
else if (-dist_tol <= origin_distance_to_plane &&
1074 origin_distance_to_plane <= dist_tol) {
1077 ccd_real_t max_distance_to_plane = -CCD_REAL_MAX;
1078 ccd_real_t min_distance_to_plane = CCD_REAL_MAX;
1088 const ccd_real_t distance_to_plane =
1089 ccdVec3Dot(&unit_dir, &(v->
v.
v)) - origin_distance_to_plane;
1090 if (distance_to_plane > dist_tol) {
1094 ccdVec3Scale(&dir, ccd_real_t(-1));
1096 }
else if (distance_to_plane < -dist_tol) {
1101 max_distance_to_plane =
1102 std::max(max_distance_to_plane, distance_to_plane);
1103 min_distance_to_plane =
1104 std::min(min_distance_to_plane, distance_to_plane);
1111 if (max_distance_to_plane > std::abs(min_distance_to_plane)) {
1112 ccdVec3Scale(&dir, ccd_real_t(-1));
1129 return ccdVec3Dot(&n, &r_VP) > 0;
1148 const std::unordered_set<ccd_pt_edge_t*>& border_edges,
1149 const std::unordered_set<ccd_pt_face_t*>& visible_faces,
1150 const std::unordered_set<ccd_pt_edge_t*>& internal_edges) {
1155 bool has_edge_internal =
false;
1156 for (
int i = 0; i < 3; ++i) {
1159 if (internal_edges.count(f->
edge[i]) > 0) {
1160 has_edge_internal =
true;
1164 if (has_edge_internal) {
1165 if (visible_faces.count(f) == 0) {
1172 if (visible_faces.count(e->
faces[0]) > 0 &&
1173 visible_faces.count(e->
faces[1]) > 0) {
1174 if (internal_edges.count(e) == 0) {
1177 }
else if (visible_faces.count(e->
faces[0]) +
1178 visible_faces.count(e->
faces[1]) ==
1180 if (border_edges.count(e) == 0) {
1185 for (
const auto b : border_edges) {
1186 if (internal_edges.count(b) > 0) {
1202 std::unordered_set<ccd_pt_edge_t*>* border_edges,
1203 std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1204 border_edges->insert(edge);
1205 if (internal_edges->count(edge) > 0) {
1207 "An edge is being classified as border that has already been "
1208 "classifed as internal");
1220 std::unordered_set<ccd_pt_edge_t*>* border_edges,
1221 std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1222 internal_edges->insert(edge);
1223 if (border_edges->count(edge) > 0) {
1225 "An edge is being classified as internal that has already been "
1226 "classified as border");
1245 const ccd_vec3_t& query_point,
1246 std::unordered_set<ccd_pt_edge_t*>* border_edges,
1247 std::unordered_set<ccd_pt_face_t*>* visible_faces,
1248 std::unordered_set<ccd_pt_face_t*>* hidden_faces,
1249 std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1253 assert(g !=
nullptr);
1255 bool is_visible = visible_faces->count(g) > 0;
1256 bool is_hidden = hidden_faces->count(g) > 0;
1257 assert(!(is_visible && is_hidden));
1294 visible_faces->insert(g);
1296 for (
int i = 0; i < 3; ++i) {
1297 if (g->
edge[i] != edge) {
1300 visible_faces, hidden_faces,
1308 hidden_faces->insert(g);
1348 const ccd_vec3_t& query_point,
1349 std::unordered_set<ccd_pt_edge_t*>* border_edges,
1350 std::unordered_set<ccd_pt_face_t*>* visible_faces,
1351 std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
1352 assert(border_edges);
1353 assert(visible_faces);
1354 assert(internal_edges);
1355 assert(border_edges->empty());
1356 assert(visible_faces->empty());
1357 assert(internal_edges->empty());
1359 std::unordered_set<ccd_pt_face_t*> hidden_faces;
1360 visible_faces->insert(&f);
1361 for (
int edge_index = 0; edge_index < 3; ++edge_index) {
1363 border_edges, visible_faces, &hidden_faces,
1369 polytope, *border_edges, *visible_faces, *internal_edges)) {
1371 "The visible patch failed its sanity check");
1426 "The visible feature is a vertex. This should already have been "
1427 "identified as a touching contact");
1442 "Both the nearest point and the new vertex are on an edge, thus the "
1443 "nearest distance should be 0. This is touching contact, and should "
1444 "already have been identified");
1448 std::unordered_set<ccd_pt_face_t*> visible_faces;
1449 std::unordered_set<ccd_pt_edge_t*> internal_edges;
1450 std::unordered_set<ccd_pt_edge_t*> border_edges;
1452 &visible_faces, &internal_edges);
1459 for (
const auto& f : visible_faces) {
1464 for (
const auto& e : internal_edges) {
1487 std::unordered_map<ccd_pt_vertex_t*, ccd_pt_edge_t*> map_vertex_to_new_edge;
1488 for (
const auto& border_edge : border_edges) {
1491 for (
int i = 0; i < 2; ++i) {
1492 auto it = map_vertex_to_new_edge.find(border_edge->vertex[i]);
1493 if (it == map_vertex_to_new_edge.end()) {
1495 e[i] =
ccdPtAddEdge(polytope, new_vertex, border_edge->vertex[i]);
1496 map_vertex_to_new_edge.emplace_hint(it, border_edge->vertex[i],
1529 if (ccdIsZero(nearest_feature->dist)) {
1531 switch (nearest_feature->type) {
1534 "The nearest point to the origin is a vertex of the polytope. This "
1535 "should be identified as a touching contact");
1559 ccdVec3Copy(&dir, &(nearest_feature->witness));
1561 ccdVec3Normalize(&dir);
1585 const void*
obj2,
const ccd_t* ccd,
1587 ccd_vec3_t *a, *b, *c;
1597 const ccd_real_t dist = ccdVec3Dot(&out->
v, &dir);
1603 if (dist - std::sqrt(el->dist) < ccd->epa_tolerance)
return -1;
1605 ccd_real_t dist_squared{};
1611 dist_squared = ccdVec3PointSegmentDist2(&out->
v, a, b, NULL);
1620 if (std::sqrt(dist_squared) < ccd->epa_tolerance)
return -1;
1629 unsigned long iterations;
1645 ccdVec3Copy(&dir, &last.
v);
1646 ccdVec3Scale(&dir, -CCD_ONE);
1649 for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
1656 if (ccdVec3Dot(&last.
v, &dir) < CCD_ZERO){
1665 do_simplex_res =
doSimplex(simplex, &dir);
1666 if (do_simplex_res == 1){
1668 }
else if (do_simplex_res == -1){
1672 if (ccdIsZero(ccdVec3Len2(&dir))){
1717 std::array<ccd_vec3_t, 2> face_normals;
1718 std::array<double, 2> origin_to_face_distance;
1723 const ccd_real_t v0_dist =
1724 std::sqrt(ccdVec3Len2(&nearest_edge->
vertex[0]->
v.
v));
1725 const ccd_real_t plane_threshold =
1726 kEps *
std::max(
static_cast<ccd_real_t
>(1.0), v0_dist);
1728 for (
int i = 0; i < 2; ++i) {
1731 ccdVec3Normalize(&face_normals[i]);
1734 origin_to_face_distance[i] =
1735 -ccdVec3Dot(&face_normals[i], &nearest_edge->
vertex[0]->
v.
v);
1747 if (origin_to_face_distance[i] > plane_threshold) {
1748 std::ostringstream oss;
1749 oss <<
"The origin is outside of the polytope by "
1750 << origin_to_face_distance[i] <<
", exceeding the threshold "
1752 <<
". This should already have been identified as separating.";
1760 const bool is_edge_closest_feature = nearest_edge->dist <
kEps *
kEps;
1762 if (!is_edge_closest_feature) {
1770 const int closest_face =
1771 origin_to_face_distance[0] < origin_to_face_distance[1] ? 1 : 0;
1775 polytope->
nearest_dist = pow(origin_to_face_distance[closest_face], 2);
1795 }
else if (size == 3) {
1806 }
else if (ret == -2){
1842 if (p1) *p1 = q->
v1;
1843 if (p2) *p2 = q->
v2;
1851 ccd_vec3_t *p1, ccd_vec3_t *p2,
1859 ccdVec3Sub2(&AB, &(b->
v), &(a->
v));
1872 ccd_real_t abs_AB_x{std::abs(ccdVec3X(&AB))};
1873 ccd_real_t abs_AB_y{std::abs(ccdVec3Y(&AB))};
1874 ccd_real_t abs_AB_z{std::abs(ccdVec3Z(&AB))};
1876 ccd_real_t A_i, AB_i, p_i;
1877 if (abs_AB_x >= abs_AB_y && abs_AB_x >= abs_AB_z) {
1878 A_i = ccdVec3X(&(a->
v));
1879 AB_i = ccdVec3X(&AB);
1881 }
else if (abs_AB_y >= abs_AB_z) {
1882 A_i = ccdVec3Y(&(a->
v));
1883 AB_i = ccdVec3Y(&AB);
1886 A_i = ccdVec3Z(&(a->
v));
1887 AB_i = ccdVec3Z(&AB);
1897 auto calc_p = [](ccd_vec3_t *p_a, ccd_vec3_t *p_b, ccd_vec3_t *p,
1900 ccdVec3Sub2(&sAB, p_b, p_a);
1901 ccdVec3Scale(&sAB, s);
1902 ccdVec3Copy(p, p_a);
1903 ccdVec3Add(p, &sAB);
1909 ccd_real_t s = (p_i - A_i) / AB_i;
1913 calc_p(&(a->
v1), &(b->
v1), p1, s);
1917 calc_p(&(a->
v2), &(b->
v2), p2, s);
1927 ccd_vec3_t *p2, ccd_vec3_t *p) {
1929 assert(simplex_size <= 3);
1930 if (simplex_size == 1)
1934 else if (simplex_size == 2)
1941 simplex->
ps[2].
v)) {
1944 int a_index, b_index;
1945 ccd_vec3_t AB, AC, BC;
1946 ccdVec3Sub2(&AB, &(simplex->
ps[1].
v), &(simplex->
ps[0].
v));
1947 ccdVec3Sub2(&AC, &(simplex->
ps[2].
v), &(simplex->
ps[0].
v));
1948 ccdVec3Sub2(&BC, &(simplex->
ps[2].
v), &(simplex->
ps[1].
v));
1949 ccd_real_t AB_len2 = ccdVec3Len2(&AB);
1950 ccd_real_t AC_len2 = ccdVec3Len2(&AC);
1951 ccd_real_t BC_len2 = ccdVec3Len2(&BC);
1952 if (AB_len2 >= AC_len2 && AB_len2 >= BC_len2) {
1955 }
else if (AC_len2 >= AB_len2 && AC_len2 >= BC_len2) {
1963 &simplex->
ps[b_index], p1, p2, p);
2003 ccd_vec3_t r_AB, r_AC, n;
2004 ccdVec3Sub2(&r_AB, &(simplex->
ps[1].
v), &(simplex->
ps[0].
v));
2005 ccdVec3Sub2(&r_AC, &(simplex->
ps[2].
v), &(simplex->
ps[0].
v));
2006 ccdVec3Cross(&n, &r_AB, &r_AC);
2007 ccd_real_t norm_squared_n{ccdVec3Len2(&n)};
2011 ccdVec3Sub2(&r_Ap, p, &(simplex->
ps[0].
v));
2014 ccd_vec3_t r_Ap_cross_r_AC, r_AB_cross_r_Ap;
2015 ccdVec3Cross(&r_Ap_cross_r_AC, &r_Ap, &r_AC);
2016 ccdVec3Cross(&r_AB_cross_r_Ap, &r_AB, &r_Ap);
2019 ccd_real_t beta{ccdVec3Dot(&n, &r_Ap_cross_r_AC) / norm_squared_n};
2020 ccd_real_t gamma{ccdVec3Dot(&n, &r_AB_cross_r_Ap) / norm_squared_n};
2024 auto interpolate = [&beta, &gamma](
const ccd_vec3_t& r_WA,
2025 const ccd_vec3_t& r_WB,
2026 const ccd_vec3_t& r_WC,
2029 ccdVec3Copy(r_WP, &r_WA);
2031 ccd_vec3_t beta_r_AB;
2032 ccdVec3Sub2(&beta_r_AB, &r_WB, &r_WA);
2033 ccdVec3Scale(&beta_r_AB, beta);
2034 ccdVec3Add(r_WP, &beta_r_AB);
2036 ccd_vec3_t gamma_r_AC;
2037 ccdVec3Sub2(&gamma_r_AC, &r_WC, &r_WA);
2038 ccdVec3Scale(&gamma_r_AC, gamma);
2039 ccdVec3Add(r_WP, &gamma_r_AC);
2043 interpolate(simplex->
ps[0].
v1, simplex->
ps[1].
v1, simplex->
ps[2].
v1, p1);
2047 interpolate(simplex->
ps[0].
v2, simplex->
ps[1].
v2, simplex->
ps[2].
v2, p2);
2070 ccd_vec3_t* p1, ccd_vec3_t* p2)
2072 ccd_real_t last_dist = CCD_REAL_MAX;
2074 for (
unsigned long iterations = 0UL; iterations < ccd->max_iterations;
2076 ccd_vec3_t closest_p;
2088 dist = CCD_SQRT(dist);
2092 dist = ccdVec3PointSegmentDist2(ccd_vec3_origin,
2096 dist = CCD_SQRT(dist);
2104 dist = CCD_SQRT(dist);
2112 if ((last_dist - dist) < ccd->dist_tolerance)
2120 ccdVec3Copy(&dir, &closest_p);
2121 ccdVec3Scale(&dir, -CCD_ONE);
2122 ccdVec3Normalize(&dir);
2134 dist = ccdVec3Len2(&last.
v);
2135 dist = CCD_SQRT(dist);
2136 if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
2146 return -CCD_REAL(1.);
2169 ccdVec3Copy(p1, &v->
v.
v1);
2170 ccdVec3Copy(p2, &v->
v.
v2);
2191 for (
int i = 0; i < 2; ++i) {
2200 throw std::logic_error(
2201 "FCL penEPAPosClosest(): Unsupported feature type. The closest point "
2202 "should be either a vertex, on an edge, or on a face.");
2211 ccdVec3Copy(&p, &(nearest->witness));
2218 const ccd_t* ccd, ccd_vec3_t* p1,
2231 if (ret == 0 && nearest)
2233 depth = -CCD_SQRT(nearest->dist);
2235 ccd_vec3_t pos1, pos2;
2281 const ccd_t *ccd, ccd_vec3_t* p1,
2295 template <
typename S>
2303 ccdVec3Set(&o->
pos, T[0], T[1], T[2]);
2304 ccdQuatSet(&o->
rot, q.x(), q.y(), q.z(), q.w());
2308 template <
typename S>
2312 box->
dim[0] = s.
side[0] / 2.0;
2313 box->
dim[1] = s.
side[1] / 2.0;
2314 box->
dim[2] = s.
side[2] / 2.0;
2317 template <
typename S>
2326 template <
typename S>
2335 template <
typename S>
2344 template <
typename S>
2352 template <
typename S>
2362 template <
typename S>
2379 auto sign = [](ccd_real_t x) -> ccd_real_t {
2380 return x >= 0 ? ccd_real_t(1.0) : ccd_real_t(-1.0);
2384 ccdVec3Copy(&dir,
dir_);
2385 ccdQuatRotVec(&dir, &o->
rot_inv);
2386 ccdVec3Set(v, sign(ccdVec3X(&dir)) * o->
dim[0],
2387 sign(ccdVec3Y(&dir)) * o->
dim[1],
2388 sign(ccdVec3Z(&dir)) * o->
dim[2]);
2389 ccdQuatRotVec(v, &o->
rot);
2390 ccdVec3Add(v, &o->
pos);
2397 ccd_vec3_t dir, pos1, pos2;
2399 ccdVec3Copy(&dir,
dir_);
2400 ccdQuatRotVec(&dir, &o->
rot_inv);
2402 ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->
height);
2403 ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->
height);
2405 ccdVec3Copy(v, &dir);
2406 ccdVec3Normalize (v);
2407 ccdVec3Scale(v, o->
radius);
2408 ccdVec3Add(&pos1, v);
2409 ccdVec3Add(&pos2, v);
2411 if (ccdVec3Z (&dir) > 0)
2412 ccdVec3Copy(v, &pos1);
2414 ccdVec3Copy(v, &pos2);
2417 ccdQuatRotVec(v, &o->
rot);
2418 ccdVec3Add(v, &o->
pos);
2428 ccdVec3Copy(&dir,
dir_);
2429 ccdQuatRotVec(&dir, &cyl->
rot_inv);
2431 zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
2432 zdist = sqrt(zdist);
2433 if (ccdIsZero(zdist))
2434 ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->
height);
2437 rad = cyl->
radius / zdist;
2439 ccdVec3Set(v, rad * ccdVec3X(&dir),
2440 rad * ccdVec3Y(&dir),
2441 ccdSign(ccdVec3Z(&dir)) * cyl->
height);
2445 ccdQuatRotVec(v, &cyl->
rot);
2446 ccdVec3Add(v, &cyl->
pos);
2455 ccdVec3Copy(&dir,
dir_);
2456 ccdQuatRotVec(&dir, &cone->
rot_inv);
2458 double zdist, len, rad;
2459 zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
2460 len = zdist + dir.v[2] * dir.v[2];
2461 zdist = sqrt(zdist);
2466 if (dir.v[2] > len * sin_a)
2467 ccdVec3Set(v, 0., 0., cone->
height);
2470 rad = cone->
radius / zdist;
2471 ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->
height);
2474 ccdVec3Set(v, 0, 0, -cone->
height);
2477 ccdQuatRotVec(v, &cone->
rot);
2478 ccdVec3Add(v, &cone->
pos);
2487 ccdVec3Copy(&dir,
dir_);
2488 ccdQuatRotVec(&dir, &s->
rot_inv);
2490 ccdVec3Copy(v, &dir);
2491 ccdVec3Scale(v, s->
radius);
2492 ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
2495 ccdQuatRotVec(v, &s->
rot);
2496 ccdVec3Add(v, &s->
pos);
2505 ccdVec3Copy(&dir,
dir_);
2506 ccdQuatRotVec(&dir, &s->
rot_inv);
2513 v->v[0] = abc2.v[0] * dir.v[0];
2514 v->v[1] = abc2.v[1] * dir.v[1];
2515 v->v[2] = abc2.v[2] * dir.v[2];
2517 ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Dot(v, &dir)));
2520 ccdQuatRotVec(v, &s->
rot);
2521 ccdVec3Add(v, &s->
pos);
2524 template <
typename S>
2533 ccdVec3Copy(&dir,
dir_);
2534 ccdQuatRotVec(&dir, &c->rot_inv);
2538 Vector3<S> dir_C{S(dir.v[0]), S(dir.v[1]), S(dir.v[2])};
2541 const Vector3<S>& p_CE = c->convex->findExtremeVertex(dir_C);
2547 ccdQuatRotVec(v, &c->rot);
2548 ccdVec3Add(v, &c->pos);
2556 ccd_real_t maxdot, dot;
2559 ccdVec3Copy(&dir,
dir_);
2560 ccdQuatRotVec(&dir, &tri->
rot_inv);
2562 maxdot = -CCD_REAL_MAX;
2564 for (i = 0; i < 3; ++i)
2566 ccdVec3Set(&p, tri->
p[i].v[0] - tri->
c.v[0], tri->
p[i].v[1] - tri->
c.v[1],
2567 tri->
p[i].v[2] - tri->
c.v[2]);
2568 dot = ccdVec3Dot(&dir, &p);
2571 ccdVec3Copy(v, &tri->
p[i]);
2577 ccdQuatRotVec(v, &tri->
rot);
2578 ccdVec3Add(v, &tri->
pos);
2584 ccdVec3Copy(c, &o->
pos);
2587 template <
typename S>
2592 ccdVec3Set(c, p[0], p[1], p[2]);
2593 ccdQuatRotVec(c, &o->rot);
2594 ccdVec3Add(c, &o->pos);
2600 ccdVec3Copy(c, &o->
c);
2601 ccdQuatRotVec(c, &o->
rot);
2602 ccdVec3Add(c, &o->
pos);
2605 template <
typename S>
2607 void*
obj2, ccd_support_fn supp2, ccd_center_fn cen2,
2608 unsigned int max_iterations, S
tolerance,
2609 Vector3<S>* contact_points, S* penetration_depth,
2615 ccd_vec3_t dir, pos;
2619 ccd.support1 = supp1;
2620 ccd.support2 = supp2;
2623 ccd.max_iterations = max_iterations;
2626 if (!contact_points)
2628 return ccdMPRIntersect(
obj1,
obj2, &ccd);
2634 res = ccdMPRPenetration(
obj1,
obj2, &ccd, &depth, &dir, &pos);
2637 *contact_points << ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos);
2638 *penetration_depth = depth;
2639 *normal << ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir);
2652 using DistanceFn = std::function<ccd_real_t (
2653 const void*,
const void*,
const ccd_t*, ccd_vec3_t*, ccd_vec3_t*)>;
2677 template <
typename S>
2679 ccd_support_fn supp2,
unsigned int max_iterations,
2685 ccd.support1 = supp1;
2686 ccd.support2 = supp2;
2688 ccd.max_iterations = max_iterations;
2692 ccd_vec3_t p1_, p2_;
2698 ccdVec3Set(&p1_, 0.0, 0.0, 0.0);
2699 ccdVec3Set(&p2_, 0.0, 0.0, 0.0);
2700 dist = distance_func(
obj1,
obj2, &ccd, &p1_, &p2_);
2701 if (p1) *p1 << ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_);
2702 if (p2) *p2 << ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_);
2703 if (res) *res = dist;
2710 template <
typename S>
2712 void*
obj2, ccd_support_fn supp2,
2713 unsigned int max_iterations, S
tolerance,
2733 template <
typename S>
2735 void*
obj2, ccd_support_fn supp2,
2736 unsigned int max_iterations,
2743 template <
typename S>
2749 template <
typename S>
2755 template <
typename S>
2764 template <
typename S>
2771 template <
typename S>
2777 template <
typename S>
2783 template <
typename S>
2792 template <
typename S>
2799 template <
typename S>
2805 template <
typename S>
2811 template <
typename S>
2820 template <
typename S>
2827 template <
typename S>
2833 template <
typename S>
2839 template <
typename S>
2848 template <
typename S>
2855 template <
typename S>
2861 template <
typename S>
2867 template <
typename S>
2876 template <
typename S>
2883 template <
typename S>
2889 template <
typename S>
2895 template <
typename S>
2904 template <
typename S>
2911 template <
typename S>
2914 return &supportConvex<S>;
2917 template <
typename S>
2920 return ¢erConvex<S>;
2923 template <
typename S>
2932 template <
typename S>
2949 template <
typename S>
2954 Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3,
2955 (P1[2] + P2[2] + P3[2]) / 3);
2957 ccdVec3Set(&o->
p[0], P1[0], P1[1], P1[2]);
2958 ccdVec3Set(&o->
p[1], P2[0], P2[1], P2[2]);
2959 ccdVec3Set(&o->
p[2], P3[0], P3[1], P3[2]);
2960 ccdVec3Set(&o->
c, center[0], center[1], center[2]);
2961 ccdVec3Set(&o->
pos, 0., 0., 0.);
2962 ccdQuatSet(&o->
rot, 0., 0., 0., 1.);
2968 template <
typename S>
2973 Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3,
2974 (P1[2] + P2[2] + P3[2]) / 3);
2976 ccdVec3Set(&o->
p[0], P1[0], P1[1], P1[2]);
2977 ccdVec3Set(&o->
p[1], P2[0], P2[1], P2[2]);
2978 ccdVec3Set(&o->
p[2], P3[0], P3[1], P3[2]);
2979 ccdVec3Set(&o->
c, center[0], center[1], center[2]);
2982 ccdVec3Set(&o->
pos, T[0], T[1], T[2]);
2983 ccdQuatSet(&o->
rot, q.x(), q.y(), q.z(), q.w());