resubmitting ode

1 files changed, 833 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp
new file mode 100644
index 0000000..07f3f8a
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_opcode.cpp
@@ -0,0 +1,833 @@
+/*************************************************************************
+ *                                                                       *
+ * 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/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_util.h"
+#define TRIMESH_INTERNAL
+#include "collision_trimesh_internal.h"
+
+#if dTRIMESH_ENABLED
+#if dTRIMESH_OPCODE
+
+// Trimesh data
+dxTriMeshData::dxTriMeshData() : UseFlags( NULL )
+{
+#if !dTRIMESH_ENABLED
+  dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work");
+#endif
+}
+
+dxTriMeshData::~dxTriMeshData()
+{
+        if ( UseFlags )
+                delete [] UseFlags;
+}
+
+void 
+dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount,
+                     const void* Indices, int IndexCount, int TriStride,
+                     const void* in_Normals,
+                     bool Single)
+{
+#if dTRIMESH_ENABLED
+
+    Mesh.SetNbTriangles(IndexCount / 3);
+    Mesh.SetNbVertices(VertexCount);
+    Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices);
+    Mesh.SetStrides(TriStride, VertexStide);
+    Mesh.Single = Single;
+    
+    // Build tree
+    BuildSettings Settings;
+    // recommended in Opcode User Manual
+    //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER;
+    // used in ODE, why?
+    //Settings.mRules = SPLIT_BEST_AXIS;
+
+    // best compromise?
+    Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER;
+
+
+    OPCODECREATE TreeBuilder;
+    TreeBuilder.mIMesh = &Mesh;
+
+    TreeBuilder.mSettings = Settings;
+    TreeBuilder.mNoLeaf = true;
+    TreeBuilder.mQuantized = false;
+
+    TreeBuilder.mKeepOriginal = false;
+    TreeBuilder.mCanRemap = false;
+
+
+
+    BVTree.Build(TreeBuilder);
+
+    // compute model space AABB
+    dVector3 AABBMax, AABBMin;
+    AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity;
+    AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity;
+    if( Single ) {
+        const char* verts = (const char*)Vertices;
+        for( int i = 0; i < VertexCount; ++i ) {
+        const float* v = (const float*)verts;
+        if( v[0] > AABBMax[0] ) AABBMax[0] = v[0];
+        if( v[1] > AABBMax[1] ) AABBMax[1] = v[1];
+        if( v[2] > AABBMax[2] ) AABBMax[2] = v[2];
+        if( v[0] < AABBMin[0] ) AABBMin[0] = v[0];
+        if( v[1] < AABBMin[1] ) AABBMin[1] = v[1];
+        if( v[2] < AABBMin[2] ) AABBMin[2] = v[2];
+        verts += VertexStide;
+        }
+    } else {
+        const char* verts = (const char*)Vertices;
+        for( int i = 0; i < VertexCount; ++i ) {
+        const double* v = (const double*)verts;
+        if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0];
+        if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1];
+        if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2];
+        if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0];
+        if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1];
+        if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2];
+        verts += VertexStide;
+        }
+    }
+    AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5);
+    AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5);
+    AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5);
+    AABBExtents[0] = AABBMax[0] - AABBCenter[0];
+    AABBExtents[1] = AABBMax[1] - AABBCenter[1];
+    AABBExtents[2] = AABBMax[2] - AABBCenter[2];
+
+    // user data (not used by OPCODE)
+    Normals = (dReal *) in_Normals;
+
+        UseFlags = 0;
+
+#endif // dTRIMESH_ENABLED
+}
+
+struct EdgeRecord
+{
+        int VertIdx1;   // Index into vertex array for this edges vertices
+        int VertIdx2;
+        int TriIdx;             // Index into triangle array for triangle this edge belongs to
+
+        uint8 EdgeFlags;        
+        uint8 Vert1Flags;
+        uint8 Vert2Flags;
+        bool Concave;
+};
+
+// Edge comparison function for qsort
+static int EdgeCompare(const void* edge1, const void* edge2)
+{
+        EdgeRecord* e1 = (EdgeRecord*)edge1;
+        EdgeRecord* e2 = (EdgeRecord*)edge2;
+
+        if (e1->VertIdx1 == e2->VertIdx1)
+                return e1->VertIdx2 - e2->VertIdx2;
+        else
+                return e1->VertIdx1 - e2->VertIdx1;
+}
+
+void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const unsigned int* vertIdxs)
+{
+        if (edgeIdx == 0)
+        {
+                edge->EdgeFlags  = dxTriMeshData::kEdge0;
+                edge->Vert1Flags = dxTriMeshData::kVert0;
+                edge->Vert2Flags = dxTriMeshData::kVert1;
+                edge->VertIdx1 = vertIdxs[0];
+                edge->VertIdx2 = vertIdxs[1];
+        }
+        else if (edgeIdx == 1)
+        {
+                edge->EdgeFlags  = dxTriMeshData::kEdge1;
+                edge->Vert1Flags = dxTriMeshData::kVert1;
+                edge->Vert2Flags = dxTriMeshData::kVert2;
+                edge->VertIdx1 = vertIdxs[1];
+                edge->VertIdx2 = vertIdxs[2];
+        }
+        else if (edgeIdx == 2)
+        {
+                edge->EdgeFlags  = dxTriMeshData::kEdge2;
+                edge->Vert1Flags = dxTriMeshData::kVert2;
+                edge->Vert2Flags = dxTriMeshData::kVert0;
+                edge->VertIdx1 = vertIdxs[2];
+                edge->VertIdx2 = vertIdxs[0];
+        }
+
+        // Make sure vert index 1 is less than index 2 (for easier sorting)
+        if (edge->VertIdx1 > edge->VertIdx2)
+        {
+                unsigned int tempIdx = edge->VertIdx1;
+                edge->VertIdx1 = edge->VertIdx2;
+                edge->VertIdx2 = tempIdx;
+
+                uint8 tempFlags = edge->Vert1Flags;
+                edge->Vert1Flags = edge->Vert2Flags;
+                edge->Vert2Flags = tempFlags;
+        }
+
+        edge->TriIdx = triIdx;
+        edge->Concave = false;
+}
+
+#if dTRIMESH_ENABLED
+
+// Get the vertex opposite this edge in the triangle
+inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[])
+{
+        if ((edge->Vert1Flags == dxTriMeshData::kVert0 && edge->Vert2Flags == dxTriMeshData::kVert1) ||
+                (edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert0))
+        {
+                return *vertices[2];
+        }
+        else if ((edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert2) ||
+                (edge->Vert1Flags == dxTriMeshData::kVert2 && edge->Vert2Flags == dxTriMeshData::kVert1))
+        {
+                return *vertices[0];
+        }
+        else
+                return *vertices[1];
+}
+
+#endif // dTRIMESH_ENABLED
+
+void dxTriMeshData::Preprocess()
+{
+
+#if dTRIMESH_ENABLED
+
+        // If this mesh has already been preprocessed, exit
+        if (UseFlags)
+                return;
+
+        udword numTris = Mesh.GetNbTriangles();
+        udword numEdges = numTris * 3;
+
+        UseFlags = new uint8[numTris];
+        memset(UseFlags, 0, sizeof(uint8) * numTris);
+
+        EdgeRecord* records = new EdgeRecord[numEdges];
+
+        // Make a list of every edge in the mesh
+        const IndexedTriangle* tris = Mesh.GetTris();
+    for (unsigned int i = 0; i < numTris; i++)
+        {
+                SetupEdge(&records[i*3],   0, i, tris->mVRef);
+                SetupEdge(&records[i*3+1], 1, i, tris->mVRef);
+                SetupEdge(&records[i*3+2], 2, i, tris->mVRef);
+
+                tris = (const IndexedTriangle*)(((uint8*)tris) + Mesh.GetTriStride());
+        }
+
+        // Sort the edges, so the ones sharing the same verts are beside each other
+        qsort(records, numEdges, sizeof(EdgeRecord), EdgeCompare);
+
+        // Go through the sorted list of edges and flag all the edges and vertices that we need to use
+        for (unsigned int i = 0; i < numEdges; i++)
+        {
+                EdgeRecord* rec1 = &records[i];
+                EdgeRecord* rec2 = 0;
+                if (i < numEdges - 1)
+                        rec2 = &records[i+1];
+
+                if (rec2 &&
+                        rec1->VertIdx1 == rec2->VertIdx1 &&
+                        rec1->VertIdx2 == rec2->VertIdx2)
+                {
+                        VertexPointers vp;
+                        Mesh.GetTriangle(vp, rec1->TriIdx);
+
+                        // Get the normal of the first triangle
+                        Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]);
+                        triNorm.Normalize();
+
+                        // Get the vert opposite this edge in the first triangle
+                        Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex);
+
+                        // Get the vert opposite this edge in the second triangle
+                        Mesh.GetTriangle(vp, rec2->TriIdx);
+                        Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex);
+
+                        float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize());
+
+                        // We let the dot threshold for concavity get slightly negative to allow for rounding errors
+                        static const float kConcaveThresh = -0.000001f;
+
+                        // This is a concave edge, leave it for the next pass
+                        if (dot >= kConcaveThresh)
+                                rec1->Concave = true;
+                        // If this is a convex edge, mark its vertices and edge as used
+                        else
+                                UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
+
+                        // Skip the second edge
+                        i++;
+                }
+                // This is a boundary edge
+                else
+                {
+                        UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
+                }
+        }
+
+        // Go through the list once more, and take any edge we marked as concave and
+        // clear it's vertices flags in any triangles they're used in
+        for (unsigned int i = 0; i < numEdges; i++)
+        {
+                EdgeRecord& er = records[i];
+
+                if (er.Concave)
+                {
+                        for (unsigned int j = 0; j < numEdges; j++)
+                        {
+                                EdgeRecord& curER = records[j];
+
+                                if (curER.VertIdx1 == er.VertIdx1 ||
+                                        curER.VertIdx1 == er.VertIdx2)
+                                        UseFlags[curER.TriIdx] &= ~curER.Vert1Flags;
+
+                                if (curER.VertIdx2 == er.VertIdx1 ||
+                                        curER.VertIdx2 == er.VertIdx2)
+                                        UseFlags[curER.TriIdx] &= ~curER.Vert2Flags;
+                        }
+                }
+        }
+
+        delete [] records;
+
+#endif // dTRIMESH_ENABLED
+
+}
+
+dTriMeshDataID dGeomTriMeshDataCreate(){
+    return new dxTriMeshData();
+}
+
+void dGeomTriMeshDataDestroy(dTriMeshDataID g){
+    delete g;
+}
+
+
+
+
+void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans )
+{
+        dAASSERT(g)
+    dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
+
+    for (int i=0; i<16; i++)
+        (((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ];
+
+    return;
+}
+
+
+dReal* dGeomTriMeshGetLastTransform( dxGeom* g )
+{
+        dAASSERT(g)
+    dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
+
+    return (dReal*)(((dxTriMesh*)g)->last_trans);
+}
+
+
+
+
+void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
+{
+    dUASSERT(g, "argument not trimesh data");
+
+    switch (data_id)
+        {
+    case TRIMESH_FACE_NORMALS:
+                g->Normals = (dReal *) in_data;
+                break;
+
+    default:
+                dUASSERT(data_id, "invalid data type");
+                break;
+    }
+
+    return;
+}
+
+
+
+void*  dGeomTriMeshDataGet(dTriMeshDataID g, int data_id)
+{
+    dUASSERT(g, "argument not trimesh data");
+
+    switch (data_id)
+        {
+    case TRIMESH_FACE_NORMALS:
+        return (void *) g->Normals;
+        break;
+
+        default:
+        dUASSERT(data_id, "invalid data type");
+        break;
+    }
+    
+    return NULL;
+}
+
+
+void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
+                                  const void* Vertices, int VertexStride, int VertexCount, 
+                                  const void* Indices, int IndexCount, int TriStride,
+                                  const void* Normals)
+{
+    dUASSERT(g, "argument not trimesh data");
+    
+    g->Build(Vertices, VertexStride, VertexCount, 
+             Indices, IndexCount, TriStride, 
+             Normals, 
+             true);
+}
+
+
+void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
+                                 const void* Vertices, int VertexStride, int VertexCount, 
+                                 const void* Indices, int IndexCount, int TriStride)
+{
+    dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
+                                 Indices, IndexCount, TriStride, (void*)NULL);
+}
+
+
+void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
+                                  const void* Vertices, int VertexStride, int VertexCount, 
+                                 const void* Indices, int IndexCount, int TriStride,
+                                 const void* Normals)
+{
+    dUASSERT(g, "argument not trimesh data");
+    
+    g->Build(Vertices, VertexStride, VertexCount, 
+             Indices, IndexCount, TriStride, 
+             Normals, 
+             false);
+}
+
+
+void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
+                                 const void* Vertices, int VertexStride, int VertexCount, 
+                                 const void* Indices, int IndexCount, int TriStride) {
+    dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
+                                 Indices, IndexCount, TriStride, NULL);
+}
+
+
+void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
+                                  const dReal* Vertices, int VertexCount, 
+                                 const int* Indices, int IndexCount,
+                                 const int* Normals){
+#ifdef dSINGLE
+    dGeomTriMeshDataBuildSingle1(g,
+                                Vertices, 4 * sizeof(dReal), VertexCount, 
+                                Indices, IndexCount, 3 * sizeof(unsigned int),
+                                Normals);
+#else
+    dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, 
+                                Indices, IndexCount, 3 * sizeof(unsigned int),
+                                Normals);
+#endif
+}
+
+
+void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
+                                 const dReal* Vertices, int VertexCount, 
+                                 const int* Indices, int IndexCount) {
+    dGeomTriMeshDataBuildSimple1(g,
+                                 Vertices, VertexCount, Indices, IndexCount,
+                                 (const int*)NULL);
+}
+
+void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
+{
+    dUASSERT(g, "argument not trimesh data");
+        g->Preprocess();
+}
+
+void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
+{
+    dUASSERT(g, "argument not trimesh data");
+#if dTRIMESH_ENABLED
+        *buf = g->UseFlags;
+        *bufLen = g->Mesh.GetNbTriangles();
+#endif // dTRIMESH_ENABLED
+}
+
+void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
+{
+    dUASSERT(g, "argument not trimesh data");
+        g->UseFlags = buf;
+}
+
+
+#if dTRIMESH_ENABLED
+
+// Trimesh Class Statics
+PlanesCollider dxTriMesh::_PlanesCollider;
+SphereCollider dxTriMesh::_SphereCollider;
+OBBCollider dxTriMesh::_OBBCollider;
+RayCollider dxTriMesh::_RayCollider;
+AABBTreeCollider dxTriMesh::_AABBTreeCollider;
+LSSCollider dxTriMesh::_LSSCollider;
+
+SphereCache dxTriMesh::defaultSphereCache;
+OBBCache dxTriMesh::defaultBoxCache;
+LSSCache dxTriMesh::defaultCapsuleCache;
+
+CollisionFaces dxTriMesh::Faces;
+
+#endif // dTRIMESH_ENABLED
+
+
+dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1)
+{
+    type = dTriMeshClass;
+
+    this->Data = Data;
+
+#if dTRIMESH_ENABLED
+
+        _RayCollider.SetDestination(&Faces);
+
+    _PlanesCollider.SetTemporalCoherence(true);
+
+        _SphereCollider.SetTemporalCoherence(true);
+        _SphereCollider.SetPrimitiveTests(false);
+
+    _OBBCollider.SetTemporalCoherence(true);
+
+    // no first-contact test (i.e. return full contact info)
+        _AABBTreeCollider.SetFirstContact( false );     
+    // temporal coherence only works with "first conact" tests
+    _AABBTreeCollider.SetTemporalCoherence(false);
+    // Perform full BV-BV tests (true) or SAT-lite tests (false)
+        _AABBTreeCollider.SetFullBoxBoxTest( true );
+    // Perform full Primitive-BV tests (true) or SAT-lite tests (false)
+        _AABBTreeCollider.SetFullPrimBoxTest( true );
+        _LSSCollider.SetTemporalCoherence(false);
+
+#endif // dTRIMESH_ENABLED
+
+        /* TC has speed/space 'issues' that don't make it a clear
+           win by default on spheres/boxes. */
+        this->doSphereTC = false;
+        this->doBoxTC = false;
+        this->doCapsuleTC = false;
+
+#if dTRIMESH_ENABLED
+
+    const char* msg;
+    if ((msg =_AABBTreeCollider.ValidateSettings()))
+        dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__);
+        _LSSCollider.SetPrimitiveTests(false);
+        _LSSCollider.SetFirstContact(false);
+
+#endif // dTRIMESH_ENABLED
+
+    for (int i=0; i<16; i++)
+        last_trans[i] = REAL( 0.0 );
+}
+
+dxTriMesh::~dxTriMesh(){
+    //
+}
+
+// Cleanup for allocations when shutting down ODE
+void opcode_collider_cleanup()
+{
+#if dTRIMESH_ENABLED
+        
+        // Clear TC caches
+        dxTriMesh::Faces.Empty();
+        dxTriMesh::defaultSphereCache.TouchedPrimitives.Empty();
+        dxTriMesh::defaultBoxCache.TouchedPrimitives.Empty();
+        dxTriMesh::defaultCapsuleCache.TouchedPrimitives.Empty();
+
+#endif // dTRIMESH_ENABLED
+}
+
+
+
+void dxTriMesh::ClearTCCache()
+{
+#if dTRIMESH_ENABLED
+  /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches -
+     but the destructor isn't called when doing this, so we would leak.
+     So, call the previous caches' containers' destructors by hand first. */
+    int i, n;
+    n = SphereTCCache.size();
+    for( i = 0; i < n; ++i ) {
+        SphereTCCache[i].~SphereTC();
+    }
+    SphereTCCache.setSize(0);
+    n = BoxTCCache.size();
+    for( i = 0; i < n; ++i ) {
+        BoxTCCache[i].~BoxTC();
+    }
+    BoxTCCache.setSize(0);
+        n = CapsuleTCCache.size();
+        for( i = 0; i < n; ++i ) {
+          CapsuleTCCache[i].~CapsuleTC();
+        }
+        CapsuleTCCache.setSize(0);
+#endif // dTRIMESH_ENABLED
+}
+
+
+int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
+    return 1;
+}
+
+
+void dxTriMesh::computeAABB() {
+    const dxTriMeshData* d = Data;
+    dVector3 c;
+    const dMatrix3& R = final_posr->R;
+    const dVector3& pos = final_posr->pos;
+    
+    dMULTIPLY0_331( c, R, d->AABBCenter );
+    
+    dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
+        dFabs(R[1] * Data->AABBExtents[1]) + 
+        dFabs(R[2] * Data->AABBExtents[2]);
+    dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
+        dFabs(R[5] * Data->AABBExtents[1]) + 
+        dFabs(R[6] * Data->AABBExtents[2]);
+    dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
+        dFabs(R[9] * Data->AABBExtents[1]) + 
+        dFabs(R[10] * Data->AABBExtents[2]);
+
+    aabb[0] = c[0] + pos[0] - xrange;
+    aabb[1] = c[0] + pos[0] + xrange;
+    aabb[2] = c[1] + pos[1] - yrange;
+    aabb[3] = c[1] + pos[1] + yrange;
+    aabb[4] = c[2] + pos[2] - zrange;
+    aabb[5] = c[2] + pos[2] + zrange;
+}
+
+
+void dxTriMeshData::UpdateData()
+{
+#if  dTRIMESH_ENABLED
+        BVTree.Refit();
+#endif // dTRIMESH_ENABLED
+}
+
+
+dGeomID dCreateTriMesh(dSpaceID space, 
+                       dTriMeshDataID Data,
+                       dTriCallback* Callback,
+                       dTriArrayCallback* ArrayCallback,
+                       dTriRayCallback* RayCallback)
+{
+    dxTriMesh* Geom = new dxTriMesh(space, Data);
+    Geom->Callback = Callback;
+    Geom->ArrayCallback = ArrayCallback;
+    Geom->RayCallback = RayCallback;
+
+    return Geom;
+}
+
+void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+        ((dxTriMesh*)g)->Callback = Callback;
+}
+
+dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+        return ((dxTriMesh*)g)->Callback;
+}
+
+void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+        ((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
+}
+
+dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+        return ((dxTriMesh*)g)->ArrayCallback;
+}
+
+void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+        ((dxTriMesh*)g)->RayCallback = Callback;
+}
+
+dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");      
+        return ((dxTriMesh*)g)->RayCallback;
+}
+
+void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+        ((dxTriMesh*)g)->Data = Data;
+        // I changed my data -- I know nothing about my own AABB anymore.
+        ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD);
+}
+
+dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
+{
+  dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+  return ((dxTriMesh*)g)->Data;
+}
+
+
+
+void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+  
+        switch (geomClass)
+        {
+                case dSphereClass: 
+                        ((dxTriMesh*)g)->doSphereTC = (1 == enable);
+                        break;
+                case dBoxClass:
+                        ((dxTriMesh*)g)->doBoxTC = (1 == enable);
+                        break;
+                case dCapsuleClass:
+                        ((dxTriMesh*)g)->doCapsuleTC = (1 == enable);
+                        break;
+        }
+}
+
+int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
+{
+        dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+  
+        switch (geomClass)
+        {
+                case dSphereClass:
+                        if (((dxTriMesh*)g)->doSphereTC)
+                                return 1;
+                        break;
+                case dBoxClass:
+                        if (((dxTriMesh*)g)->doBoxTC)
+                                return 1;
+                        break;
+                case dCapsuleClass:
+                        if (((dxTriMesh*)g)->doCapsuleTC)
+                                return 1;
+                        break;
+        }
+        return 0;
+}
+
+void dGeomTriMeshClearTCCache(dGeomID g){
+    dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+
+    dxTriMesh* Geom = (dxTriMesh*)g;
+    Geom->ClearTCCache();
+}
+
+/*
+ * returns the TriMeshDataID
+ */
+dTriMeshDataID
+dGeomTriMeshGetTriMeshDataID(dGeomID g)
+{
+    dxTriMesh* Geom = (dxTriMesh*) g;
+    return Geom->Data;
+}
+
+// Getting data
+void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){
+    dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+
+    dxTriMesh* Geom = (dxTriMesh*)g;
+
+    const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
+    const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
+
+    dVector3 v[3];
+        FetchTriangle(Geom, Index, Position, Rotation, v);
+
+    if (v0){
+        (*v0)[0] = v[0][0];
+        (*v0)[1] = v[0][1];
+        (*v0)[2] = v[0][2];
+        (*v0)[3] = v[0][3];
+    }
+    if (v1){
+        (*v1)[0] = v[1][0];
+        (*v1)[1] = v[1][1];
+        (*v1)[2] = v[1][2];
+        (*v1)[3] = v[1][3];
+    }
+    if (v2){
+        (*v2)[0] = v[2][0];
+        (*v2)[1] = v[2][1];
+        (*v2)[2] = v[2][2];
+        (*v2)[3] = v[2][3];
+    }
+}
+
+void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
+    dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+
+    dxTriMesh* Geom = (dxTriMesh*)g;
+
+    const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
+    const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
+
+    dVector3 dv[3];
+    FetchTriangle(Geom, Index, Position, Rotation, dv);
+
+    GetPointFromBarycentric(dv, u, v, Out);
+}
+
+int dGeomTriMeshGetTriangleCount (dGeomID g)            
+{               
+#if dTRIMESH_ENABLED
+    dxTriMesh* Geom = (dxTriMesh*)g;            
+    return Geom->Data->Mesh.GetNbTriangles();           
+#else
+        return 0;
+#endif // dTRIMESH_ENABLED
+}
+
+void dGeomTriMeshDataUpdate(dTriMeshDataID g) {
+    dUASSERT(g, "argument not trimesh data");
+    g->UpdateData();
+}
+
+#endif // dTRIMESH_OPCODE
+#endif // dTRIMESH_ENABLED