/*************************************************************************
*                                                                       *
* 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/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"

#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"

#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;

typedef struct _sCylinderTrimeshColliderData
{
	// cylinder data
	dMatrix3	mCylinderRot;
	dQuaternion	qCylinderRot;
	dQuaternion	qInvCylinderRot;
	dVector3	vCylinderPos;
	dVector3	vCylinderAxis;
	dReal		fCylinderRadius;
	dReal		fCylinderSize;
	dVector3	avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS];

	// mesh data
	dQuaternion	qTrimeshRot;
	dQuaternion	qInvTrimeshRot;
	dMatrix3	mTrimeshRot;
	dVector3	vTrimeshPos;

	// global collider data
	dVector3	vBestPoint;
	dReal		fBestDepth;
	dReal		fBestCenter;
	dReal		fBestrt;
	int			iBestAxis;
	dVector3	vContactNormal;
	dVector3	vNormal;
	dVector3	vE0;
	dVector3	vE1;
	dVector3	vE2;

	// ODE stuff
	dGeomID				gCylinder;
	dxTriMesh*			gTrimesh;
	dContactGeom*		gContact;
	int					iFlags;
	int					iSkip;
	int					nContacts;// = 0;
	sLocalContactData*	gLocalContacts;
} sCylinderTrimeshColliderData;

// Short type name
typedef  sCylinderTrimeshColliderData sData;

// 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"
inline void	_OptimizeLocalContacts(sData& cData)
{
	int nContacts = cData.nContacts;

	for (int i = 0; i < nContacts-1; i++)
	{
		for (int j = i+1; j < nContacts; j++)
		{
			if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j]))
			{
				// If they are seem to be the samed then filtered 
				// out the least penetrate one
				if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i]))
				{
					cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact
				}
				else
				{
					cData.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???
			}
		}
	}
}

inline int	_ProcessLocalContacts(sData& cData)
{
	if (cData.nContacts == 0)
	{
		return 0;
	}

#ifdef OPTIMIZE_CONTACTS
	if (cData.nContacts > 1)
	{
		// Can be optimized...
		_OptimizeLocalContacts(cData);
	}
#endif		

	int iContact = 0;
	dContactGeom* Contact = 0;

	int nFinalContact = 0;

	for (iContact = 0; iContact < cData.nContacts; iContact ++)
	{
		if (1 == cData.gLocalContacts[iContact].nFlags)
		{
			Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip);
			Contact->depth = cData.gLocalContacts[iContact].fDepth;
			dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal);
			dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos);
			Contact->g1 = cData.gCylinder;
			Contact->g2 = cData.gTrimesh;
			Contact->side2 = cData.gLocalContacts[iContact].triIndex;
			dVector3Inv(Contact->normal);

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

	return nFinalContact;
}


bool _cldTestAxis(sData& cData,
				  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 < 1e-5f )
	{
		// do nothing
		return true;
	}

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

	dReal fdot1 = dVector3Dot(cData.vCylinderAxis,vAxis);
	// project capsule on vAxis
	dReal frc;

	if (dFabs(fdot1) > REAL(1.0) ) 
	{
//		fdot1 = REAL(1.0);
		frc = dFabs(cData.fCylinderSize* REAL(0.5));
	}
	else
	{
		frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1)
			+ cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
	}
  
	dVector3 vV0;
	dVector3Subtract(v0,cData.vCylinderPos,vV0);
	dVector3 vV1;
	dVector3Subtract(v1,cData.vCylinderPos,vV1);
	dVector3 vV2;
	dVector3Subtract(v2,cData.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 < cData.fBestDepth ) 
	{
		// remember depth
		cData.fBestDepth			= fDepth;
		cData.fBestCenter		    = fCenter;
		cData.fBestrt				= frc;
		dVector3Copy(vAxis,cData.vContactNormal);
		cData.iBestAxis				= iAxis;
	  
		// flip normal if interval is wrong faced
		if ( fCenter< REAL(0.0) && !bNoFlip) 
		{ 
			dVector3Inv(cData.vContactNormal);
			cData.fBestCenter = -fCenter;
		}
	}
  
	return true;
}

// intersection test between edge and circle
bool _cldTestCircleToEdgeAxis(sData& cData,
							  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)<1e-5f)
	{
		// 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( cData ,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 _cldTestSeparatingAxes(sData& cData,
							const dVector3 &v0,
							const dVector3 &v1,
							const dVector3 &v2) 
{

	// calculate edge vectors
	dVector3Subtract(v1 ,v0 , cData.vE0);
	// cData.vE1 has been calculated before -> so save some cycles here
	dVector3Subtract(v0 ,v2 , cData.vE2);

	// calculate caps centers in absolute space
	dVector3 vCp0;
	vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
	vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
	vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));

	dVector3 vCp1;
	vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
	vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
	vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));

	// reset best axis
	cData.iBestAxis = 0;
	dVector3 vAxis;

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

	// axis CxE0
	// vAxis = ( cData.vCylinderAxis cross cData.vE0 );
	dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis);
	if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2)) 
	{ 
		return false; 
	}

	// axis CxE1
	// vAxis = ( cData.vCylinderAxis cross cData.vE1 );
	dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis);
	if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3)) 
	{ 
		return false; 
	}

	// axis CxE2
	// vAxis = ( cData.vCylinderAxis cross cData.vE2 );
	dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis);
	if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4)) 
	{ 
		return false; 
	}

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

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

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

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

	// Test top and bottom circle ring of cylinder for separation
	dVector3 vccATop;
	vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
	vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
	vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));

	dVector3 vccABottom;
	vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
	vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
	vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));


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

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

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

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

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

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

  return true;
}

bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
{
	// translate cylinder
	dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal);
	dVector3 vN2;
	vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp;
	vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp;
	vN2[2] = cData.vContactNormal[2] - cData.vCylinderAxis[2]*fTemp;

	fTemp = dVector3Length(vN2);
	if (fTemp < 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] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius;
	vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius;
	vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius;
	  
	dVector3 vCEdgePoint0;
	vCEdgePoint0[0]  = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
	vCEdgePoint0[1]  = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
	vCEdgePoint0[2]  = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5));

	dVector3 vCEdgePoint1;
	vCEdgePoint1[0]  = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
	vCEdgePoint1[1]  = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
	vCEdgePoint1[2]  = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.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( -cData.vNormal, 0);
	vPlaneNormal[0] = -cData.vNormal[0];
	vPlaneNormal[1] = -cData.vNormal[1];
	vPlaneNormal[2] = -cData.vNormal[2];
	dConstructPlane(vPlaneNormal,REAL(0.0),plPlane);
	if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
	{ 
		return false; 
	}

	// plane with edge 0
	//plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), 1e-5f);
	dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal);
	dConstructPlane(vPlaneNormal,1e-5f,plPlane);
	if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
	{ 
		return false; 
	}
  
	// plane with edge 1
	//dVector3 vTemp = ( cData.vNormal cross cData.vE1 );
	dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal);
	fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - dReal(1e-5);
	//plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-1e-5f));
	dConstructPlane(vPlaneNormal,-fTemp,plPlane);
	if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
	{
		return false;
	}

	// plane with edge 2
	// plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), 1e-5f);
	dVector3Cross(cData.vNormal,cData.vE2,vPlaneNormal);
	dConstructPlane(vPlaneNormal,1e-5f,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,cData.vCylinderPos, vTemp);
	dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;
	dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp);
	dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;
	
	dReal fDepth0 = cData.fBestDepth - (fRestDepth0);
	dReal fDepth1 = cData.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
	{
		cData.gLocalContacts[cData.nContacts].fDepth = fDepth0;
		dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
		dVector3Copy(vCEdgePoint0,cData.gLocalContacts[cData.nContacts].vPos);
		cData.gLocalContacts[cData.nContacts].nFlags = 1;
		cData.nContacts++;
		if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) 
			return true;
	}

	// Generate contact 1
	{
		// generate contacts
		cData.gLocalContacts[cData.nContacts].fDepth = fDepth1;
		dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
		dVector3Copy(vCEdgePoint1,cData.gLocalContacts[cData.nContacts].vPos);
		cData.gLocalContacts[cData.nContacts].nFlags = 1;
		cData.nContacts++;		
	}

	return true;
}

void _cldClipCylinderToTriangle(sData& cData,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(cData.vCylinderAxis , cData.vContactNormal) > REAL(0.0)) 
	{
		// get top circle
		vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));

		vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
	} 
	else 
	{
		// get bottom circle
		vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
		vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));

		vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
	}

	dVector3 vTemp;
	dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot);
	// transform triangle points to space of cylinder circle
	for(i=0; i<3; i++) 
	{
		dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp);
		dQuatTransform(cData.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(cData.avCylinderNormals[nCircleSegment],cData.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(cData.qCylinderRot,avTempArray2[i], vPoint);
			vPoint[0] += vCylinderCirclePos[0];
			vPoint[1] += vCylinderCirclePos[1];
			vPoint[2] += vCylinderCirclePos[2];

			dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
			ftmpdot	 = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
			fTempDepth = cData.fBestrt - ftmpdot;
			// Depth must be positive
			if (fTempDepth > REAL(0.0))
			{
				cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
				dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
				dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
				cData.gLocalContacts[cData.nContacts].nFlags = 1;
				cData.nContacts++;
				if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) 
					return;;
			}
		}
	}
	else
	{
		for( i=0; i<iTmpCounter1; i++)
		{
			dQuatTransform(cData.qCylinderRot,avTempArray1[i], vPoint);
			vPoint[0] += vCylinderCirclePos[0];
			vPoint[1] += vCylinderCirclePos[1];
			vPoint[2] += vCylinderCirclePos[2];

			dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
			ftmpdot	 = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
			fTempDepth = cData.fBestrt - ftmpdot;
			// Depth must be positive
			if (fTempDepth > REAL(0.0))
			{
				cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
				dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
				dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
				cData.gLocalContacts[cData.nContacts].nFlags = 1;
				cData.nContacts++;
				if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) 
					return;;
			}
		}
	}
}

void TestOneTriangleVsCylinder(   sData& cData, 
								  const dVector3 &v0, 
                                  const dVector3 &v1, 
                                  const dVector3 &v2, 
                                  const bool bDoubleSided)
{

	// calculate triangle normal
	dVector3Subtract( v2 , v1 ,cData.vE1);
	dVector3 vTemp;
	dVector3Subtract( v0 , v1 ,vTemp);
	dVector3Cross(cData.vE1 , vTemp , cData.vNormal );

	dNormalize3( cData.vNormal);

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

	 // calculate sphere distance to plane
	dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.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);
	}

	cData.fBestDepth = MAX_REAL;

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

	// if best separation axis is not found
	if ( cData.iBestAxis == 0 ) 
	{
		// this should not happen (we should already exit in that case)
		dIASSERT(false);
		// do nothing
		return;
	}

	dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis );

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

}

void _InitCylinderTrimeshData(sData& cData)
{
	// get cylinder information
	// Rotation
	const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); 
	dMatrix3Copy(pRotCyc,cData.mCylinderRot);
	dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);
	
	// Position
	const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
	dVector3Copy(*pPosCyc,cData.vCylinderPos);
	// Cylinder axis
	dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
	// get cylinder radius and size
	dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
	
	// get trimesh position and orientation
	const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh); 
	dMatrix3Copy(pRotTris,cData.mTrimeshRot);
	dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);
	
	// Position
	const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
	dVector3Copy(*pPosTris,cData.vTrimeshPos);


	// calculate basic angle for 8-gon
	dReal fAngle = 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++) 
	{
		cData.avCylinderNormals[i][0] = -dCos(fAngle);
		cData.avCylinderNormals[i][1] = -dSin(fAngle);
		cData.avCylinderNormals[i][2] = REAL(0.0);

		fAngle += fAngleIncrement;
	}

	dSetZero(cData.vBestPoint,4);
	// reset best depth
	cData.fBestCenter = REAL(0.0);	
}

#if dTRIMESH_ENABLED

// OPCODE version of cylinder to mesh collider
#if dTRIMESH_OPCODE
int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
	// Main data holder
	sData cData;

	// Assign ODE stuff
	cData.gCylinder	 = o1;
	cData.gTrimesh	 = (dxTriMesh*)o2;
	cData.iFlags	 = flags;
	cData.iSkip		 = skip;
	cData.gContact	 = contact;
	cData.nContacts  = 0;

	_InitCylinderTrimeshData(cData);

	OBBCollider& Collider = cData.gTrimesh->_OBBCollider;

	Point cCenter(cData.vCylinderPos[0],cData.vCylinderPos[1],cData.vCylinderPos[2]);

	Point cExtents(cData.fCylinderRadius,cData.fCylinderRadius,cData.fCylinderRadius);
	cExtents[nCYLINDER_AXIS] = cData.fCylinderSize * REAL(0.5);

	Matrix3x3 obbRot;

	obbRot[0][0] = (float)cData.mCylinderRot[0];
	obbRot[1][0] = (float)cData.mCylinderRot[1];
	obbRot[2][0] = (float)cData.mCylinderRot[2];

	obbRot[0][1] = (float)cData.mCylinderRot[4];
	obbRot[1][1] = (float)cData.mCylinderRot[5];
	obbRot[2][1] = (float)cData.mCylinderRot[6];

	obbRot[0][2] = (float)cData.mCylinderRot[8];
	obbRot[1][2] = (float)cData.mCylinderRot[9];
	obbRot[2][2] = (float)cData.mCylinderRot[10];

	OBB obbCapsule(cCenter,cExtents,obbRot);

	Matrix4x4 CapsuleMatrix;
	MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CapsuleMatrix);

	Matrix4x4 MeshMatrix;
	MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix);

	// TC results
	if (cData.gTrimesh->doBoxTC) 
	{
		dxTriMesh::BoxTC* BoxTC = 0;
		for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++)
		{
			if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder)
			{
				BoxTC = &cData.gTrimesh->BoxTCCache[i];
				break;
			}
		}
		if (!BoxTC)
		{
			cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC());

			BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1];
			BoxTC->Geom = cData.gCylinder;
			BoxTC->FatCoeff = REAL(1.0);
		}

		// Intersect
		Collider.SetTemporalCoherence(true);
		Collider.Collide(*BoxTC, obbCapsule, cData.gTrimesh->Data->BVTree, null, &MeshMatrix);
	}
	else 
	{
		Collider.SetTemporalCoherence(false);
		Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, cData.gTrimesh->Data->BVTree, null,&MeshMatrix);
	}

	// Retrieve data
	int TriCount = Collider.GetNbTouchedPrimitives();
	const int* Triangles = (const int*)Collider.GetTouchedPrimitives();


	if (TriCount != 0)
	{
		if (cData.gTrimesh->ArrayCallback != null)
		{
			cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount);
		}

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

	    int ctContacts0 = 0;

		// loop through all intersecting triangles
		for (int i = 0; i < TriCount; i++)
		{
			if(cData.nContacts	>= (cData.iFlags & NUMC_MASK)) 
			{
				break;
			}

			const int& Triint = Triangles[i];
			if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue;


			dVector3 dv[3];
			FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv);
			
			// test this triangle
			TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);

			// fill-in tri index for generated contacts
			for (; ctContacts0<cData.nContacts; ctContacts0++)
				cData.gLocalContacts[ctContacts0].triIndex = Triint;
		}
	}

	return _ProcessLocalContacts(cData);
}
#endif

// GIMPACT version of cylinder to mesh collider
#if dTRIMESH_GIMPACT
int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
	// Main data holder
	sData cData;

	// Assign ODE stuff
	cData.gCylinder	 = o1;
	cData.gTrimesh	 = (dxTriMesh*)o2;
	cData.iFlags	 = flags;
	cData.iSkip		 = skip;
	cData.gContact	 = contact;
	cData.nContacts  = 0;

	_InitCylinderTrimeshData(cData);

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

	aabb3f test_aabb;

	test_aabb.minX = o1->aabb[0];
	test_aabb.maxX = o1->aabb[1];
	test_aabb.minY = o1->aabb[2];
	test_aabb.maxY = o1->aabb[3];
	test_aabb.minZ = o1->aabb[4];
	test_aabb.maxZ = o1->aabb[5];


	GDYNAMIC_ARRAY collision_result;
	GIM_CREATE_BOXQUERY_LIST(collision_result);

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

	if(collision_result.m_size==0)
	{
	    GIM_DYNARRAY_DESTROY(collision_result);
	    return 0;
	}
//*****Set globals for box collision******//

	int ctContacts0 = 0;
	cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK));

	GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
	GIM_TRIMESH * ptrimesh = &cData.gTrimesh->m_collision_trimesh;

	gim_trimesh_locks_work_data(ptrimesh);


	for(unsigned int i=0;i<collision_result.m_size;i++)
	{
	    if(cData.nContacts	>= (cData.iFlags & NUMC_MASK))
        {
            break;
        }

		dVector3 dv[3];
		gim_trimesh_get_triangle_vertices(ptrimesh, boxesresult[i],dv[0],dv[1],dv[2]);
        // test this triangle
        TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);

        // fill-in tri index for generated contacts
        for (; ctContacts0<cData.nContacts; ctContacts0++)
            cData.gLocalContacts[ctContacts0].triIndex =  boxesresult[i];
	}

	gim_trimesh_unlocks_work_data(ptrimesh);
	GIM_DYNARRAY_DESTROY(collision_result);

	return _ProcessLocalContacts(cData);
}
#endif

#endif // dTRIMESH_ENABLED