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

/*
 *	Cylinder-trimesh collider by Alen Ladavac
 *   Ported to ODE by Nguyen Binh
 */


#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"
#include "util.h"

#if dTRIMESH_ENABLED

#define MAX_REAL	dInfinity
static const int	nCYLINDER_AXIS				= 2;
static const int    nCYLINDER_CIRCLE_SEGMENTS	= 8;
static const int    nMAX_CYLINDER_TRIANGLE_CLIP_POINTS	= 12;

#define OPTIMIZE_CONTACTS 1

// Local contacts data
typedef struct _sLocalContactData
{
    dVector3	vPos;
    dVector3	vNormal;
    dReal		fDepth;
    int			triIndex;
    int			nFlags; // 0 = filtered out, 1 = OK
}sLocalContactData;

struct sCylinderTrimeshColliderData
{
    sCylinderTrimeshColliderData(int flags, int skip): m_iFlags(flags), m_iSkip(skip), m_nContacts(0), m_gLocalContacts(NULL) {}

#ifdef OPTIMIZE_CONTACTS
    void _OptimizeLocalContacts();
#endif
    void _InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh);
    int	_ProcessLocalContacts(dContactGeom *contact, dxGeom *Cylinder, dxTriMesh *Trimesh);

    bool _cldTestAxis(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, 
        dVector3& vAxis, int iAxis, bool bNoFlip = false);
    bool _cldTestCircleToEdgeAxis(
        const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
        const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1,
        const dVector3 &vVx0, const dVector3 &vVx1, int iAxis);
    bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2);
    bool _cldClipCylinderEdgeToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2);
    void _cldClipCylinderToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2);
    void TestOneTriangleVsCylinder(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, 
        const bool bDoubleSided);
    int TestCollisionForSingleTriangle(int ctContacts0, int Triint, dVector3 dv[3], 
        bool &bOutFinishSearching);

    // cylinder data
    dMatrix3	m_mCylinderRot;
    dQuaternion	m_qCylinderRot;
    dQuaternion	m_qInvCylinderRot;
    dVector3	m_vCylinderPos;
    dVector3	m_vCylinderAxis;
    dReal		m_fCylinderRadius;
    dReal		m_fCylinderSize;
    dVector3	m_avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS];

    // mesh data
    dQuaternion	m_qTrimeshRot;
    dQuaternion	m_qInvTrimeshRot;
    dMatrix3	m_mTrimeshRot;
    dVector3	m_vTrimeshPos;

    // global collider data
    dVector3	m_vBestPoint;
    dReal		m_fBestDepth;
    dReal		m_fBestCenter;
    dReal		m_fBestrt;
    int			m_iBestAxis;
    dVector3	m_vContactNormal;
    dVector3	m_vNormal;
    dVector3	m_vE0;
    dVector3	m_vE1;
    dVector3	m_vE2;

    // ODE stuff
    int					m_iFlags;
    int					m_iSkip;
    int					m_nContacts;// = 0;
    sLocalContactData*	m_gLocalContacts;
};


#ifdef OPTIMIZE_CONTACTS

// Use to classify contacts to be "near" in position
static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
// Use to classify contacts to be "near" in normal direction
static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4

// If this two contact can be classified as "near"
inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
{
    int bPosNear = 0;
    int bSameDir = 0;
    dVector3	vDiff;

    // First check if they are "near" in position
    dVector3Subtract(c1.vPos,c2.vPos,vDiff);
    if (  (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
        &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
        &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
    {
        bPosNear = 1;
    }

    // Second check if they are "near" in normal direction
    dVector3Subtract(c1.vNormal,c2.vNormal,vDiff);
    if (  (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
        &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
        &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
    {
        bSameDir = 1;
    }

    // Will be "near" if position and normal direction are "near"
    return (bPosNear && bSameDir);
}

inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
{
    // The not better will be throw away
    // You can change the selection criteria here
    return (c1.fDepth > c2.fDepth);
}

// iterate through gLocalContacts and filtered out "near contact"
void sCylinderTrimeshColliderData::_OptimizeLocalContacts()
{
    int nContacts = m_nContacts;

    for (int i = 0; i < nContacts-1; i++)
    {
        for (int j = i+1; j < nContacts; j++)
        {
            if (_IsNearContacts(m_gLocalContacts[i],m_gLocalContacts[j]))
            {
                // If they are seem to be the same then filtered 
                // out the least penetrate one
                if (_IsBetter(m_gLocalContacts[j],m_gLocalContacts[i]))
                {
                    m_gLocalContacts[i].nFlags = 0; // filtered 1st contact
                }
                else
                {
                    m_gLocalContacts[j].nFlags = 0; // filtered 2nd contact
                }

                // NOTE
                // There is other way is to add two depth together but
                // it not work so well. Why???
            }
        }
    }
}
#endif // OPTIMIZE_CONTACTS

int	sCylinderTrimeshColliderData::_ProcessLocalContacts(dContactGeom *contact, 
                                                        dxGeom *Cylinder, dxTriMesh *Trimesh)
{
#ifdef OPTIMIZE_CONTACTS
    if (m_nContacts > 1 && !(m_iFlags & CONTACTS_UNIMPORTANT))
    {
        // Can be optimized...
        _OptimizeLocalContacts();
    }
#endif		

    int iContact = 0;
    dContactGeom* Contact = 0;

    int nFinalContact = 0;

    for (iContact = 0; iContact < m_nContacts; iContact ++)
    {
        if (1 == m_gLocalContacts[iContact].nFlags)
        {
            Contact = SAFECONTACT(m_iFlags, contact, nFinalContact, m_iSkip);
            Contact->depth = m_gLocalContacts[iContact].fDepth;
            dVector3Copy(m_gLocalContacts[iContact].vNormal,Contact->normal);
            dVector3Copy(m_gLocalContacts[iContact].vPos,Contact->pos);
            Contact->g1 = Cylinder;
            Contact->g2 = Trimesh;
            Contact->side1 = -1;
            Contact->side2 = m_gLocalContacts[iContact].triIndex;
            dVector3Inv(Contact->normal);

            nFinalContact++;
        }
    }
    // debug
    //if (nFinalContact != m_nContacts)
    //{
    //	printf("[Info] %d contacts generated,%d  filtered.\n",m_nContacts,m_nContacts-nFinalContact);
    //}

    return nFinalContact;
}


bool sCylinderTrimeshColliderData::_cldTestAxis(
    const dVector3 &v0,
    const dVector3 &v1,
    const dVector3 &v2, 
    dVector3& vAxis, 
    int iAxis,
    bool bNoFlip/* = false*/)
{

    // calculate length of separating axis vector
    dReal fL = dVector3Length(vAxis);
    // if not long enough
    if ( fL < REAL(1e-5) )
    {
        // do nothing
        return true;
    }

    // otherwise normalize it
    vAxis[0] /= fL;
    vAxis[1] /= fL;
    vAxis[2] /= fL;

    dReal fdot1 = dVector3Dot(m_vCylinderAxis,vAxis);
    // project capsule on vAxis
    dReal frc;

    if (dFabs(fdot1) > REAL(1.0) ) 
    {
        //		fdot1 = REAL(1.0);
        frc = dFabs(m_fCylinderSize* REAL(0.5));
    }
    else
    {
        frc = dFabs((m_fCylinderSize* REAL(0.5)) * fdot1)
            + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
    }

    dVector3 vV0;
    dVector3Subtract(v0,m_vCylinderPos,vV0);
    dVector3 vV1;
    dVector3Subtract(v1,m_vCylinderPos,vV1);
    dVector3 vV2;
    dVector3Subtract(v2,m_vCylinderPos,vV2);

    // project triangle on vAxis
    dReal afv[3];
    afv[0] = dVector3Dot( vV0 , vAxis );
    afv[1] = dVector3Dot( vV1 , vAxis );
    afv[2] = dVector3Dot( vV2 , vAxis );

    dReal fMin = MAX_REAL;
    dReal fMax = -MAX_REAL;

    // for each vertex 
    for(int i = 0; i < 3; i++) 
    {
        // find minimum
        if (afv[i]<fMin) 
        {
            fMin = afv[i];
        }
        // find maximum
        if (afv[i]>fMax) 
        {
            fMax = afv[i];
        }
    }

    // find capsule's center of interval on axis
    dReal fCenter = (fMin+fMax)* REAL(0.5);
    // calculate triangles halfinterval 
    dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);

    // if they do not overlap, 
    if( dFabs(fCenter) > (frc+fTriangleRadius) ) 
    { 
        // exit, we have no intersection
        return false; 
    }

    // calculate depth 
    dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) );

    // if greater then best found so far
    if ( fDepth < m_fBestDepth ) 
    {
        // remember depth
        m_fBestDepth			= fDepth;
        m_fBestCenter		    = fCenter;
        m_fBestrt				= frc;
        dVector3Copy(vAxis,m_vContactNormal);
        m_iBestAxis				= iAxis;

        // flip normal if interval is wrong faced
        if ( fCenter< REAL(0.0) && !bNoFlip) 
        { 
            dVector3Inv(m_vContactNormal);
            m_fBestCenter = -fCenter;
        }
    }

    return true;
}

// intersection test between edge and circle
bool sCylinderTrimeshColliderData::_cldTestCircleToEdgeAxis(
    const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
    const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1,
    const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) 
{
    // calculate direction of edge
    dVector3 vkl;
    dVector3Subtract( vVx1 , vVx0 , vkl);
    dNormalize3(vkl);
    // starting point of edge 
    dVector3 vol;
    dVector3Copy(vVx0,vol);

    // calculate angle cosine between cylinder axis and edge
    dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1);

    // if edge is perpendicular to cylinder axis
    if(dFabs(fdot2)<REAL(1e-5))
    {
        // this can't be separating axis, because edge is parallel to circle plane
        return true;
    }

    // find point of intersection between edge line and circle plane
    dVector3 vTemp;
    dVector3Subtract(vCenterPoint,vol,vTemp);
    dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1);
    dVector3 vpnt;// = vol + vkl * (fdot1/fdot2);
    vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2;
    vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2;
    vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2;

    // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt)
    dVector3 vTangent;
    dVector3Subtract(vCenterPoint,vpnt,vTemp);
    dVector3Cross(vTemp,vCylinderAxis1,vTangent);

    // find vector orthogonal both to tangent and edge direction
    dVector3 vAxis;
    dVector3Cross(vTangent,vkl,vAxis);

    // use that vector as separating axis
    return _cldTestAxis( v0, v1, v2, vAxis, iAxis );
}

// helper for less key strokes
// r = ( (v1 - v2) cross v3 ) cross v3
inline void _CalculateAxis(const dVector3& v1,
                           const dVector3& v2,
                           const dVector3& v3,
                           dVector3& r)
{
    dVector3 t1;
    dVector3 t2;

    dVector3Subtract(v1,v2,t1);
    dVector3Cross(t1,v3,t2);
    dVector3Cross(t2,v3,r);
}

bool sCylinderTrimeshColliderData::_cldTestSeparatingAxes(
    const dVector3 &v0,
    const dVector3 &v1,
    const dVector3 &v2) 
{

    // calculate edge vectors
    dVector3Subtract(v1 ,v0 , m_vE0);
    // m_vE1 has been calculated before -> so save some cycles here
    dVector3Subtract(v0 ,v2 , m_vE2);

    // calculate caps centers in absolute space
    dVector3 vCp0;
    vCp0[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5));
    vCp0[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5));
    vCp0[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5));

#if 0
    dVector3 vCp1;
    vCp1[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5));
    vCp1[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5));
    vCp1[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5));
#endif

    // reset best axis
    m_iBestAxis = 0;
    dVector3 vAxis;

    // axis m_vNormal
    //vAxis = -m_vNormal;
    vAxis[0] = -m_vNormal[0];
    vAxis[1] = -m_vNormal[1];
    vAxis[2] = -m_vNormal[2];
    if (!_cldTestAxis(v0, v1, v2, vAxis, 1, true)) 
    { 
        return false; 
    }

    // axis CxE0
    // vAxis = ( m_vCylinderAxis cross m_vE0 );
    dVector3Cross(m_vCylinderAxis, m_vE0,vAxis);
    if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) 
    { 
        return false; 
    }

    // axis CxE1
    // vAxis = ( m_vCylinderAxis cross m_vE1 );
    dVector3Cross(m_vCylinderAxis, m_vE1,vAxis);
    if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) 
    { 
        return false; 
    }

    // axis CxE2
    // vAxis = ( m_vCylinderAxis cross m_vE2 );
    dVector3Cross(m_vCylinderAxis, m_vE2,vAxis);
    if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) 
    { 
        return false; 
    }

    // first vertex on triangle
    // axis ((V0-Cp0) x C) x C
    //vAxis = ( ( v0-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis;
    _CalculateAxis(v0 , vCp0 , m_vCylinderAxis , vAxis);
    if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) 
    { 
        return false; 
    }

    // second vertex on triangle
    // axis ((V1-Cp0) x C) x C
    // vAxis = ( ( v1-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis;
    _CalculateAxis(v1 , vCp0 , m_vCylinderAxis , vAxis);
    if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) 
    { 
        return false; 
    }

    // third vertex on triangle
    // axis ((V2-Cp0) x C) x C
    //vAxis = ( ( v2-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis;
    _CalculateAxis(v2 , vCp0 , m_vCylinderAxis , vAxis);
    if (!_cldTestAxis(v0, v1, v2, vAxis, 13))
    { 
        return false; 
    }

    // test cylinder axis
    // vAxis = m_vCylinderAxis;
    dVector3Copy(m_vCylinderAxis , vAxis);
    if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) 
    { 
        return false; 
    }

    // Test top and bottom circle ring of cylinder for separation
    dVector3 vccATop;
    vccATop[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5));
    vccATop[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5));
    vccATop[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5));

    dVector3 vccABottom;
    vccABottom[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5));
    vccABottom[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5));
    vccABottom[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5));


    if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v1, 15)) 
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v1, v2, 16)) 
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v2, 17)) 
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v1, 18)) 
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v1, v2, 19)) 
    {
        return false;
    }

    if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v2, 20)) 
    {
        return false;
    }

    return true;
}

bool sCylinderTrimeshColliderData::_cldClipCylinderEdgeToTriangle(
    const dVector3 &v0, const dVector3 &/*v1*/, const dVector3 &/*v2*/)
{
    // translate cylinder
    dReal fTemp = dVector3Dot(m_vCylinderAxis , m_vContactNormal);
    dVector3 vN2;
    vN2[0] = m_vContactNormal[0] - m_vCylinderAxis[0]*fTemp;
    vN2[1] = m_vContactNormal[1] - m_vCylinderAxis[1]*fTemp;
    vN2[2] = m_vContactNormal[2] - m_vCylinderAxis[2]*fTemp;

    fTemp = dVector3Length(vN2);
    if (fTemp < REAL(1e-5))
    {
        return false;
    }

    // Normalize it
    vN2[0] /= fTemp;
    vN2[1] /= fTemp;
    vN2[2] /= fTemp;

    // calculate caps centers in absolute space
    dVector3 vCposTrans;
    vCposTrans[0] = m_vCylinderPos[0] + vN2[0]*m_fCylinderRadius;
    vCposTrans[1] = m_vCylinderPos[1] + vN2[1]*m_fCylinderRadius;
    vCposTrans[2] = m_vCylinderPos[2] + vN2[2]*m_fCylinderRadius;

    dVector3 vCEdgePoint0;
    vCEdgePoint0[0]  = vCposTrans[0] + m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5));
    vCEdgePoint0[1]  = vCposTrans[1] + m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5));
    vCEdgePoint0[2]  = vCposTrans[2] + m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5));

    dVector3 vCEdgePoint1;
    vCEdgePoint1[0]  = vCposTrans[0] - m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5));
    vCEdgePoint1[1]  = vCposTrans[1] - m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5));
    vCEdgePoint1[2]  = vCposTrans[2] - m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5));

    // transform cylinder edge points into triangle space
    vCEdgePoint0[0] -= v0[0];
    vCEdgePoint0[1] -= v0[1];
    vCEdgePoint0[2] -= v0[2];

    vCEdgePoint1[0] -= v0[0];
    vCEdgePoint1[1] -= v0[1];
    vCEdgePoint1[2] -= v0[2];

    dVector4 plPlane;
    dVector3 vPlaneNormal;

    // triangle plane
    //plPlane = Plane4f( -m_vNormal, 0);
    vPlaneNormal[0] = -m_vNormal[0];
    vPlaneNormal[1] = -m_vNormal[1];
    vPlaneNormal[2] = -m_vNormal[2];
    dConstructPlane(vPlaneNormal,REAL(0.0),plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
    { 
        return false; 
    }

    // plane with edge 0
    //plPlane = Plane4f( ( m_vNormal cross m_vE0 ), REAL(1e-5));
    dVector3Cross(m_vNormal,m_vE0,vPlaneNormal);
    dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
    { 
        return false; 
    }

    // plane with edge 1
    //dVector3 vTemp = ( m_vNormal cross m_vE1 );
    dVector3Cross(m_vNormal,m_vE1,vPlaneNormal);
    fTemp = dVector3Dot(m_vE0 , vPlaneNormal) - REAL(1e-5);
    //plPlane = Plane4f( vTemp, -(( m_vE0 dot vTemp )-REAL(1e-5)));
    dConstructPlane(vPlaneNormal,-fTemp,plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
    {
        return false;
    }

    // plane with edge 2
    // plPlane = Plane4f( ( m_vNormal cross m_vE2 ), REAL(1e-5));
    dVector3Cross(m_vNormal,m_vE2,vPlaneNormal);
    dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
    if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
    { 
        return false; 
    }

    // return capsule edge points into absolute space
    vCEdgePoint0[0] += v0[0];
    vCEdgePoint0[1] += v0[1];
    vCEdgePoint0[2] += v0[2];

    vCEdgePoint1[0] += v0[0];
    vCEdgePoint1[1] += v0[1];
    vCEdgePoint1[2] += v0[2];

    // calculate depths for both contact points
    dVector3 vTemp;
    dVector3Subtract(vCEdgePoint0,m_vCylinderPos, vTemp);
    dReal fRestDepth0 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt;
    dVector3Subtract(vCEdgePoint1,m_vCylinderPos, vTemp);
    dReal fRestDepth1 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt;

    dReal fDepth0 = m_fBestDepth - (fRestDepth0);
    dReal fDepth1 = m_fBestDepth - (fRestDepth1);

    // clamp depths to zero
    if(fDepth0 < REAL(0.0) ) 
    {
        fDepth0 = REAL(0.0);
    }

    if(fDepth1<REAL(0.0)) 
    {
        fDepth1 = REAL(0.0);
    }

    // Generate contact 0
    {
        m_gLocalContacts[m_nContacts].fDepth = fDepth0;
        dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal);
        dVector3Copy(vCEdgePoint0,m_gLocalContacts[m_nContacts].vPos);
        m_gLocalContacts[m_nContacts].nFlags = 1;
        m_nContacts++;
        if(m_nContacts >= (m_iFlags & NUMC_MASK)) 
            return true;
    }

    // Generate contact 1
    {
        // generate contacts
        m_gLocalContacts[m_nContacts].fDepth = fDepth1;
        dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal);
        dVector3Copy(vCEdgePoint1,m_gLocalContacts[m_nContacts].vPos);
        m_gLocalContacts[m_nContacts].nFlags = 1;
        m_nContacts++;		
    }

    return true;
}

void sCylinderTrimeshColliderData::_cldClipCylinderToTriangle(
    const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
{
    int i = 0;
    dVector3 avPoints[3];
    dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];
    dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];

    dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);
    dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);

    // setup array of triangle vertices
    dVector3Copy(v0,avPoints[0]);
    dVector3Copy(v1,avPoints[1]);
    dVector3Copy(v2,avPoints[2]);

    dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
    dSetZero(vCylinderCircleNormal_Rel,4);
    // check which circle from cylinder we take for clipping
    if ( dVector3Dot(m_vCylinderAxis , m_vContactNormal) > REAL(0.0)) 
    {
        // get top circle
        vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
        vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
        vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));

        vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
    } 
    else 
    {
        // get bottom circle
        vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
        vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
        vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));

        vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
    }

    dVector3 vTemp;
    dQuatInv(m_qCylinderRot , m_qInvCylinderRot);
    // transform triangle points to space of cylinder circle
    for(i=0; i<3; i++) 
    {
        dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp);
        dQuatTransform(m_qInvCylinderRot,vTemp,avPoints[i]);
    }

    int iTmpCounter1 = 0;
    int iTmpCounter2 = 0;
    dVector4 plPlane;

    // plane of cylinder that contains circle for intersection
    //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f );
    dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
    dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane);

    // Body of base circle of Cylinder
    int nCircleSegment = 0;
    for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++)
    {
        dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane);

        if (0 == (nCircleSegment % 2))
        {
            dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
        }
        else
        {
            dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
        }

        dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
        dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
    }

    // back transform clipped points to absolute space
    dReal ftmpdot;	
    dReal fTempDepth;
    dVector3 vPoint;

    if (nCircleSegment %2)
    {
        for( i=0; i<iTmpCounter2; i++)
        {
            dQuatTransform(m_qCylinderRot,avTempArray2[i], vPoint);
            vPoint[0] += vCylinderCirclePos[0];
            vPoint[1] += vCylinderCirclePos[1];
            vPoint[2] += vCylinderCirclePos[2];

            dVector3Subtract(vPoint,m_vCylinderPos,vTemp);
            ftmpdot	 = dFabs(dVector3Dot(vTemp, m_vContactNormal));
            fTempDepth = m_fBestrt - ftmpdot;
            // Depth must be positive
            if (fTempDepth > REAL(0.0))
            {
                m_gLocalContacts[m_nContacts].fDepth = fTempDepth;
                dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal);
                dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos);
                m_gLocalContacts[m_nContacts].nFlags = 1;
                m_nContacts++;
                if(m_nContacts >= (m_iFlags & NUMC_MASK)) 
                    return;;
            }
        }
    }
    else
    {
        for( i=0; i<iTmpCounter1; i++)
        {
            dQuatTransform(m_qCylinderRot,avTempArray1[i], vPoint);
            vPoint[0] += vCylinderCirclePos[0];
            vPoint[1] += vCylinderCirclePos[1];
            vPoint[2] += vCylinderCirclePos[2];

            dVector3Subtract(vPoint,m_vCylinderPos,vTemp);
            ftmpdot	 = dFabs(dVector3Dot(vTemp, m_vContactNormal));
            fTempDepth = m_fBestrt - ftmpdot;
            // Depth must be positive
            if (fTempDepth > REAL(0.0))
            {
                m_gLocalContacts[m_nContacts].fDepth = fTempDepth;
                dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal);
                dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos);
                m_gLocalContacts[m_nContacts].nFlags = 1;
                m_nContacts++;
                if(m_nContacts >= (m_iFlags & NUMC_MASK)) 
                    return;;
            }
        }
    }
}

void sCylinderTrimeshColliderData::TestOneTriangleVsCylinder(
    const dVector3 &v0, 
    const dVector3 &v1, 
    const dVector3 &v2, 
    const bool bDoubleSided)
{
    // calculate triangle normal
    dVector3Subtract( v2 , v1 , m_vE1);
    dVector3 vTemp;
    dVector3Subtract( v0 , v1 ,vTemp);
    dVector3Cross(m_vE1 , vTemp , m_vNormal );

    // Even though all triangles might be initially valid, 
    // a triangle may degenerate into a segment after applying 
    // space transformation.
    if (!dSafeNormalize3( m_vNormal))
    {
        return;
    }

    // create plane from triangle
    //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); 
    dReal plDistance = -dVector3Dot(v0, m_vNormal);
    dVector4 plTrianglePlane;
    dConstructPlane( m_vNormal,plDistance,plTrianglePlane);

    // calculate sphere distance to plane
    dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(m_vCylinderPos , plTrianglePlane);

    // Sphere must be over positive side of triangle
    if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) 
    {
        // if not don't generate contacts
        return;
    }

    dVector3 vPnt0;
    dVector3 vPnt1;
    dVector3 vPnt2;

    if (fDistanceCylinderCenterToPlane < REAL(0.0) )
    {
        // flip it
        dVector3Copy(v0 , vPnt0);
        dVector3Copy(v1 , vPnt2);
        dVector3Copy(v2 , vPnt1);
    }
    else
    {
        dVector3Copy(v0 , vPnt0);
        dVector3Copy(v1 , vPnt1);
        dVector3Copy(v2 , vPnt2);
    }

    m_fBestDepth = MAX_REAL;

    // do intersection test and find best separating axis
    if(!_cldTestSeparatingAxes(vPnt0, vPnt1, vPnt2) ) 
    {
        // if not found do nothing
        return;
    }

    // if best separation axis is not found
    if ( m_iBestAxis == 0 ) 
    {
        // this should not happen (the function should have already returned in this case)
        dIASSERT(false);
        // do nothing
        return;
    }

    dReal fdot = dVector3Dot( m_vContactNormal , m_vCylinderAxis );

    // choose which clipping method are we going to apply
    if (dFabs(fdot) < REAL(0.9) ) 
    {
        if (!_cldClipCylinderEdgeToTriangle(vPnt0, vPnt1, vPnt2)) 
        {
            return;
        }
    }
    else 
    {
        _cldClipCylinderToTriangle(vPnt0, vPnt1, vPnt2);
    }
}

void sCylinderTrimeshColliderData::_InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh)
{
    // get cylinder information
    // Rotation
    const dReal* pRotCyc = dGeomGetRotation(Cylinder); 
    dMatrix3Copy(pRotCyc,m_mCylinderRot);
    dGeomGetQuaternion(Cylinder,m_qCylinderRot);

    // Position
    const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(Cylinder);
    dVector3Copy(*pPosCyc,m_vCylinderPos);
    // Cylinder axis
    dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis);
    // get cylinder radius and size
    dGeomCylinderGetParams(Cylinder,&m_fCylinderRadius,&m_fCylinderSize);

    // get trimesh position and orientation
    const dReal* pRotTris = dGeomGetRotation(Trimesh); 
    dMatrix3Copy(pRotTris,m_mTrimeshRot);
    dGeomGetQuaternion(Trimesh,m_qTrimeshRot);

    // Position
    const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(Trimesh);
    dVector3Copy(*pPosTris,m_vTrimeshPos);


    // calculate basic angle for 8-gon
    dReal fAngle = (dReal) (M_PI / nCYLINDER_CIRCLE_SEGMENTS);
    // calculate angle increment
    dReal fAngleIncrement = fAngle*REAL(2.0); 

    // calculate plane normals
    // axis dependant code
    for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++) 
    {
        m_avCylinderNormals[i][0] = -dCos(fAngle);
        m_avCylinderNormals[i][1] = -dSin(fAngle);
        m_avCylinderNormals[i][2] = REAL(0.0);

        fAngle += fAngleIncrement;
    }

    dSetZero(m_vBestPoint,4);
    // reset best depth
    m_fBestCenter = REAL(0.0);	
}

int sCylinderTrimeshColliderData::TestCollisionForSingleTriangle(int ctContacts0, 
                                                                 int Triint, dVector3 dv[3], bool &bOutFinishSearching)
{
    // test this triangle
    TestOneTriangleVsCylinder(dv[0],dv[1],dv[2], false);

    // fill-in tri index for generated contacts
    for (; ctContacts0<m_nContacts; ctContacts0++)
        m_gLocalContacts[ctContacts0].triIndex = Triint;

    // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
    bOutFinishSearching = (m_nContacts >= (m_iFlags & NUMC_MASK));

    return ctContacts0;
}

// OPCODE version of cylinder to mesh collider
#if dTRIMESH_OPCODE
static void dQueryCTLPotentialCollisionTriangles(OBBCollider &Collider, 
                                                 sCylinderTrimeshColliderData &cData, dxGeom *Cylinder, dxTriMesh *Trimesh,
                                                 OBBCache &BoxCache)
{
    Matrix4x4 MeshMatrix;
    const dVector3 vZeroVector3 = { REAL(0.0), };
    MakeMatrix(vZeroVector3, cData.m_mTrimeshRot, MeshMatrix);

    const dVector3 &vCylinderPos = cData.m_vCylinderPos;
    const dMatrix3 &mCylinderRot = cData.m_mCylinderRot;

    dVector3 vCylinderOffsetPos;
    dSubtractVectors3(vCylinderOffsetPos, vCylinderPos, cData.m_vTrimeshPos);

    const dReal fCylinderRadius = cData.m_fCylinderRadius, fCylinderHalfAxis = cData.m_fCylinderSize * REAL(0.5);

    OBB obbCylinder;
    obbCylinder.mCenter.Set(vCylinderOffsetPos[0], vCylinderOffsetPos[1], vCylinderOffsetPos[2]);
    obbCylinder.mExtents.Set(
        0 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius,
        1 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius,
        2 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius);
    obbCylinder.mRot.Set(
        mCylinderRot[0], mCylinderRot[4], mCylinderRot[8],
        mCylinderRot[1], mCylinderRot[5], mCylinderRot[9],
        mCylinderRot[2], mCylinderRot[6], mCylinderRot[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 == Cylinder)
            {
                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 = Cylinder;
            BoxTC->FatCoeff = REAL(1.0);
        }

        // Intersect
        Collider.SetTemporalCoherence(true);
        Collider.Collide(*BoxTC, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
    }
    else 
    {
        Collider.SetTemporalCoherence(false);
        Collider.Collide(BoxCache, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
    }
}

int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    dIASSERT( skip >= (int)sizeof( dContactGeom ) );
    dIASSERT( o1->type == dCylinderClass );
    dIASSERT( o2->type == dTriMeshClass );
    dIASSERT ((flags & NUMC_MASK) >= 1);

    int nContactCount = 0;

    dxGeom *Cylinder = o1;
    dxTriMesh *Trimesh = (dxTriMesh *)o2;

    // Main data holder
    sCylinderTrimeshColliderData cData(flags, skip);
    cData._InitCylinderTrimeshData(Cylinder, Trimesh);

    const unsigned uiTLSKind = Trimesh->getParentSpaceTLSKind();
    dIASSERT(uiTLSKind == Cylinder->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
    TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
    OBBCollider& Collider = pccColliderCache->m_OBBCollider;

    dQueryCTLPotentialCollisionTriangles(Collider, cData, Cylinder, Trimesh, pccColliderCache->m_DefaultBoxCache);

    // Retrieve data
    int TriCount = Collider.GetNbTouchedPrimitives();

    if (TriCount != 0)
    {
        const int* Triangles = (const int*)Collider.GetTouchedPrimitives();

        if (Trimesh->m_ArrayCallback != NULL)
        {
            Trimesh->m_ArrayCallback(Trimesh, Cylinder, Triangles, TriCount);
        }

        // allocate buffer for local contacts on stack
        cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK));

        int ctContacts0 = 0;

        // loop through all intersecting triangles
        for (int i = 0; i < TriCount; i++)
        {
            const int Triint = Triangles[i];
            if (!Trimesh->invokeCallback(Cylinder, Triint)) continue;


            dVector3 dv[3];
            Trimesh->fetchMeshTriangle(dv, Triint, cData.m_vTrimeshPos, cData.m_mTrimeshRot);

            bool bFinishSearching;
            ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching);

            if (bFinishSearching) 
            {
                break;
            }
        }

        if (cData.m_nContacts != 0)
        {
            nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh);
        }
    }

    return nContactCount;
}
#endif

// GIMPACT version of cylinder to mesh collider
#if dTRIMESH_GIMPACT
int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
    dIASSERT( skip >= (int)sizeof( dContactGeom ) );
    dIASSERT( o1->type == dCylinderClass );
    dIASSERT( o2->type == dTriMeshClass );
    dIASSERT ((flags & NUMC_MASK) >= 1);

    int nContactCount = 0;

    dxGeom *Cylinder = o1;
    dxTriMesh *Trimesh = (dxTriMesh *)o2;

    // Main data holder
    sCylinderTrimeshColliderData cData(flags, skip);
    cData._InitCylinderTrimeshData(Cylinder, Trimesh);

    //*****at first , collide box aabb******//

    aabb3f test_aabb(o1->aabb[0], o1->aabb[1], o1->aabb[2], o1->aabb[3], o1->aabb[4], o1->aabb[5]);


    GDYNAMIC_ARRAY collision_result;
    GIM_CREATE_BOXQUERY_LIST(collision_result);

    gim_aabbset_box_collision(&test_aabb, &Trimesh->m_collision_trimesh.m_aabbset , &collision_result);

    if (collision_result.m_size != 0)
    {
        //*****Set globals for box collision******//

        int ctContacts0 = 0;
        cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK));

        GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
        GIM_TRIMESH * ptrimesh = &Trimesh->m_collision_trimesh;

        gim_trimesh_locks_work_data(ptrimesh);

        for(unsigned int i=0;i<collision_result.m_size;i++)
        {
            const int Triint = boxesresult[i];

            dVector3 dv[3];
            gim_trimesh_get_triangle_vertices(ptrimesh, Triint, dv[0], dv[1], dv[2]);

            bool bFinishSearching;
            ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching);

            if (bFinishSearching) 
            {
                break;
            }
        }

        gim_trimesh_unlocks_work_data(ptrimesh);

        if (cData.m_nContacts != 0)
        {
            nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh);
        }
    }

    GIM_DYNARRAY_DESTROY(collision_result);

    return nContactCount;
}
#endif

#endif // dTRIMESH_ENABLED