summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/libccd/src/ccd.c
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/libccd/src/ccd.c')
-rw-r--r--libs/ode-0.16.1/libccd/src/ccd.c955
1 files changed, 955 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/libccd/src/ccd.c b/libs/ode-0.16.1/libccd/src/ccd.c
new file mode 100644
index 0000000..5221b8f
--- /dev/null
+++ b/libs/ode-0.16.1/libccd/src/ccd.c
@@ -0,0 +1,955 @@
+/***
+ * libccd
+ * ---------------------------------
+ * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
+ *
+ *
+ * This file is part of libccd.
+ *
+ * Distributed under the OSI-approved BSD License (the "License");
+ * see accompanying file BDS-LICENSE for details or see
+ * <http://www.opensource.org/licenses/bsd-license.php>.
+ *
+ * This software is distributed WITHOUT ANY WARRANTY; without even the
+ * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the License for more information.
+ */
+
+#include <stdio.h>
+#include <float.h>
+#include <ccd/ccd.h>
+#include <ccdcustom/vec3.h>
+#include <ccd/simplex.h>
+#include <ccd/polytope.h>
+#include <ccd/alloc.h>
+#include <ccd/dbg.h>
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+
+/** Performs GJK algorithm. Returns 0 if intersection was found and simplex
+ * is filled with resulting polytope. */
+static int __ccdGJK(const void *obj1, const void *obj2,
+ const ccd_t *ccd, ccd_simplex_t *simplex);
+
+/** Performs GJK+EPA algorithm. Returns 0 if intersection was found and
+ * pt is filled with resulting polytope and nearest with pointer to
+ * nearest element (vertex, edge, face) of polytope to origin. */
+static int __ccdGJKEPA(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ ccd_pt_t *pt, ccd_pt_el_t **nearest);
+
+
+/** Returns true if simplex contains origin.
+ * This function also alteres simplex and dir according to further
+ * processing of GJK algorithm. */
+static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir);
+static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir);
+static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir);
+static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir);
+
+/** d = a x b x c */
+_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
+ const ccd_vec3_t *c, ccd_vec3_t *d);
+
+
+/** Transforms simplex to polytope. It is assumed that simplex has 4
+ * vertices. */
+static int simplexToPolytope4(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ ccd_simplex_t *simplex,
+ ccd_pt_t *pt, ccd_pt_el_t **nearest);
+
+/** Transforms simplex to polytope, three vertices required */
+static int simplexToPolytope3(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ const ccd_simplex_t *simplex,
+ ccd_pt_t *pt, ccd_pt_el_t **nearest);
+
+/** Transforms simplex to polytope, two vertices required */
+static int simplexToPolytope2(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ const ccd_simplex_t *simplex,
+ ccd_pt_t *pt, ccd_pt_el_t **nearest);
+
+/** Expands polytope using new vertex v. */
+static void expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el,
+ const ccd_support_t *newv);
+
+/** Finds next support point (at stores it in out argument).
+ * Returns 0 on success, -1 otherwise */
+static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd,
+ const ccd_pt_el_t *el,
+ ccd_support_t *out);
+
+
+
+void ccdFirstDirDefault(const void *o1, const void *o2, ccd_vec3_t *dir)
+{
+ ccdVec3Set(dir, CCD_ONE, CCD_ZERO, CCD_ZERO);
+}
+
+int ccdGJKIntersect(const void *obj1, const void *obj2, const ccd_t *ccd)
+{
+ ccd_simplex_t simplex;
+ return __ccdGJK(obj1, obj2, ccd, &simplex) == 0;
+}
+
+int ccdGJKSeparate(const void *obj1, const void *obj2, const ccd_t *ccd,
+ ccd_vec3_t *sep)
+{
+ ccd_pt_t polytope;
+ ccd_pt_el_t *nearest;
+ int ret;
+
+ ccdPtInit(&polytope);
+
+ ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest);
+
+ // set separation vector
+ if (nearest)
+ ccdVec3Copy(sep, &nearest->witness);
+
+ ccdPtDestroy(&polytope);
+
+ return ret;
+}
+
+
+static int penEPAPosCmp(const void *a, const void *b)
+{
+ ccd_pt_vertex_t *v1, *v2;
+ v1 = *(ccd_pt_vertex_t **)a;
+ v2 = *(ccd_pt_vertex_t **)b;
+
+ if (ccdEq(v1->dist, v2->dist)){
+ return 0;
+ }else if (v1->dist < v2->dist){
+ return -1;
+ }else{
+ return 1;
+ }
+}
+
+static void penEPAPos(const ccd_pt_t *pt, const ccd_pt_el_t *nearest,
+ ccd_vec3_t *pos)
+{
+ ccd_pt_vertex_t *v;
+ ccd_pt_vertex_t **vs;
+ size_t i, len;
+ ccd_real_t scale;
+
+ // compute median
+ len = 0;
+ ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
+ len++;
+ }
+
+ vs = CCD_ALLOC_ARR(ccd_pt_vertex_t *, len);
+ i = 0;
+ ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
+ vs[i++] = v;
+ }
+
+ qsort(vs, len, sizeof(ccd_pt_vertex_t *), penEPAPosCmp);
+
+ ccdVec3Set(pos, CCD_ZERO, CCD_ZERO, CCD_ZERO);
+ scale = CCD_ZERO;
+ if (len % 2 == 1)
+ len++;
+
+ for (i = 0; i < len / 2; i++){
+ ccdVec3Add(pos, &vs[i]->v.v1);
+ ccdVec3Add(pos, &vs[i]->v.v2);
+ scale += CCD_REAL(2.);
+ }
+ ccdVec3Scale(pos, CCD_ONE / scale);
+
+ free(vs);
+}
+
+int ccdGJKPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
+ ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
+{
+ ccd_pt_t polytope;
+ ccd_pt_el_t *nearest;
+ int ret;
+
+ ccdPtInit(&polytope);
+
+ ret = __ccdGJKEPA(obj1, obj2, ccd, &polytope, &nearest);
+
+ // set separation vector
+ if (ret == 0 && nearest){
+ // store normalized direction vector
+ ccdVec3Copy(dir, &nearest->witness);
+ ret = ccdVec3SafeNormalize(dir);
+
+ if (ret == 0) {
+ // compute depth of penetration
+ *depth = CCD_SQRT(nearest->dist);
+ // compute position
+ penEPAPos(&polytope, nearest, pos);
+ }
+ }
+
+ ccdPtDestroy(&polytope);
+
+ return ret;
+}
+
+
+
+
+static int __ccdGJK(const void *obj1, const void *obj2,
+ const ccd_t *ccd, ccd_simplex_t *simplex)
+{
+ unsigned long iterations;
+ ccd_vec3_t dir; // direction vector
+ ccd_support_t last; // last support point
+ int do_simplex_res;
+
+ // initialize simplex struct
+ ccdSimplexInit(simplex);
+
+ // get first direction
+ ccd->first_dir(obj1, obj2, &dir);
+ // get first support point
+ __ccdSupport(obj1, obj2, &dir, ccd, &last);
+ // and add this point to simplex as last one
+ ccdSimplexAdd(simplex, &last);
+
+ // set up direction vector to as (O - last) which is exactly -last
+ ccdVec3Copy(&dir, &last.v);
+ ccdVec3Scale(&dir, -CCD_ONE);
+
+ // start iterations
+ for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
+ // obtain support point
+ __ccdSupport(obj1, obj2, &dir, ccd, &last);
+
+ // check if farthest point in Minkowski difference in direction dir
+ // isn't somewhere before origin (the test on negative dot product)
+ // - because if it is, objects are not intersecting at all.
+ if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){
+ return -1; // intersection not found
+ }
+
+ // add last support vector to simplex
+ ccdSimplexAdd(simplex, &last);
+
+ // if doSimplex returns 1 if objects intersect, -1 if objects don't
+ // intersect and 0 if algorithm should continue
+ do_simplex_res = doSimplex(simplex, &dir);
+ if (do_simplex_res == 1){
+ return 0; // intersection found
+ }else if (do_simplex_res == -1){
+ return -1; // intersection not found
+ }
+
+ if (ccdIsZero(ccdVec3Len2(&dir))){
+ return -1; // intersection not found
+ }
+ }
+
+ // intersection wasn't found
+ return -1;
+}
+
+static int __ccdGJKEPA(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ ccd_pt_t *polytope, ccd_pt_el_t **nearest)
+{
+ ccd_simplex_t simplex;
+ ccd_support_t supp; // support point
+ int ret, size;
+
+ *nearest = NULL;
+
+ // run GJK and obtain terminal simplex
+ ret = __ccdGJK(obj1, obj2, ccd, &simplex);
+ if (ret != 0)
+ return -1;
+
+ // transform simplex to polytope - simplex won't be used anymore
+ size = ccdSimplexSize(&simplex);
+ if (size == 4){
+ if (simplexToPolytope4(obj1, obj2, ccd, &simplex, polytope, nearest) != 0){
+ return 0;// touch contact
+ }
+ }else if (size == 3){
+ if (simplexToPolytope3(obj1, obj2, ccd, &simplex, polytope, nearest) != 0){
+ return 0; // touch contact
+ }
+ }else{ // size == 2
+ if (simplexToPolytope2(obj1, obj2, ccd, &simplex, polytope, nearest) != 0){
+ return 0; // touch contact
+ }
+ }
+
+ while (1){
+ // get triangle nearest to origin
+ *nearest = ccdPtNearest(polytope);
+
+ // get next support point
+ if (nextSupport(obj1, obj2, ccd, *nearest, &supp) != 0)
+ break;
+
+ // expand nearest triangle using new point - supp
+ expandPolytope(polytope, *nearest, &supp);
+ }
+
+ return 0;
+}
+
+
+
+static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
+{
+ const ccd_support_t *A, *B;
+ ccd_vec3_t AB, AO, tmp;
+ ccd_real_t dot;
+
+ // get last added as A
+ A = ccdSimplexLast(simplex);
+ // get the other point
+ B = ccdSimplexPoint(simplex, 0);
+ // compute AB oriented segment
+ ccdVec3Sub2(&AB, &B->v, &A->v);
+ // compute AO vector
+ ccdVec3Copy(&AO, &A->v);
+ ccdVec3Scale(&AO, -CCD_ONE);
+
+ // dot product AB . AO
+ dot = ccdVec3Dot(&AB, &AO);
+
+ // check if origin doesn't lie on AB segment
+ ccdVec3Cross(&tmp, &AB, &AO);
+ if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
+ return 1;
+ }
+
+ // check if origin is in area where AB segment is
+ if (ccdIsZero(dot) || dot < CCD_ZERO){
+ // origin is in outside are of A
+ ccdSimplexSet(simplex, 0, A);
+ ccdSimplexSetSize(simplex, 1);
+ ccdVec3Copy(dir, &AO);
+ }else{
+ // origin is in area where AB segment is
+
+ // keep simplex untouched and set direction to
+ // AB x AO x AB
+ tripleCross(&AB, &AO, &AB, dir);
+ }
+
+ return 0;
+}
+
+static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
+{
+ const ccd_support_t *A, *B, *C;
+ ccd_vec3_t AO, AB, AC, ABC, tmp;
+ ccd_real_t dot, dist;
+
+ // get last added as A
+ A = ccdSimplexLast(simplex);
+ // get the other points
+ B = ccdSimplexPoint(simplex, 1);
+ C = ccdSimplexPoint(simplex, 0);
+
+ // check touching contact
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
+ if (ccdIsZero(dist)){
+ return 1;
+ }
+
+ // check if triangle is really triangle (has area > 0)
+ // if not simplex can't be expanded and thus no itersection is found
+ if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){
+ return -1;
+ }
+
+ // compute AO vector
+ ccdVec3Copy(&AO, &A->v);
+ ccdVec3Scale(&AO, -CCD_ONE);
+
+ // compute AB and AC segments and ABC vector (perpendircular to triangle)
+ ccdVec3Sub2(&AB, &B->v, &A->v);
+ ccdVec3Sub2(&AC, &C->v, &A->v);
+ ccdVec3Cross(&ABC, &AB, &AC);
+
+ ccdVec3Cross(&tmp, &ABC, &AC);
+ dot = ccdVec3Dot(&tmp, &AO);
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
+ dot = ccdVec3Dot(&AC, &AO);
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
+ // C is already in place
+ ccdSimplexSet(simplex, 1, A);
+ ccdSimplexSetSize(simplex, 2);
+ tripleCross(&AC, &AO, &AC, dir);
+ }else{
+ccd_do_simplex3_45:
+ dot = ccdVec3Dot(&AB, &AO);
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
+ ccdSimplexSet(simplex, 0, B);
+ ccdSimplexSet(simplex, 1, A);
+ ccdSimplexSetSize(simplex, 2);
+ tripleCross(&AB, &AO, &AB, dir);
+ }else{
+ ccdSimplexSet(simplex, 0, A);
+ ccdSimplexSetSize(simplex, 1);
+ ccdVec3Copy(dir, &AO);
+ }
+ }
+ }else{
+ ccdVec3Cross(&tmp, &AB, &ABC);
+ dot = ccdVec3Dot(&tmp, &AO);
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
+ goto ccd_do_simplex3_45;
+ }else{
+ dot = ccdVec3Dot(&ABC, &AO);
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
+ ccdVec3Copy(dir, &ABC);
+ }else{
+ ccd_support_t Ctmp;
+ ccdSupportCopy(&Ctmp, C);
+ ccdSimplexSet(simplex, 0, B);
+ ccdSimplexSet(simplex, 1, &Ctmp);
+
+ ccdVec3Copy(dir, &ABC);
+ ccdVec3Scale(dir, -CCD_ONE);
+ }
+ }
+ }
+
+ return 0;
+}
+
+static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
+{
+ const ccd_support_t *A, *B, *C, *D;
+ ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
+ int B_on_ACD, C_on_ADB, D_on_ABC;
+ int AB_O, AC_O, AD_O;
+ ccd_real_t dist;
+
+ // get last added as A
+ A = ccdSimplexLast(simplex);
+ // get the other points
+ B = ccdSimplexPoint(simplex, 2);
+ C = ccdSimplexPoint(simplex, 1);
+ D = ccdSimplexPoint(simplex, 0);
+
+ // check if tetrahedron is really tetrahedron (has volume > 0)
+ // if it is not simplex can't be expanded and thus no intersection is
+ // found
+ dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
+ if (ccdIsZero(dist)){
+ return -1;
+ }
+
+ // check if origin lies on some of tetrahedron's face - if so objects
+ // intersect
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
+ if (ccdIsZero(dist))
+ return 1;
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
+ if (ccdIsZero(dist))
+ return 1;
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
+ if (ccdIsZero(dist))
+ return 1;
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
+ if (ccdIsZero(dist))
+ return 1;
+
+ // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
+ ccdVec3Copy(&AO, &A->v);
+ ccdVec3Scale(&AO, -CCD_ONE);
+ ccdVec3Sub2(&AB, &B->v, &A->v);
+ ccdVec3Sub2(&AC, &C->v, &A->v);
+ ccdVec3Sub2(&AD, &D->v, &A->v);
+ ccdVec3Cross(&ABC, &AB, &AC);
+ ccdVec3Cross(&ACD, &AC, &AD);
+ ccdVec3Cross(&ADB, &AD, &AB);
+
+ // side (positive or negative) of B, C, D relative to planes ACD, ADB
+ // and ABC respectively
+ B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
+ C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
+ D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
+
+ // whether origin is on same side of ACD, ADB, ABC as B, C, D
+ // respectively
+ AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
+ AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
+ AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
+
+ if (AB_O && AC_O && AD_O){
+ // origin is in tetrahedron
+ return 1;
+
+ // rearrange simplex to triangle and call doSimplex3()
+ }else if (!AB_O){
+ // B is farthest from the origin among all of the tetrahedron's
+ // points, so remove it from the list and go on with the triangle
+ // case
+
+ // D and C are in place
+ ccdSimplexSet(simplex, 2, A);
+ ccdSimplexSetSize(simplex, 3);
+ }else if (!AC_O){
+ // C is farthest
+ ccdSimplexSet(simplex, 1, D);
+ ccdSimplexSet(simplex, 0, B);
+ ccdSimplexSet(simplex, 2, A);
+ ccdSimplexSetSize(simplex, 3);
+ }else{ // (!AD_O)
+ ccdSimplexSet(simplex, 0, C);
+ ccdSimplexSet(simplex, 1, B);
+ ccdSimplexSet(simplex, 2, A);
+ ccdSimplexSetSize(simplex, 3);
+ }
+
+ return doSimplex3(simplex, dir);
+}
+
+static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir)
+{
+ if (ccdSimplexSize(simplex) == 2){
+ // simplex contains segment only one segment
+ return doSimplex2(simplex, dir);
+ }else if (ccdSimplexSize(simplex) == 3){
+ // simplex contains triangle
+ return doSimplex3(simplex, dir);
+ }else{ // ccdSimplexSize(simplex) == 4
+ // tetrahedron - this is the only shape which can encapsule origin
+ // so doSimplex4() also contains test on it
+ return doSimplex4(simplex, dir);
+ }
+}
+
+_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
+ const ccd_vec3_t *c, ccd_vec3_t *d)
+{
+ ccd_vec3_t e;
+ ccdVec3Cross(&e, a, b);
+ ccdVec3Cross(d, &e, c);
+}
+
+
+
+/** Transforms simplex to polytope. It is assumed that simplex has 4
+ * vertices! */
+static int simplexToPolytope4(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ ccd_simplex_t *simplex,
+ ccd_pt_t *pt, ccd_pt_el_t **nearest)
+{
+ const ccd_support_t *a, *b, *c, *d;
+ int use_polytope3;
+ ccd_real_t dist;
+ ccd_pt_vertex_t *v[4];
+ ccd_pt_edge_t *e[6];
+ size_t i;
+
+ a = ccdSimplexPoint(simplex, 0);
+ b = ccdSimplexPoint(simplex, 1);
+ c = ccdSimplexPoint(simplex, 2);
+ d = ccdSimplexPoint(simplex, 3);
+
+ // check if origin lies on some of tetrahedron's face - if so use
+ // simplexToPolytope3()
+ use_polytope3 = 0;
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &c->v, NULL);
+ if (ccdIsZero(dist)){
+ use_polytope3 = 1;
+ }
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL);
+ if (ccdIsZero(dist)){
+ use_polytope3 = 1;
+ ccdSimplexSet(simplex, 1, c);
+ ccdSimplexSet(simplex, 2, d);
+ }
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &b->v, &d->v, NULL);
+ if (ccdIsZero(dist)){
+ use_polytope3 = 1;
+ ccdSimplexSet(simplex, 2, d);
+ }
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &b->v, &c->v, &d->v, NULL);
+ if (ccdIsZero(dist)){
+ use_polytope3 = 1;
+ ccdSimplexSet(simplex, 0, b);
+ ccdSimplexSet(simplex, 1, c);
+ ccdSimplexSet(simplex, 2, d);
+ }
+
+ if (use_polytope3){
+ ccdSimplexSetSize(simplex, 3);
+ return simplexToPolytope3(obj1, obj2, ccd, simplex, pt, nearest);
+ }
+
+ // no touching contact - simply create tetrahedron
+ for (i = 0; i < 4; i++){
+ v[i] = ccdPtAddVertex(pt, ccdSimplexPoint(simplex, i));
+ }
+
+ e[0] = ccdPtAddEdge(pt, v[0], v[1]);
+ e[1] = ccdPtAddEdge(pt, v[1], v[2]);
+ e[2] = ccdPtAddEdge(pt, v[2], v[0]);
+ e[3] = ccdPtAddEdge(pt, v[3], v[0]);
+ e[4] = ccdPtAddEdge(pt, v[3], v[1]);
+ e[5] = ccdPtAddEdge(pt, v[3], v[2]);
+
+ ccdPtAddFace(pt, e[0], e[1], e[2]);
+ ccdPtAddFace(pt, e[3], e[4], e[0]);
+ ccdPtAddFace(pt, e[4], e[5], e[1]);
+ ccdPtAddFace(pt, e[5], e[3], e[2]);
+
+ return 0;
+}
+
+/** Transforms simplex to polytope, three vertices required */
+static int simplexToPolytope3(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ const ccd_simplex_t *simplex,
+ ccd_pt_t *pt, ccd_pt_el_t **nearest)
+{
+ const ccd_support_t *a, *b, *c;
+ ccd_support_t d, d2;
+ ccd_vec3_t ab, ac, dir;
+ ccd_pt_vertex_t *v[5];
+ ccd_pt_edge_t *e[9];
+ ccd_real_t dist, dist2;
+
+ *nearest = NULL;
+
+ a = ccdSimplexPoint(simplex, 0);
+ b = ccdSimplexPoint(simplex, 1);
+ c = ccdSimplexPoint(simplex, 2);
+
+ // If only one triangle left from previous GJK run origin lies on this
+ // triangle. So it is necessary to expand triangle into two
+ // tetrahedrons connected with base (which is exactly abc triangle).
+
+ // get next support point in direction of normal of triangle
+ ccdVec3Sub2(&ab, &b->v, &a->v);
+ ccdVec3Sub2(&ac, &c->v, &a->v);
+ ccdVec3Cross(&dir, &ab, &ac);
+ __ccdSupport(obj1, obj2, &dir, ccd, &d);
+ dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v, NULL);
+
+ // and second one take in opposite direction
+ ccdVec3Scale(&dir, -CCD_ONE);
+ __ccdSupport(obj1, obj2, &dir, ccd, &d2);
+ dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v, NULL);
+
+ // check if face isn't already on edge of minkowski sum and thus we
+ // have touching contact
+ if (ccdIsZero(dist) || ccdIsZero(dist2)){
+ v[0] = ccdPtAddVertex(pt, a);
+ v[1] = ccdPtAddVertex(pt, b);
+ v[2] = ccdPtAddVertex(pt, c);
+ e[0] = ccdPtAddEdge(pt, v[0], v[1]);
+ e[1] = ccdPtAddEdge(pt, v[1], v[2]);
+ e[2] = ccdPtAddEdge(pt, v[2], v[0]);
+ *nearest = (ccd_pt_el_t *)ccdPtAddFace(pt, e[0], e[1], e[2]);
+
+ return -1;
+ }
+
+ // form polyhedron
+ v[0] = ccdPtAddVertex(pt, a);
+ v[1] = ccdPtAddVertex(pt, b);
+ v[2] = ccdPtAddVertex(pt, c);
+ v[3] = ccdPtAddVertex(pt, &d);
+ v[4] = ccdPtAddVertex(pt, &d2);
+
+ e[0] = ccdPtAddEdge(pt, v[0], v[1]);
+ e[1] = ccdPtAddEdge(pt, v[1], v[2]);
+ e[2] = ccdPtAddEdge(pt, v[2], v[0]);
+
+ e[3] = ccdPtAddEdge(pt, v[3], v[0]);
+ e[4] = ccdPtAddEdge(pt, v[3], v[1]);
+ e[5] = ccdPtAddEdge(pt, v[3], v[2]);
+
+ e[6] = ccdPtAddEdge(pt, v[4], v[0]);
+ e[7] = ccdPtAddEdge(pt, v[4], v[1]);
+ e[8] = ccdPtAddEdge(pt, v[4], v[2]);
+
+ ccdPtAddFace(pt, e[3], e[4], e[0]);
+ ccdPtAddFace(pt, e[4], e[5], e[1]);
+ ccdPtAddFace(pt, e[5], e[3], e[2]);
+
+ ccdPtAddFace(pt, e[6], e[7], e[0]);
+ ccdPtAddFace(pt, e[7], e[8], e[1]);
+ ccdPtAddFace(pt, e[8], e[6], e[2]);
+
+ return 0;
+}
+
+/** Transforms simplex to polytope, two vertices required */
+static int simplexToPolytope2(const void *obj1, const void *obj2,
+ const ccd_t *ccd,
+ const ccd_simplex_t *simplex,
+ ccd_pt_t *pt, ccd_pt_el_t **nearest)
+{
+ const ccd_support_t *a, *b;
+ ccd_vec3_t ab, ac, dir;
+ ccd_support_t supp[4];
+ ccd_pt_vertex_t *v[6];
+ ccd_pt_edge_t *e[12];
+ size_t i;
+ int found;
+
+ a = ccdSimplexPoint(simplex, 0);
+ b = ccdSimplexPoint(simplex, 1);
+
+ // This situation is a bit tricky. If only one segment comes from
+ // previous run of GJK - it means that either this segment is on
+ // minkowski edge (and thus we have touch contact) or it it isn't and
+ // therefore segment is somewhere *inside* minkowski sum and it *must*
+ // be possible to fully enclose this segment with polyhedron formed by
+ // at least 8 triangle faces.
+
+ // get first support point (any)
+ found = 0;
+ for (i = 0; i < ccd_points_on_sphere_len; i++){
+ __ccdSupport(obj1, obj2, &ccd_points_on_sphere[i], ccd, &supp[0]);
+ if (!ccdVec3Eq(&a->v, &supp[0].v) && !ccdVec3Eq(&b->v, &supp[0].v)){
+ found = 1;
+ break;
+ }
+ }
+ if (!found)
+ goto simplexToPolytope2_touching_contact;
+
+ // get second support point in opposite direction than supp[0]
+ ccdVec3Copy(&dir, &supp[0].v);
+ ccdVec3Scale(&dir, -CCD_ONE);
+ __ccdSupport(obj1, obj2, &dir, ccd, &supp[1]);
+ if (ccdVec3Eq(&a->v, &supp[1].v) || ccdVec3Eq(&b->v, &supp[1].v))
+ goto simplexToPolytope2_touching_contact;
+
+ // next will be in direction of normal of triangle a,supp[0],supp[1]
+ ccdVec3Sub2(&ab, &supp[0].v, &a->v);
+ ccdVec3Sub2(&ac, &supp[1].v, &a->v);
+ ccdVec3Cross(&dir, &ab, &ac);
+ __ccdSupport(obj1, obj2, &dir, ccd, &supp[2]);
+ if (ccdVec3Eq(&a->v, &supp[2].v) || ccdVec3Eq(&b->v, &supp[2].v))
+ goto simplexToPolytope2_touching_contact;
+
+ // and last one will be in opposite direction
+ ccdVec3Scale(&dir, -CCD_ONE);
+ __ccdSupport(obj1, obj2, &dir, ccd, &supp[3]);
+ if (ccdVec3Eq(&a->v, &supp[3].v) || ccdVec3Eq(&b->v, &supp[3].v))
+ goto simplexToPolytope2_touching_contact;
+
+ goto simplexToPolytope2_not_touching_contact;
+simplexToPolytope2_touching_contact:
+ v[0] = ccdPtAddVertex(pt, a);
+ v[1] = ccdPtAddVertex(pt, b);
+ *nearest = (ccd_pt_el_t *)ccdPtAddEdge(pt, v[0], v[1]);
+ return -1;
+
+simplexToPolytope2_not_touching_contact:
+ // form polyhedron
+ v[0] = ccdPtAddVertex(pt, a);
+ v[1] = ccdPtAddVertex(pt, &supp[0]);
+ v[2] = ccdPtAddVertex(pt, b);
+ v[3] = ccdPtAddVertex(pt, &supp[1]);
+ v[4] = ccdPtAddVertex(pt, &supp[2]);
+ v[5] = ccdPtAddVertex(pt, &supp[3]);
+
+ e[0] = ccdPtAddEdge(pt, v[0], v[1]);
+ e[1] = ccdPtAddEdge(pt, v[1], v[2]);
+ e[2] = ccdPtAddEdge(pt, v[2], v[3]);
+ e[3] = ccdPtAddEdge(pt, v[3], v[0]);
+
+ e[4] = ccdPtAddEdge(pt, v[4], v[0]);
+ e[5] = ccdPtAddEdge(pt, v[4], v[1]);
+ e[6] = ccdPtAddEdge(pt, v[4], v[2]);
+ e[7] = ccdPtAddEdge(pt, v[4], v[3]);
+
+ e[8] = ccdPtAddEdge(pt, v[5], v[0]);
+ e[9] = ccdPtAddEdge(pt, v[5], v[1]);
+ e[10] = ccdPtAddEdge(pt, v[5], v[2]);
+ e[11] = ccdPtAddEdge(pt, v[5], v[3]);
+
+ ccdPtAddFace(pt, e[4], e[5], e[0]);
+ ccdPtAddFace(pt, e[5], e[6], e[1]);
+ ccdPtAddFace(pt, e[6], e[7], e[2]);
+ ccdPtAddFace(pt, e[7], e[4], e[3]);
+
+ ccdPtAddFace(pt, e[8], e[9], e[0]);
+ ccdPtAddFace(pt, e[9], e[10], e[1]);
+ ccdPtAddFace(pt, e[10], e[11], e[2]);
+ ccdPtAddFace(pt, e[11], e[8], e[3]);
+
+ return 0;
+}
+
+/** Expands polytope's tri by new vertex v. Triangle tri is replaced by
+ * three triangles each with one vertex in v. */
+static void expandPolytope(ccd_pt_t *pt, ccd_pt_el_t *el,
+ const ccd_support_t *newv)
+{
+ ccd_pt_vertex_t *v[5];
+ ccd_pt_edge_t *e[8] = {0};
+ ccd_pt_face_t *f[2];
+
+
+ // element can be either segment or triangle
+ if (el->type == CCD_PT_EDGE){
+ // In this case, segment should be replaced by new point.
+ // Simpliest case is when segment stands alone and in this case
+ // this segment is replaced by two other segments both connected to
+ // newv.
+ // Segment can be also connected to max two faces and in that case
+ // each face must be replaced by two other faces. To do this
+ // correctly it is necessary to have correctly ordered edges and
+ // vertices which is exactly what is done in following code.
+ //
+
+ ccdPtEdgeVertices((const ccd_pt_edge_t *)el, &v[0], &v[2]);
+
+ ccdPtEdgeFaces((ccd_pt_edge_t *)el, &f[0], &f[1]);
+
+ if (f[0]){
+ ccdPtFaceEdges(f[0], &e[0], &e[1], &e[2]);
+ if (e[0] == (ccd_pt_edge_t *)el){
+ e[0] = e[2];
+ }else if (e[1] == (ccd_pt_edge_t *)el){
+ e[1] = e[2];
+ }
+ ccdPtEdgeVertices(e[0], &v[1], &v[3]);
+ if (v[1] != v[0] && v[3] != v[0]){
+ e[2] = e[0];
+ e[0] = e[1];
+ e[1] = e[2];
+ if (v[1] == v[2])
+ v[1] = v[3];
+ }else{
+ if (v[1] == v[0])
+ v[1] = v[3];
+ }
+
+ if (f[1]){
+ ccdPtFaceEdges(f[1], &e[2], &e[3], &e[4]);
+ if (e[2] == (ccd_pt_edge_t *)el){
+ e[2] = e[4];
+ }else if (e[3] == (ccd_pt_edge_t *)el){
+ e[3] = e[4];
+ }
+ ccdPtEdgeVertices(e[2], &v[3], &v[4]);
+ if (v[3] != v[2] && v[4] != v[2]){
+ e[4] = e[2];
+ e[2] = e[3];
+ e[3] = e[4];
+ if (v[3] == v[0])
+ v[3] = v[4];
+ }else{
+ if (v[3] == v[2])
+ v[3] = v[4];
+ }
+ }
+
+
+ v[4] = ccdPtAddVertex(pt, newv);
+
+ ccdPtDelFace(pt, f[0]);
+ if (f[1]){
+ ccdPtDelFace(pt, f[1]);
+ ccdPtDelEdge(pt, (ccd_pt_edge_t *)el);
+ }
+
+ e[4] = ccdPtAddEdge(pt, v[4], v[2]);
+ e[5] = ccdPtAddEdge(pt, v[4], v[0]);
+ e[6] = ccdPtAddEdge(pt, v[4], v[1]);
+ if (f[1])
+ e[7] = ccdPtAddEdge(pt, v[4], v[3]);
+
+ ccdPtAddFace(pt, e[1], e[4], e[6]);
+ ccdPtAddFace(pt, e[0], e[6], e[5]);
+ if (f[1]){
+ ccdPtAddFace(pt, e[3], e[5], e[7]);
+ ccdPtAddFace(pt, e[4], e[7], e[2]);
+ }else{
+ ccdPtAddFace(pt, e[4], e[5], (ccd_pt_edge_t *)el);
+ }
+ }
+ }else{ // el->type == CCD_PT_FACE
+ // replace triangle by tetrahedron without base (base would be the
+ // triangle that will be removed)
+
+ // get triplet of surrounding edges and vertices of triangle face
+ ccdPtFaceEdges((const ccd_pt_face_t *)el, &e[0], &e[1], &e[2]);
+ ccdPtEdgeVertices(e[0], &v[0], &v[1]);
+ ccdPtEdgeVertices(e[1], &v[2], &v[3]);
+
+ // following code sorts edges to have e[0] between vertices 0-1,
+ // e[1] between 1-2 and e[2] between 2-0
+ if (v[2] != v[1] && v[3] != v[1]){
+ // swap e[1] and e[2]
+ e[3] = e[1];
+ e[1] = e[2];
+ e[2] = e[3];
+ }
+ if (v[3] != v[0] && v[3] != v[1])
+ v[2] = v[3];
+
+ // remove triangle face
+ ccdPtDelFace(pt, (ccd_pt_face_t *)el);
+
+ // expand triangle to tetrahedron
+ v[3] = ccdPtAddVertex(pt, newv);
+ e[3] = ccdPtAddEdge(pt, v[3], v[0]);
+ e[4] = ccdPtAddEdge(pt, v[3], v[1]);
+ e[5] = ccdPtAddEdge(pt, v[3], v[2]);
+
+ ccdPtAddFace(pt, e[3], e[4], e[0]);
+ ccdPtAddFace(pt, e[4], e[5], e[1]);
+ ccdPtAddFace(pt, e[5], e[3], e[2]);
+ }
+}
+
+/** Finds next support point (at stores it in out argument).
+ * Returns 0 on success, -1 otherwise */
+static int nextSupport(const void *obj1, const void *obj2, const ccd_t *ccd,
+ const ccd_pt_el_t *el,
+ ccd_support_t *out)
+{
+ ccd_vec3_t *a, *b, *c;
+ ccd_real_t dist;
+
+ if (el->type == CCD_PT_VERTEX)
+ return -1;
+
+ // touch contact
+ if (ccdIsZero(el->dist))
+ return -1;
+
+ __ccdSupport(obj1, obj2, &el->witness, ccd, out);
+
+ if (el->type == CCD_PT_EDGE){
+ // fetch end points of edge
+ ccdPtEdgeVec3((ccd_pt_edge_t *)el, &a, &b);
+
+ // get distance from segment
+ dist = ccdVec3PointSegmentDist2(&out->v, a, b, NULL);
+ }else{ // el->type == CCD_PT_FACE
+ // fetch vertices of triangle face
+ ccdPtFaceVec3((ccd_pt_face_t *)el, &a, &b, &c);
+
+ // check if new point can significantly expand polytope
+ dist = ccdVec3PointTriDist2(&out->v, a, b, c, NULL);
+ }
+
+ if (dist < ccd->epa_tolerance)
+ return -1;
+
+ return 0;
+}
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 02:28:21 +0000