resubmitting ode

1 files changed, 1497 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
new file mode 100644
index 0000000..0194d02
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
@@ -0,0 +1,1497 @@
+/*************************************************************************
+ *                                                                       *
+ * 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/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
+
+
+static void
+GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
+                dxGeom* in_g1,  dxGeom* in_g2,
+                const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
+                int& OutTriCount);
+
+
+// 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) do{ \
+  (r)[0]=(a)[0] - (b)[0]; \
+  (r)[1]=(a)[1] - (b)[1]; \
+  (r)[2]=(a)[2] - (b)[2]; }while(0)
+
+
+// dVector3
+// a=b
+#define SET(a,b) do{ \
+  (a)[0]=(b)[0]; \
+  (a)[1]=(b)[1]; \
+  (a)[2]=(b)[2]; }while(0)
+
+
+// dMatrix3
+// a=b
+#define SETM(a,b) do{ \
+  (a)[0]=(b)[0]; \
+  (a)[1]=(b)[1]; \
+  (a)[2]=(b)[2]; \
+  (a)[3]=(b)[3]; \
+  (a)[4]=(b)[4]; \
+  (a)[5]=(b)[5]; \
+  (a)[6]=(b)[6]; \
+  (a)[7]=(b)[7]; \
+  (a)[8]=(b)[8]; \
+  (a)[9]=(b)[9]; \
+  (a)[10]=(b)[10]; \
+                     (a)[11]=(b)[11]; }while(0)
+
+
+// dVector3
+// r=a+b
+#define ADD(a,b,r) do{ \
+  (r)[0]=(a)[0] + (b)[0]; \
+  (r)[1]=(a)[1] + (b)[1]; \
+  (r)[2]=(a)[2] + (b)[2]; }while(0)
+
+
+// dMatrix3, int, dVector3
+// v=column a from m
+#define GETCOL(m,a,v) do{ \
+  (v)[0]=(m)[(a)+0]; \
+  (v)[1]=(m)[(a)+4]; \
+  (v)[2]=(m)[(a)+8]; }while(0)
+
+
+// dVector4, dVector3
+// distance between plane p and point v
+#define POINTDISTANCE(p,v) \
+  ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] )
+
+
+// dVector4, dVector3, dReal
+// construct plane from normal and d
+#define CONSTRUCTPLANE(plane,normal,d) do{ \
+  plane[0]=normal[0];\
+  plane[1]=normal[1];\
+  plane[2]=normal[2];\
+  plane[3]=d; }while(0)
+
+
+// dVector3
+// length of vector a
+#define LENGTHOF(a) \
+  dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2])
+
+
+// box data
+static dMatrix3 mHullBoxRot;
+static dVector3 vHullBoxPos;
+static dVector3 vBoxHalfSize;
+
+// mesh data
+static dVector3   vHullDstPos;
+
+// global collider data
+static dVector3 vBestNormal;
+static dReal    fBestDepth;
+static int    iBestAxis = 0;
+static int    iExitAxis = 0;
+static dVector3 vE0, vE1, vE2, vN;
+
+// global info for contact creation
+static int iFlags;
+static dContactGeom *ContactGeoms;
+static int iStride;
+static dxGeom *Geom1;
+static dxGeom *Geom2;
+static int ctContacts = 0;
+
+
+
+// Test normal of mesh face as separating axis for intersection
+static bool _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<fBestDepth) {
+      vBestNormal[0] = -vNormal[0]*fOneOverLength;
+      vBestNormal[1] = -vNormal[1]*fOneOverLength;
+      vBestNormal[2] = -vNormal[2]*fOneOverLength;
+      iBestAxis = iAxis;
+      //dAASSERT(fDepth>=0);
+      fBestDepth = fDepth;
+    }
+
+  }
+
+  return true;
+}
+
+
+
+
+// Test box axis as separating axis
+static bool _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<fBestDepth) {
+    // remember current axis as best axis
+    vBestNormal[0]  = vNormal[0];
+    vBestNormal[1]  = vNormal[1];
+    vBestNormal[2]  = vNormal[2];
+    iBestAxis    = iAxis;
+    //dAASSERT(fDepth>=0);
+    fBestDepth   = fDepth;
+  }
+
+  return true;
+}
+
+
+
+
+
+// Test cross products of box axis and triangle edges as separating axis
+static bool _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<fBestDepth) {
+      // remember current axis as best axis
+      vBestNormal[0]  = vNormal[0]*fOneOverLength;
+      vBestNormal[1]  = vNormal[1]*fOneOverLength;
+      vBestNormal[2]  = vNormal[2]*fOneOverLength;
+      iBestAxis    = iAxis;
+      //dAASSERT(fDepth>=0);
+      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++;
+    }
+  }
+
+}
+
+
+
+
+static bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
+  // reset best axis
+  iBestAxis = 0;
+  iExitAxis = -1;
+  fBestDepth = MAXVALUE;
+
+  // calculate edges
+  SUBTRACT(v1,v0,vE0);
+  SUBTRACT(v2,v0,vE1);
+  SUBTRACT(vE1,vE0,vE2);
+
+  // calculate poly normal
+  dCROSS(vN,=,vE0,vE1);
+
+  // extract box axes as vectors
+  dVector3 vA0,vA1,vA2;
+  GETCOL(mHullBoxRot,0,vA0);
+  GETCOL(mHullBoxRot,1,vA1);
+  GETCOL(mHullBoxRot,2,vA2);
+
+  // box halfsizes
+  dReal fa0 = vBoxHalfSize[0];
+  dReal fa1 = vBoxHalfSize[1];
+  dReal fa2 = vBoxHalfSize[2];
+
+  // calculate relative position between box and triangle
+  dVector3 vD;
+  SUBTRACT(v0,vHullBoxPos,vD);
+
+  // calculate length of face normal
+  dReal fNLen = LENGTHOF( vN );
+
+  dVector3 vL;
+  dReal fp0, fp1, fp2, fR, fD;
+
+  // Test separating axes for intersection
+  // ************************************************
+  // Axis 1 - Triangle Normal
+  SET(vL,vN);
+  fp0  = dDOT(vL,vD);
+  fp1  = fp0;
+  fp2  = fp0;
+  fR=fa0*dFabs( dDOT(vN,vA0) ) + fa1 * dFabs( dDOT(vN,vA1) ) + fa2 * dFabs( dDOT(vN,vA2) );
+
+
+  if( !_cldTestNormal( fp0, fR, vL, 1) ) {
+    iExitAxis=1;
+    return false;
+  }
+
+  // ************************************************
+
+  // Test Faces
+  // ************************************************
+  // Axis 2 - Box X-Axis
+  SET(vL,vA0);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 + dDOT(vA0,vE0);
+  fp2 = fp0 + dDOT(vA0,vE1);
+  fR  = fa0;
+
+
+  if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 2) ) {
+    iExitAxis=2;
+    return false;
+  }
+  // ************************************************
+
+  // ************************************************
+  // Axis 3 - Box Y-Axis
+  SET(vL,vA1);
+  fD = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 + dDOT(vA1,vE0);
+  fp2 = fp0 + dDOT(vA1,vE1);
+  fR  = fa1;
+
+
+  if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 3) ) {
+    iExitAxis=3;
+    return false;
+  }
+
+  // ************************************************
+
+  // ************************************************
+  // Axis 4 - Box Z-Axis
+  SET(vL,vA2);
+  fD = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 + dDOT(vA2,vE0);
+  fp2 = fp0 + dDOT(vA2,vE1);
+  fR  = fa2;
+
+
+  if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 4) ) {
+    iExitAxis=4;
+    return false;
+  }
+
+  // ************************************************
+
+  // Test Edges
+  // ************************************************
+  // Axis 5 - Box X-Axis cross Edge0
+  dCROSS(vL,=,vA0,vE0);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0;
+  fp2 = fp0 + dDOT(vA0,vN);
+  fR  = fa1 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA1,vE0));
+
+
+  if( !_cldTestEdge( fp1, fp2, fR, fD, vL, 5) ) {
+    iExitAxis=5;
+    return false;
+  }
+  // ************************************************
+
+  // ************************************************
+  // Axis 6 - Box X-Axis cross Edge1
+  dCROSS(vL,=,vA0,vE1);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 - dDOT(vA0,vN);
+  fp2 = fp0;
+  fR  = fa1 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA1,vE1));
+
+
+  if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 6) ) {
+    iExitAxis=6;
+    return false;
+  }
+  // ************************************************
+
+  // ************************************************
+  // Axis 7 - Box X-Axis cross Edge2
+  dCROSS(vL,=,vA0,vE2);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 - dDOT(vA0,vN);
+  fp2 = fp0 - dDOT(vA0,vN);
+  fR  = fa1 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA1,vE2));
+
+
+  if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 7) ) {
+    iExitAxis=7;
+    return false;
+  }
+
+  // ************************************************
+
+  // ************************************************
+  // Axis 8 - Box Y-Axis cross Edge0
+  dCROSS(vL,=,vA1,vE0);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0;
+  fp2 = fp0 + dDOT(vA1,vN);
+  fR  = fa0 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA0,vE0));
+
+
+  if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 8) ) {
+    iExitAxis=8;
+    return false;
+  }
+
+  // ************************************************
+
+  // ************************************************
+  // Axis 9 - Box Y-Axis cross Edge1
+  dCROSS(vL,=,vA1,vE1);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 - dDOT(vA1,vN);
+  fp2 = fp0;
+  fR  = fa0 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA0,vE1));
+
+
+  if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 9) ) {
+    iExitAxis=9;
+    return false;
+  }
+
+  // ************************************************
+
+  // ************************************************
+  // Axis 10 - Box Y-Axis cross Edge2
+  dCROSS(vL,=,vA1,vE2);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 - dDOT(vA1,vN);
+  fp2 = fp0 - dDOT(vA1,vN);
+  fR  = fa0 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA0,vE2));
+
+
+  if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 10) ) {
+    iExitAxis=10;
+    return false;
+  }
+
+  // ************************************************
+
+  // ************************************************
+  // Axis 11 - Box Z-Axis cross Edge0
+  dCROSS(vL,=,vA2,vE0);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0;
+  fp2 = fp0 + dDOT(vA2,vN);
+  fR  = fa0 * dFabs(dDOT(vA1,vE0)) + fa1 * dFabs(dDOT(vA0,vE0));
+
+
+  if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 11) ) {
+    iExitAxis=11;
+    return false;
+  }
+  // ************************************************
+
+  // ************************************************
+  // Axis 12 - Box Z-Axis cross Edge1
+  dCROSS(vL,=,vA2,vE1);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 - dDOT(vA2,vN);
+  fp2 = fp0;
+  fR  = fa0 * dFabs(dDOT(vA1,vE1)) + fa1 * dFabs(dDOT(vA0,vE1));
+
+
+  if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 12) ) {
+    iExitAxis=12;
+    return false;
+  }
+  // ************************************************
+
+  // ************************************************
+  // Axis 13 - Box Z-Axis cross Edge2
+  dCROSS(vL,=,vA2,vE2);
+  fD  = dDOT(vL,vN)/fNLen;
+  fp0 = dDOT(vL,vD);
+  fp1 = fp0 - dDOT(vA2,vN);
+  fp2 = fp0 - dDOT(vA2,vN);
+  fR  = fa0 * dFabs(dDOT(vA1,vE2)) + fa1 * dFabs(dDOT(vA0,vE2));
+
+
+  if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 13) ) {
+    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)
+{
+  // calulate denominator
+  dVector3 vp;
+  SUBTRACT(vPoint2,vPoint1,vp);
+  dReal fuaub  = dDOT(vLenVec1,vLenVec2);
+  dReal fq1    = dDOT(vLenVec1,vp);
+  dReal fq2    = -dDOT(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
+static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
+  dIASSERT( !(iFlags & CONTACTS_UNIMPORTANT) || ctContacts < (iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results
+
+  // if we have edge/edge intersection
+  if ( iBestAxis > 4 ) {
+
+    dVector3 vub,vPb,vPa;
+
+    SET(vPa,vHullBoxPos);
+
+    // calculate point on box edge
+    for( int i=0; i<3; i++) {
+      dVector3 vRotCol;
+      GETCOL(mHullBoxRot,i,vRotCol);
+      dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
+
+      vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
+      vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
+      vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
+    }
+
+    int iEdge = (iBestAxis-5)%3;
+
+    // decide which edge is on triangle
+    if ( iEdge == 0 ) {
+      SET(vPb,v0);
+      SET(vub,vE0);
+    } else if ( iEdge == 1) {
+      SET(vPb,v2);
+      SET(vub,vE1);
+    } else {
+      SET(vPb,v1);
+      SET(vub,vE2);
+    }
+
+
+    // setup direction parameter for face edge
+    dNormalize3(vub);
+
+    dReal fParam1, fParam2;
+
+    // setup direction parameter for box edge
+    dVector3 vua;
+    int col=(iBestAxis-5)/3;
+    GETCOL(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
+#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
+        dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
+        Contact->depth = fBestDepth;
+        SET(Contact->normal,vBestNormal);
+        SET(Contact->pos,vPntTmp);
+        Contact->g1 = Geom1;
+        Contact->g2 = Geom2;
+        ctContacts++;
+#endif
+    GenerateContact(iFlags, ContactGeoms, iStride,  Geom1, Geom2,
+                    vPntTmp, vBestNormal, fBestDepth, ctContacts);
+
+
+  // if triangle is the referent face then clip box to triangle face
+  } else if ( iBestAxis == 1 ) {
+
+
+    dVector3 vNormal2;
+    vNormal2[0]=-vBestNormal[0];
+    vNormal2[1]=-vBestNormal[1];
+    vNormal2[2]=-vBestNormal[2];
+
+
+    // vNr is normal in box frame, pointing from triangle to box
+    dMatrix3 mTransposed;
+    mTransposed[0*4+0]=mHullBoxRot[0*4+0];
+    mTransposed[0*4+1]=mHullBoxRot[1*4+0];
+    mTransposed[0*4+2]=mHullBoxRot[2*4+0];
+
+    mTransposed[1*4+0]=mHullBoxRot[0*4+1];
+    mTransposed[1*4+1]=mHullBoxRot[1*4+1];
+    mTransposed[1*4+2]=mHullBoxRot[2*4+1];
+
+    mTransposed[2*4+0]=mHullBoxRot[0*4+2];
+    mTransposed[2*4+1]=mHullBoxRot[1*4+2];
+    mTransposed[2*4+2]=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(mHullBoxRot,iB0,vRotCol);
+
+    if (vNr[iB0] > 0) {
+        vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
+      vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
+      vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
+    } else {
+      vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
+      vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
+      vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
+    }
+
+    // Here find 4 corner points of box
+    dVector3 avPoints[4];
+
+    dVector3 vRotCol2;
+    GETCOL(mHullBoxRot,iB1,vRotCol);
+    GETCOL(mHullBoxRot,iB2,vRotCol2);
+
+    for(int x=0;x<3;x++) {
+        avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
+        avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
+        avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
+        avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (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]=-vN[0];
+    vTemp[1]=-vN[1];
+    vTemp[2]=-vN[2];
+    dNormalize3(vTemp);
+    CONSTRUCTPLANE(plPlane,vTemp,0);
+
+    _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane  );
+
+
+    // Plane p0
+    dVector3 vTemp2;
+    SUBTRACT(v1,v0,vTemp2);
+    dCROSS(vTemp,=,vN,vTemp2);
+    dNormalize3(vTemp);
+    CONSTRUCTPLANE(plPlane,vTemp,0);
+
+    _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane  );
+
+
+    // Plane p1
+    SUBTRACT(v2,v1,vTemp2);
+    dCROSS(vTemp,=,vN,vTemp2);
+    dNormalize3(vTemp);
+    SUBTRACT(v0,v2,vTemp2);
+    CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));
+
+    _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane  );
+
+
+    // Plane p2
+    SUBTRACT(v0,v2,vTemp2);
+    dCROSS(vTemp,=,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 = dDOT(vNormal2,avTempArray2[i]);
+
+      // clamp depth to zero
+      if (fTempDepth > 0) {
+        fTempDepth = 0;
+      }
+
+      dVector3 vPntTmp;
+      ADD(avTempArray2[i],v0,vPntTmp);
+
+#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
+      dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
+          
+      Contact->depth = -fTempDepth;
+      SET(Contact->normal,vBestNormal);
+      SET(Contact->pos,vPntTmp);
+      Contact->g1 = Geom1;
+      Contact->g2 = Geom2;
+      ctContacts++;
+#endif
+                GenerateContact(iFlags, ContactGeoms, iStride,  Geom1, Geom2,
+                                                vPntTmp, vBestNormal, -fTempDepth, ctContacts);
+
+                if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
+                        break;
+                }
+    }
+
+    //dAASSERT(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,vBestNormal);
+
+    // get indices of box axes in correct order
+    int iA0,iA1,iA2;
+    iA0 = 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,vHullBoxPos,avPoints[0]);
+    SUBTRACT(v1,vHullBoxPos,avPoints[1]);
+    SUBTRACT(v2,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,vBoxHalfSize[iA0]);
+
+    _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane );
+
+
+    // Plane p0
+    GETCOL(mHullBoxRot,iA1,vTemp);
+    CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
+
+    _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
+
+
+    // Plane p1
+    GETCOL(mHullBoxRot,iA1,vTemp);
+    vTemp[0]=-vTemp[0];
+    vTemp[1]=-vTemp[1];
+    vTemp[2]=-vTemp[2];
+    CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
+
+    _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
+
+
+    // Plane p2
+    GETCOL(mHullBoxRot,iA2,vTemp);
+    CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
+
+    _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
+
+
+    // Plane p3
+    GETCOL(mHullBoxRot,iA2,vTemp);
+    vTemp[0]=-vTemp[0];
+    vTemp[1]=-vTemp[1];
+    vTemp[2]=-vTemp[2];
+    CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
+
+    _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
+
+
+    // for each generated contact point
+    for ( int i=0; i<iTempCnt1; i++ ) {
+      // calculate depth
+      dReal fTempDepth = dDOT(vNormal2,avTempArray1[i])-vBoxHalfSize[iA0];
+
+      // clamp depth to zero
+      if (fTempDepth > 0) {
+        fTempDepth = 0;
+      }
+
+      // generate contact data
+      dVector3 vPntTmp;
+      ADD(avTempArray1[i],vHullBoxPos,vPntTmp);
+
+#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
+      dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
+
+      Contact->depth = -fTempDepth;
+      SET(Contact->normal,vBestNormal);
+      SET(Contact->pos,vPntTmp);
+      Contact->g1 = Geom1;
+      Contact->g2 = Geom2;
+      ctContacts++;
+#endif
+                GenerateContact(iFlags, ContactGeoms, iStride,  Geom1, Geom2,
+                                          vPntTmp, vBestNormal, -fTempDepth, ctContacts);
+
+                if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
+                        break;
+                }
+    }
+
+    //dAASSERT(ctContacts>0);
+  }
+
+}
+
+
+
+
+
+// test one mesh triangle on intersection with given box
+static void _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)//, void *pvUser)
+{
+  // do intersection test and find best separating axis
+  if(!_cldTestSeparatingAxes(v0, v1, v2) ) {
+     // if not found do nothing
+    return;
+  }
+
+  // if best separation axis is not found
+  if ( iBestAxis == 0 ) {
+    // this should not happen (we should already exit in that case)
+    //dMessage (0, "best separation axis not found");
+    // do nothing
+    return;
+  }
+
+  _cldClipping(v0, v1, v2);
+}
+
+
+
+
+
+// OPCODE version of box to mesh collider
+#if dTRIMESH_OPCODE
+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;
+
+
+  // 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(mHullBoxRot,mRotBox);
+  SET(vHullBoxPos,vPosBox);
+
+  dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
+  vBoxHalfSize[0] *= 0.5f;
+  vBoxHalfSize[1] *= 0.5f;
+  vBoxHalfSize[2] *= 0.5f;
+
+
+
+  // get destination hull position and orientation
+  const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
+  const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
+
+  // to global
+  SET(vHullDstPos,vPosMesh);
+
+
+
+  // global info for contact creation
+  ctContacts = 0;
+  iStride=Stride;
+  iFlags=Flags;
+  ContactGeoms=Contacts;
+  Geom1=TriMesh;
+  Geom2=BoxGeom;
+
+
+
+  // reset stuff
+  fBestDepth = MAXVALUE;
+  vBestNormal[0]=0;
+  vBestNormal[1]=0;
+  vBestNormal[2]=0;
+
+  OBBCollider& Collider = TriMesh->_OBBCollider;
+
+
+
+
+  // Make OBB
+  OBB Box;
+  Box.mCenter.x = vPosBox[0];
+  Box.mCenter.y = vPosBox[1];
+  Box.mCenter.z = vPosBox[2];
+
+  // It is a potential issue to explicitly cast to float 
+  // if custom width floating point type is introduced in OPCODE.
+  // It is necessary to make a typedef and cast to it
+  // (e.g. typedef float opc_float;)
+  // However I'm not sure in what header it should be added.
+
+  Box.mExtents.x = /*(float)*/vBoxHalfSize[0];
+  Box.mExtents.y = /*(float)*/vBoxHalfSize[1];
+  Box.mExtents.z = /*(float)*/vBoxHalfSize[2];
+
+  Box.mRot.m[0][0] = /*(float)*/mRotBox[0];
+  Box.mRot.m[1][0] = /*(float)*/mRotBox[1];
+  Box.mRot.m[2][0] = /*(float)*/mRotBox[2];
+
+  Box.mRot.m[0][1] = /*(float)*/mRotBox[4];
+  Box.mRot.m[1][1] = /*(float)*/mRotBox[5];
+  Box.mRot.m[2][1] = /*(float)*/mRotBox[6];
+
+  Box.mRot.m[0][2] = /*(float)*/mRotBox[8];
+  Box.mRot.m[1][2] = /*(float)*/mRotBox[9];
+  Box.mRot.m[2][2] = /*(float)*/mRotBox[10];
+
+  Matrix4x4 amatrix;
+  Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix);
+
+  Matrix4x4 InvBoxMatrix;
+  InvertPRMatrix(InvBoxMatrix, BoxMatrix);
+
+  // TC results
+  if (TriMesh->doBoxTC) {
+        dxTriMesh::BoxTC* BoxTC = 0;
+        for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
+                if (TriMesh->BoxTCCache[i].Geom == BoxGeom){
+                        BoxTC = &TriMesh->BoxTCCache[i];
+                        break;
+                }
+        }
+        if (!BoxTC){
+                TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
+
+                BoxTC = &TriMesh->BoxTCCache[TriMesh->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->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix));
+  }
+  else {
+                Collider.SetTemporalCoherence(false);
+                Collider.Collide(dxTriMesh::defaultBoxCache, Box, TriMesh->Data->BVTree, null,
+                                                 &MakeMatrix(vPosMesh, mRotMesh, amatrix));
+        }
+
+  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->ArrayCallback != null){
+         TriMesh->ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount);
+    }
+
+    int ctContacts0 = 0;
+
+    // loop through all intersecting triangles
+    for (int i = 0; i < TriCount; i++){
+
+
+        const int Triint = Triangles[i];
+        if (!Callback(TriMesh, BoxGeom, Triint)) continue;
+
+
+        dVector3 dv[3];
+        FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv);
+
+
+        // test this triangle
+        _cldTestOneTriangle(dv[0],dv[1],dv[2]);
+
+                // fill-in tri index for generated contacts
+                for (; ctContacts0<ctContacts; ctContacts0++)
+                        SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = 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()
+                */
+                // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
+                if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
+                        break;
+    }
+  }
+
+
+  return 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;
+
+
+  // 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(mHullBoxRot,mRotBox);
+  SET(vHullBoxPos,vPosBox);
+
+  dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
+  vBoxHalfSize[0] *= 0.5f;
+  vBoxHalfSize[1] *= 0.5f;
+  vBoxHalfSize[2] *= 0.5f;
+
+  // get destination hull position and orientation
+  /*const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
+  const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
+
+  // to global
+  SET(vHullDstPos,vPosMesh);*/
+
+  // global info for contact creation
+  ctContacts = 0;
+  iStride=Stride;
+  iFlags=Flags;
+  ContactGeoms=Contacts;
+  Geom1=TriMesh;
+  Geom2=BoxGeom;
+
+
+  // reset stuff
+  fBestDepth = MAXVALUE;
+  vBestNormal[0]=0;
+  vBestNormal[1]=0;
+  vBestNormal[2]=0;
+
+
+//*****at first , collide box aabb******//
+
+    GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh;
+        aabb3f test_aabb;
+
+        test_aabb.minX = BoxGeom->aabb[0];
+        test_aabb.maxX = BoxGeom->aabb[1];
+        test_aabb.minY = BoxGeom->aabb[2];
+        test_aabb.maxY = BoxGeom->aabb[3];
+        test_aabb.minZ = BoxGeom->aabb[4];
+        test_aabb.maxZ = 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
+
+        GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
+        gim_trimesh_locks_work_data(ptrimesh);
+
+    int ctContacts0 = 0;
+        
+        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]);
+        // test this triangle
+        _cldTestOneTriangle(dv[0],dv[1],dv[2]);
+                
+                // fill-in tri index for generated contacts
+                for (; ctContacts0<ctContacts; ctContacts0++)
+                        SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = 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()
+                */
+                // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
+                if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
+                        break;
+        }
+
+        gim_trimesh_unlocks_work_data(ptrimesh);
+        GIM_DYNARRAY_DESTROY(collision_result);
+
+        return ctContacts;
+}
+#endif
+
+
+// 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
+//
+static void
+GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
+                dxGeom* in_g1,  dxGeom* in_g2,
+                const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
+                int& OutTriCount)
+{
+        /*
+                NOTE by Oleh_Derevenko:
+                This function is called after maximal number of contacts has already been 
+                collected because it has a side effect of replacing penetration depth of
+                existing contact with larger penetration depth of another matching normal contact.
+                If this logic is not necessary any more, you can bail out on reach of contact
+                number maximum immediately in dCollideBTL(). You will also need to correct 
+                conditional statements after invocations of GenerateContact() in _cldClipping().
+        */
+        do 
+        {
+                dContactGeom* Contact;
+                dVector3 diff;
+                
+                if (!(in_Flags & CONTACTS_UNIMPORTANT))
+                {
+                        bool duplicate = false;
+                        for (int i=0; i<OutTriCount; i++)
+                        {
+                                Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
+
+                                // same position?
+                                for (int j=0; j<3; j++)
+                                        diff[j] = in_ContactPos[j] - Contact->pos[j];
+                                if (dDOT(diff, diff) < dEpsilon)
+                                {
+                                        // same normal?
+                                        if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon))
+                                        {
+                                                if (in_Depth > Contact->depth)
+                                                        Contact->depth = in_Depth;
+                                                duplicate = true;
+                                                /*
+                                                        NOTE by Oleh_Derevenko:
+                                                        There may be a case when two normals are close to each other but not duplicate
+                                                        while third normal is detected to be duplicate for both of them.
+                                                        This is the only reason I can think of, there is no "break" statement.
+                                                        Perhaps author considered it to be logical that the third normal would 
+                                                        replace the depth in both of initial contacts. 
+                                                        However, I consider it a questionable practice which should not
+                                                        be applied without deep understanding of underlaying physics.
+                                                        Even more, is this situation with close normal triplet acceptable at all?
+                                                        Should not be two initial contacts reduced to one (replaced with the latter)?
+                                                        If you know the answers for these questions, you may want to change this code.
+                                                        See the same statement in GenerateContact() of collision_trimesh_trimesh.cpp
+                                                */
+                                        }
+                                }
+                        }
+                        if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
+                        {
+                                break;
+                        }
+                }
+                else
+                {
+                        dIASSERT(OutTriCount < (in_Flags & NUMC_MASK));
+                }
+
+                // Add a new contact
+                Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride);
+
+                Contact->pos[0] = in_ContactPos[0];
+                Contact->pos[1] = in_ContactPos[1];
+                Contact->pos[2] = in_ContactPos[2];
+                Contact->pos[3] = 0.0;
+
+                Contact->normal[0] = in_Normal[0];
+                Contact->normal[1] = in_Normal[1];
+                Contact->normal[2] = in_Normal[2];
+                Contact->normal[3] = 0.0;
+
+                Contact->depth = in_Depth;
+
+                Contact->g1 = in_g1;
+                Contact->g2 = in_g2;
+
+                OutTriCount++;
+        }
+        while (false);
+}
+
+#endif // dTRIMESH_ENABLED