1 files changed, 495 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_internal.h b/libraries/ode-0.9/ode/src/collision_trimesh_internal.h
new file mode 100644
index 0000000..bf474a2
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_internal.h
@@ -0,0 +1,495 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.
+// Modified for FreeSOLID Compatibility by Rodrigo Hernandez
+
+#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
+#define _ODE_COLLISION_TRIMESH_INTERNAL_H_
+
+int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideTrimeshPlane(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags,
+                                                                        dContactGeom *contact, int skip )
+{
+        // Swapped case, for code that needs it (heightfield initially)
+        // The other ray-geom colliders take geoms in a swapped order to the
+        // dCollideRTL function which is annoying when using function pointers.
+        return dCollideRTL( trimesh, ray, flags, contact, skip );
+}
+
+//****************************************************************************
+// dxTriMesh class
+
+#ifdef TRIMESH_INTERNAL
+
+#include "collision_kernel.h"
+#include <ode/collision_trimesh.h>
+
+#if dTRIMESH_OPCODE
+#define BAN_OPCODE_AUTOLINK
+#include "Opcode.h"
+using namespace Opcode;
+#endif // dTRIMESH_OPCODE
+
+#if dTRIMESH_GIMPACT
+#include <GIMPACT/gimpact.h>
+#endif
+
+struct dxTriMeshData  : public dBase 
+{
+    /* Array of flags for which edges and verts should be used on each triangle */
+    enum UseFlags
+    {
+        kEdge0 = 0x1,
+        kEdge1 = 0x2,
+        kEdge2 = 0x4,
+        kVert0 = 0x8,
+        kVert1 = 0x10,
+        kVert2 = 0x20,
+
+        kUseAll = 0xFF,
+    };
+
+    /* Setup the UseFlags array */
+    void Preprocess();
+    /* For when app changes the vertices */
+    void UpdateData();
+
+#if dTRIMESH_OPCODE
+        Model BVTree;
+        MeshInterface Mesh;
+
+    dxTriMeshData();
+    ~dxTriMeshData();
+    
+    void Build(const void* Vertices, int VertexStide, int VertexCount, 
+               const void* Indices, int IndexCount, int TriStride, 
+               const void* Normals, 
+               bool Single);
+    
+        /* aabb in model space */
+        dVector3 AABBCenter;
+        dVector3 AABBExtents;
+
+    // data for use in collision resolution
+    const void* Normals;
+    uint8* UseFlags;
+#endif  // dTRIMESH_OPCODE
+
+#if dTRIMESH_GIMPACT
+        const char* m_Vertices;
+        int m_VertexStride;
+        int m_VertexCount;
+        const char* m_Indices;
+        int m_TriangleCount;
+        int m_TriStride;
+        bool m_single;
+
+   dxTriMeshData()
+        {
+                m_Vertices=NULL;
+                m_VertexStride = 12;
+                m_VertexCount = 0;
+                m_Indices = 0;
+                m_TriangleCount = 0;
+                m_TriStride = 12;
+                m_single = true;
+        }
+
+    void Build(const void* Vertices, int VertexStride, int VertexCount,
+               const void* Indices, int IndexCount, int TriStride,
+               const void* Normals,
+              bool Single)
+        {
+                dIASSERT(Vertices);
+                dIASSERT(Indices);
+                dIASSERT(VertexStride);
+                dIASSERT(TriStride);
+                dIASSERT(IndexCount);
+                m_Vertices=(const char *)Vertices;
+                m_VertexStride = VertexStride;
+                m_VertexCount = VertexCount;
+                m_Indices = (const char *)Indices;
+                m_TriangleCount = IndexCount/3;
+                m_TriStride = TriStride;
+                m_single = Single;
+        }
+
+        inline void GetVertex(unsigned int i, dVector3 Out)
+        {
+                if(m_single)
+                {
+                        const float * fverts = (const float * )(m_Vertices + m_VertexStride*i);
+                        Out[0] = fverts[0];
+                        Out[1] = fverts[1];
+                        Out[2] = fverts[2];
+                        Out[3] = 1.0f;
+                }
+                else
+                {
+                        const double * dverts = (const double * )(m_Vertices + m_VertexStride*i);
+                        Out[0] = (float)dverts[0];
+                        Out[1] = (float)dverts[1];
+                        Out[2] = (float)dverts[2];
+                        Out[3] = 1.0f;
+
+                }
+        }
+
+        inline void GetTriIndices(unsigned int itriangle, unsigned int triindices[3])
+        {
+                const unsigned int * ind = (const unsigned int * )(m_Indices + m_TriStride*itriangle);
+                triindices[0] = ind[0];
+                triindices[1] = ind[1];
+                triindices[2] = ind[2];
+        }
+#endif  // dTRIMESH_GIMPACT
+};
+
+
+struct dxTriMesh : public dxGeom{
+        // Callbacks
+        dTriCallback* Callback;
+        dTriArrayCallback* ArrayCallback;
+        dTriRayCallback* RayCallback;
+
+        // Data types
+        dxTriMeshData* Data;
+
+        bool doSphereTC;
+        bool doBoxTC;
+        bool doCapsuleTC;
+
+        // Functions
+        dxTriMesh(dSpaceID Space, dTriMeshDataID Data);
+        ~dxTriMesh();
+
+        void ClearTCCache();
+
+        int AABBTest(dxGeom* g, dReal aabb[6]);
+        void computeAABB();
+
+#if dTRIMESH_OPCODE
+        // Instance data for last transform.
+    dMatrix4 last_trans;
+
+        // Colliders
+        static PlanesCollider _PlanesCollider;
+        static SphereCollider _SphereCollider;
+        static OBBCollider _OBBCollider;
+        static RayCollider _RayCollider;
+        static AABBTreeCollider _AABBTreeCollider;
+        static LSSCollider _LSSCollider;
+
+        // Some constants
+        static CollisionFaces Faces;
+        // Temporal coherence
+        struct SphereTC : public SphereCache{
+                dxGeom* Geom;
+        };
+        dArray<SphereTC> SphereTCCache;
+        static SphereCache defaultSphereCache;
+
+        struct BoxTC : public OBBCache{
+                dxGeom* Geom;
+        };
+        dArray<BoxTC> BoxTCCache;
+        static OBBCache defaultBoxCache;
+        
+        struct CapsuleTC : public LSSCache{
+                dxGeom* Geom;
+        };
+        dArray<CapsuleTC> CapsuleTCCache;
+        static LSSCache defaultCapsuleCache;
+#endif // dTRIMESH_OPCODE
+
+#if dTRIMESH_GIMPACT
+    GIM_TRIMESH  m_collision_trimesh;
+#endif  // dTRIMESH_GIMPACT
+};
+
+#if 0
+#include "collision_kernel.h"
+// Fetches a contact
+inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
+        dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
+        return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
+}
+#endif
+
+#if dTRIMESH_OPCODE
+inline void FetchTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
+        VertexPointers VP;
+        TriMesh->Data->Mesh.GetTriangle(VP, Index);
+        for (int i = 0; i < 3; i++){
+                Out[i][0] = VP.Vertex[i]->x;
+                Out[i][1] = VP.Vertex[i]->y;
+                Out[i][2] = VP.Vertex[i]->z;
+                Out[i][3] = 0;
+        }
+}
+
+inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
+        VertexPointers VP;
+        TriMesh->Data->Mesh.GetTriangle(VP, Index);
+        for (int i = 0; i < 3; i++){
+                dVector3 v;
+                v[0] = VP.Vertex[i]->x;
+                v[1] = VP.Vertex[i]->y;
+                v[2] = VP.Vertex[i]->z;
+                v[3] = 0;
+
+                dMULTIPLY0_331(Out[i], Rotation, v);
+                Out[i][0] += Position[0];
+                Out[i][1] += Position[1];
+                Out[i][2] += Position[2];
+                Out[i][3] = 0;
+        }
+}
+
+inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
+        Out.m[0][0] = (float) Rotation[0];
+        Out.m[1][0] = (float) Rotation[1];
+        Out.m[2][0] = (float) Rotation[2];
+
+        Out.m[0][1] = (float) Rotation[4];
+        Out.m[1][1] = (float) Rotation[5];
+        Out.m[2][1] = (float) Rotation[6];
+
+        Out.m[0][2] = (float) Rotation[8];
+        Out.m[1][2] = (float) Rotation[9];
+        Out.m[2][2] = (float) Rotation[10];
+
+        Out.m[3][0] = (float) Position[0];
+        Out.m[3][1] = (float) Position[1];
+        Out.m[3][2] = (float) Position[2];
+
+        Out.m[0][3] = 0.0f;
+        Out.m[1][3] = 0.0f;
+        Out.m[2][3] = 0.0f;
+        Out.m[3][3] = 1.0f;
+
+        return Out;
+}
+
+inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
+        const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
+        const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
+        return MakeMatrix(Position, Rotation, Out);
+}
+#endif // dTRIMESH_OPCODE
+
+#if dTRIMESH_GIMPACT
+inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
+        // why is this not implemented?
+        dAASSERT(false);
+}
+
+inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m)
+{
+        m[0][0] = (float) Rotation[0];
+        m[0][1] = (float) Rotation[1];
+        m[0][2] = (float) Rotation[2];
+
+        m[1][0] = (float) Rotation[4];
+        m[1][1] = (float) Rotation[5];
+        m[1][2] = (float) Rotation[6];
+
+        m[2][0] = (float) Rotation[8];
+        m[2][1] = (float) Rotation[9];
+        m[2][2] = (float) Rotation[10];
+
+        m[0][3] = (float) Position[0];
+        m[1][3] = (float) Position[1];
+        m[2][3] = (float) Position[2];
+
+}
+
+inline void MakeMatrix(dxGeom* g, mat4f Out){
+        const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
+        const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
+        MakeMatrix(Position, Rotation, Out);
+}
+#endif // dTRIMESH_GIMPACT
+
+// Outputs a matrix to 3 vectors
+inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
+        Right[0] = Matrix[0 * 4 + 0];
+        Right[1] = Matrix[1 * 4 + 0];
+        Right[2] = Matrix[2 * 4 + 0];
+        Right[3] = REAL(0.0);
+        Up[0] = Matrix[0 * 4 + 1];
+        Up[1] = Matrix[1 * 4 + 1];
+        Up[2] = Matrix[2 * 4 + 1];
+        Up[3] = REAL(0.0);
+        Direction[0] = Matrix[0 * 4 + 2];
+        Direction[1] = Matrix[1 * 4 + 2];
+        Direction[2] = Matrix[2 * 4 + 2];
+        Direction[3] = REAL(0.0);
+}
+
+// Outputs a matrix to 3 vectors
+inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
+        Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
+}
+
+// Finds barycentric
+inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
+        dReal w = REAL(1.0) - u - v;
+
+        Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
+        Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
+        Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
+        Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
+}
+
+// Performs a callback
+inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
+        if (TriMesh->Callback != NULL){
+                return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0);
+        }
+        else return true;
+}
+
+// Some utilities
+template<class T> const T& dcMAX(const T& x, const T& y){
+        return x > y ? x : y;
+}
+
+template<class T> const T& dcMIN(const T& x, const T& y){
+        return x < y ? x : y;
+}
+
+dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, 
+                           const dVector3 triEdge1, const dVector3 triEdge2,
+                           dReal* pfSParam = 0, dReal* pfTParam = 0 );
+
+dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, 
+                           const dVector3 seg2Origin, const dVector3 seg2Direction,
+                           dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 );
+
+dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, 
+                         const dVector3 triOrigin, 
+                         const dVector3 triEdge1, const dVector3 triEdge2,
+                         dReal* t = 0, dReal* u = 0, dReal* v = 0 );
+
+inline
+void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result )
+{
+    result[0] = left[0] - right[0];
+    result[1] = left[1] - right[1];
+    result[2] = left[2] - right[2];
+    result[3] = REAL(0.0);
+}
+
+inline
+void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
+{
+    result[0] = left[0] + right[0];
+    result[1] = left[1] + right[1];
+    result[2] = left[2] + right[2];
+    result[3] = REAL(0.0);
+}
+
+inline
+void Vector3Negate( const dVector3 in, dVector3 out )
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+    out[2] = -in[2];
+    out[3] = REAL(0.0);
+}
+
+inline
+void Vector3Copy( const dVector3 in, dVector3 out )
+{
+    out[0] = in[0];
+    out[1] = in[1];
+    out[2] = in[2];
+    out[3] = REAL(0.0);
+}
+
+inline
+void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
+{
+    out[0] = in[0] * scalar;
+    out[1] = in[1] * scalar;
+    out[2] = in[2] * scalar;
+    out[3] = REAL(0.0);
+}
+
+inline
+void TransformVector3( const dVector3 in, 
+                       const dMatrix3 orientation, const dVector3 position, 
+                       dVector3 out )
+{
+    dMULTIPLY0_331( out, orientation, in );
+    out[0] += position[0];
+    out[1] += position[1];
+    out[2] += position[2];
+}
+
+//------------------------------------------------------------------------------
+/**
+  @brief Check for intersection between triangle and capsule.
+  
+  @param dist [out] Shortest distance squared between the triangle and 
+                    the capsule segment (central axis).
+  @param t    [out] t value of point on segment that's the shortest distance 
+                    away from the triangle, the coordinates of this point 
+                    can be found by (cap.seg.end - cap.seg.start) * t,
+                    or cap.seg.ipol(t).
+  @param u    [out] Barycentric coord on triangle.
+  @param v    [out] Barycentric coord on triangle.
+  @return True if intersection exists.
+  
+  The third Barycentric coord is implicit, ie. w = 1.0 - u - v
+  The Barycentric coords give the location of the point on the triangle
+  closest to the capsule (where the distance between the two shapes
+  is the shortest).
+*/
+inline
+bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd, 
+                          const dReal radius, const dVector3 triOrigin, 
+                          const dVector3 triEdge0, const dVector3 triEdge1,
+                          dReal* dist, dReal* t, dReal* u, dReal* v )
+{
+    dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1, 
+                                       t, u, v );
+  
+    if ( dist )
+      *dist = sqrDist;
+    
+    return ( sqrDist <= (radius * radius) );
+}
+
+#endif  //TRIMESH_INTERNAL
+
+#endif  //_ODE_COLLISION_TRIMESH_INTERNAL_H_