/*************************************************************************

*                                                                       *

* 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-box 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"

#include <ode/lookup_tables.h>

static const int MAX_CYLBOX_CLIP_POINTS  = 16;

static const int nCYLINDER_AXIS			 = 2;

// Number of segment of cylinder base circle.

// Must be divisible by 4.

static const int nCYLINDER_SEGMENT		 = 8;

#define MAX_FLOAT	dInfinity

// Data that passed through the collider's functions

typedef struct _sCylinderBoxData

{

	// cylinder parameters

	dMatrix3			mCylinderRot;

	dVector3			vCylinderPos;

	dVector3			vCylinderAxis;

	dReal				fCylinderRadius;

	dReal				fCylinderSize;

	dVector3			avCylinderNormals[nCYLINDER_SEGMENT];

	// box parameters

	dMatrix3			mBoxRot;

	dVector3			vBoxPos;

	dVector3			vBoxHalfSize;

	// box vertices array : 8 vertices

	dVector3			avBoxVertices[8];

	// global collider data

	dVector3			vDiff;			

	dVector3			vNormal;

	dReal				fBestDepth;

	dReal				fBestrb;

	dReal				fBestrc;

	int					iBestAxis;

	// contact data

	dVector3			vEp0, vEp1;

	dReal				fDepth0, fDepth1;

	// ODE stuff

	dGeomID				gBox;

	dGeomID				gCylinder;

	dContactGeom*		gContact;

	int					iFlags;

	int					iSkip;

	int					nContacts;

} sCylinderBoxData;

// initialize collision data

void _cldInitCylinderBox(sCylinderBoxData& cData) 

{

	// get cylinder position, orientation

	const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); 

	dMatrix3Copy(pRotCyc,cData.mCylinderRot);

	const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);

	dVector3Copy(*pPosCyc,cData.vCylinderPos);

	dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);

	// get cylinder radius and size

	dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);

	// get box position, orientation, size

	const dReal* pRotBox = dGeomGetRotation(cData.gBox);

	dMatrix3Copy(pRotBox,cData.mBoxRot);

	const dVector3* pPosBox  = (const dVector3*)dGeomGetPosition(cData.gBox);

	dVector3Copy(*pPosBox,cData.vBoxPos);

	dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);

	cData.vBoxHalfSize[0] = dMUL(cData.vBoxHalfSize[0],REAL(0.5));

	cData.vBoxHalfSize[1] = dMUL(cData.vBoxHalfSize[1],REAL(0.5));

	cData.vBoxHalfSize[2] = dMUL(cData.vBoxHalfSize[2],REAL(0.5));

	// vertex 0

	cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];

	cData.avBoxVertices[0][1] =  cData.vBoxHalfSize[1];

	cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];

	// vertex 1

	cData.avBoxVertices[1][0] =  cData.vBoxHalfSize[0];

	cData.avBoxVertices[1][1] =  cData.vBoxHalfSize[1];

	cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];

	// vertex 2

	cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];

	cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];

	cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];

	// vertex 3

	cData.avBoxVertices[3][0] =  cData.vBoxHalfSize[0];

	cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];

	cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];

	// vertex 4

	cData.avBoxVertices[4][0] =  cData.vBoxHalfSize[0];

	cData.avBoxVertices[4][1] =  cData.vBoxHalfSize[1];

	cData.avBoxVertices[4][2] =  cData.vBoxHalfSize[2];

	// vertex 5

	cData.avBoxVertices[5][0] =  cData.vBoxHalfSize[0];

	cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];

	cData.avBoxVertices[5][2] =  cData.vBoxHalfSize[2];

	// vertex 6

	cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];

	cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];

	cData.avBoxVertices[6][2] =  cData.vBoxHalfSize[2];

	// vertex 7

	cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];

	cData.avBoxVertices[7][1] =  cData.vBoxHalfSize[1];

	cData.avBoxVertices[7][2] =  cData.vBoxHalfSize[2];

	// temp index

	int i = 0;

	dVector3	vTempBoxVertices[8];

	// transform vertices in absolute space

	for(i=0; i < 8; i++) 

	{

		dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);

		dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);

	}

	// find relative position

	dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);

	cData.fBestDepth = MAX_FLOAT;

	cData.vNormal[0] = REAL(0.0);

	cData.vNormal[1] = REAL(0.0);

	cData.vNormal[2] = REAL(0.0);

	// calculate basic angle for nCYLINDER_SEGMENT-gon

	dReal fAngle = dPI/nCYLINDER_SEGMENT;

	// calculate angle increment

	dReal fAngleIncrement = dMUL(fAngle,REAL(2.0)); 

	// calculate nCYLINDER_SEGMENT-gon points

	for(i = 0; i < nCYLINDER_SEGMENT; i++) 

	{

		cData.avCylinderNormals[i][0] = -dCos(fAngle);

		cData.avCylinderNormals[i][1] = -dSin(fAngle);

		cData.avCylinderNormals[i][2] = 0;

		fAngle += fAngleIncrement;

	}

	cData.fBestrb		= 0;

	cData.fBestrc		= 0;

	cData.iBestAxis		= 0;

	cData.nContacts		= 0;

}

// test for given separating axis

int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis ) 

{

	// check length of input normal

	dReal fL = dVector3Length(vInputNormal);

	// if not long enough

	if ( fL < REAL(2e-5f) ) 

	{

		// do nothing

		return 1;

	}

	// otherwise make it unit for sure

	dNormalize3(vInputNormal);

	// project box and Cylinder on mAxis

	dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal);

	dReal frc;

	if (fdot1 > REAL(1.0)) 

	{

		fdot1 = REAL(1.0);

		frc = dFabs(dMUL(cData.fCylinderSize,REAL(0.5)));

	}

	// project box and capsule on iAxis

	frc = dFabs( dMUL(fdot1,(cData.fCylinderSize*REAL(0.5)))) + dMUL(cData.fCylinderRadius,dSqrt(REAL(1.0)-(dMUL(fdot1,fdot1))));

	dVector3	vTemp1;

	dReal frb = REAL(0.0);

	dMat3GetCol(cData.mBoxRot,0,vTemp1);

	frb = dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[0]);

	dMat3GetCol(cData.mBoxRot,1,vTemp1);

	frb += dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[1]);

	dMat3GetCol(cData.mBoxRot,2,vTemp1);

	frb += dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[2]);

	// project their distance on separating axis

	dReal fd  = dVector3Dot(cData.vDiff,vInputNormal);

	// if they do not overlap exit, we have no intersection

	if ( dFabs(fd) > frc+frb )

	{ 

		return 0; 

	} 

	// get depth

	dReal fDepth = - dFabs(fd) + (frc+frb);

	// get maximum depth

	if ( fDepth < cData.fBestDepth ) 

	{

		cData.fBestDepth = fDepth;

		dVector3Copy(vInputNormal,cData.vNormal);

		cData.iBestAxis  = iAxis;

		cData.fBestrb    = frb;

		cData.fBestrc    = frc;

		// flip normal if interval is wrong faced

		if (fd > 0) 

		{ 

			dVector3Inv(cData.vNormal);

		}

	}

	return 1;

}

// check for separation between box edge and cylinder circle edge

int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,

							const dVector3 &vCenterPoint, 

							const dVector3 &vVx0, const dVector3 &vVx1, 

							int iAxis ) 

{

	// calculate direction of edge

	dVector3 vDirEdge;

	dVector3Subtract(vVx1,vVx0,vDirEdge);

	dNormalize3(vDirEdge);

	// starting point of edge 

	dVector3 vEStart;

	dVector3Copy(vVx0,vEStart);

	// calculate angle cosine between cylinder axis and edge

	dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);

	// if edge is perpendicular to cylinder axis

	if(dFabs(fdot2) < REAL(2e-5f)) 

	{

		// this can't be separating axis, because edge is parallel to circle plane

		return 1;

	}

	// find point of intersection between edge line and circle plane

	dVector3 vTemp1;

	dVector3Subtract(vCenterPoint,vEStart,vTemp1);

	dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);

	dVector3 vpnt;

	vpnt[0]= vEStart[0] + dMUL(vDirEdge[0],dDIV(fdot1,fdot2));

	vpnt[1]= vEStart[1] + dMUL(vDirEdge[1],dDIV(fdot1,fdot2));

	vpnt[2]= vEStart[2] + dMUL(vDirEdge[2],dDIV(fdot1,fdot2));

	// find tangent vector on circle with same center (vCenterPoint) that

	// touches point of intersection (vpnt)

	dVector3 vTangent;

	dVector3Subtract(vCenterPoint,vpnt,vTemp1);

	dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);

	// find vector orthogonal both to tangent and edge direction

	dVector3 vAxis;

	dVector3Cross(vTangent,vDirEdge,vAxis);

	// use that vector as separating axis

	return _cldTestAxis( cData, vAxis, iAxis );

}

// Test separating axis for collision

int _cldTestSeparatingAxes(sCylinderBoxData& cData) 

{

	// reset best axis

	cData.fBestDepth = MAX_FLOAT;

	cData.iBestAxis = 0;

	cData.fBestrb = 0;

	cData.fBestrc = 0;

	cData.nContacts = 0;

	dVector3  vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};

	// Epsilon value for checking axis vector length 

	const dReal fEpsilon = REAL(2e-5f);

	// axis A0

	dMat3GetCol(cData.mBoxRot, 0 , vAxis);

	if (!_cldTestAxis( cData, vAxis, 1 )) 

	{

		return 0;

	}

	// axis A1

	dMat3GetCol(cData.mBoxRot, 1 , vAxis);

	if (!_cldTestAxis( cData, vAxis, 2 )) 

	{

		return 0;

	}

	// axis A2

	dMat3GetCol(cData.mBoxRot, 2 , vAxis);

	if (!_cldTestAxis( cData, vAxis, 3 )) 

	{

		return 0;

	}

	// axis C - Cylinder Axis

	//vAxis = vCylinderAxis;

	dVector3Copy(cData.vCylinderAxis , vAxis);

	if (!_cldTestAxis( cData, vAxis, 4 )) 

	{

		return 0;

	}

	// axis CxA0

	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 ));

	dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis);

	if(dVector3Length2( vAxis ) > fEpsilon ) 

	{

		if (!_cldTestAxis( cData, vAxis, 5 ))

		{

			return 0;

		}

	}

	// axis CxA1

	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 ));

	dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis);

	if(dVector3Length2( vAxis ) > fEpsilon ) 

	{

		if (!_cldTestAxis( cData, vAxis, 6 )) 

		{

			return 0;

		}

	}

	// axis CxA2

	//vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 ));

	dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis);

	if(dVector3Length2( vAxis ) > fEpsilon ) 

	{

		if (!_cldTestAxis( cData, vAxis, 7 ))

		{

			return 0;

		}

	}

	int i = 0;

	dVector3	vTemp1;

	dVector3	vTemp2;

	// here we check box's vertices axis

	for(i=0; i< 8; i++) 

	{

		//vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos));

		dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1);

		dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2);

		//vAxis = ( vCylinderAxis cross vAxis );

		dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis);

		if(dVector3Length2( vAxis ) > fEpsilon ) 

		{

			if (!_cldTestAxis( cData, vAxis, 8 + i ))

			{

				return 0;

			}

		}

	}

	// ************************************

	// this is defined for first 12 axes

	// normal of plane that contains top circle of cylinder

	// center of top circle of cylinder

	dVector3 vcc;

	vcc[0] = (cData.vCylinderPos)[0] + dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));

	vcc[1] = (cData.vCylinderPos)[1] + dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));

	vcc[2] = (cData.vCylinderPos)[2] + dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));

	// ************************************

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18))

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20))

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21))

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27)) 

	{

		return 0;

	}

	// ************************************

	// this is defined for second 12 axes

	// normal of plane that contains bottom circle of cylinder

	// center of bottom circle of cylinder

	//	vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5));

	vcc[0] = (cData.vCylinderPos)[0] - dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));

	vcc[1] = (cData.vCylinderPos)[1] - dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));

	vcc[2] = (cData.vCylinderPos)[2] - dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));

	// ************************************

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35)) 

	{

		return 0;

	}

	if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36))