//Benoit CHAPEROT 2003-2004 www.jstarlab.com
//some code inspired by Magic Software
#include <ode/common.h>
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_kernel.h"
#include "collision_std.h"
#include "collision_std_internal.h"
#include "collision_util.h"
#include <drawstuff/drawstuff.h>
#include "windows.h"
#include "ode\ode.h"

#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
const dReal fEPSILON = 1e-9f;

dxCone::dxCone (dSpaceID space, dReal _radius,dReal _length) :
dxGeom (space,1)
{
	dAASSERT(_radius > 0.f);
	dAASSERT(_length > 0.f);
	type = dConeClass;
	radius = _radius;
	lz = _length;
}

dxCone::~dxCone()
{
}

void dxCone::computeAABB()
{
  const dMatrix3& R = final_posr->R;
  const dVector3& pos = final_posr->pos;

	dReal xrange = dFabs(R[2]  * lz) + radius;
	dReal yrange = dFabs(R[6]  * lz) + radius;
	dReal zrange = dFabs(R[10] * lz) + radius;
	aabb[0] = pos[0] - xrange;
	aabb[1] = pos[0] + xrange;
	aabb[2] = pos[1] - yrange;
	aabb[3] = pos[1] + yrange;
	aabb[4] = pos[2] - zrange;
	aabb[5] = pos[2] + zrange;
}

dGeomID dCreateCone(dSpaceID space, dReal _radius,dReal _length)
{
	return new dxCone(space,_radius,_length);
}

void dGeomConeSetParams (dGeomID g, dReal _radius, dReal _length)
{
	dUASSERT (g && g->type == dConeClass,"argument not a cone");
	dAASSERT (_radius > 0.f);
	dAASSERT (_length > 0.f);
  g->recomputePosr();
	dxCone *c = (dxCone*) g;
	c->radius = _radius;
	c->lz = _length;
	dGeomMoved (g);
}


void dGeomConeGetParams (dGeomID g, dReal *_radius, dReal *_length)
{
	dUASSERT (g && g->type == dConeClass,"argument not a cone");
  g->recomputePosr();
	dxCone *c = (dxCone*) g;
	*_radius = c->radius;
	*_length = c->lz;
}

//positive inside
dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z)
{
	dUASSERT (g && g->type == dConeClass,"argument not a cone");

   g->recomputePosr();
	dxCone *cone = (dxCone*) g;

	dVector3 tmp,q;
	tmp[0] = x - cone->final_posr->pos[0];
	tmp[1] = y - cone->final_posr->pos[1];
	tmp[2] = z - cone->final_posr->pos[2];
	dMULTIPLY1_331 (q,cone->final_posr->R,tmp);

	dReal r = cone->radius;
	dReal h = cone->lz;

	dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]);
	dReal d1 = q[2];
	dReal d2 = h-q[2];
	
	if (d0 < d1) {
    if (d0 < d2) return d0; else return d2;
	}
	else {
	if (d1 < d2) return d1; else return d2;
	}
}

//plane plane
bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
	dVector3 LinePos,dVector3 LineDir)
{
    // If Cross(N0,N1) is zero, then either planes are parallel and separated
    // or the same plane.  In both cases, 'false' is returned.  Otherwise,
    // the intersection line is
    //
    //   L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
    //
    // for some coefficients c0 and c1 and for t any real number (the line
    // parameter).  Taking dot products with the normals,
    //
    //   d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
    //   d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
    //
    // which are two equations in two unknowns.  The solution is
    //
    //   c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
    //   c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
    //
    // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
/*
    Real fN00 = rkPlane0.Normal().SquaredLength();
    Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
    Real fN11 = rkPlane1.Normal().SquaredLength();
    Real fDet = fN00*fN11 - fN01*fN01;

    if ( Math::FAbs(fDet) < gs_fEpsilon )
        return false;

    Real fInvDet = 1.0f/fDet;
    Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
    Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;

    rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
    rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
    return true;
*/
	dReal fN00 = dLENGTHSQUARED(Plane0);
    dReal fN01 = dDOT(Plane0,Plane1);
    dReal fN11 = dLENGTHSQUARED(Plane1);
    dReal fDet = fN00*fN11 - fN01*fN01;

    if ( fabs(fDet) < fEPSILON)
        return false;

    dReal fInvDet = 1.0f/fDet;
    dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
    dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;

    dCROSS(LineDir,=,Plane0,Plane1);
	dNormalize3(LineDir);

	dVector3 Temp0,Temp1;
	dOPC(Temp0,*,Plane0,fC0);
	dOPC(Temp1,*,Plane1,fC1);
	dOP(LinePos,+,Temp0,Temp1);

    return true;
}

//plane ray
bool FindIntersectionPlaneRay(const dReal Plane[4],
					  const dVector3 &LinePos,const dVector3 &LineDir,
					  dReal &u,dVector3 &Pos)
{
/*
	u = (A*X1 + B*Y1 + C*Z1 + D) / (A*(X1-X2) + B*(Y1-Y2)+C*(Z1-Z2))	
*/	
	dReal fDet = -dDot(Plane,LineDir,3);

	if ( fabs(fDet) < fEPSILON)
        return false;

	u = (dDot(Plane,LinePos,3) - Plane[3]) / fDet;
	dOPC(Pos,*,LineDir,u);
	dOPE(Pos,+=,LinePos);

	return true;
}

int SolveQuadraticPolynomial(dReal a,dReal b,dReal c,dReal &x0,dReal &x1)
{
	dReal d = b*b - 4*a*c;
	int NumRoots = 0;
	dReal dr;

	if (d < 0.f)
		return NumRoots;

	if (d == 0.f)
	{
		NumRoots = 1;
		dr = 0.f;
	}
	else
	{
		NumRoots = 2;
		dr = sqrtf(d);
	}

	x0 = (-b -dr) / (2.f * a);
	x1 = (-b +dr) / (2.f * a);

	return NumRoots;
}
/*
const int VALID_INTERSECTION	= 1<<0;
const int POS_TEST_FAILEDT0		= 1<<0;
const int POS_TEST_FAILEDT1		= 1<<1;
*/
int ProcessConeRayIntersectionPoint(	dReal r,dReal h,
										const dVector3 &q,const dVector3 &v,dReal t,
										dVector3 &p,
										dVector3 &n,
										int &f)
{
	dOPC(p,*,v,t);
	dOPE(p,+=,q);
	n[0] = 2*p[0];
	n[1] = 2*p[1];
	n[2] = -2*p[2]*r*r/(h*h);

	f = 0;
	if (p[2] > h)	return 0;
	if (p[2] < 0)	return 0;
	if (t > 1)		return 0;
	if (t < 0)		return 0;

	return 1;
}

//cone ray
//line in cone space (position,direction)
//distance from line position (direction normalized)(if any)
//return the number of intersection
int FindIntersectionConeRay(dReal r,dReal h,	
					 const dVector3 &q,const dVector3 &v,dContactGeom *pContact)
{
	dVector3 qp,vp;
	dOPE(qp,=,q);
	dOPE(vp,=,v);
	qp[2] = h-q[2];
	vp[2] = -v[2];
	dReal ts = (r/h);
	ts *= ts;
	dReal a = vp[0]*vp[0] + vp[1]*vp[1] - ts*vp[2]*vp[2];
	dReal b = 2.f*qp[0]*vp[0] + 2.f*qp[1]*vp[1] - 2.f*ts*qp[2]*vp[2];
	dReal c = qp[0]*qp[0] + qp[1]*qp[1] - ts*qp[2]*qp[2];

/*
	dReal a = v[0]*v[0] + v[1]*v[1] - (v[2]*v[2]*r*r) / (h*h);
	dReal b = 2.f*q[0]*v[0] + 2.f*q[1]*v[1] + 2.f*r*r*v[2]/h - 2*r*r*q[0]*v[0]/(h*h);
	dReal c = q[0]*q[0] + q[1]*q[1] + 2*r*r*q[2]/h - r*r*q[2]/(h*h) - r*r;
*/
	int nNumRoots=SolveQuadraticPolynomial(a,b,c,pContact[0].depth,pContact[1].depth);
	int flag = 0;

	dContactGeom ValidContact[2];

	int nNumValidContacts = 0;
	for (int i=0;i<nNumRoots;i++)
	{
		if (ProcessConeRayIntersectionPoint(r,h,q,v,pContact[i].depth,pContact[i].pos,
			pContact[i].normal,flag))
		{
			ValidContact[nNumValidContacts] = pContact[i];
			nNumValidContacts++;
		}
	}

	dOP(qp,+,q,v);

	if ((nNumValidContacts < 2) && (v[2] != 0.f))
	{
		dReal d = (0.f-q[2]) / (v[2]); 
		if ((d>=0) && (d<=1))
		{
			dOPC(vp,*,v,d);
			dOP(qp,+,q,vp);

			if (qp[0]*qp[0]+qp[1]*qp[1] < r*r)
			{
				dOPE(ValidContact[nNumValidContacts].pos,=,qp);
				ValidContact[nNumValidContacts].normal[0] = 0.f;
				ValidContact[nNumValidContacts].normal[1] = 0.f;
				ValidContact[nNumValidContacts].normal[2] = -1.f;
				ValidContact[nNumValidContacts].depth = d;
				nNumValidContacts++;
			}
		}
	}

	if (nNumValidContacts == 2)
	{
		if (ValidContact[0].depth > ValidContact[1].depth)
		{
			pContact[0] = ValidContact[1];
			pContact[1] = ValidContact[0];
		}
		else
		{
			pContact[0] = ValidContact[0];
			pContact[1] = ValidContact[1];
		}
	}
	else if (nNumValidContacts == 1)
	{
		pContact[0] = ValidContact[0];
	}

	return nNumValidContacts;
}

int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags,
						 dContactGeom *contact, int skip)
{
	dIASSERT (skip >= (int)sizeof(dContactGeom));
	dIASSERT (o1->type == dConeClass);
	dIASSERT (o2->type == dPlaneClass);
	dxCone *cone = (dxCone*) o1;
	dxPlane *plane = (dxPlane*) o2;

	contact->g1 = o1;
	contact->g2 = o2;

	dVector3 p0,p1,pp0,pp1;
	dOPE(p0,=,cone->final_posr->pos);
	p1[0] = cone->final_posr->R[0*4+2] * cone->lz + p0[0];
	p1[1] = cone->final_posr->R[1*4+2] * cone->lz + p0[1];
	p1[2] = cone->final_posr->R[2*4+2] * cone->lz + p0[2];

	dReal u;
	FindIntersectionPlaneRay(plane->p,p0,plane->p,u,pp0);
	FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);

	if (dDISTANCE(pp0,pp1) < fEPSILON)
	{
		p1[0] = cone->final_posr->R[0*4+0] * cone->lz + p0[0];
		p1[1] = cone->final_posr->R[1*4+0] * cone->lz + p0[1];
		p1[2] = cone->final_posr->R[2*4+0] * cone->lz + p0[2];
		FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);
		dIASSERT(dDISTANCE(pp0,pp1) >= fEPSILON);
	}
	dVector3 h,r0,r1;
	h[0] = cone->final_posr->R[0*4+2];
	h[1] = cone->final_posr->R[1*4+2];
	h[2] = cone->final_posr->R[2*4+2];
	
	dOP(r0,-,pp0,pp1);
	dCROSS(r1,=,h,r0);
	dCROSS(r0,=,r1,h);
	dNormalize3(r0);
	dOPEC(h,*=,cone->lz);
	dOPEC(r0,*=,cone->radius);

	dVector3 p[3];
	dOP(p[0],+,cone->final_posr->pos,h);
	dOP(p[1],+,cone->final_posr->pos,r0);
	dOP(p[2],-,cone->final_posr->pos,r0);
	
	int numMaxContacts = flags & 0xffff;
	if (numMaxContacts == 0) 
		numMaxContacts = 1;

	int n=0;
	for (int i=0;i<3;i++)
	{
		dReal d = dGeomPlanePointDepth(o2, p[i][0], p[i][1], p[i][2]);

		if (d>0.f)
		{
			CONTACT(contact,n*skip)->g1 = o1;
			CONTACT(contact,n*skip)->g2 = o2;
			dOPE(CONTACT(contact,n*skip)->normal,=,plane->p); 
			dOPE(CONTACT(contact,n*skip)->pos,=,p[i]); 
			CONTACT(contact,n*skip)->depth = d;
			n++;

			if (n == numMaxContacts)
				return n;
		}
	}
	
	return n;
}

int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags,
						 dContactGeom *contact, int skip)
{
	dIASSERT (skip >= (int)sizeof(dContactGeom));
	dIASSERT (o1->type == dRayClass);
	dIASSERT (o2->type == dConeClass);
	dxRay *ray = (dxRay*) o1;
	dxCone *cone = (dxCone*) o2;

	contact->g1 = o1;
	contact->g2 = o2;

	dVector3 tmp,q,v;
	tmp[0] = ray->final_posr->pos[0] - cone->final_posr->pos[0];
	tmp[1] = ray->final_posr->pos[1] - cone->final_posr->pos[1];
	tmp[2] = ray->final_posr->pos[2] - cone->final_posr->pos[2];
	dMULTIPLY1_331 (q,cone->final_posr->R,tmp);
	tmp[0] = ray->final_posr->R[0*4+2] * ray->length;
	tmp[1] = ray->final_posr->R[1*4+2] * ray->length;
	tmp[2] = ray->final_posr->R[2*4+2] * ray->length;
	dMULTIPLY1_331 (v,cone->final_posr->R,tmp);

	dReal r = cone->radius;
	dReal h = cone->lz;

	dContactGeom Contact[2];

	if (FindIntersectionConeRay(r,h,q,v,Contact))
	{
		dMULTIPLY0_331(contact->normal,cone->final_posr->R,Contact[0].normal);
		dMULTIPLY0_331(contact->pos,cone->final_posr->R,Contact[0].pos);
		dOPE(contact->pos,+=,cone->final_posr->pos);
		contact->depth = Contact[0].depth * dLENGTH(v);
/*
		dMatrix3 RI;
		dRSetIdentity (RI);
		dVector3 ss;
		ss[0] = 0.01f;
		ss[1] = 0.01f;
		ss[2] = 0.01f;

		dsSetColorAlpha (1,0,0,0.8f);
		dsDrawBox(contact->pos,RI,ss);
*/		
		return 1;
	}

	return 0;
}

int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
	dIASSERT (skip >= (int)sizeof(dContactGeom));
	dIASSERT (o1->type == dConeClass);
	dIASSERT (o2->type == dSphereClass);
	dxCone		*cone = (dxCone*) o1;
	
	dxSphere ASphere(0,cone->radius);
	dGeomSetRotation(&ASphere,cone->final_posr->R);
	dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);

	return dCollideSphereSphere(&ASphere, o2, flags, contact, skip);
}

int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
	dIASSERT (skip >= (int)sizeof(dContactGeom));
	dIASSERT (o1->type == dConeClass);
	dIASSERT (o2->type == dBoxClass);
	dxCone		*cone = (dxCone*) o1;
	
	dxSphere ASphere(0,cone->radius);
	dGeomSetRotation(&ASphere,cone->final_posr->R);
	dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);

	return dCollideSphereBox(&ASphere, o2, flags, contact, skip);
}

int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
	dIASSERT (skip >= (int)sizeof(dContactGeom));
	dIASSERT (o1->type == dCCylinderClass);
	dIASSERT (o2->type == dConeClass);
	dxCone		*cone = (dxCone*) o2;
	
	dxSphere ASphere(0,cone->radius);
	dGeomSetRotation(&ASphere,cone->final_posr->R);
	dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);

	return dCollideCCylinderSphere(o1, &ASphere, flags, contact, skip);
}

extern int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);

int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
	dIASSERT (skip >= (int)sizeof(dContactGeom));
	dIASSERT (o1->type == dTriMeshClass);
	dIASSERT (o2->type == dConeClass);
	dxCone		*cone = (dxCone*) o2;

	dxSphere ASphere(0,cone->radius);
	dGeomSetRotation(&ASphere,cone->final_posr->R);
	dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);

	return dCollideSTL(o1, &ASphere, flags, contact, skip);
}