38 #ifndef FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H 39 #define FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H 41 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h" 51 class GJKInitializer<double, Cylinder<double>>;
55 class GJKInitializer<double, Sphere<double>>;
59 class GJKInitializer<double, Ellipsoid<double>>;
63 class GJKInitializer<double, Box<double>>;
67 class GJKInitializer<double, Capsule<double>>;
71 class GJKInitializer<double, Cone<double>>;
75 class GJKInitializer<double, Convex<double>>;
79 void* triCreateGJKObject(
80 const Vector3d& P1,
const Vector3d& P2,
const Vector3d& P3);
84 void* triCreateGJKObject(
88 const Transform3d& tf);
99 unsigned int max_iterations,
101 Vector3d* contact_points,
102 double* penetration_depth,
109 ccd_support_fn supp1,
111 ccd_support_fn supp2,
112 unsigned int max_iterations,
121 ccd_quat_t rot, rot_inv;
131 ccd_real_t radius, height;
136 ccd_real_t radius, height;
141 ccd_real_t radius, height;
154 template <
typename S>
166 namespace libccd_extension
169 static ccd_real_t simplexReduceToTriangle(
ccd_simplex_t *simplex,
171 ccd_vec3_t *best_witness)
179 for (i = 0; i < 3; i++){
180 newdist = ccdVec3PointTriDist2(ccd_vec3_origin,
181 &ccdSimplexPoint(simplex, (i == 0 ? 3 : 0))->v,
182 &ccdSimplexPoint(simplex, (i == 1 ? 3 : 1))->v,
183 &ccdSimplexPoint(simplex, (i == 2 ? 3 : 2))->v,
185 newdist = CCD_SQRT(newdist);
191 ccdVec3Copy(best_witness, &witness);
196 ccdSimplexSet(simplex, best, ccdSimplexPoint(simplex, 3));
198 ccdSimplexSetSize(simplex, 3);
203 _ccd_inline
void tripleCross(
const ccd_vec3_t *a,
const ccd_vec3_t *b,
204 const ccd_vec3_t *c, ccd_vec3_t *d)
207 ccdVec3Cross(&e, a, b);
208 ccdVec3Cross(d, &e, c);
211 static int doSimplex2(
ccd_simplex_t *simplex, ccd_vec3_t *dir)
214 ccd_vec3_t AB, AO, tmp;
218 A = ccdSimplexLast(simplex);
220 B = ccdSimplexPoint(simplex, 0);
222 ccdVec3Sub2(&AB, &B->
v, &A->
v);
224 ccdVec3Copy(&AO, &A->
v);
225 ccdVec3Scale(&AO, -CCD_ONE);
228 dot = ccdVec3Dot(&AB, &AO);
231 ccdVec3Cross(&tmp, &AB, &AO);
232 if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
237 if (ccdIsZero(dot) || dot < CCD_ZERO){
239 ccdSimplexSet(simplex, 0, A);
240 ccdSimplexSetSize(simplex, 1);
241 ccdVec3Copy(dir, &AO);
247 tripleCross(&AB, &AO, &AB, dir);
253 static int doSimplex3(
ccd_simplex_t *simplex, ccd_vec3_t *dir)
256 ccd_vec3_t AO, AB, AC, ABC, tmp;
257 ccd_real_t dot, dist;
260 A = ccdSimplexLast(simplex);
262 B = ccdSimplexPoint(simplex, 1);
263 C = ccdSimplexPoint(simplex, 0);
266 dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->
v, &B->
v, &C->
v,
nullptr);
267 if (ccdIsZero(dist)){
273 if (ccdVec3Eq(&A->
v, &B->
v) || ccdVec3Eq(&A->
v, &C->
v)){
278 ccdVec3Copy(&AO, &A->
v);
279 ccdVec3Scale(&AO, -CCD_ONE);
282 ccdVec3Sub2(&AB, &B->
v, &A->
v);
283 ccdVec3Sub2(&AC, &C->
v, &A->
v);
284 ccdVec3Cross(&ABC, &AB, &AC);
286 ccdVec3Cross(&tmp, &ABC, &AC);
287 dot = ccdVec3Dot(&tmp, &AO);
288 if (ccdIsZero(dot) || dot > CCD_ZERO){
289 dot = ccdVec3Dot(&AC, &AO);
290 if (ccdIsZero(dot) || dot > CCD_ZERO){
292 ccdSimplexSet(simplex, 1, A);
293 ccdSimplexSetSize(simplex, 2);
294 tripleCross(&AC, &AO, &AC, dir);
297 dot = ccdVec3Dot(&AB, &AO);
298 if (ccdIsZero(dot) || dot > CCD_ZERO){
299 ccdSimplexSet(simplex, 0, B);
300 ccdSimplexSet(simplex, 1, A);
301 ccdSimplexSetSize(simplex, 2);
302 tripleCross(&AB, &AO, &AB, dir);
304 ccdSimplexSet(simplex, 0, A);
305 ccdSimplexSetSize(simplex, 1);
306 ccdVec3Copy(dir, &AO);
310 ccdVec3Cross(&tmp, &AB, &ABC);
311 dot = ccdVec3Dot(&tmp, &AO);
312 if (ccdIsZero(dot) || dot > CCD_ZERO){
313 goto ccd_do_simplex3_45;
315 dot = ccdVec3Dot(&ABC, &AO);
316 if (ccdIsZero(dot) || dot > CCD_ZERO){
317 ccdVec3Copy(dir, &ABC);
320 ccdSupportCopy(&Ctmp, C);
321 ccdSimplexSet(simplex, 0, B);
322 ccdSimplexSet(simplex, 1, &Ctmp);
324 ccdVec3Copy(dir, &ABC);
325 ccdVec3Scale(dir, -CCD_ONE);
333 static int doSimplex4(
ccd_simplex_t *simplex, ccd_vec3_t *dir)
336 ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
337 int B_on_ACD, C_on_ADB, D_on_ABC;
338 int AB_O, AC_O, AD_O;
342 A = ccdSimplexLast(simplex);
344 B = ccdSimplexPoint(simplex, 2);
345 C = ccdSimplexPoint(simplex, 1);
346 D = ccdSimplexPoint(simplex, 0);
351 dist = ccdVec3PointTriDist2(&A->
v, &B->
v, &C->
v, &D->
v,
nullptr);
352 if (ccdIsZero(dist)){
358 dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->
v, &B->
v, &C->
v,
nullptr);
361 dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->
v, &C->
v, &D->
v,
nullptr);
364 dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->
v, &B->
v, &D->
v,
nullptr);
367 dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->
v, &C->
v, &D->
v,
nullptr);
372 ccdVec3Copy(&AO, &A->
v);
373 ccdVec3Scale(&AO, -CCD_ONE);
374 ccdVec3Sub2(&AB, &B->
v, &A->
v);
375 ccdVec3Sub2(&AC, &C->
v, &A->
v);
376 ccdVec3Sub2(&AD, &D->
v, &A->
v);
377 ccdVec3Cross(&ABC, &AB, &AC);
378 ccdVec3Cross(&ACD, &AC, &AD);
379 ccdVec3Cross(&ADB, &AD, &AB);
383 B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
384 C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
385 D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
389 AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
390 AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
391 AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
393 if (AB_O && AC_O && AD_O){
404 ccdSimplexSet(simplex, 2, A);
405 ccdSimplexSetSize(simplex, 3);
408 ccdSimplexSet(simplex, 1, D);
409 ccdSimplexSet(simplex, 0, B);
410 ccdSimplexSet(simplex, 2, A);
411 ccdSimplexSetSize(simplex, 3);
413 ccdSimplexSet(simplex, 0, C);
414 ccdSimplexSet(simplex, 1, B);
415 ccdSimplexSet(simplex, 2, A);
416 ccdSimplexSetSize(simplex, 3);
419 return doSimplex3(simplex, dir);
422 static int doSimplex(
ccd_simplex_t *simplex, ccd_vec3_t *dir)
424 if (ccdSimplexSize(simplex) == 2){
426 return doSimplex2(simplex, dir);
427 }
else if (ccdSimplexSize(simplex) == 3){
429 return doSimplex3(simplex, dir);
433 return doSimplex4(simplex, dir);
437 static int __ccdGJK(
const void *obj1,
const void *obj2,
440 unsigned long iterations;
446 ccdSimplexInit(simplex);
449 ccd->first_dir(obj1, obj2, &dir);
451 __ccdSupport(obj1, obj2, &dir, ccd, &last);
453 ccdSimplexAdd(simplex, &last);
456 ccdVec3Copy(&dir, &last.
v);
457 ccdVec3Scale(&dir, -CCD_ONE);
460 for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
462 __ccdSupport(obj1, obj2, &dir, ccd, &last);
467 if (ccdVec3Dot(&last.
v, &dir) < CCD_ZERO){
472 ccdSimplexAdd(simplex, &last);
476 do_simplex_res = doSimplex(simplex, &dir);
477 if (do_simplex_res == 1){
479 }
else if (do_simplex_res == -1){
483 if (ccdIsZero(ccdVec3Len2(&dir))){
493 static inline ccd_real_t ccdGJKDist2(
const void *obj1,
const void *obj2,
const ccd_t *ccd, ccd_vec3_t* p1, ccd_vec3_t* p2)
495 unsigned long iterations;
499 ccd_real_t dist, last_dist;
502 if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0)
505 last_dist = CCD_REAL_MAX;
507 for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
513 if (ccdSimplexSize(&simplex) == 1){
514 ccdVec3Copy(&dir, &ccdSimplexPoint(&simplex, 0)->v);
515 dist = ccdVec3Len2(&ccdSimplexPoint(&simplex, 0)->v);
516 dist = CCD_SQRT(dist);
517 }
else if (ccdSimplexSize(&simplex) == 2){
518 dist = ccdVec3PointSegmentDist2(ccd_vec3_origin,
519 &ccdSimplexPoint(&simplex, 0)->v,
520 &ccdSimplexPoint(&simplex, 1)->v,
522 dist = CCD_SQRT(dist);
523 }
else if(ccdSimplexSize(&simplex) == 3){
524 dist = ccdVec3PointTriDist2(ccd_vec3_origin,
525 &ccdSimplexPoint(&simplex, 0)->v,
526 &ccdSimplexPoint(&simplex, 1)->v,
527 &ccdSimplexPoint(&simplex, 2)->v,
529 dist = CCD_SQRT(dist);
531 dist = simplexReduceToTriangle(&simplex, last_dist, &dir);
540 if ((last_dist - dist) < ccd->dist_tolerance)
542 if(p1) *p1 = last.
v1;
543 if(p2) *p2 = last.
v2;
548 ccdVec3Scale(&dir, -CCD_ONE);
549 ccdVec3Normalize(&dir);
552 __ccdSupport(obj1, obj2, &dir, ccd, &last);
560 dist = ccdVec3Len2(&last.
v);
561 dist = CCD_SQRT(dist);
562 if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
564 if(p1) *p1 = last.
v1;
565 if(p2) *p2 = last.
v2;
570 ccdSimplexAdd(&simplex, &last);
573 return -CCD_REAL(1.);
579 template <
typename S>
582 const Quaternion<S> q(tf.linear());
583 const Vector3<S>& T = tf.translation();
584 ccdVec3Set(&o->pos, T[0], T[1], T[2]);
585 ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
586 ccdQuatInvert2(&o->rot_inv, &o->rot);
589 template <
typename S>
590 static void boxToGJK(
const Box<S>& s,
const Transform3<S>& tf,
ccd_box_t* box)
592 shapeToGJK(s, tf, box);
593 box->dim[0] = s.
side[0] / 2.0;
594 box->dim[1] = s.
side[1] / 2.0;
595 box->dim[2] = s.
side[2] / 2.0;
598 template <
typename S>
601 shapeToGJK(s, tf, cap);
603 cap->height = s.
lz / 2;
606 template <
typename S>
609 shapeToGJK(s, tf, cyl);
611 cyl->height = s.
lz / 2;
614 template <
typename S>
615 static void coneToGJK(
const Cone<S>& s,
const Transform3<S>& tf,
ccd_cone_t* cone)
617 shapeToGJK(s, tf, cone);
619 cone->height = s.
lz / 2;
622 template <
typename S>
625 shapeToGJK(s, tf, sph);
629 template <
typename S>
632 shapeToGJK(s, tf, ellipsoid);
633 ellipsoid->radii[0] = s.
radii[0];
634 ellipsoid->radii[1] = s.
radii[1];
635 ellipsoid->radii[2] = s.
radii[2];
638 template <
typename S>
641 shapeToGJK(s, tf, conv);
646 static inline void supportBox(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
650 ccdVec3Copy(&dir, dir_);
651 ccdQuatRotVec(&dir, &o->rot_inv);
652 ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
653 ccdSign(ccdVec3Y(&dir)) * o->dim[1],
654 ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
655 ccdQuatRotVec(v, &o->rot);
656 ccdVec3Add(v, &o->pos);
659 static inline void supportCap(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
662 ccd_vec3_t dir, pos1, pos2;
664 ccdVec3Copy(&dir, dir_);
665 ccdQuatRotVec(&dir, &o->rot_inv);
667 ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
668 ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
670 ccdVec3Copy(v, &dir);
671 ccdVec3Normalize (v);
672 ccdVec3Scale(v, o->radius);
673 ccdVec3Add(&pos1, v);
674 ccdVec3Add(&pos2, v);
676 if(ccdVec3Z (&dir) > 0)
677 ccdVec3Copy(v, &pos1);
679 ccdVec3Copy(v, &pos2);
682 ccdQuatRotVec(v, &o->rot);
683 ccdVec3Add(v, &o->pos);
686 static inline void supportCyl(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
692 ccdVec3Copy(&dir, dir_);
693 ccdQuatRotVec(&dir, &cyl->rot_inv);
695 zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
698 ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
701 rad = cyl->radius / zdist;
703 ccdVec3Set(v, rad * ccdVec3X(&dir),
704 rad * ccdVec3Y(&dir),
705 ccdSign(ccdVec3Z(&dir)) * cyl->height);
709 ccdQuatRotVec(v, &cyl->rot);
710 ccdVec3Add(v, &cyl->pos);
713 static inline void supportCone(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
718 ccdVec3Copy(&dir, dir_);
719 ccdQuatRotVec(&dir, &cone->rot_inv);
721 double zdist, len, rad;
722 zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
723 len = zdist + dir.v[2] * dir.v[2];
727 double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
729 if(dir.v[2] > len * sin_a)
730 ccdVec3Set(v, 0., 0., cone->height);
733 rad = cone->radius / zdist;
734 ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
737 ccdVec3Set(v, 0, 0, -cone->height);
740 ccdQuatRotVec(v, &cone->rot);
741 ccdVec3Add(v, &cone->pos);
744 static inline void supportSphere(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
749 ccdVec3Copy(&dir, dir_);
750 ccdQuatRotVec(&dir, &s->rot_inv);
752 ccdVec3Copy(v, &dir);
753 ccdVec3Scale(v, s->radius);
754 ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
757 ccdQuatRotVec(v, &s->rot);
758 ccdVec3Add(v, &s->pos);
761 static inline void supportEllipsoid(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
766 ccdVec3Copy(&dir, dir_);
767 ccdQuatRotVec(&dir, &s->rot_inv);
770 abc2.v[0] = s->radii[0] * s->radii[0];
771 abc2.v[1] = s->radii[1] * s->radii[1];
772 abc2.v[2] = s->radii[2] * s->radii[2];
774 v->v[0] = abc2.v[0] * dir.v[0];
775 v->v[1] = abc2.v[1] * dir.v[1];
776 v->v[2] = abc2.v[2] * dir.v[2];
778 ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Dot(v, &dir)));
781 ccdQuatRotVec(v, &s->rot);
782 ccdVec3Add(v, &s->pos);
785 template <
typename S>
786 static void supportConvex(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
790 ccd_real_t maxdot, dot;
793 const auto& center = c->convex->center;
795 ccdVec3Copy(&dir, dir_);
796 ccdQuatRotVec(&dir, &c->rot_inv);
798 maxdot = -CCD_REAL_MAX;
799 curp = c->convex->points;
801 for(i = 0; i < c->convex->num_points; ++i, curp += 1)
803 ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]);
804 dot = ccdVec3Dot(&dir, &p);
807 ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]);
813 ccdQuatRotVec(v, &c->rot);
814 ccdVec3Add(v, &c->pos);
817 static void supportTriangle(
const void* obj,
const ccd_vec3_t* dir_, ccd_vec3_t* v)
821 ccd_real_t maxdot, dot;
824 ccdVec3Copy(&dir, dir_);
825 ccdQuatRotVec(&dir, &tri->rot_inv);
827 maxdot = -CCD_REAL_MAX;
829 for(i = 0; i < 3; ++i)
831 ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]);
832 dot = ccdVec3Dot(&dir, &p);
835 ccdVec3Copy(v, &tri->p[i]);
841 ccdQuatRotVec(v, &tri->rot);
842 ccdVec3Add(v, &tri->pos);
845 static inline void centerShape(
const void* obj, ccd_vec3_t* c)
848 ccdVec3Copy(c, &o->pos);
851 template <
typename S>
852 static void centerConvex(
const void* obj, ccd_vec3_t* c)
855 ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]);
856 ccdQuatRotVec(c, &o->rot);
857 ccdVec3Add(c, &o->pos);
860 static void centerTriangle(
const void* obj, ccd_vec3_t* c)
863 ccdVec3Copy(c, &o->c);
864 ccdQuatRotVec(c, &o->rot);
865 ccdVec3Add(c, &o->pos);
868 template <
typename S>
869 bool GJKCollide(
void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
870 void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
871 unsigned int max_iterations, S tolerance,
872 Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal)
881 ccd.support1 = supp1;
882 ccd.support2 = supp2;
885 ccd.max_iterations = max_iterations;
886 ccd.mpr_tolerance = tolerance;
890 return ccdMPRIntersect(obj1, obj2, &ccd);
895 res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
898 *contact_points << ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos);
899 *penetration_depth = depth;
900 *normal << ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir);
910 template <
typename S>
911 bool GJKDistance(
void* obj1, ccd_support_fn supp1,
912 void* obj2, ccd_support_fn supp2,
913 unsigned int max_iterations, S tolerance,
914 S* res, Vector3<S>* p1, Vector3<S>* p2)
919 ccd.support1 = supp1;
920 ccd.support2 = supp2;
922 ccd.max_iterations = max_iterations;
923 ccd.dist_tolerance = tolerance;
931 ccdVec3Set(&p1_, 0.0, 0.0, 0.0);
932 ccdVec3Set(&p2_, 0.0, 0.0, 0.0);
933 dist = libccd_extension::ccdGJKDist2(obj1, obj2, &ccd, &p1_, &p2_);
934 if(p1) *p1 << ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_);
935 if(p2) *p2 << ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_);
937 if(dist < 0)
return false;
941 template <
typename S>
947 template <
typename S>
953 template <
typename S>
961 template <
typename S>
968 template <
typename S>
971 return &supportSphere;
974 template <
typename S>
980 template <
typename S>
984 sphereToGJK(s, tf, o);
988 template <
typename S>
995 template <
typename S>
998 return &supportEllipsoid;
1001 template <
typename S>
1004 return ¢erShape;
1007 template <
typename S>
1011 ellipsoidToGJK(s, tf, o);
1015 template <
typename S>
1022 template <
typename S>
1028 template <
typename S>
1031 return ¢erShape;
1034 template <
typename S>
1042 template <
typename S>
1049 template <
typename S>
1055 template <
typename S>
1058 return ¢erShape;
1061 template <
typename S>
1069 template <
typename S>
1076 template <
typename S>
1079 return &supportCone;
1082 template <
typename S>
1085 return ¢erShape;
1088 template <
typename S>
1092 coneToGJK(s, tf, o);
1096 template <
typename S>
1103 template <
typename S>
1106 return &supportConvex<S>;
1109 template <
typename S>
1112 return ¢erConvex<S>;
1115 template <
typename S>
1119 convexToGJK(s, tf, o);
1123 template <
typename S>
1130 inline GJKSupportFunction triGetSupportFunction()
1132 return &supportTriangle;
1135 inline GJKCenterFunction triGetCenterFunction()
1137 return ¢erTriangle;
1140 template <
typename S>
1141 void* triCreateGJKObject(
const Vector3<S>& P1,
const Vector3<S>& P2,
const Vector3<S>& P3)
1144 Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
1146 ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
1147 ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
1148 ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
1149 ccdVec3Set(&o->c, center[0], center[1], center[2]);
1150 ccdVec3Set(&o->pos, 0., 0., 0.);
1151 ccdQuatSet(&o->rot, 0., 0., 0., 1.);
1152 ccdQuatInvert2(&o->rot_inv, &o->rot);
1157 template <
typename S>
1158 void* triCreateGJKObject(
const Vector3<S>& P1,
const Vector3<S>& P2,
const Vector3<S>& P3,
const Transform3<S>& tf)
1161 Vector3<S> center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
1163 ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
1164 ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
1165 ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
1166 ccdVec3Set(&o->c, center[0], center[1], center[2]);
1167 const Quaternion<S> q(tf.linear());
1168 const Vector3<S>& T = tf.translation();
1169 ccdVec3Set(&o->pos, T[0], T[1], T[2]);
1170 ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
1171 ccdQuatInvert2(&o->rot_inv, &o->rot);
1176 inline void triDeleteGJKObject(
void* o_)
Definition: gjk_libccd-inl.h:144
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
Definition: gjk_libccd-inl.h:160
S lz
Length along z axis.
Definition: capsule.h:61
S radius
Radius of the cylinder.
Definition: cylinder.h:58
Definition: gjk_libccd-inl.h:124
Center at zero point ellipsoid.
Definition: ellipsoid.h:48
S lz
Length along z axis.
Definition: cylinder.h:61
initialize GJK stuffs
Definition: gjk_libccd.h:70
Definition: gjk_libccd-inl.h:118
Center at zero point capsule.
Definition: capsule.h:48
ccd_vec3_t v
Support point in minkowski sum.
Definition: support.h:28
S lz
Length along z axis.
Definition: cone.h:60
Center at zero point, axis aligned box.
Definition: box.h:48
S radius
Radius of the cone.
Definition: cone.h:57
Definition: gjk_libccd-inl.h:139
Vector3< S > side
box side length
Definition: box.h:64
Definition: gjk_libccd-inl.h:155
Definition: gjk_libccd-inl.h:129
ccd_vec3_t v1
Support point in obj1.
Definition: support.h:29
Center at zero cylinder.
Definition: cylinder.h:48
Vector3< S > radii
Radii of the ellipsoid.
Definition: ellipsoid.h:61
Center at zero cone.
Definition: cone.h:48
ccd_vec3_t v2
Support point in obj2.
Definition: support.h:30
Definition: gjk_libccd-inl.h:134
S radius
Radius of the sphere.
Definition: sphere.h:57
Center at zero point sphere.
Definition: sphere.h:48
Definition: gjk_libccd-inl.h:149
S radius
Radius of capsule.
Definition: capsule.h:58