add ode

1 files changed, 596 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_sphere.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_sphere.cpp
new file mode 100644
index 0000000..8076411
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/collision_trimesh_sphere.cpp
@@ -0,0 +1,596 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_util.h"
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+
+#if dTRIMESH_ENABLED
+
+#include "collision_trimesh_internal.h"
+
+
+#if dTRIMESH_OPCODE
+
+// Ripped from Opcode 1.1.
+static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){
+
+    // now onto the bulk of the collision...
+
+    dVector3 Diff;
+    Diff[0] = Origin[0] - Center[0];
+    Diff[1] = Origin[1] - Center[1];
+    Diff[2] = Origin[2] - Center[2];
+    Diff[3] = Origin[3] - Center[3];
+
+    dReal A00 = dCalcVectorDot3(Edge0, Edge0);
+    dReal A01 = dCalcVectorDot3(Edge0, Edge1);
+    dReal A11 = dCalcVectorDot3(Edge1, Edge1);
+
+    dReal B0 = dCalcVectorDot3(Diff, Edge0);
+    dReal B1 = dCalcVectorDot3(Diff, Edge1);
+
+    dReal C = dCalcVectorDot3(Diff, Diff);
+
+    dReal Det = dFabs(A00 * A11 - A01 * A01);
+    u = A01 * B1 - A11 * B0;
+    v = A01 * B0 - A00 * B1;
+
+    dReal DistSq;
+
+    if (u + v <= Det){
+        if(u < REAL(0.0)){
+            if(v < REAL(0.0)){  // region 4
+                if(B0 < REAL(0.0)){
+                    v = REAL(0.0);
+                    if (-B0 >= A00){
+                        u = REAL(1.0);
+                        DistSq = A00 + REAL(2.0) * B0 + C;
+                    }
+                    else{
+                        u = -B0 / A00;
+                        DistSq = B0 * u + C;
+                    }
+                }
+                else{
+                    u = REAL(0.0);
+                    if(B1 >= REAL(0.0)){
+                        v = REAL(0.0);
+                        DistSq = C;
+                    }
+                    else if(-B1 >= A11){
+                        v = REAL(1.0);
+                        DistSq = A11 + REAL(2.0) * B1 + C;
+                    }
+                    else{
+                        v = -B1 / A11;
+                        DistSq = B1 * v + C;
+                    }
+                }
+            }
+            else{  // region 3
+                u = REAL(0.0);
+                if(B1 >= REAL(0.0)){
+                    v = REAL(0.0);
+                    DistSq = C;
+                }
+                else if(-B1 >= A11){
+                    v = REAL(1.0);
+                    DistSq = A11 + REAL(2.0) * B1 + C;
+                }
+                else{
+                    v = -B1 / A11;
+                    DistSq = B1 * v + C;
+                }
+            }
+        }
+        else if(v < REAL(0.0)){  // region 5
+            v = REAL(0.0);
+            if (B0 >= REAL(0.0)){
+                u = REAL(0.0);
+                DistSq = C;
+            }
+            else if (-B0 >= A00){
+                u = REAL(1.0);
+                DistSq = A00 + REAL(2.0) * B0 + C;
+            }
+            else{
+                u = -B0 / A00;
+                DistSq = B0 * u + C;
+            }
+        }
+        else{  // region 0
+            // minimum at interior point
+            if (Det == REAL(0.0)){
+                u = REAL(0.0);
+                v = REAL(0.0);
+                DistSq = FLT_MAX;
+            }
+            else{
+                dReal InvDet = REAL(1.0) / Det;
+                u *= InvDet;
+                v *= InvDet;
+                DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+            }
+        }
+    }
+    else{
+        dReal Tmp0, Tmp1, Numer, Denom;
+
+        if(u < REAL(0.0)){  // region 2
+            Tmp0 = A01 + B0;
+            Tmp1 = A11 + B1;
+            if (Tmp1 > Tmp0){
+                Numer = Tmp1 - Tmp0;
+                Denom = A00 - REAL(2.0) * A01 + A11;
+                if (Numer >= Denom){
+                    u = REAL(1.0);
+                    v = REAL(0.0);
+                    DistSq = A00 + REAL(2.0) * B0 + C;
+                }
+                else{
+                    u = Numer / Denom;
+                    v = REAL(1.0) - u;
+                    DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+                }
+            }
+            else{
+                u = REAL(0.0);
+                if(Tmp1 <= REAL(0.0)){
+                    v = REAL(1.0);
+                    DistSq = A11 + REAL(2.0) * B1 + C;
+                }
+                else if(B1 >= REAL(0.0)){
+                    v = REAL(0.0);
+                    DistSq = C;
+                }
+                else{
+                    v = -B1 / A11;
+                    DistSq = B1 * v + C;
+                }
+            }
+        }
+        else if(v < REAL(0.0)){  // region 6
+            Tmp0 = A01 + B1;
+            Tmp1 = A00 + B0;
+            if (Tmp1 > Tmp0){
+                Numer = Tmp1 - Tmp0;
+                Denom = A00 - REAL(2.0) * A01 + A11;
+                if (Numer >= Denom){
+                    v = REAL(1.0);
+                    u = REAL(0.0);
+                    DistSq = A11 + REAL(2.0) * B1 + C;
+                }
+                else{
+                    v = Numer / Denom;
+                    u = REAL(1.0) - v;
+                    DistSq =  u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+                }
+            }
+            else{
+                v = REAL(0.0);
+                if (Tmp1 <= REAL(0.0)){
+                    u = REAL(1.0);
+                    DistSq = A00 + REAL(2.0) * B0 + C;
+                }
+                else if(B0 >= REAL(0.0)){
+                    u = REAL(0.0);
+                    DistSq = C;
+                }
+                else{
+                    u = -B0 / A00;
+                    DistSq = B0 * u + C;
+                }
+            }
+        }
+        else{  // region 1
+            Numer = A11 + B1 - A01 - B0;
+            if (Numer <= REAL(0.0)){
+                u = REAL(0.0);
+                v = REAL(1.0);
+                DistSq = A11 + REAL(2.0) * B1 + C;
+            }
+            else{
+                Denom = A00 - REAL(2.0) * A01 + A11;
+                if (Numer >= Denom){
+                    u = REAL(1.0);
+                    v = REAL(0.0);
+                    DistSq = A00 + REAL(2.0) * B0 + C;
+                }
+                else{
+                    u = Numer / Denom;
+                    v = REAL(1.0) - u;
+                    DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+                }
+            }
+        }
+    }
+
+    Dist = dSqrt(dFabs(DistSq));
+
+    if (Dist <= Radius){
+        Dist = Radius - Dist;
+        return true;
+    }
+    else return false;
+}
+
+int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (SphereGeom->type == dSphereClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+    const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
+    dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
+    TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
+    SphereCollider& Collider = pccColliderCache->m_SphereCollider;
+
+    const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
+    const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
+
+    Matrix4x4 MeshMatrix;
+    const dVector3 ZeroVector3 = { REAL(0.0), };
+    MakeMatrix(ZeroVector3, TLRotation, MeshMatrix);
+
+    const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
+    dReal Radius = dGeomSphereGetRadius(SphereGeom);
+
+    dVector3 OffsetPosition;
+    dSubtractVectors3(OffsetPosition, Position, TLPosition);
+
+    // Sphere
+    Sphere Sphere;
+    Sphere.mCenter.Set(OffsetPosition[0], OffsetPosition[1], OffsetPosition[2]);
+    Sphere.mRadius = Radius;
+
+
+    // TC results
+    if (TriMesh->getDoTC(dxTriMesh::TTC_SPHERE)) {
+        dxTriMesh::SphereTC* sphereTC = 0;
+        const int sphereCacheSize = TriMesh->m_SphereTCCache.size();
+        for (int i = 0; i != sphereCacheSize; i++){
+            if (TriMesh->m_SphereTCCache[i].Geom == SphereGeom){
+                sphereTC = &TriMesh->m_SphereTCCache[i];
+                break;
+            }
+        }
+
+        if (!sphereTC) {
+            TriMesh->m_SphereTCCache.push(dxTriMesh::SphereTC());
+
+            sphereTC = &TriMesh->m_SphereTCCache[TriMesh->m_SphereTCCache.size() - 1];
+            sphereTC->Geom = SphereGeom;
+        }
+
+        // Intersect
+        Collider.SetTemporalCoherence(true);
+        Collider.Collide(*sphereTC, Sphere, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
+    }
+    else {
+        Collider.SetTemporalCoherence(false);
+        Collider.Collide(pccColliderCache->m_DefaultSphereCache, Sphere, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
+    }
+
+    if (! Collider.GetContactStatus()) {
+        // no collision occurred
+        return 0;
+    }
+
+    // get results
+    int TriCount = Collider.GetNbTouchedPrimitives();
+    const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
+
+    if (TriCount != 0){
+        if (TriMesh->m_ArrayCallback != null){
+            TriMesh->m_ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
+        }
+
+        int OutTriCount = 0;
+        for (int i = 0; i < TriCount; i++){
+            if (OutTriCount == (Flags & NUMC_MASK)){
+                break;
+            }
+
+            const int TriIndex = Triangles[i];
+
+            dVector3 dv[3];
+            if (!TriMesh->invokeCallback(SphereGeom, TriIndex))
+                continue;
+
+            TriMesh->fetchMeshTriangle(dv, TriIndex, TLPosition, TLRotation);
+
+            dVector3& v0 = dv[0];
+            dVector3& v1 = dv[1];
+            dVector3& v2 = dv[2];
+
+            dVector3 vu;
+            vu[0] = v1[0] - v0[0];
+            vu[1] = v1[1] - v0[1];
+            vu[2] = v1[2] - v0[2];
+            vu[3] = REAL(0.0);
+
+            dVector3 vv;
+            vv[0] = v2[0] - v0[0];
+            vv[1] = v2[1] - v0[1];
+            vv[2] = v2[2] - v0[2];
+            vv[3] = REAL(0.0);
+
+            // Get plane coefficients
+            dVector4 Plane;
+            dCalcVectorCross3(Plane, vu, vv);
+
+            // Even though all triangles might be initially valid, 
+            // a triangle may degenerate into a segment after applying 
+            // space transformation.
+            if (!dSafeNormalize3(Plane)) {
+                continue;
+            }
+
+            /* If the center of the sphere is within the positive halfspace of the
+            * triangle's plane, allow a contact to be generated.
+            * If the center of the sphere made it into the positive halfspace of a
+            * back-facing triangle, then the physics update and/or velocity needs
+            * to be adjusted (penetration has occured anyway).
+            */
+
+            dReal side = dCalcVectorDot3(Plane,Position) - dCalcVectorDot3(Plane, v0);
+
+            if(side < REAL(0.0)) {
+                continue;
+            }
+
+            dReal Depth;
+            dReal u, v;
+            if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
+                continue;	// Sphere doesn't hit triangle
+            }
+
+            if (Depth < REAL(0.0)){
+                continue; // Negative depth does not produce a contact
+            }
+
+            dVector3 ContactPos;
+
+            dReal w = REAL(1.0) - u - v;
+            ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
+            ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
+            ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
+
+            // Depth returned from GetContactData is depth along 
+            // contact point - sphere center direction
+            // we'll project it to contact normal
+            dVector3 dir;
+            dir[0] = Position[0]-ContactPos[0];
+            dir[1] = Position[1]-ContactPos[1];
+            dir[2] = Position[2]-ContactPos[2];
+            dReal dirProj = dCalcVectorDot3(dir, Plane) / dSqrt(dCalcVectorDot3(dir, dir));
+
+            // Since Depth already had a requirement to be non-negative,
+            // negative direction projections should not be allowed as well,
+            // as otherwise the multiplication will result in negative contact depth.
+            if (dirProj < REAL(0.0)){
+                continue; // Zero contact depth could be ignored
+            }
+
+            dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
+
+            Contact->pos[0] = ContactPos[0];
+            Contact->pos[1] = ContactPos[1];
+            Contact->pos[2] = ContactPos[2];
+            Contact->pos[3] = REAL(0.0);
+
+            // Using normal as plane (reversed)
+            Contact->normal[0] = -Plane[0];
+            Contact->normal[1] = -Plane[1];
+            Contact->normal[2] = -Plane[2];
+            Contact->normal[3] = REAL(0.0);
+
+            Contact->depth = Depth * dirProj;
+            //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
+
+            // We need to set these unconditionally, as the merging may fail! - Bram
+            Contact->g1 = TriMesh;
+            Contact->g2 = SphereGeom;
+            Contact->side2 = -1;
+
+            Contact->side1 = TriIndex;
+
+            OutTriCount++;
+        }
+        if (OutTriCount > 0){
+            if (TriMesh->m_SphereContactsMergeOption == MERGE_CONTACTS_FULLY) {
+                dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
+                Contact->g1 = TriMesh;
+                Contact->g2 = SphereGeom;
+                Contact->side2 = -1;
+
+                if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){
+                    dVector3 pos;
+                    pos[0] = Contact->pos[0];
+                    pos[1] = Contact->pos[1];
+                    pos[2] = Contact->pos[2];
+
+                    dVector3 normal;
+                    normal[0] = Contact->normal[0] * Contact->depth;
+                    normal[1] = Contact->normal[1] * Contact->depth;
+                    normal[2] = Contact->normal[2] * Contact->depth;
+                    normal[3] = REAL(0.0);
+
+                    int TriIndex = Contact->side1;
+
+                    for (int i = 1; i < OutTriCount; i++){
+                        dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+                        pos[0] += TempContact->pos[0];
+                        pos[1] += TempContact->pos[1];
+                        pos[2] += TempContact->pos[2];
+
+                        normal[0] += TempContact->normal[0] * TempContact->depth;
+                        normal[1] += TempContact->normal[1] * TempContact->depth;
+                        normal[2] += TempContact->normal[2] * TempContact->depth;
+
+                        TriIndex = (TriMesh->m_TriMergeCallback) ? TriMesh->m_TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
+                    }
+
+                    Contact->side1 = TriIndex;
+
+                    Contact->pos[0] = pos[0] / OutTriCount;
+                    Contact->pos[1] = pos[1] / OutTriCount;
+                    Contact->pos[2] = pos[2] / OutTriCount;
+
+                    if ( !dSafeNormalize3(normal) )
+                        return OutTriCount;	// Cannot merge in this pathological case
+
+                    // Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection.
+                    // That is why we need to correct this by increasing the depth for each intersection.
+                    // The maximum of the adjusted depths is our newly merged depth value - Bram.
+
+                    dReal mergedDepth = REAL(0.0);
+                    dReal minEffectiveness = REAL(0.5);
+                    for ( int i = 0; i < OutTriCount; ++i )
+                    {
+                        dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
+                        dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal);
+                        if ( effectiveness < dEpsilon )
+                            return OutTriCount; // Cannot merge this pathological case
+                        // Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal.
+                        effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness;
+                        dReal adjusted = TempContact->depth / effectiveness;
+                        mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth;
+                    }
+                    Contact->depth = mergedDepth;
+                    Contact->normal[0] = normal[0];
+                    Contact->normal[1] = normal[1];
+                    Contact->normal[2] = normal[2];
+                    Contact->normal[3] = normal[3];
+                }
+
+                return 1;
+            }
+            else if (TriMesh->m_SphereContactsMergeOption == MERGE_CONTACT_NORMALS) {
+                if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
+                    dVector3 Normal;
+
+                    dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
+                    Normal[0] = FirstContact->normal[0] * FirstContact->depth;
+                    Normal[1] = FirstContact->normal[1] * FirstContact->depth;
+                    Normal[2] = FirstContact->normal[2] * FirstContact->depth;
+                    Normal[3] = FirstContact->normal[3] * FirstContact->depth;
+
+                    for (int i = 1; i < OutTriCount; i++){
+                        dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+                        Normal[0] += Contact->normal[0] * Contact->depth;
+                        Normal[1] += Contact->normal[1] * Contact->depth;
+                        Normal[2] += Contact->normal[2] * Contact->depth;
+                        Normal[3] += Contact->normal[3] * Contact->depth;
+                    }
+
+                    dNormalize3(Normal);
+
+                    for (int i = 0; i < OutTriCount; i++){
+                        dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+                        Contact->normal[0] = Normal[0];
+                        Contact->normal[1] = Normal[1];
+                        Contact->normal[2] = Normal[2];
+                        Contact->normal[3] = Normal[3];
+                    }
+                }
+
+                return OutTriCount;
+            }
+            else {
+                dIASSERT(TriMesh->m_SphereContactsMergeOption == DONT_MERGE_CONTACTS);
+                return OutTriCount;
+            }
+        }
+        else return 0;
+    }
+    else return 0;
+}
+
+
+#endif // dTRIMESH_OPCODE
+
+
+#if dTRIMESH_GIMPACT
+
+#include "gimpact_contact_export_helper.h"
+#include "gimpact_gim_contact_accessor.h"
+
+
+int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride)
+{
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (SphereGeom->type == dSphereClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+    dVector3& Position = *(dVector3*)dGeomGetPosition(SphereGeom);
+    dReal Radius = dGeomSphereGetRadius(SphereGeom);
+    //Create contact list
+    GDYNAMIC_ARRAY trimeshcontacts;
+    GIM_CREATE_CONTACT_LIST(trimeshcontacts);
+
+    g1 -> recomputeAABB();
+    SphereGeom -> recomputeAABB();
+
+    //Collide trimeshes
+    gim_trimesh_sphere_collisionODE(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts);
+
+    if(trimeshcontacts.m_size == 0)
+    {
+        GIM_DYNARRAY_DESTROY(trimeshcontacts);
+        return 0;
+    }
+
+    GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
+    unsigned contactcount = trimeshcontacts.m_size;
+
+    dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, g1, SphereGeom, -1);
+    contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, Flags, Contacts, Stride);
+
+    GIM_DYNARRAY_DESTROY(trimeshcontacts);
+
+    return (int)contactcount;
+}
+
+
+#endif // dTRIMESH_GIMPACT
+
+
+#endif // dTRIMESH_ENABLED