add ode

1 files changed, 1380 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_box.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_box.cpp
new file mode 100644
index 0000000..521ed43
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/collision_trimesh_box.cpp
@@ -0,0 +1,1380 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+/*************************************************************************
+ *                                                                       *
+ * Triangle-box collider by Alen Ladavac and Vedran Klanac.              *
+ * Ported to ODE by Oskari Nyman.                                        *
+ *                                                                       *
+ *************************************************************************/
+
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_util.h"
+#include "collision_trimesh_internal.h"
+
+#if dTRIMESH_ENABLED
+
+
+// largest number, double or float
+#if defined(dSINGLE)
+#define MAXVALUE FLT_MAX
+#else
+#define MAXVALUE DBL_MAX
+#endif
+
+
+// dVector3
+// r=a-b
+#define SUBTRACT(a,b,r) dSubtractVectors3(r, a, b)
+
+
+// dVector3
+// a=b
+#define SET(a,b) dCopyVector3(a, b)
+
+
+// dMatrix3
+// a=b
+#define SETM(a,b) dCopyMatrix4x4(a, b)
+
+
+// dVector3
+// r=a+b
+#define ADD(a,b,r) dAddVectors3(r, a, b)
+
+
+// dMatrix3, int, dVector3
+// v=column a from m
+#define GETCOL(m,a,v) dGetMatrixColumn3(v, m, a)
+
+
+// dVector4, dVector3
+// distance between plane p and point v
+#define POINTDISTANCE(p,v) dPointPlaneDistance(v, p)
+
+
+// dVector4, dVector3, dReal
+// construct plane from normal and d
+#define CONSTRUCTPLANE(plane,normal,d) dConstructPlane(normal, d, plane)
+
+
+// dVector3
+// length of vector a
+#define LENGTHOF(a) dCalcVectorLength3(a)
+
+
+struct sTrimeshBoxColliderData
+{
+    sTrimeshBoxColliderData(): m_iBestAxis(0), m_iExitAxis(0), m_ctContacts(0) {}
+
+    void SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom,
+        int Flags, dContactGeom* Contacts, int Stride);
+    void TestCollisionForSingleTriangle(int Triint, dVector3 dv[3], bool &bOutFinishSearching);
+
+    bool _cldTestNormal(dReal fp0, dReal fR, dVector3 vNormal, int iAxis);
+    bool _cldTestFace(dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD,
+        dVector3 vNormal, int iAxis);
+    bool _cldTestEdge(dReal fp0, dReal fp1, dReal fR, dReal fD,
+        dVector3 vNormal, int iAxis);
+    bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2);
+    void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex);
+    bool _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex);
+
+    void GenerateContact(int TriIndex, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth);
+
+    // box data
+    dMatrix3 m_mHullBoxRot;
+    dVector3 m_vHullBoxPos;
+    dVector3 m_vBoxHalfSize;
+
+    // mesh data
+    dVector3   m_vHullDstPos;
+
+    // global collider data
+    dVector3 m_vBestNormal;
+    dReal    m_fBestDepth;
+    int    m_iBestAxis;
+    int    m_iExitAxis;
+    dVector3 m_vE0, m_vE1, m_vE2, m_vN;
+
+    // global info for contact creation
+    int m_iFlags;
+    dContactGeom *m_ContactGeoms;
+    int m_iStride;
+    dxGeom *m_Geom1;
+    dxGeom *m_Geom2;
+    int m_ctContacts;
+};
+
+// Test normal of mesh face as separating axis for intersection
+bool sTrimeshBoxColliderData::_cldTestNormal(dReal fp0, dReal fR, dVector3 vNormal, int iAxis)
+{
+    // calculate overlapping interval of box and triangle
+    dReal fDepth = fR+fp0;
+
+    // if we do not overlap
+    if ( fDepth<0 ) {
+        // do nothing
+        return false;
+    }
+
+    // calculate normal's length
+    dReal fLength = LENGTHOF(vNormal);
+    // if long enough
+    if ( fLength > 0.0f ) {
+
+        dReal fOneOverLength = 1.0f/fLength;
+        // normalize depth
+        fDepth = fDepth*fOneOverLength;
+
+        // get minimum depth
+        if (fDepth < m_fBestDepth) {
+            m_vBestNormal[0] = -vNormal[0]*fOneOverLength;
+            m_vBestNormal[1] = -vNormal[1]*fOneOverLength;
+            m_vBestNormal[2] = -vNormal[2]*fOneOverLength;
+            m_iBestAxis = iAxis;
+            //dAASSERT(fDepth>=0);
+            m_fBestDepth = fDepth;
+        }
+    }
+
+    return true;
+}
+
+
+
+
+// Test box axis as separating axis
+bool sTrimeshBoxColliderData::_cldTestFace(dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD,
+                                           dVector3 vNormal, int iAxis)
+{
+    dReal fMin, fMax;
+
+    // find min of triangle interval
+    if ( fp0 < fp1 ) {
+        if ( fp0 < fp2 ) {
+            fMin = fp0;
+        } else {
+            fMin = fp2;
+        }
+    } else {
+        if( fp1 < fp2 ) {
+            fMin = fp1;
+        } else {
+            fMin = fp2;
+        }
+    }
+
+    // find max of triangle interval
+    if ( fp0 > fp1 ) {
+        if ( fp0 > fp2 ) {
+            fMax = fp0;
+        } else {
+            fMax = fp2;
+        }
+    } else {
+        if( fp1 > fp2 ) {
+            fMax = fp1;
+        } else {
+            fMax = fp2;
+        }
+    }
+
+    // calculate minimum and maximum depth
+    dReal fDepthMin = fR - fMin;
+    dReal fDepthMax = fMax + fR;
+
+    // if we dont't have overlapping interval
+    if ( fDepthMin < 0 || fDepthMax < 0 ) {
+        // do nothing
+        return false;
+    }
+
+    dReal fDepth = 0;
+
+    // if greater depth is on negative side
+    if ( fDepthMin > fDepthMax ) {
+        // use smaller depth (one from positive side)
+        fDepth = fDepthMax;
+        // flip normal direction
+        vNormal[0] = -vNormal[0];
+        vNormal[1] = -vNormal[1];
+        vNormal[2] = -vNormal[2];
+        fD = -fD;
+        // if greater depth is on positive side
+    } else {
+        // use smaller depth (one from negative side)
+        fDepth = fDepthMin;
+    }
+
+    // if lower depth than best found so far
+    if (fDepth < m_fBestDepth) {
+        // remember current axis as best axis
+        m_vBestNormal[0]  = vNormal[0];
+        m_vBestNormal[1]  = vNormal[1];
+        m_vBestNormal[2]  = vNormal[2];
+        m_iBestAxis    = iAxis;
+        //dAASSERT(fDepth>=0);
+        m_fBestDepth   = fDepth;
+    }
+
+    return true;
+}
+
+// Test cross products of box axis and triangle edges as separating axis
+bool sTrimeshBoxColliderData::_cldTestEdge(dReal fp0, dReal fp1, dReal fR, dReal fD,
+                                           dVector3 vNormal, int iAxis)
+{
+    dReal fMin, fMax;
+
+    // ===== Begin Patch by Francisco Leon, 2006/10/28 =====
+
+    // Fixed Null Normal. This prevents boxes passing
+    // through trimeshes at certain contact angles
+
+    fMin = vNormal[0] * vNormal[0] +
+        vNormal[1] * vNormal[1] +
+        vNormal[2] * vNormal[2];
+
+    if ( fMin <= dEpsilon ) /// THIS NORMAL WOULD BE DANGEROUS
+        return true;
+
+    // ===== Ending Patch by Francisco Leon =====
+
+
+    // calculate min and max interval values
+    if ( fp0 < fp1 ) {
+        fMin = fp0;
+        fMax = fp1;
+    } else {
+        fMin = fp1;
+        fMax = fp0;
+    }
+
+    // check if we overlapp
+    dReal fDepthMin = fR - fMin;
+    dReal fDepthMax = fMax + fR;
+
+    // if we don't overlapp
+    if ( fDepthMin < 0 || fDepthMax < 0 ) {
+        // do nothing
+        return false;
+    }
+
+    dReal fDepth;
+
+    // if greater depth is on negative side
+    if ( fDepthMin > fDepthMax ) {
+        // use smaller depth (one from positive side)
+        fDepth = fDepthMax;
+        // flip normal direction
+        vNormal[0] = -vNormal[0];
+        vNormal[1] = -vNormal[1];
+        vNormal[2] = -vNormal[2];
+        fD = -fD;
+        // if greater depth is on positive side
+    } else {
+        // use smaller depth (one from negative side)
+        fDepth = fDepthMin;
+    }
+
+    // calculate normal's length
+    dReal fLength = LENGTHOF(vNormal);
+
+    // if long enough
+    if ( fLength > 0.0f ) {
+
+        // normalize depth
+        dReal fOneOverLength = 1.0f/fLength;
+        fDepth = fDepth*fOneOverLength;
+        fD*=fOneOverLength;
+
+        // if lower depth than best found so far (favor face over edges)
+        if (fDepth*1.5f < m_fBestDepth) {
+            // remember current axis as best axis
+            m_vBestNormal[0]  = vNormal[0]*fOneOverLength;
+            m_vBestNormal[1]  = vNormal[1]*fOneOverLength;
+            m_vBestNormal[2]  = vNormal[2]*fOneOverLength;
+            m_iBestAxis    = iAxis;
+            //dAASSERT(fDepth>=0);
+            m_fBestDepth   = fDepth;
+        }
+    }
+
+    return true;
+}
+
+
+// clip polygon with plane and generate new polygon points
+static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn,
+                                dVector3 avArrayOut[], int &ctOut,
+                                const dVector4 &plPlane )
+{
+    // start with no output points
+    ctOut = 0;
+
+    int i0 = ctIn-1;
+
+    // for each edge in input polygon
+    for (int i1=0; i1<ctIn; i0=i1, i1++) {
+
+
+        // calculate distance of edge points to plane
+        dReal fDistance0 = POINTDISTANCE( plPlane ,avArrayIn[i0] );
+        dReal fDistance1 = POINTDISTANCE( plPlane ,avArrayIn[i1] );
+
+
+        // if first point is in front of plane
+        if( fDistance0 >= 0 ) {
+            // emit point
+            avArrayOut[ctOut][0] = avArrayIn[i0][0];
+            avArrayOut[ctOut][1] = avArrayIn[i0][1];
+            avArrayOut[ctOut][2] = avArrayIn[i0][2];
+            ctOut++;
+        }
+
+        // if points are on different sides
+        if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
+
+            // find intersection point of edge and plane
+            dVector3 vIntersectionPoint;
+            vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
+            vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
+            vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
+
+            // emit intersection point
+            avArrayOut[ctOut][0] = vIntersectionPoint[0];
+            avArrayOut[ctOut][1] = vIntersectionPoint[1];
+            avArrayOut[ctOut][2] = vIntersectionPoint[2];
+            ctOut++;
+        }
+    }
+
+}
+
+
+
+
+bool sTrimeshBoxColliderData::_cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
+    // reset best axis
+    m_iBestAxis = 0;
+    m_iExitAxis = -1;
+    m_fBestDepth = MAXVALUE;
+
+    // calculate edges
+    SUBTRACT(v1,v0,m_vE0);
+    SUBTRACT(v2,v0,m_vE1);
+    SUBTRACT(m_vE1,m_vE0,m_vE2);
+
+    // calculate poly normal
+    dCalcVectorCross3(m_vN,m_vE0,m_vE1);
+
+    // calculate length of face normal
+    dReal fNLen = LENGTHOF(m_vN);
+
+    // Even though all triangles might be initially valid, 
+    // a triangle may degenerate into a segment after applying 
+    // space transformation.
+    if (!fNLen) {
+        return false;
+    }
+
+    // extract box axes as vectors
+    dVector3 vA0,vA1,vA2;
+    GETCOL(m_mHullBoxRot,0,vA0);
+    GETCOL(m_mHullBoxRot,1,vA1);
+    GETCOL(m_mHullBoxRot,2,vA2);
+
+    // box halfsizes
+    dReal fa0 = m_vBoxHalfSize[0];
+    dReal fa1 = m_vBoxHalfSize[1];
+    dReal fa2 = m_vBoxHalfSize[2];
+
+    // calculate relative position between box and triangle
+    dVector3 vD;
+    SUBTRACT(v0,m_vHullBoxPos,vD);
+
+    dVector3 vL;
+    dReal fp0, fp1, fp2, fR, fD;
+
+    // Test separating axes for intersection
+    // ************************************************
+    // Axis 1 - Triangle Normal
+    SET(vL,m_vN);
+    fp0  = dCalcVectorDot3(vL,vD);
+    fp1  = fp0;
+    fp2  = fp0;
+    fR=fa0*dFabs( dCalcVectorDot3(m_vN,vA0) ) + fa1 * dFabs( dCalcVectorDot3(m_vN,vA1) ) + fa2 * dFabs( dCalcVectorDot3(m_vN,vA2) );
+
+    if (!_cldTestNormal(fp0, fR, vL, 1)) {
+        m_iExitAxis=1;
+        return false;
+    }
+
+    // ************************************************
+
+    // Test Faces
+    // ************************************************
+    // Axis 2 - Box X-Axis
+    SET(vL,vA0);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 + dCalcVectorDot3(vA0,m_vE0);
+    fp2 = fp0 + dCalcVectorDot3(vA0,m_vE1);
+    fR  = fa0;
+
+    if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 2)) {
+        m_iExitAxis=2;
+        return false;
+    }
+    // ************************************************
+
+    // ************************************************
+    // Axis 3 - Box Y-Axis
+    SET(vL,vA1);
+    fD = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 + dCalcVectorDot3(vA1,m_vE0);
+    fp2 = fp0 + dCalcVectorDot3(vA1,m_vE1);
+    fR  = fa1;
+
+    if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 3)) {
+        m_iExitAxis=3;
+        return false;
+    }
+
+    // ************************************************
+
+    // ************************************************
+    // Axis 4 - Box Z-Axis
+    SET(vL,vA2);
+    fD = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 + dCalcVectorDot3(vA2,m_vE0);
+    fp2 = fp0 + dCalcVectorDot3(vA2,m_vE1);
+    fR  = fa2;
+
+    if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 4)) {
+        m_iExitAxis=4;
+        return false;
+    }
+
+    // ************************************************
+
+    // Test Edges
+    // ************************************************
+    // Axis 5 - Box X-Axis cross Edge0
+    dCalcVectorCross3(vL,vA0,m_vE0);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0;
+    fp2 = fp0 + dCalcVectorDot3(vA0,m_vN);
+    fR  = fa1 * dFabs(dCalcVectorDot3(vA2,m_vE0)) + fa2 * dFabs(dCalcVectorDot3(vA1,m_vE0));
+
+    if (!_cldTestEdge(fp1, fp2, fR, fD, vL, 5)) {
+        m_iExitAxis=5;
+        return false;
+    }
+    // ************************************************
+
+    // ************************************************
+    // Axis 6 - Box X-Axis cross Edge1
+    dCalcVectorCross3(vL,vA0,m_vE1);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 - dCalcVectorDot3(vA0,m_vN);
+    fp2 = fp0;
+    fR  = fa1 * dFabs(dCalcVectorDot3(vA2,m_vE1)) + fa2 * dFabs(dCalcVectorDot3(vA1,m_vE1));
+
+    if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 6)) {
+        m_iExitAxis=6;
+        return false;
+    }
+    // ************************************************
+
+    // ************************************************
+    // Axis 7 - Box X-Axis cross Edge2
+    dCalcVectorCross3(vL,vA0,m_vE2);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 - dCalcVectorDot3(vA0,m_vN);
+    fp2 = fp0 - dCalcVectorDot3(vA0,m_vN);
+    fR  = fa1 * dFabs(dCalcVectorDot3(vA2,m_vE2)) + fa2 * dFabs(dCalcVectorDot3(vA1,m_vE2));
+
+    if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 7)) {
+        m_iExitAxis=7;
+        return false;
+    }
+
+    // ************************************************
+
+    // ************************************************
+    // Axis 8 - Box Y-Axis cross Edge0
+    dCalcVectorCross3(vL,vA1,m_vE0);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0;
+    fp2 = fp0 + dCalcVectorDot3(vA1,m_vN);
+    fR  = fa0 * dFabs(dCalcVectorDot3(vA2,m_vE0)) + fa2 * dFabs(dCalcVectorDot3(vA0,m_vE0));
+
+    if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 8)) {
+        m_iExitAxis=8;
+        return false;
+    }
+
+    // ************************************************
+
+    // ************************************************
+    // Axis 9 - Box Y-Axis cross Edge1
+    dCalcVectorCross3(vL,vA1,m_vE1);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 - dCalcVectorDot3(vA1,m_vN);
+    fp2 = fp0;
+    fR  = fa0 * dFabs(dCalcVectorDot3(vA2,m_vE1)) + fa2 * dFabs(dCalcVectorDot3(vA0,m_vE1));
+
+    if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 9)) {
+        m_iExitAxis=9;
+        return false;
+    }
+
+    // ************************************************
+
+    // ************************************************
+    // Axis 10 - Box Y-Axis cross Edge2
+    dCalcVectorCross3(vL,vA1,m_vE2);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 - dCalcVectorDot3(vA1,m_vN);
+    fp2 = fp0 - dCalcVectorDot3(vA1,m_vN);
+    fR  = fa0 * dFabs(dCalcVectorDot3(vA2,m_vE2)) + fa2 * dFabs(dCalcVectorDot3(vA0,m_vE2));
+
+    if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 10)) {
+        m_iExitAxis=10;
+        return false;
+    }
+
+    // ************************************************
+
+    // ************************************************
+    // Axis 11 - Box Z-Axis cross Edge0
+    dCalcVectorCross3(vL,vA2,m_vE0);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0;
+    fp2 = fp0 + dCalcVectorDot3(vA2,m_vN);
+    fR  = fa0 * dFabs(dCalcVectorDot3(vA1,m_vE0)) + fa1 * dFabs(dCalcVectorDot3(vA0,m_vE0));
+
+    if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 11)) {
+        m_iExitAxis=11;
+        return false;
+    }
+    // ************************************************
+
+    // ************************************************
+    // Axis 12 - Box Z-Axis cross Edge1
+    dCalcVectorCross3(vL,vA2,m_vE1);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 - dCalcVectorDot3(vA2,m_vN);
+    fp2 = fp0;
+    fR  = fa0 * dFabs(dCalcVectorDot3(vA1,m_vE1)) + fa1 * dFabs(dCalcVectorDot3(vA0,m_vE1));
+
+    if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 12)) {
+        m_iExitAxis=12;
+        return false;
+    }
+    // ************************************************
+
+    // ************************************************
+    // Axis 13 - Box Z-Axis cross Edge2
+    dCalcVectorCross3(vL,vA2,m_vE2);
+    fD  = dCalcVectorDot3(vL,m_vN)/fNLen;
+    fp0 = dCalcVectorDot3(vL,vD);
+    fp1 = fp0 - dCalcVectorDot3(vA2,m_vN);
+    fp2 = fp0 - dCalcVectorDot3(vA2,m_vN);
+    fR  = fa0 * dFabs(dCalcVectorDot3(vA1,m_vE2)) + fa1 * dFabs(dCalcVectorDot3(vA0,m_vE2));
+
+    if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 13)) {
+        m_iExitAxis=13;
+        return false;
+    }
+
+    // ************************************************
+    return true;
+}
+
+
+
+
+
+// find two closest points on two lines
+static bool _cldClosestPointOnTwoLines(
+    dVector3 vPoint1, dVector3 vLenVec1, dVector3 vPoint2, dVector3 vLenVec2,
+    dReal &fvalue1, dReal &fvalue2)
+{
+    // calculate denominator
+    dVector3 vp;
+    SUBTRACT(vPoint2,vPoint1,vp);
+    dReal fuaub  = dCalcVectorDot3(vLenVec1,vLenVec2);
+    dReal fq1    = dCalcVectorDot3(vLenVec1,vp);
+    dReal fq2    = -dCalcVectorDot3(vLenVec2,vp);
+    dReal fd     = 1.0f - fuaub * fuaub;
+
+    // if denominator is positive
+    if (fd > 0.0f) {
+        // calculate points of closest approach
+        fd = 1.0f/fd;
+        fvalue1 = (fq1 + fuaub*fq2)*fd;
+        fvalue2 = (fuaub*fq1 + fq2)*fd;
+        return true;
+        // otherwise
+    } else {
+        // lines are parallel
+        fvalue1 = 0.0f;
+        fvalue2 = 0.0f;
+        return false;
+    }
+}
+
+
+
+
+
+// clip and generate contacts
+void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex) {
+    dIASSERT( !(m_iFlags & CONTACTS_UNIMPORTANT) || m_ctContacts < (m_iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results
+
+    // if we have edge/edge intersection
+    if (m_iBestAxis > 4 ) {
+        dVector3 vub,vPb,vPa;
+
+        SET(vPa,m_vHullBoxPos);
+
+        // calculate point on box edge
+        for( int i=0; i<3; i++) {
+            dVector3 vRotCol;
+            GETCOL(m_mHullBoxRot,i,vRotCol);
+            dReal fSign = dCalcVectorDot3(m_vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
+
+            vPa[0] += fSign * m_vBoxHalfSize[i] * vRotCol[0];
+            vPa[1] += fSign * m_vBoxHalfSize[i] * vRotCol[1];
+            vPa[2] += fSign * m_vBoxHalfSize[i] * vRotCol[2];
+        }
+
+        int iEdge = (m_iBestAxis-5)%3;
+
+        // decide which edge is on triangle
+        if ( iEdge == 0 ) {
+            SET(vPb,v0);
+            SET(vub,m_vE0);
+        } else if ( iEdge == 1) {
+            SET(vPb,v2);
+            SET(vub,m_vE1);
+        } else {
+            SET(vPb,v1);
+            SET(vub,m_vE2);
+        }
+
+
+        // setup direction parameter for face edge
+        dNormalize3(vub);
+
+        dReal fParam1, fParam2;
+
+        // setup direction parameter for box edge
+        dVector3 vua;
+        int col=(m_iBestAxis-5)/3;
+        GETCOL(m_mHullBoxRot,col,vua);
+
+        // find two closest points on both edges
+        _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
+        vPa[0] += vua[0]*fParam1;
+        vPa[1] += vua[1]*fParam1;
+        vPa[2] += vua[2]*fParam1;
+
+        vPb[0] += vub[0]*fParam2;
+        vPb[1] += vub[1]*fParam2;
+        vPb[2] += vub[2]*fParam2;
+
+        // calculate collision point
+        dVector3 vPntTmp;
+        ADD(vPa,vPb,vPntTmp);
+
+        vPntTmp[0]*=0.5f;
+        vPntTmp[1]*=0.5f;
+        vPntTmp[2]*=0.5f;
+
+        // generate contact point between two closest points
+        GenerateContact(TriIndex, vPntTmp, m_vBestNormal, m_fBestDepth);
+
+
+        // if triangle is the referent face then clip box to triangle face
+    } else if (m_iBestAxis == 1) {
+
+        dVector3 vNormal2;
+        vNormal2[0]=-m_vBestNormal[0];
+        vNormal2[1]=-m_vBestNormal[1];
+        vNormal2[2]=-m_vBestNormal[2];
+
+
+        // vNr is normal in box frame, pointing from triangle to box
+        dMatrix3 mTransposed;
+        mTransposed[0*4+0]=m_mHullBoxRot[0*4+0];
+        mTransposed[0*4+1]=m_mHullBoxRot[1*4+0];
+        mTransposed[0*4+2]=m_mHullBoxRot[2*4+0];
+
+        mTransposed[1*4+0]=m_mHullBoxRot[0*4+1];
+        mTransposed[1*4+1]=m_mHullBoxRot[1*4+1];
+        mTransposed[1*4+2]=m_mHullBoxRot[2*4+1];
+
+        mTransposed[2*4+0]=m_mHullBoxRot[0*4+2];
+        mTransposed[2*4+1]=m_mHullBoxRot[1*4+2];
+        mTransposed[2*4+2]=m_mHullBoxRot[2*4+2];
+
+        dVector3 vNr;
+        vNr[0]=mTransposed[0*4+0]*vNormal2[0]+  mTransposed[0*4+1]*vNormal2[1]+  mTransposed[0*4+2]*vNormal2[2];
+        vNr[1]=mTransposed[1*4+0]*vNormal2[0]+  mTransposed[1*4+1]*vNormal2[1]+  mTransposed[1*4+2]*vNormal2[2];
+        vNr[2]=mTransposed[2*4+0]*vNormal2[0]+  mTransposed[2*4+1]*vNormal2[1]+  mTransposed[2*4+2]*vNormal2[2];
+
+
+        dVector3 vAbsNormal;
+        vAbsNormal[0] = dFabs( vNr[0] );
+        vAbsNormal[1] = dFabs( vNr[1] );
+        vAbsNormal[2] = dFabs( vNr[2] );
+
+        // get closest face from box
+        int iB0, iB1, iB2;
+        if (vAbsNormal[1] > vAbsNormal[0]) {
+            if (vAbsNormal[1] > vAbsNormal[2]) {
+                iB1 = 0;  iB0 = 1;  iB2 = 2;
+            } else {
+                iB1 = 0;  iB2 = 1;  iB0 = 2;
+            }
+        } else {
+
+            if (vAbsNormal[0] > vAbsNormal[2]) {
+                iB0 = 0;  iB1 = 1;  iB2 = 2;
+            } else {
+                iB1 = 0;  iB2 = 1;  iB0 = 2;
+            }
+        }
+
+        // Here find center of box face we are going to project
+        dVector3 vCenter;
+        dVector3 vRotCol;
+        GETCOL(m_mHullBoxRot,iB0,vRotCol);
+
+        if (vNr[iB0] > 0) {
+            vCenter[0] = m_vHullBoxPos[0] - v0[0] - m_vBoxHalfSize[iB0] * vRotCol[0];
+            vCenter[1] = m_vHullBoxPos[1] - v0[1] - m_vBoxHalfSize[iB0] * vRotCol[1];
+            vCenter[2] = m_vHullBoxPos[2] - v0[2] - m_vBoxHalfSize[iB0] * vRotCol[2];
+        } else {
+            vCenter[0] = m_vHullBoxPos[0] - v0[0] + m_vBoxHalfSize[iB0] * vRotCol[0];
+            vCenter[1] = m_vHullBoxPos[1] - v0[1] + m_vBoxHalfSize[iB0] * vRotCol[1];
+            vCenter[2] = m_vHullBoxPos[2] - v0[2] + m_vBoxHalfSize[iB0] * vRotCol[2];
+        }
+
+        // Here find 4 corner points of box
+        dVector3 avPoints[4];
+
+        dVector3 vRotCol2;
+        GETCOL(m_mHullBoxRot,iB1,vRotCol);
+        GETCOL(m_mHullBoxRot,iB2,vRotCol2);
+
+        for(int x=0;x<3;x++) {
+            avPoints[0][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]);
+            avPoints[1][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]);
+            avPoints[2][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]);
+            avPoints[3][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]);
+        }
+
+        // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
+        dVector3 avTempArray1[9];
+        dVector3 avTempArray2[9];
+        dVector4 plPlane;
+
+        int iTempCnt1=0;
+        int iTempCnt2=0;
+
+        // zeroify vectors - necessary?
+        for(int i=0; i<9; i++) {
+            avTempArray1[i][0]=0;
+            avTempArray1[i][1]=0;
+            avTempArray1[i][2]=0;
+
+            avTempArray2[i][0]=0;
+            avTempArray2[i][1]=0;
+            avTempArray2[i][2]=0;
+        }
+
+
+        // Normal plane
+        dVector3 vTemp;
+        vTemp[0]=-m_vN[0];
+        vTemp[1]=-m_vN[1];
+        vTemp[2]=-m_vN[2];
+        dNormalize3(vTemp);
+        CONSTRUCTPLANE(plPlane,vTemp,0);
+
+        _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane  );
+
+
+        // Plane p0
+        dVector3 vTemp2;
+        SUBTRACT(v1,v0,vTemp2);
+        dCalcVectorCross3(vTemp,m_vN,vTemp2);
+        dNormalize3(vTemp);
+        CONSTRUCTPLANE(plPlane,vTemp,0);
+
+        _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane  );
+
+        // Plane p1
+        SUBTRACT(v2,v1,vTemp2);
+        dCalcVectorCross3(vTemp,m_vN,vTemp2);
+        dNormalize3(vTemp);
+        SUBTRACT(v0,v2,vTemp2);
+        CONSTRUCTPLANE(plPlane,vTemp,dCalcVectorDot3(vTemp2,vTemp));
+
+        _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane  );
+
+        // Plane p2
+        SUBTRACT(v0,v2,vTemp2);
+        dCalcVectorCross3(vTemp,m_vN,vTemp2);
+        dNormalize3(vTemp);
+        CONSTRUCTPLANE(plPlane,vTemp,0);
+
+        _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane  );
+
+        // END of clipping polygons
+
+        // for each generated contact point
+        for ( int i=0; i<iTempCnt2; i++ ) {
+            // calculate depth
+            dReal fTempDepth = dCalcVectorDot3(vNormal2,avTempArray2[i]);
+
+            // clamp depth to zero
+            if (fTempDepth > 0) {
+                fTempDepth = 0;
+            }
+
+            dVector3 vPntTmp;
+            ADD(avTempArray2[i],v0,vPntTmp);
+
+            GenerateContact(TriIndex, vPntTmp, m_vBestNormal, -fTempDepth);
+
+            if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
+                break;
+            }
+        }
+
+        //dAASSERT(m_ctContacts>0);
+
+        // if box face is the referent face, then clip triangle on box face
+    } else { // 2 <= if iBestAxis <= 4
+
+        // get normal of box face
+        dVector3 vNormal2;
+        SET(vNormal2,m_vBestNormal);
+
+        // get indices of box axes in correct order
+        int iA0,iA1,iA2;
+        iA0 = m_iBestAxis-2;
+        if ( iA0 == 0 ) {
+            iA1 = 1; iA2 = 2;
+        } else if ( iA0 == 1 ) {
+            iA1 = 0; iA2 = 2;
+        } else {
+            iA1 = 0; iA2 = 1;
+        }
+
+        dVector3 avPoints[3];
+        // calculate triangle vertices in box frame
+        SUBTRACT(v0,m_vHullBoxPos,avPoints[0]);
+        SUBTRACT(v1,m_vHullBoxPos,avPoints[1]);
+        SUBTRACT(v2,m_vHullBoxPos,avPoints[2]);
+
+        // CLIP Polygons
+        // define temp data for clipping
+        dVector3 avTempArray1[9];
+        dVector3 avTempArray2[9];
+
+        int iTempCnt1, iTempCnt2;
+
+        // zeroify vectors - necessary?
+        for(int i=0; i<9; i++) {
+            avTempArray1[i][0]=0;
+            avTempArray1[i][1]=0;
+            avTempArray1[i][2]=0;
+
+            avTempArray2[i][0]=0;
+            avTempArray2[i][1]=0;
+            avTempArray2[i][2]=0;
+        }
+
+        // clip triangle with 5 box planes (1 face plane, 4 edge planes)
+
+        dVector4 plPlane;
+
+        // Normal plane
+        dVector3 vTemp;
+        vTemp[0]=-vNormal2[0];
+        vTemp[1]=-vNormal2[1];
+        vTemp[2]=-vNormal2[2];
+        CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA0]);
+
+        _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane );
+
+
+        // Plane p0
+        GETCOL(m_mHullBoxRot,iA1,vTemp);
+        CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA1]);
+
+        _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
+
+
+        // Plane p1
+        GETCOL(m_mHullBoxRot,iA1,vTemp);
+        vTemp[0]=-vTemp[0];
+        vTemp[1]=-vTemp[1];
+        vTemp[2]=-vTemp[2];
+        CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA1]);
+
+        _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
+
+        // Plane p2
+        GETCOL(m_mHullBoxRot,iA2,vTemp);
+        CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA2]);
+
+        _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
+
+        // Plane p3
+        GETCOL(m_mHullBoxRot,iA2,vTemp);
+        vTemp[0]=-vTemp[0];
+        vTemp[1]=-vTemp[1];
+        vTemp[2]=-vTemp[2];
+        CONSTRUCTPLANE(plPlane,vTemp,m_vBoxHalfSize[iA2]);
+
+        _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
+
+
+        // for each generated contact point
+        for ( int i=0; i<iTempCnt1; i++ ) {
+            // calculate depth
+            dReal fTempDepth = dCalcVectorDot3(vNormal2,avTempArray1[i])-m_vBoxHalfSize[iA0];
+
+            // clamp depth to zero
+            if (fTempDepth > 0) {
+                fTempDepth = 0;
+            }
+
+            // generate contact data
+            dVector3 vPntTmp;
+            ADD(avTempArray1[i],m_vHullBoxPos,vPntTmp);
+
+            GenerateContact(TriIndex, vPntTmp, m_vBestNormal, -fTempDepth);
+
+            if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
+                break;
+            }
+        }
+
+        //dAASSERT(m_ctContacts>0);
+    }
+}
+
+// GenerateContact - Written by Jeff Smith (jeff@burri.to)
+//   Generate a "unique" contact.  A unique contact has a unique
+//   position or normal.  If the potential contact has the same
+//   position and normal as an existing contact, but a larger
+//   penetration depth, this new depth is used instead
+//
+void sTrimeshBoxColliderData::GenerateContact(int TriIndex, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth)
+{
+    int TriCount = m_ctContacts;
+
+    do
+    {
+        dContactGeom* TgtContact = NULL;
+        bool deeper = false;
+
+        if (!(m_iFlags & CONTACTS_UNIMPORTANT))
+        {
+            dReal MinDepth = dInfinity;
+            dContactGeom* MinContact = NULL;
+
+            bool duplicate = false;
+            for (int i = 0; i < TriCount; i++)
+            {
+                dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, i, m_iStride);
+
+                // same position?
+                dVector3 diff;
+                dSubtractVectors3(diff, in_ContactPos, Contact->pos);
+
+                if (dCalcVectorDot3(diff, diff) < dEpsilon)
+                {
+                    // same normal?
+                    if (REAL(1.0) - dCalcVectorDot3(in_Normal, Contact->normal) < dEpsilon)
+                    {
+                        if (in_Depth > Contact->depth)
+                        {
+                            Contact->depth = in_Depth;
+                            Contact->side1 = TriIndex;
+                        }
+
+                        duplicate = true;
+                        break;
+                    }
+                }
+
+                if (Contact->depth < MinDepth)
+                {
+                    MinDepth = Contact->depth;
+                    MinContact = Contact;
+                }
+            }
+            if (duplicate)
+            {
+                break;
+            }
+
+            if (TriCount == (m_iFlags & NUMC_MASK))
+            {
+                if (!(MinDepth < in_Depth))
+                {
+                    break;
+                }
+
+                TgtContact = MinContact;
+                deeper = true;
+            }
+        }
+        else
+        {
+            dIASSERT(TriCount < (m_iFlags & NUMC_MASK));
+        }
+
+        if (!deeper)
+        {
+            // Add a new contact
+            TgtContact = SAFECONTACT(m_iFlags, m_ContactGeoms, TriCount, m_iStride);
+            TriCount++;
+
+            TgtContact->pos[3] = 0.0;
+
+            TgtContact->normal[3] = 0.0;
+
+            TgtContact->g1 = m_Geom1;
+            TgtContact->g2 = m_Geom2;
+
+            TgtContact->side2 = -1;
+        }
+
+        TgtContact->pos[0] = in_ContactPos[0];
+        TgtContact->pos[1] = in_ContactPos[1];
+        TgtContact->pos[2] = in_ContactPos[2];
+
+        TgtContact->normal[0] = in_Normal[0];
+        TgtContact->normal[1] = in_Normal[1];
+        TgtContact->normal[2] = in_Normal[2];
+
+        TgtContact->depth = in_Depth;
+
+        TgtContact->side1 = TriIndex;
+
+        m_ctContacts = TriCount;
+    }
+    while (false);
+}
+
+
+
+
+
+void sTrimeshBoxColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *BoxGeom,
+                                                  int Flags, dContactGeom* Contacts, int Stride)
+{
+    // get source hull position, orientation and half size
+    const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
+    const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
+
+    // to global
+    SETM(m_mHullBoxRot,mRotBox);
+    SET(m_vHullBoxPos,vPosBox);
+
+    dGeomBoxGetLengths(BoxGeom, m_vBoxHalfSize);
+    m_vBoxHalfSize[0] *= 0.5f;
+    m_vBoxHalfSize[1] *= 0.5f;
+    m_vBoxHalfSize[2] *= 0.5f;
+
+    // get destination hull position and orientation
+    const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
+
+    // to global
+    SET(m_vHullDstPos,vPosMesh);
+
+    // global info for contact creation
+    m_ctContacts = 0;
+    m_iStride=Stride;
+    m_iFlags=Flags;
+    m_ContactGeoms=Contacts;
+    m_Geom1=TriMesh;
+    m_Geom2=BoxGeom;
+
+    // reset stuff
+    m_fBestDepth = MAXVALUE;
+    m_vBestNormal[0]=0;
+    m_vBestNormal[1]=0;
+    m_vBestNormal[2]=0;
+}
+
+void sTrimeshBoxColliderData::TestCollisionForSingleTriangle(int Triint, dVector3 dv[3], bool &bOutFinishSearching)
+{
+    bool finish = false;
+
+    // test this triangle
+    if (_cldTestOneTriangle(dv[0], dv[1], dv[2], Triint))
+    {
+        /*
+        NOTE by Oleh_Derevenko:
+        The function continues checking triangles after maximal number
+        of contacts is reached because it selects maximal penetration depths.
+        See also comments in GenerateContact()
+        */
+        finish = ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)));
+    }
+
+    bOutFinishSearching = finish;
+}
+
+// test one mesh triangle on intersection with given box
+bool sTrimeshBoxColliderData::_cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex)//, void *pvUser)
+{
+    // do intersection test and find best separating axis
+    if (!_cldTestSeparatingAxes(v0, v1, v2)) {
+        // if not found do nothing
+        return false;
+    }
+
+    // if best separation axis is not found
+    if (m_iBestAxis == 0) {
+        // this should not happen (we should already exit in that case)
+        //dMessage (0, "best separation axis not found");
+        // do nothing
+        return false;
+    }
+
+    _cldClipping(v0, v1, v2, TriIndex);
+    return true;
+}
+
+
+// OPCODE version of box to mesh collider
+#if dTRIMESH_OPCODE
+static void dQueryBTLPotentialCollisionTriangles(OBBCollider &Collider, 
+                                                 const sTrimeshBoxColliderData &cData, dxTriMesh *TriMesh, dxGeom *BoxGeom,
+                                                 OBBCache &BoxCache)
+{
+    // get destination hull position and orientation
+    const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
+    const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
+
+    Matrix4x4 MeshMatrix;
+    const dVector3 vZeroVector3 = { REAL(0.0), };
+    MakeMatrix(vZeroVector3, mRotMesh, MeshMatrix);
+
+    // get source hull position, orientation and half size
+    const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
+    const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
+
+    dVector3 vOffsetPosBox;
+    dSubtractVectors3(vOffsetPosBox, vPosBox, vPosMesh);
+
+    // Make OBB
+    OBB Box;
+    Box.mCenter.Set(vOffsetPosBox[0], vOffsetPosBox[1], vOffsetPosBox[2]);
+    Box.mExtents.Set(cData.m_vBoxHalfSize[0], cData.m_vBoxHalfSize[1], cData.m_vBoxHalfSize[2]);
+    Box.mRot.Set(
+        mRotBox[0], mRotBox[4], mRotBox[8], 
+        mRotBox[1], mRotBox[5], mRotBox[9],
+        mRotBox[2], mRotBox[6], mRotBox[10]);
+
+    // TC results
+    if (TriMesh->getDoTC(dxTriMesh::TTC_BOX)) {
+        dxTriMesh::BoxTC* BoxTC = 0;
+        const int iBoxCacheSize = TriMesh->m_BoxTCCache.size();
+        for (int i = 0; i != iBoxCacheSize; i++){
+            if (TriMesh->m_BoxTCCache[i].Geom == BoxGeom){
+                BoxTC = &TriMesh->m_BoxTCCache[i];
+                break;
+            }
+        }
+        if (!BoxTC){
+            TriMesh->m_BoxTCCache.push(dxTriMesh::BoxTC());
+
+            BoxTC = &TriMesh->m_BoxTCCache[TriMesh->m_BoxTCCache.size() - 1];
+            BoxTC->Geom = BoxGeom;
+            BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0
+        }
+
+        // Intersect
+        Collider.SetTemporalCoherence(true);
+        Collider.Collide(*BoxTC, Box, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
+    }
+    else {
+        Collider.SetTemporalCoherence(false);
+        Collider.Collide(BoxCache, Box, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
+    }
+}
+
+int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (BoxGeom->type == dBoxClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+    sTrimeshBoxColliderData cData;
+    cData.SetupInitialContext(TriMesh, BoxGeom, Flags, Contacts, Stride);
+
+    const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
+    dIASSERT(uiTLSKind == BoxGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
+    TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
+    OBBCollider& Collider = pccColliderCache->m_OBBCollider;
+
+    dQueryBTLPotentialCollisionTriangles(Collider, cData, TriMesh, BoxGeom,
+        pccColliderCache->m_DefaultBoxCache);
+
+    if (!Collider.GetContactStatus()) {
+        // no collision occurred
+        return 0;
+    }
+
+    // Retrieve data
+    int TriCount = Collider.GetNbTouchedPrimitives();
+    const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
+
+    if (TriCount != 0){
+        if (TriMesh->m_ArrayCallback != null){
+            TriMesh->m_ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount);
+        }
+
+        // get destination hull position and orientation
+        const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
+        const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
+
+        // loop through all intersecting triangles
+        for (int i = 0; i < TriCount; i++){
+            const int Triint = Triangles[i];
+            if (!TriMesh->invokeCallback(BoxGeom, Triint)) continue;
+
+            dVector3 dv[3];
+            TriMesh->fetchMeshTriangle(dv, Triint, vPosMesh, mRotMesh);
+
+            bool bFinishSearching;
+            cData.TestCollisionForSingleTriangle(Triint, dv, bFinishSearching);
+
+            if (bFinishSearching) {
+                break;
+            }
+        }
+    }
+
+    return cData.m_ctContacts;
+}
+#endif
+
+// GIMPACT version of box to mesh collider
+#if dTRIMESH_GIMPACT
+int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride)
+{
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (BoxGeom->type == dBoxClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+    g1 -> recomputeAABB();
+    BoxGeom -> recomputeAABB();
+
+
+    sTrimeshBoxColliderData cData;
+    cData.SetupInitialContext(TriMesh, BoxGeom, Flags, Contacts, Stride);
+
+    //*****at first , collide box aabb******//
+
+    GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh;
+    aabb3f test_aabb(BoxGeom->aabb[0], BoxGeom->aabb[1], BoxGeom->aabb[2], BoxGeom->aabb[3], BoxGeom->aabb[4], BoxGeom->aabb[5]);
+
+    GDYNAMIC_ARRAY collision_result;
+    GIM_CREATE_BOXQUERY_LIST(collision_result);
+
+    gim_aabbset_box_collision(&test_aabb, &ptrimesh->m_aabbset , &collision_result);
+
+    if(collision_result.m_size==0)
+    {
+        GIM_DYNARRAY_DESTROY(collision_result);
+        return 0;
+    }
+    //*****Set globals for box collision******//
+
+    //collide triangles
+
+    GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
+    gim_trimesh_locks_work_data(ptrimesh);
+
+    for(unsigned int i=0;i<collision_result.m_size;i++)
+    {
+        dVector3 dv[3];
+
+        int Triint = boxesresult[i];
+        gim_trimesh_get_triangle_vertices(ptrimesh, Triint, dv[0], dv[1], dv[2]);
+
+        bool bFinishSearching;
+        cData.TestCollisionForSingleTriangle(Triint, dv, bFinishSearching);
+
+        if (bFinishSearching)
+        {
+            break;
+        }
+    }
+
+    gim_trimesh_unlocks_work_data(ptrimesh);
+    GIM_DYNARRAY_DESTROY(collision_result);
+
+    return cData.m_ctContacts;
+}
+#endif
+
+
+
+#endif // dTRIMESH_ENABLED