add ode

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;
+}