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

 *                                                                       *

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

 *                                                                       *

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

/*

Code for Convex Collision Detection

By Rodrigo Hernandez

*/

//#include <algorithm>

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

#include "set.h"

#include <ode/ode.h>

#define dMIN(A,B)  ((A)>(B) ? B : A)

#define dMAX(A,B)  ((A)>(B) ? A : B)

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

// Convex public API

dxConvex::dxConvex (dSpaceID space,

		    dReal *_planes,

		    unsigned int _planecount,

		    dReal *_points,

		    unsigned int _pointcount,

		    unsigned int *_polygons) : 

  dxGeom (space,1)

{

  //fprintf(stdout,"dxConvex Constructor planes %X\n",_planes);

  type = dConvexClass;

  planes = _planes;

  planecount = _planecount;

  // we need points as well

  points = _points;

  pointcount = _pointcount;

  polygons=_polygons;

  FillEdges();

}

void dxConvex::computeAABB()

{

  // this can, and should be optimized

  dVector3 point;

  dMULTIPLY0_331 (point,final_posr->R,points);

  aabb[0] = point[0]+final_posr->pos[0];

  aabb[1] = point[0]+final_posr->pos[0];

  aabb[2] = point[1]+final_posr->pos[1];

  aabb[3] = point[1]+final_posr->pos[1];

  aabb[4] = point[2]+final_posr->pos[2];

  aabb[5] = point[2]+final_posr->pos[2];

  for(unsigned int i=3;i<(pointcount*3);i+=3)

    {

      dMULTIPLY0_331 (point,final_posr->R,&points[i]);

      aabb[0] = dMIN(aabb[0],point[0]+final_posr->pos[0]);

      aabb[1] = dMAX(aabb[1],point[0]+final_posr->pos[0]);

      aabb[2] = dMIN(aabb[2],point[1]+final_posr->pos[1]);

      aabb[3] = dMAX(aabb[3],point[1]+final_posr->pos[1]);

      aabb[4] = dMIN(aabb[4],point[2]+final_posr->pos[2]);

      aabb[5] = dMAX(aabb[5],point[2]+final_posr->pos[2]);

    }

}

/*! \brief Populates the edges set, should be called only once whenever

  the polygon array gets updated */

void dxConvex::FillEdges()

{

	unsigned int *points_in_poly=polygons;

	unsigned int *index=polygons+1;

	for(unsigned int i=0;i<planecount;++i)

	{

		for(unsigned int j=0;j<*points_in_poly;++j)

		{

			pair* p = new pair(dMIN(index[j],index[(j+1)%*points_in_poly]),

					dMAX(index[j],index[(j+1)%*points_in_poly]));

			edges->addElem(*p);

		}

		points_in_poly+=(*points_in_poly+1);

		index=points_in_poly+1;

	}

}

EXPORT_C dGeomID dCreateConvex (dSpaceID space,dReal *_planes,unsigned int _planecount,

		    dReal *_points,

		       unsigned int _pointcount,

		       unsigned int *_polygons)

{

  //fprintf(stdout,"dxConvex dCreateConvex\n");

  return new dxConvex(space,_planes, _planecount,

		      _points,

		      _pointcount,

		      _polygons);

}

EXPORT_C void dGeomSetConvex (dGeomID g,dReal *_planes,unsigned int _planecount,

		     dReal *_points,

		     unsigned int _pointcount,

		     unsigned int *_polygons)

{

  //fprintf(stdout,"dxConvex dGeomSetConvex\n");

  dxConvex *s = (dxConvex*) g;

  s->planes = _planes;

  s->planecount = _planecount;

  s->points = _points;

  s->pointcount = _pointcount;

  s->polygons=_polygons;

}

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

// Helper Inlines

//

/*! \brief Returns Whether or not the segment ab intersects plane p

  \param a origin of the segment

  \param b segment destination

  \param p plane to test for intersection

  \param t returns the time "t" in the segment ray that gives us the intersecting 

  point

  \param q returns the intersection point

  \return true if there is an intersection, otherwise false.

*/

bool IntersectSegmentPlane(dVector3 a, 

			   dVector3 b, 

			   dVector4 p, 

			   dReal &t, 

			   dVector3 q)

{

  // Compute the t value for the directed line ab intersecting the plane

  dVector3 ab;

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

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

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

  t = dDIV((p[3] - dDOT(p,a)),dDOT(p,ab));

  // If t in [0..1] compute and return intersection point

  if (t >= REAL(0.0) && t <= REAL(1.0)) 

    {

      q[0] = a[0] + dMUL(t,ab[0]);

      q[1] = a[1] + dMUL(t,ab[1]);

      q[2] = a[2] + dMUL(t,ab[2]);

      return true;

    }

  // Else no intersection

  return false;

}

/*! \brief Returns the Closest Point in Ray 1 to Ray 2

  \param Origin1 The origin of Ray 1

  \param Direction1 The direction of Ray 1

  \param Origin1 The origin of Ray 2

  \param Direction1 The direction of Ray 3

  \param t the time "t" in Ray 1 that gives us the closest point 

  (closest_point=Origin1+(Direction*t).

  \return true if there is a closest point, false if the rays are paralell.

*/

inline bool ClosestPointInRay(const dVector3 Origin1,

			      const dVector3 Direction1,

			      const dVector3 Origin2,

			      const dVector3 Direction2,

			      dReal& t)

{

  dVector3 w = {Origin1[0]-Origin2[0],

		Origin1[1]-Origin2[1],

		Origin1[2]-Origin2[2]};

  dReal a = dDOT(Direction1 , Direction1);

  dReal b = dDOT(Direction1 , Direction2);

  dReal c = dDOT(Direction2 , Direction2);

  dReal d = dDOT(Direction1 , w);

  dReal e = dDOT(Direction2 , w);

  dReal denominator = dMUL(a,c)-dMUL(b,b);

  if(denominator==REAL(0.0f))

    {

      return false;

    }

  t = dDIV((dMUL(a,e)-dMUL(b,d)),denominator);

  return true;

}

/*! \brief Returns the Ray on which 2 planes intersect if they do.

  \param p1 Plane 1

  \param p2 Plane 2

  \param p Contains the origin of the ray upon returning if planes intersect

  \param d Contains the direction of the ray upon returning if planes intersect

  \return true if the planes intersect, false if paralell.

*/

inline bool IntersectPlanes(const dVector4 p1, const dVector4 p2, dVector3 p, dVector3 d)

{

  // Compute direction of intersection line

  //Cross(p1, p2,d);

  dCROSS(d,=,p1,p2);

  // If d is (near) zero, the planes are parallel (and separated)

  // or coincident, so they're not considered intersecting

  dReal denom = dDOT(d, d);

  if (denom < dEpsilon) return false;

  dVector3 n;

  n[0]=dMUL(p1[3],p2[0]) - dMUL(p2[3],p1[0]);

  n[1]=dMUL(p1[3],p2[1]) - dMUL(p2[3],p1[1]);

  n[2]=dMUL(p1[3],p2[2]) - dMUL(p2[3],p1[2]);

  // Compute point on intersection line

  //Cross(n, d,p);

  dCROSS(p,=,n,d);

  p[0] = dDIV(p[0],denom);

  p[1] = dDIV(p[1],denom);

  p[2] = dDIV(p[2],denom);

  return true;

}

/*! \brief Finds out if a point lies inside a 2D polygon

  \param p Point to test

  \param polygon a pointer to the start of the convex polygon index buffer

  \param out the closest point in the polygon if the point is not inside

  \return true if the point lies inside of the polygon, false if not.

*/

inline bool IsPointInPolygon(dVector3 p,

			     unsigned int *polygon,

			     dxConvex *convex,

			     dVector3 out)

{

  // p is the point we want to check,

  // polygon is a pointer to the polygon we

  // are checking against, remember it goes

  // number of vertices then that many indexes

  // out returns the closest point on the border of the

  // polygon if the point is not inside it.

  size_t pointcount=polygon[0];

  dVector3 a;

  dVector3 b;

  dVector3 c;

  dVector3 ab;

  dVector3 ac;

  dVector3 ap;

  dVector3 bp;

  dReal d1;

  dReal d2;

  dReal d3;

  dReal d4;

  dReal vc;

  polygon++; // skip past pointcount

  for(size_t i=0;i<pointcount;++i)

    {

      dMULTIPLY0_331 (a,convex->final_posr->R,&convex->points[(polygon[i]*3)]);

      a[0]=convex->final_posr->pos[0]+a[0];

      a[1]=convex->final_posr->pos[1]+a[1];

      a[2]=convex->final_posr->pos[2]+a[2];

      dMULTIPLY0_331 (b,convex->final_posr->R,

		      &convex->points[(polygon[(i+1)%pointcount]*3)]);

      b[0]=convex->final_posr->pos[0]+b[0];

      b[1]=convex->final_posr->pos[1]+b[1];

      b[2]=convex->final_posr->pos[2]+b[2];

      dMULTIPLY0_331 (c,convex->final_posr->R,

		      &convex->points[(polygon[(i+2)%pointcount]*3)]);

      c[0]=convex->final_posr->pos[0]+c[0];

      c[1]=convex->final_posr->pos[1]+c[1];

      c[2]=convex->final_posr->pos[2]+c[2];

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

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

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

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

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

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

      ap[0] = p[0] - a[0];

      ap[1] = p[1] - a[1];

      ap[2] = p[2] - a[2];

      d1 = dDOT(ab,ap);

      d2 = dDOT(ac,ap);

      if (d1 <= REAL(0.0f) && d2 <= REAL(0.0f))

	{

	  out[0]=a[0];

	  out[1]=a[1];

	  out[2]=a[2];

	  return false;

	}

      bp[0] = p[0] - b[0];

      bp[1] = p[1] - b[1];

      bp[2] = p[2] - b[2];

      d3 = dDOT(ab,bp);

      d4 = dDOT(ac,bp);

      if (d3 >= REAL(0.0f) && d4 <= d3)

	{

	  out[0]=b[0];

	  out[1]=b[1];

	  out[2]=b[2];

	  return false;

	}      

      vc = dMUL(d1,d4) - dMUL(d3,d2);

      if (vc <= REAL(0.0f) && d1 >= REAL(0.0f) && d3 <= REAL(0.0f)) 

	{

	  dReal v = dDIV(d1,(d1 - d3));

	  out[0] = a[0] + dMUL(ab[0],v);

	  out[1] = a[1] + dMUL(ab[1],v);

	  out[2] = a[2] + dMUL(ab[2],v);

	  return false;

	}

    }

  return true;

}

int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,

						 dContactGeom *contact, int skip)

{

	dxConvex *Convex = (dxConvex*) o1;

	dxPlane *Plane = (dxPlane*) o2;

	unsigned int contacts=0;

	unsigned int maxc = flags & NUMC_MASK;

	dVector3 v1;

	dVector3 v2;

	bool Hit=false;

	dMULTIPLY0_331 (v1,Convex->final_posr->R,Convex->points);

	v1[0]=Convex->final_posr->pos[0]+v1[0];

	v1[1]=Convex->final_posr->pos[1]+v1[1];

	v1[2]=Convex->final_posr->pos[2]+v1[2];

	dReal distance1 = (dMUL(Plane->p[0],v1[0])   + // Ax +

		dMUL(Plane->p[1],v1[1])   + // Bx +

		dMUL(Plane->p[2],v1[2])) - Plane->p[3]; // Cz - D

	if(distance1<=0)

	{

		CONTACT(contact,skip*contacts)->normal[0] = Plane->p[0];

		CONTACT(contact,skip*contacts)->normal[1] = Plane->p[1];

		CONTACT(contact,skip*contacts)->normal[2] = Plane->p[2];

		CONTACT(contact,skip*contacts)->pos[0] = v1[0];

		CONTACT(contact,skip*contacts)->pos[1] = v1[1];

		CONTACT(contact,skip*contacts)->pos[2] = v1[2];

		CONTACT(contact,skip*contacts)->depth = -distance1;

		CONTACT(contact,skip*contacts)->g1 = Convex;

		CONTACT(contact,skip*contacts)->g2 = Plane;

		contacts++;

	}

	for(unsigned int i=1;i<Convex->pointcount;++i)

	{

		dMULTIPLY0_331 (v2,Convex->final_posr->R,&Convex->points[(i*3)]);

		v2[0]=Convex->final_posr->pos[0]+v2[0];

		v2[1]=Convex->final_posr->pos[1]+v2[1];

		v2[2]=Convex->final_posr->pos[2]+v2[2];

		dReal distance2 = (dMUL(Plane->p[0],v2[0]) + // Ax +

			dMUL(Plane->p[1],v2[1])  + // Bx +

			dMUL(Plane->p[2],v2[2])) - Plane->p[3]; // Cz + D

		if(!Hit) 

			/* 

			Avoid multiplication 

			if we have already determined there is a hit 

			*/

		{

			if(dMUL(distance1,distance2) <= REAL(0.0))

			{

				// there is a hit.

				Hit=true;

			}

		}

		if((distance2<=0)&&(contacts<maxc))

		{

			CONTACT(contact,skip*contacts)->normal[0] = Plane->p[0];

			CONTACT(contact,skip*contacts)->normal[1] = Plane->p[1];

			CONTACT(contact,skip*contacts)->normal[2] = Plane->p[2];

			CONTACT(contact,skip*contacts)->pos[0] = v2[0];

			CONTACT(contact,skip*contacts)->pos[1] = v2[1];

			CONTACT(contact,skip*contacts)->pos[2] = v2[2];

			CONTACT(contact,skip*contacts)->depth = -distance2;

			CONTACT(contact,skip*contacts)->g1 = Convex;

			CONTACT(contact,skip*contacts)->g2 = Plane;

			contacts++;

		}

	}

	if(Hit) return contacts;

	return 0;

}

int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int /*flags*/,

			  dContactGeom *contact, int /*skip*/)

{

  dxSphere *Sphere = (dxSphere*) o1;

  dxConvex *Convex = (dxConvex*) o2;

  dReal dist,closestdist=dInfinity;

  dVector4 plane;

  // dVector3 contactpoint;

  dVector3 offsetpos,out,temp;

  unsigned int *pPoly=Convex->polygons;

  int closestplane=0;

  bool sphereinside=true;

  /* 

     Do a good old sphere vs plane check first,

     if a collision is found then check if the contact point

     is within the polygon

  */

  // offset the sphere final_posr->position into the convex space

  offsetpos[0]=Sphere->final_posr->pos[0]-Convex->final_posr->pos[0];

  offsetpos[1]=Sphere->final_posr->pos[1]-Convex->final_posr->pos[1];

  offsetpos[2]=Sphere->final_posr->pos[2]-Convex->final_posr->pos[2];

  //fprintf(stdout,"Begin Check\n");  

  for(unsigned int i=0;i<Convex->planecount;++i)

    {

      // apply rotation to the plane

      dMULTIPLY0_331(plane,Convex->final_posr->R,&Convex->planes[(i*4)]);

      plane[3]=(&Convex->planes[(i*4)])[3];

      // Get the distance from the sphere origin to the plane

      dist = (dMUL(plane[0],offsetpos[0]) + // Ax +

	      dMUL(plane[1],offsetpos[1])  + // Bx +

	      dMUL(plane[2],offsetpos[2])) - plane[3]; // Cz - D

      if(dist>REAL(0.0))

	{

	  // if we get here, we know the center of the sphere is

	  // outside of the convex hull.

	  if(dist<Sphere->radius)

	    {

	      // if we get here we know the sphere surface penetrates

	      // the plane

	      if(IsPointInPolygon(Sphere->final_posr->pos,pPoly,Convex,out))

		{

		  // finally if we get here we know that the

		  // sphere is directly touching the inside of the polyhedron

		  //fprintf(stdout,"Contact! distance=%f\n",dist);

		  contact->normal[0] = plane[0];

		  contact->normal[1] = plane[1];

		  contact->normal[2] = plane[2];

		  contact->pos[0] = Sphere->final_posr->pos[0]+

		    (-dMUL(contact->normal[0],Sphere->radius));

		  contact->pos[1] = Sphere->final_posr->pos[1]+

		    (-dMUL(contact->normal[1],Sphere->radius));

		  contact->pos[2] = Sphere->final_posr->pos[2]+

		    (-dMUL(contact->normal[2],Sphere->radius));

		  contact->depth = Sphere->radius-dist;

		  contact->g1 = Sphere;

		  contact->g2 = Convex;

		  return 1;

		}

	      else

		{

		  // the sphere may not be directly touching

		  // the polyhedron, but it may be touching

		  // a point or an edge, if the distance between

		  // the closest point on the poly (out) and the 

		  // center of the sphere is less than the sphere 

		  // radius we have a hit.

		  temp[0] = (Sphere->final_posr->pos[0]-out[0]);

		  temp[1] = (Sphere->final_posr->pos[1]-out[1]);

		  temp[2] = (Sphere->final_posr->pos[2]-out[2]);

		  dist=(dMUL(temp[0],temp[0])+dMUL(temp[1],temp[1])+dMUL(temp[2],temp[2]));

		  // avoid the sqrt unless really necesary

		  if(dist<dMUL(Sphere->radius,Sphere->radius))

		    {

		      // We got an indirect hit

		      dist=dSqrt(dist);

		      contact->normal[0] = dDIV(temp[0],dist);

		      contact->normal[1] = dDIV(temp[1],dist);

		      contact->normal[2] = dDIV(temp[2],dist);

		      contact->pos[0] = Sphere->final_posr->pos[0]+

			(-dMUL(contact->normal[0],Sphere->radius));

		      contact->pos[1] = Sphere->final_posr->pos[1]+

			(-dMUL(contact->normal[1],Sphere->radius));

		      contact->pos[2] = Sphere->final_posr->pos[2]+

			(-dMUL(contact->normal[2],Sphere->radius));

		      contact->depth = Sphere->radius-dist;

		      contact->g1 = Sphere;

		      contact->g2 = Convex;

		      return 1;

		    }

		}

	    }

	  sphereinside=false;

	}

      if(sphereinside)

	{

	  if(closestdist>dFabs(dist))

	    {

	      closestdist=dFabs(dist);

	      closestplane=i;

	    }

	}

      pPoly+=pPoly[0]+1;

    }

    if(sphereinside)

      {

	// if the center of the sphere is inside 

	// the Convex, we need to pop it out

	dMULTIPLY0_331(contact->normal,

		       Convex->final_posr->R,

		       &Convex->planes[(closestplane*4)]);

	contact->pos[0] = Sphere->final_posr->pos[0];

	contact->pos[1] = Sphere->final_posr->pos[1];

	contact->pos[2] = Sphere->final_posr->pos[2];

	contact->depth = closestdist+Sphere->radius;

	contact->g1 = Sphere;

	contact->g2 = Convex;

	return 1;

      }

  return 0;

}

int dCollideConvexBox (dxGeom */*o1*/, dxGeom */*o2*/, int /*flags*/,

		       dContactGeom */*contact*/, int /*skip*/)

{

  return 0;

}