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

 *                                                                       *

 * Open Dynamics Engine, Copyright (C) 2001,2002 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.                     *

 *                                                                       *

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

/*

some useful collision utility stuff.

*/

#ifndef _ODE_COLLISION_UTIL_H_

#define _ODE_COLLISION_UTIL_H_

#include <ode/common.h>

#include <ode/contact.h>

#include <ode/odemath.h>

#include <ode/rotation.h>

// given a pointer `p' to a dContactGeom, return the dContactGeom at

// p + skip bytes.

#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))

// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and

// return 1, else return 0.

int dCollideSpheres (dVector3 p1, dReal r1,

		     dVector3 p2, dReal r2, dContactGeom *c);

// given two lines

//    qa = pa + alpha* ua

//    qb = pb + beta * ub

// where pa,pb are two points, ua,ub are two unit length vectors, and alpha,

// beta go from [-inf,inf], return alpha and beta such that qa and qb are

// as close as possible

void dLineClosestApproach (const dVector3 pa, const dVector3 ua,

			   const dVector3 pb, const dVector3 ub,

			   dReal *alpha, dReal *beta);

// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length

// vector 'side'), compute the points of closest approach between the box

// and the line. return these points in 'lret' (the point on the line) and

// 'bret' (the point on the box). if the line actually penetrates the box

// then the solution is not unique, but only one solution will be returned.

// in this case the solution points will coincide.

void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,

			    const dVector3 c, const dMatrix3 R,

			    const dVector3 side,

			    dVector3 lret, dVector3 bret);

// 20 Apr 2004

// Start code by Nguyen Binh

int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);

// clip polygon with plane and generate new polygon points

void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane );

void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);

// Some vector math

inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)

{

	c[0] = a[0] - b[0];

	c[1] = a[1] - b[1];

	c[2] = a[2] - b[2];

}

// Some vector math

inline void dVector3Scale(dVector3& a,dReal nScale)

{

	a[0] = dMUL(a[0],nScale);

	a[1] = dMUL(a[1],nScale);

	a[2] = dMUL(a[2],nScale);

}

inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)

{

	c[0] = a[0] + b[0];

	c[1] = a[1] + b[1];

	c[2] = a[2] + b[2];

}

inline void dVector3Copy(const dVector3& a,dVector3& c)

{

	c[0] = a[0];

	c[1] = a[1];

	c[2] = a[2];

}

inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)

{

	dCROSS(c,=,a,b);

}

inline dReal dVector3Length(const dVector3& a)

{

	return dSqrt(dMUL(a[0],a[0])+dMUL(a[1],a[1])+dMUL(a[2],a[2]));

}

inline dReal dVector3Dot(const dVector3& a,const dVector3& b)

{

	return dDOT(a,b);

}

inline void dVector3Inv(dVector3& a)

{

	a[0] = -a[0];

	a[1] = -a[1];

	a[2] = -a[2];

}

inline dReal dVector3Length2(const dVector3& a)

{

	return (dMUL(a[0],a[0])+dMUL(a[1],a[1])+dMUL(a[2],a[2]));

}

inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)

{

	v[0] = m[col + 0];

	v[1] = m[col + 4];

	v[2] = m[col + 8];

}

inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)

{

	r[0] =  dMUL(v[1],m[2*4 + col]) - dMUL(v[2],m[1*4 + col]); 

	r[1] =  dMUL(v[2],m[0*4 + col]) - dMUL(v[0],m[2*4 + col]); 

	r[2] =  dMUL(v[0],m[1*4 + col]) - dMUL(v[1],m[0*4 + col]);

}

inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)

{

	r[0] =   dMUL(v[2],m[1*4 + col]) - dMUL(v[1],m[2*4 + col]); 

	r[1] =   dMUL(v[0],m[2*4 + col]) - dMUL(v[2],m[0*4 + col]); 

	r[2] =   dMUL(v[1],m[0*4 + col]) - dMUL(v[0],m[1*4 + col]);

}

inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)

{

	dMULTIPLY0_331(r,m,v);

}

inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)

{

	return (dMUL(plane[0],point[0]) + dMUL(plane[1],point[1]) + dMUL(plane[2],point[2]) + plane[3]);

}

inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)

{

	plane[0] = normal[0];

	plane[1] = normal[1];

	plane[2] = normal[2];

	plane[3] = distance;

}

inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)

{

	dest[0]	=	source[0];

	dest[1]	=	source[1];

	dest[2]	=	source[2];

	dest[4]	=	source[4];

	dest[5]	=	source[5];

	dest[6]	=	source[6];

	dest[8]	=	source[8];

	dest[9]	=	source[9];

	dest[10]=	source[10];

}

inline dReal dMatrix3Det( const dMatrix3& mat )

{

	dReal det;

	det = dMUL(mat[0],( dMUL(mat[5],mat[10]) - dMUL(mat[9],mat[6]) ))

		- dMUL(mat[1],( dMUL(mat[4],mat[10]) - dMUL(mat[8],mat[6]) ))

		+ dMUL(mat[2],( dMUL(mat[4],mat[9])  - dMUL(mat[8],mat[5]) ));

	return( det );

}

inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )

{

	dReal det = dMatrix3Det( ma );

	if ( dFabs( det ) < REAL(0.0005) )

	{

		dRSetIdentity( dst );

		return;

	}

	dst[0] =    dMUL(ma[5],ma[10]) - dDIV(dMUL(ma[6],ma[9]),det);

	dst[1] = -dDIV(( dMUL(ma[1],ma[10]) - dMUL(ma[9],ma[2]) ),det);

	dst[2] =    dMUL(ma[1],ma[6])  - dDIV(dMUL(ma[5],ma[2]),det);

	dst[4] = -dDIV(( dMUL(ma[4],ma[10]) - dMUL(ma[6],ma[8]) ),det);

	dst[5] =    dMUL(ma[0],ma[10]) - dDIV(dMUL(ma[8],ma[2]),det);

	dst[6] = -dDIV(( dMUL(ma[0],ma[6]) - dMUL(ma[4],ma[2]) ),det);

	dst[8] =    dMUL(ma[4],ma[9]) - dDIV(dMUL(ma[8],ma[5]),det);

	dst[9] = -dDIV(( dMUL(ma[0],ma[9]) - dMUL(ma[8],ma[1]) ),det);

	dst[10] =    dMUL(ma[0],ma[5]) - dDIV(dMUL(ma[1],ma[4]),det);

}

inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)

{

	// Nguyen Binh : this code seem to be the fastest.

	dReal x0 = 	dMUL(source[0],quat[0]) + dMUL(source[2],quat[2]) - dMUL(source[1],quat[3]);

	dReal x1 = 	dMUL(source[1],quat[0]) + dMUL(source[0],quat[3]) - dMUL(source[2],quat[1]);

	dReal x2 = 	dMUL(source[2],quat[0]) + dMUL(source[1],quat[1]) - dMUL(source[0],quat[2]);

	dReal x3 = 	dMUL(source[0],quat[1]) + dMUL(source[1],quat[2]) + dMUL(source[2],quat[3]);

	dest[0]  = 	dMUL(quat[0],x0) + dMUL(quat[1],x3) + dMUL(quat[2],x2) - dMUL(quat[3],x1);

	dest[1]  = 	dMUL(quat[0],x1) + dMUL(quat[2],x3) + dMUL(quat[3],x0) - dMUL(quat[1],x2);

	dest[2]  = 	dMUL(quat[0],x2) + dMUL(quat[3],x3) + dMUL(quat[1],x1) - dMUL(quat[2],x0);

	/*

	// nVidia SDK implementation

	dVector3 uv, uuv; 

	dVector3 qvec;

	qvec[0] = quat[1];

	qvec[1] = quat[2];

	qvec[2] = quat[3];

	dVector3Cross(qvec,source,uv);

	dVector3Cross(qvec,uv,uuv);

	dVector3Scale(uv,REAL(2.0)*quat[0]);

	dVector3Scale(uuv,REAL(2.0));

	dest[0] = source[0] + uv[0] + uuv[0];

	dest[1] = source[1] + uv[1] + uuv[1];

	dest[2] = source[2] + uv[2] + uuv[2];   

	*/

}

inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)

{

	dReal norm = dMUL(quat[0],quat[0]) + dMUL(quat[1],quat[1]) + dMUL(quat[2],quat[2]) + dMUL(quat[3],quat[3]);

	if (norm > REAL(0.0))

	{

		dQuaternion invQuat;

		invQuat[0] =  dDIV(quat[0],norm);

		invQuat[1] = -dDIV(quat[1],norm);

		invQuat[2] = -dDIV(quat[2],norm);

		invQuat[3] = -dDIV(quat[3],norm);	

		dQuatTransform(invQuat,source,dest);

	}

	else

	{

		// Singular -> return identity

		dVector3Copy(source,dest);

	}

}

inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)

{

	rY = asin(mRot[0 * 4 + 2]);

	if (rY < dPI /2)

	{

		if (rY > -dPI /2)

		{

			rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);

			rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);

		}

		else

		{

			// not unique

			rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);

			rZ = REAL(0.0);

		}

	}

	else

	{

		// not unique

		rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);

		rZ = REAL(0.0);

	}

}

inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)

{

	dReal norm = dMUL(source[0],source[0]) + dMUL(source[1],source[1]) + dMUL(source[2],source[2]) + dMUL(source[3],source[3]);

	if (norm > REAL(0.0f))

	{

		dest[0] = dDIV(source[0],norm);

		dest[1] = -dDIV(source[1],norm);

		dest[2] = -dDIV(source[2],norm);

		dest[3] = -dDIV(source[3],norm);	

	}

	else

	{

		// Singular -> return identity

		dest[0] = REAL(1.0);

		dest[1] = REAL(0.0);

		dest[2] = REAL(0.0);

		dest[3] = REAL(0.0);

	}

}

#if 1

// Fetches a contact

inline dContactGeom* SAFECONTACT(int /*Flags*/, dContactGeom* Contacts, int Index, int Stride){

	return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));

}

#endif

#endif