/*************************************************************************
 *                                                                       *
 * 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


static void
GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
                dxGeom* in_g1,  dxGeom* in_g2, int TriIndex,
                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) 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->doBoxTC) {
        dxTriMesh::BoxTC* BoxTC = 0;
        const int iBoxCacheSize = TriMesh->BoxTCCache.size();
        for (int i = 0; i != iBoxCacheSize; 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, &MeshMatrix);
    }
    else {
        Collider.SetTemporalCoherence(false);
        Collider.Collide(BoxCache, Box, TriMesh->Data->BVTree, 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->_OBBCollider;

    dQueryBTLPotentialCollisionTriangles(Collider, cData, TriMesh, BoxGeom,
        pccColliderCache->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->ArrayCallback != null){
            TriMesh->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 (!Callback(TriMesh, BoxGeom, Triint)) continue;

            dVector3 dv[3];
            FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv);

            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;

    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

    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
