add ode

1 files changed, 572 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/libccd/src/mpr.c b/libs/ode-0.16.1/libccd/src/mpr.c
new file mode 100644
index 0000000..072a99b
--- /dev/null
+++ b/libs/ode-0.16.1/libccd/src/mpr.c
@@ -0,0 +1,572 @@
+/***
+ * libccd
+ * ---------------------------------
+ * Copyright (c)2010,2011 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 <stdlib.h>
+#include <ccd/ccd.h>
+#include <ccdcustom/vec3.h>
+#include <ccd/simplex.h>
+#include <ccd/dbg.h>
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+/** Finds origin (center) of Minkowski difference (actually it can be any
+ *  interior point of Minkowski difference. */
+_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd,
+                            ccd_support_t *center);
+
+/** Discovers initial portal - that is tetrahedron that intersects with
+ *  origin ray (ray from center of Minkowski diff to (0,0,0).
+ *
+ *  Returns -1 if already recognized that origin is outside Minkowski
+ *  portal.
+ *  Returns 1 if origin lies on v1 of simplex (only v0 and v1 are present
+ *  in simplex).
+ *  Returns 2 if origin lies on v0-v1 segment.
+ *  Returns 0 if portal was built.
+ */
+static int discoverPortal(const void *obj1, const void *obj2,
+                          const ccd_t *ccd, ccd_simplex_t *portal);
+
+
+/** Expands portal towards origin and determine if objects intersect.
+ *  Already established portal must be given as argument.
+ *  If intersection is found 0 is returned, -1 otherwise */
+static int refinePortal(const void *obj1, const void *obj2,
+                        const ccd_t *ccd, ccd_simplex_t *portal);
+
+/** Finds penetration info by expanding provided portal. */
+static int findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
+                       ccd_simplex_t *portal,
+                       ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);
+
+/** Finds penetration info if origin lies on portal's v1 */
+static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd,
+                            ccd_simplex_t *portal,
+                            ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);
+
+/** Find penetration info if origin lies on portal's segment v0-v1 */
+static int findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd,
+                              ccd_simplex_t *portal,
+                              ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);
+
+/** Finds position vector from fully established portal */
+static int findPos(const void *obj1, const void *obj2, const ccd_t *ccd,
+                    const ccd_simplex_t *portal, ccd_vec3_t *pos);
+
+/** Extends portal with new support point.
+ *  Portal must have face v1-v2-v3 arranged to face outside portal. */
+_ccd_inline void expandPortal(ccd_simplex_t *portal,
+                              const ccd_support_t *v4);
+
+/** Fill dir with direction outside portal. Portal's v1-v2-v3 face must be
+ *  arranged in correct order! */
+_ccd_inline int portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir);
+
+/** Returns true if portal encapsules origin (0,0,0), dir is direction of
+ *  v1-v2-v3 face. */
+_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal,
+                                       const ccd_vec3_t *dir);
+
+/** Returns true if portal with new point v4 would reach specified
+ *  tolerance (i.e. returns true if portal can _not_ significantly expand
+ *  within Minkowski difference).
+ *
+ *  v4 is candidate for new point in portal, dir is direction in which v4
+ *  was obtained. */
+_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal,
+                                     const ccd_support_t *v4,
+                                     const ccd_vec3_t *dir,
+                                     const ccd_t *ccd);
+
+/** Returns true if portal expanded by new point v4 could possibly contain
+ *  origin, dir is direction in which v4 was obtained. */
+_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal,   
+                                         const ccd_support_t *v4,
+                                         const ccd_vec3_t *dir);
+
+
+int ccdMPRIntersect(const void *obj1, const void *obj2, const ccd_t *ccd)
+{
+    ccd_simplex_t portal;
+    int res;
+
+    // Phase 1: Portal discovery - find portal that intersects with origin
+    // ray (ray from center of Minkowski diff to origin of coordinates)
+    res = discoverPortal(obj1, obj2, ccd, &portal);
+    if (res < 0)
+        return 0;
+    if (res > 0)
+        return 1;
+
+    // Phase 2: Portal refinement
+    res = refinePortal(obj1, obj2, ccd, &portal);
+    return (res == 0 ? 1 : 0);
+}
+
+int ccdMPRPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
+                      ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
+{
+    ccd_simplex_t portal;
+    int res;
+
+    // Phase 1: Portal discovery
+    res = discoverPortal(obj1, obj2, ccd, &portal);
+    if (res < 0){
+        // Origin isn't inside portal - no collision.
+        return -1;
+
+    }else if (res == 1){
+        // Touching contact on portal's v1.
+        findPenetrTouch(obj1, obj2, ccd, &portal, depth, dir, pos);
+
+    }else if (res == 2){
+        // Origin lies on v0-v1 segment.
+        if (findPenetrSegment(obj1, obj2, ccd, &portal, depth, dir, pos) != 0) {
+            return -1;
+        }
+
+    }else if (res == 0){
+        // Phase 2: Portal refinement
+        res = refinePortal(obj1, obj2, ccd, &portal);
+        if (res < 0) {
+            return -1;
+        }
+
+        // Phase 3. Penetration info
+        if (findPenetr(obj1, obj2, ccd, &portal, depth, dir, pos) != 0) {
+            return -1;
+        }
+    }
+
+    return 0;
+}
+
+
+
+_ccd_inline void findOrigin(const void *obj1, const void *obj2, const ccd_t *ccd,
+                            ccd_support_t *center)
+{
+    ccd->center1(obj1, &center->v1);
+    ccd->center2(obj2, &center->v2);
+    ccdVec3Sub2(&center->v, &center->v1, &center->v2);
+}
+
+static int discoverPortal(const void *obj1, const void *obj2,
+                          const ccd_t *ccd, ccd_simplex_t *portal)
+{
+    ccd_vec3_t dir, va, vb;
+    ccd_real_t dot;
+    int cont;
+
+    // vertex 0 is center of portal
+    findOrigin(obj1, obj2, ccd, ccdSimplexPointW(portal, 0));
+    ccdSimplexSetSize(portal, 1);
+
+    if (ccdVec3Eq(&ccdSimplexPoint(portal, 0)->v, ccd_vec3_origin)){
+        // Portal's center lies on origin (0,0,0) => we know that objects
+        // intersect but we would need to know penetration info.
+        // So move center little bit...
+        ccdVec3Set(&va, CCD_EPS * CCD_REAL(10.), CCD_ZERO, CCD_ZERO);
+        ccdVec3Add(&ccdSimplexPointW(portal, 0)->v, &va);
+    }
+
+
+    // vertex 1 = support in direction of origin
+    ccdVec3Copy(&dir, &ccdSimplexPoint(portal, 0)->v);
+    ccdVec3Scale(&dir, CCD_REAL(-1.));
+    if (ccdVec3SafeNormalize(&dir) != 0) {
+        return -1;
+    }
+    __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 1));
+    ccdSimplexSetSize(portal, 2);
+
+    // test if origin isn't outside of v1
+    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &dir);
+    if (ccdIsZero(dot) || dot < CCD_ZERO)
+        return -1;
+
+
+    // vertex 2
+    ccdVec3Cross(&dir, &ccdSimplexPoint(portal, 0)->v,
+                       &ccdSimplexPoint(portal, 1)->v);
+    if (ccdIsZero(ccdVec3Len2(&dir))){
+        if (ccdVec3Eq(&ccdSimplexPoint(portal, 1)->v, ccd_vec3_origin)){
+            // origin lies on v1
+            return 1;
+        }else{
+            // origin lies on v0-v1 segment
+            return 2;
+        }
+    }
+
+    if (ccdVec3SafeNormalize(&dir) != 0) {
+        return -1;
+    }
+    __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 2));
+    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &dir);
+    if (ccdIsZero(dot) || dot < CCD_ZERO) {
+        return -1;
+    }
+
+    ccdSimplexSetSize(portal, 3);
+
+    // vertex 3 direction
+    ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
+                     &ccdSimplexPoint(portal, 0)->v);
+    ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
+                     &ccdSimplexPoint(portal, 0)->v);
+    ccdVec3Cross(&dir, &va, &vb);
+    if (ccdVec3SafeNormalize(&dir) != 0) {
+        return -1;
+    }
+
+    // it is better to form portal faces to be oriented "outside" origin
+    dot = ccdVec3Dot(&dir, &ccdSimplexPoint(portal, 0)->v);
+    if (dot > CCD_ZERO){
+        ccdSimplexSwap(portal, 1, 2);
+        ccdVec3Scale(&dir, CCD_REAL(-1.));
+    }
+
+    while (ccdSimplexSize(portal) < 4){
+        __ccdSupport(obj1, obj2, &dir, ccd, ccdSimplexPointW(portal, 3));
+        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &dir);
+        if (ccdIsZero(dot) || dot < CCD_ZERO) {
+            return -1;
+        }
+
+        cont = 0;
+
+        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
+        // continue
+        ccdVec3Cross(&va, &ccdSimplexPoint(portal, 1)->v,
+                          &ccdSimplexPoint(portal, 3)->v);
+        dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
+        if (dot < CCD_ZERO && !ccdIsZero(dot)){
+            ccdSimplexSet(portal, 2, ccdSimplexPoint(portal, 3));
+            cont = 1;
+        }
+
+        if (!cont){
+            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
+            // continue
+            ccdVec3Cross(&va, &ccdSimplexPoint(portal, 3)->v,
+                              &ccdSimplexPoint(portal, 2)->v);
+            dot = ccdVec3Dot(&va, &ccdSimplexPoint(portal, 0)->v);
+            if (dot < CCD_ZERO && !ccdIsZero(dot)){
+                ccdSimplexSet(portal, 1, ccdSimplexPoint(portal, 3));
+                cont = 1;
+            }
+        }
+
+        if (cont){
+            ccdVec3Sub2(&va, &ccdSimplexPoint(portal, 1)->v,
+                             &ccdSimplexPoint(portal, 0)->v);
+            ccdVec3Sub2(&vb, &ccdSimplexPoint(portal, 2)->v,
+                             &ccdSimplexPoint(portal, 0)->v);
+            ccdVec3Cross(&dir, &va, &vb);
+            if (ccdVec3SafeNormalize(&dir) != 0) {
+                return -1;
+            }
+        }else{
+            ccdSimplexSetSize(portal, 4);
+        }
+    }
+
+    return 0;
+}
+
+static int refinePortal(const void *obj1, const void *obj2,
+                        const ccd_t *ccd, ccd_simplex_t *portal)
+{
+    ccd_vec3_t dir;
+    ccd_support_t v4;
+
+    while (1){
+        // compute direction outside the portal (from v0 throught v1,v2,v3
+        // face)
+        if (portalDir(portal, &dir) != 0) {
+            return -1;
+        }
+
+        // test if origin is inside the portal
+        if (portalEncapsulesOrigin(portal, &dir))
+            return 0;
+
+        // get next support point
+        __ccdSupport(obj1, obj2, &dir, ccd, &v4);
+
+        // test if v4 can expand portal to contain origin and if portal
+        // expanding doesn't reach given tolerance
+        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
+                || portalReachTolerance(portal, &v4, &dir, ccd)){
+            return -1;
+        }
+
+        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
+        // difference (direction from v0).
+        expandPortal(portal, &v4);
+    }
+
+    return -1;
+}
+
+
+static int findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
+                       ccd_simplex_t *portal,
+                       ccd_real_t *depth, ccd_vec3_t *pdir, ccd_vec3_t *pos)
+{
+    ccd_vec3_t dir;
+    ccd_support_t v4;
+    unsigned long iterations;
+
+    iterations = 0UL;
+    while (1){
+        // compute portal direction and obtain next support point
+        if (portalDir(portal, &dir) != 0) {
+            return -1;
+        }
+
+        __ccdSupport(obj1, obj2, &dir, ccd, &v4);
+
+        // reached tolerance -> find penetration info
+        if (portalReachTolerance(portal, &v4, &dir, ccd)
+                || iterations > ccd->max_iterations){
+            *depth = ccdVec3PointTriDist2(ccd_vec3_origin,
+                                          &ccdSimplexPoint(portal, 1)->v,
+                                          &ccdSimplexPoint(portal, 2)->v,
+                                          &ccdSimplexPoint(portal, 3)->v,
+                                          pdir);
+            *depth = CCD_SQRT(*depth);
+            if (ccdVec3SafeNormalize(pdir) != 0) {
+                return -1;
+            }
+
+            // barycentric coordinates:
+            if (findPos(obj1, obj2, ccd, portal, pos) != 0) {
+                return -1;
+            }
+
+            return 0;
+        }
+
+        expandPortal(portal, &v4);
+
+        iterations++;
+    }
+}
+
+static void findPenetrTouch(const void *obj1, const void *obj2, const ccd_t *ccd,
+                            ccd_simplex_t *portal,
+                            ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
+{
+    // Touching contact on portal's v1 - so depth is zero and direction
+    // is unimportant and pos can be guessed
+    *depth = CCD_REAL(0.);
+    ccdVec3Copy(dir, ccd_vec3_origin);
+
+    ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
+    ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
+    ccdVec3Scale(pos, 0.5);
+}
+
+static int findPenetrSegment(const void *obj1, const void *obj2, const ccd_t *ccd,
+                              ccd_simplex_t *portal,
+                              ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos)
+{
+    /*
+    ccd_vec3_t vec;
+    ccd_real_t k;
+    */
+
+    // Origin lies on v0-v1 segment.
+    // Depth is distance to v1, direction also and position must be
+    // computed
+
+    ccdVec3Copy(pos, &ccdSimplexPoint(portal, 1)->v1);
+    ccdVec3Add(pos, &ccdSimplexPoint(portal, 1)->v2);
+    ccdVec3Scale(pos, CCD_REAL(0.5));
+
+    /*
+    ccdVec3Sub2(&vec, &ccdSimplexPoint(portal, 1)->v,
+                      &ccdSimplexPoint(portal, 0)->v);
+    k  = CCD_SQRT(ccdVec3Len2(&ccdSimplexPoint(portal, 0)->v));
+    k /= CCD_SQRT(ccdVec3Len2(&vec));
+    ccdVec3Scale(&vec, -k);
+    ccdVec3Add(pos, &vec);
+    */
+
+    ccdVec3Copy(dir, &ccdSimplexPoint(portal, 1)->v);
+    *depth = CCD_SQRT(ccdVec3Len2(dir));
+    if (ccdVec3SafeNormalize(dir) != 0) {
+        return -1;
+    }
+    return 0;
+}
+
+
+static int findPos(const void *obj1, const void *obj2, const ccd_t *ccd,
+                    const ccd_simplex_t *portal, ccd_vec3_t *pos)
+{
+    ccd_vec3_t dir;
+    size_t i;
+    ccd_real_t b[4], sum, inv;
+    ccd_vec3_t vec, p1, p2;
+
+    if (portalDir(portal, &dir) != 0) {
+        return -1;
+    }
+
+    // use barycentric coordinates of tetrahedron to find origin
+    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
+                       &ccdSimplexPoint(portal, 2)->v);
+    b[0] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);
+
+    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
+                       &ccdSimplexPoint(portal, 2)->v);
+    b[1] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);
+
+    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 0)->v,
+                       &ccdSimplexPoint(portal, 1)->v);
+    b[2] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 3)->v);
+
+    ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
+                       &ccdSimplexPoint(portal, 1)->v);
+    b[3] = ccdVec3Dot(&vec, &ccdSimplexPoint(portal, 0)->v);
+
+	sum = b[0] + b[1] + b[2] + b[3];
+
+    if (ccdIsZero(sum) || sum < CCD_ZERO){
+		b[0] = CCD_REAL(0.);
+
+        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 2)->v,
+                           &ccdSimplexPoint(portal, 3)->v);
+        b[1] = ccdVec3Dot(&vec, &dir);
+        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 3)->v,
+                           &ccdSimplexPoint(portal, 1)->v);
+        b[2] = ccdVec3Dot(&vec, &dir);
+        ccdVec3Cross(&vec, &ccdSimplexPoint(portal, 1)->v,
+                           &ccdSimplexPoint(portal, 2)->v);
+        b[3] = ccdVec3Dot(&vec, &dir);
+
+		sum = b[1] + b[2] + b[3];
+	}
+
+	inv = CCD_REAL(1.) / sum;
+
+    ccdVec3Copy(&p1, ccd_vec3_origin);
+    ccdVec3Copy(&p2, ccd_vec3_origin);
+    for (i = 0; i < 4; i++){
+        ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v1);
+        ccdVec3Scale(&vec, b[i]);
+        ccdVec3Add(&p1, &vec);
+
+        ccdVec3Copy(&vec, &ccdSimplexPoint(portal, i)->v2);
+        ccdVec3Scale(&vec, b[i]);
+        ccdVec3Add(&p2, &vec);
+    }
+    ccdVec3Scale(&p1, inv);
+    ccdVec3Scale(&p2, inv);
+
+    ccdVec3Copy(pos, &p1);
+    ccdVec3Add(pos, &p2);
+    ccdVec3Scale(pos, 0.5);
+    return 0;
+}
+
+_ccd_inline void expandPortal(ccd_simplex_t *portal,
+                              const ccd_support_t *v4)
+{
+    ccd_real_t dot;
+    ccd_vec3_t v4v0;
+
+    ccdVec3Cross(&v4v0, &v4->v, &ccdSimplexPoint(portal, 0)->v);
+    dot = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, &v4v0);
+    if (dot > CCD_ZERO){
+        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, &v4v0);
+        if (dot > CCD_ZERO){
+            ccdSimplexSet(portal, 1, v4);
+        }else{
+            ccdSimplexSet(portal, 3, v4);
+        }
+    }else{
+        dot = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, &v4v0);
+        if (dot > CCD_ZERO){
+            ccdSimplexSet(portal, 2, v4);
+        }else{
+            ccdSimplexSet(portal, 1, v4);
+        }
+    }
+}
+
+_ccd_inline int portalDir(const ccd_simplex_t *portal, ccd_vec3_t *dir)
+{
+    ccd_vec3_t v2v1, v3v1;
+
+    ccdVec3Sub2(&v2v1, &ccdSimplexPoint(portal, 2)->v,
+                       &ccdSimplexPoint(portal, 1)->v);
+    ccdVec3Sub2(&v3v1, &ccdSimplexPoint(portal, 3)->v,
+                       &ccdSimplexPoint(portal, 1)->v);
+    ccdVec3Cross(dir, &v2v1, &v3v1);
+    if (ccdVec3SafeNormalize(dir) != 0) {
+        return -1;
+    }
+    return 0;
+}
+
+_ccd_inline int portalEncapsulesOrigin(const ccd_simplex_t *portal,
+                                       const ccd_vec3_t *dir)
+{
+    ccd_real_t dot;
+    dot = ccdVec3Dot(dir, &ccdSimplexPoint(portal, 1)->v);
+    return ccdIsZero(dot) || dot > CCD_ZERO;
+}
+
+_ccd_inline int portalReachTolerance(const ccd_simplex_t *portal,
+                                     const ccd_support_t *v4,
+                                     const ccd_vec3_t *dir,
+                                     const ccd_t *ccd)
+{
+    ccd_real_t dv1, dv2, dv3, dv4;
+    ccd_real_t dot1, dot2, dot3;
+
+    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
+
+    dv1 = ccdVec3Dot(&ccdSimplexPoint(portal, 1)->v, dir);
+    dv2 = ccdVec3Dot(&ccdSimplexPoint(portal, 2)->v, dir);
+    dv3 = ccdVec3Dot(&ccdSimplexPoint(portal, 3)->v, dir);
+    dv4 = ccdVec3Dot(&v4->v, dir);
+
+    dot1 = dv4 - dv1;
+    dot2 = dv4 - dv2;
+    dot3 = dv4 - dv3;
+
+    dot1 = CCD_FMIN(dot1, dot2);
+    dot1 = CCD_FMIN(dot1, dot3);
+
+    return ccdEq(dot1, ccd->mpr_tolerance) || dot1 < ccd->mpr_tolerance;
+}
+
+_ccd_inline int portalCanEncapsuleOrigin(const ccd_simplex_t *portal,   
+                                         const ccd_support_t *v4,
+                                         const ccd_vec3_t *dir)
+{
+    ccd_real_t dot;
+    dot = ccdVec3Dot(&v4->v, dir);
+    return ccdIsZero(dot) || dot > CCD_ZERO;
+}