diff -r 000000000000 -r 2f259fa3e83a ode/src/convex.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/src/convex.cpp	Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,1206 @@
+/*************************************************************************
+ *                                                                       *
+ * 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;
+}
+
+int dCollideConvexCapsule (dxGeom */*o1*/, dxGeom */*o2*/,
+			     int /*flags*/, dContactGeom */*contact*/, int /*skip*/)
+{
+  return 0;
+}
+
+/*!
+  \brief Retrieves the proper convex and plane index between 2 convex objects.
+
+  Seidel's Algorithm does not discriminate between 2 different Convex Hulls,
+  it only cares about planes, so we feed it the planes of Convex 1 followed
+  by the planes of Convex 2 as a single collection of planes, 
+  given an index into the single collection, 
+  this function determines the correct convex object and index to retrieve
+  the current plane.
+  
+  \param c1 Convex 1
+  \param c2 Convex 2
+  \param i Plane Index to retrieve
+  \param index contains the translated index uppon return
+  \return a pointer to the convex object containing plane index "i"
+*/
+inline dxConvex* GetPlaneIndex(dxConvex& c1, 
+			       dxConvex& c2,
+			       unsigned int i,unsigned int& index)
+{
+  if(i>=c1.planecount)
+    {
+      index = i-c1.planecount;
+      return &c2;
+    }
+  index=i;
+  return &c1;
+}
+
+inline void dumpplanes(dxConvex& cvx)
+{
+	// This is just a dummy debug function
+	dVector4 plane;
+	fprintf(stdout,"DUMP PLANES\n");
+	for (unsigned int i=0;i<cvx.planecount;++i)
+	{
+		dMULTIPLY0_331(plane,cvx.final_posr->R,&cvx.planes[(i*4)]);      
+		// Translate
+		plane[3]=
+			(cvx.planes[(i*4)+3])+
+			(dMUL(plane[0],cvx.final_posr->pos[0]) + 
+			dMUL(plane[1],cvx.final_posr->pos[1]) + 
+			dMUL(plane[2],cvx.final_posr->pos[2]));
+		fprintf(stdout,"%f %f %f %f\n",plane[0],plane[1],plane[2],plane[3]);
+	}
+}
+
+// this variable is for debuggin purpuses only, should go once everything works
+/*
+  \brief Tests whether 2 convex objects collide
+
+  Seidel's algorithm is a method to solve linear programs, 
+  it finds the optimum vertex "v" of a set of functions, 
+  in our case, the set of functions are the plane functions
+  for the 2 convex objects being tested.
+  We don't really care about the value optimum vertex though,
+  but the 2 convex objects only collide if this vertex exists,
+  otherwise, the set of functions is said to be "empty" or "void".
+
+  Seidel's Original algorithm is recursive and not bound to any number
+  of dimensions, the one I present here is Iterative rather than recursive
+  and bound to 3 dimensions, which is what we care about.
+  
+  If you're interested (and you should be!) on the algorithm, the paper
+  by Raimund Seidel himself should explain a lot better than I did, you can
+  find it here: http://www.cs.berkeley.edu/~jrs/meshpapers/SeidelLP.pdf
+
+  If posible, read the section about Linear Programming in 
+  Christer Ericson's RealTime Collision Detection book.
+  
+  \note currently there seem to be some issues with this function since
+  it doesn't detect collisions except for the most simple tests :(. 
+*/
+bool SeidelLP(dxConvex& cvx1,dxConvex& cvx2)
+{
+	dVector3 c={1,0,0}; // The Objective vector can be anything
+	dVector3 solution;    // We dont really need the solution so its local
+	dxConvex *cvx;
+	unsigned int index;
+	unsigned int planecount=cvx1.planecount+cvx2.planecount;
+	dReal sum,min,max,med;
+	dVector3 c1; // ,c2;
+	dVector4 aoveral,aoveram; // these will contain cached computations
+	unsigned int l,m,n; // l and m are the axes to the zerod dimensions, n is the axe for the last dimension
+	unsigned int i,j,k;
+	dVector4 eq1,eq2,eq3; // cached equations for 3d,2d and 1d respectivelly
+	// Get the support mapping for a HUGE bounding box in direction c
+	solution[0]= (c[0]>0) ? dInfinity : -dInfinity;
+	solution[1]= (c[1]>0) ? dInfinity : -dInfinity;
+	solution[2]= (c[2]>0) ? dInfinity : -dInfinity;
+	for( i=0;i<planecount;++i)
+	{
+		// Isolate plane equation
+		cvx=GetPlaneIndex(cvx1,cvx2,i,index);
+		// Rotate
+		dMULTIPLY0_331(eq1,cvx->final_posr->R,&cvx->planes[(index*4)]);
+
+		// Translate
+		eq1[3]=(cvx->planes[(index*4)+3])+
+			(dMUL(eq1[0],cvx->final_posr->pos[0]) + 
+			dMUL(eq1[1],cvx->final_posr->pos[1]) + 
+			dMUL(eq1[2],cvx->final_posr->pos[2]));
+		//       if(!hit)
+		// 	{
+		// 	  fprintf(stdout,"Plane I %d: %f %f %f %f\n",i,
+		// 		  cvx->planes[(index*4)+0],
+		// 		  cvx->planes[(index*4)+1],
+		// 		  cvx->planes[(index*4)+2],
+		// 		  cvx->planes[(index*4)+3]);
+		// 	  fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",i,
+		// 		  eq1[0],
+		// 		  eq1[1],eq1[2],eq1[3]);
+		// 	  fprintf(stdout,"POS %f,%f,%f\n",
+		// 		  cvx->final_posr->pos[0],
+		// 		  cvx->final_posr->pos[1],
+		// 		  cvx->final_posr->pos[2]);
+		// 	}
+		// find if the solution is behind the current plane
+		sum=
+			(dMUL(eq1[0],solution[0])+
+			dMUL(eq1[1],solution[1])+
+			dMUL(eq1[2],solution[2]))-eq1[3];
+		// if not we need to discard the current solution
+		if(sum>0)
+		{
+			// go down a dimension by eliminating a variable
+			// first find l
+			l=0;
+			for( j=0;j<3;++j)
+			{
+				if(dFabs(eq1[j])>dFabs(eq1[l]))
+				{
+					l=j;
+				}
+			}
+			if(eq1[l]==0)
+			{
+				if(!GetGlobalData()->hit) 
+				{
+					fprintf(stdout,"Plane %d: %f %f %f %f is invalid\n",i,
+						eq1[0],eq1[1],eq1[2],eq1[3]);
+				}
+				return false; 
+			}
+			// then compute a/a[l] c1 and solution
+			aoveral[0]=dDIV(eq1[0],eq1[l]);
+			aoveral[1]=dDIV(eq1[1],eq1[l]);
+			aoveral[2]=dDIV(eq1[2],eq1[l]);
+			aoveral[3]=dDIV(eq1[3],eq1[l]);
+			c1[0]=c[0]-dMUL(dDIV(c[l],eq1[l]),eq1[0]);
+			c1[1]=c[1]-dMUL(dDIV(c[l],eq1[l]),eq1[1]);
+			c1[2]=c[2]-dMUL(dDIV(c[l],eq1[l]),eq1[2]);
+			solution[0]=solution[0]-dMUL(dDIV(solution[l],eq1[l]),eq1[0]);
+			solution[1]=solution[1]-dMUL(dDIV(solution[l],eq1[l]),eq1[1]);
+			solution[2]=solution[2]-dMUL(dDIV(solution[l],eq1[l]),eq1[2]);
+			// iterate a to get the new equations with the help of a/a[l]
+			for( j=0;j<planecount;++j)
+			{
+				if(i!=j)
+				{
+					cvx=GetPlaneIndex(cvx1,cvx2,j,index);
+					// Rotate
+					dMULTIPLY0_331(eq2,cvx->final_posr->R,&cvx->planes[(index*4)]);
+					// Translate
+					eq2[3]=(cvx->planes[(index*4)+3])+
+						(dMUL(eq2[0],cvx->final_posr->pos[0]) + 
+						dMUL(eq2[1],cvx->final_posr->pos[1]) + 
+						dMUL(eq2[2],cvx->final_posr->pos[2]));
+
+					//       if(!hit)
+					// 	{
+					// 	  fprintf(stdout,"Plane J %d: %f %f %f %f\n",j,
+					// 		  cvx->planes[(index*4)+0],
+					// 		  cvx->planes[(index*4)+1],
+					// 		  cvx->planes[(index*4)+2],
+					// 		  cvx->planes[(index*4)+3]);
+					// 	  fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",j,
+					// 		  eq2[0],
+					// 		  eq2[1],
+					// 		  eq2[2],
+					// 		  eq2[3]);
+					// 	  fprintf(stdout,"POS %f,%f,%f\n",
+					// 		  cvx->final_posr->pos[0],
+					// 		  cvx->final_posr->pos[1],
+					// 		  cvx->final_posr->pos[2]);
+					// 	}
+
+					// Take The equation down a dimension
+					eq2[0]-=dMUL(cvx->planes[(index*4)+l],aoveral[0]);
+					eq2[1]-=dMUL(cvx->planes[(index*4)+l],aoveral[1]);
+					eq2[2]-=dMUL(cvx->planes[(index*4)+l],aoveral[2]);
+					eq2[3]-=dMUL(cvx->planes[(index*4)+l],aoveral[3]);
+					sum=
+						(dMUL(eq2[0],solution[0])+
+						dMUL(eq2[1],solution[1])+
+						dMUL(eq2[2],solution[2]))-eq2[3];
+					if(sum>0)
+					{
+						m=0;
+						for( k=0;k<3;++k)
+						{
+							if(dFabs(eq2[k])>dFabs(eq2[m]))
+							{
+								m=k;
+							}
+						}
+						if(eq2[m]==0)
+						{
+							/* 
+							if(!hit) fprintf(stdout,
+							"Plane %d: %f %f %f %f is invalid\n",j,
+							eq2[0],eq2[1],eq2[2],eq2[3]);
+							*/
+							return false; 
+						}
+						// then compute a/a[m] c1 and solution
+						aoveram[0]=dDIV(eq2[0],eq2[m]);
+						aoveram[1]=dDIV(eq2[1],eq2[m]);
+						aoveram[2]=dDIV(eq2[2],eq2[m]);
+						aoveram[3]=dDIV(eq2[3],eq2[m]);
+						c1[0]=c[0]-dMUL(dDIV(c[m],eq2[m]),eq2[0]);
+						c1[1]=c[1]-dMUL(dDIV(c[m],eq2[m]),eq2[1]);
+						c1[2]=c[2]-dMUL(dDIV(c[m],eq2[m]),eq2[2]);
+						solution[0]=solution[0]-dMUL(dDIV(solution[m],eq2[m]),eq2[0]);
+						solution[1]=solution[1]-dMUL(dDIV(solution[m],eq2[m]),eq2[1]);
+						solution[2]=solution[2]-dMUL(dDIV(solution[m],eq2[m]),eq2[2]);
+						// figure out the value for n by elimination
+						n = (l==0) ? ((m==1)? 2:1):((m==0)?((l==1)?2:1):0);
+						// iterate a to get the new equations with the help of a/a[l]
+						min=-dInfinity;
+						max=med=dInfinity;
+						for(k=0;k<planecount;++k)
+						{
+							if((i!=k)&&(j!=k))
+							{
+								cvx=GetPlaneIndex(cvx1,cvx2,k,index);
+								// Rotate
+								dMULTIPLY0_331(eq3,cvx->final_posr->R,&cvx->planes[(index*4)]);
+								// Translate
+								eq3[3]=(cvx->planes[(index*4)+3])+
+									(dMUL(eq3[0],cvx->final_posr->pos[0]) + 
+									dMUL(eq3[1],cvx->final_posr->pos[1]) + 
+									dMUL(eq3[2],cvx->final_posr->pos[2]));
+								//       if(!hit)
+								// 	{
+								// 	  fprintf(stdout,"Plane K %d: %f %f %f %f\n",k,
+								// 		  cvx->planes[(index*4)+0],
+								// 		  cvx->planes[(index*4)+1],
+								// 		  cvx->planes[(index*4)+2],
+								// 		  cvx->planes[(index*4)+3]);
+								// 	  fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",k,
+								// 		  eq3[0],
+								// 		  eq3[1],
+								// 		  eq3[2],
+								// 		  eq3[3]);
+								// 	  fprintf(stdout,"POS %f,%f,%f\n",
+								// 		  cvx->final_posr->pos[0],
+								// 		  cvx->final_posr->pos[1],
+								// 		  cvx->final_posr->pos[2]);
+								// 	}
+
+								// Take the equation Down to 1D
+								eq3[0]-=dMUL(cvx->planes[(index*4)+m],aoveram[0]);
+								eq3[1]-=dMUL(cvx->planes[(index*4)+m],aoveram[1]);
+								eq3[2]-=dMUL(cvx->planes[(index*4)+m],aoveram[2]);
+								eq3[3]-=dMUL(cvx->planes[(index*4)+m],aoveram[3]);
+								if(eq3[n]>REAL(0.0))
+								{
+									max=dMIN(max,dDIV(eq3[3],eq3[n]));
+								}
+								else if(eq3[n]<0)
+								{
+									min=dMAX(min,dDIV(eq3[3],eq3[n]));
+								}
+								else
+								{
+									med=dMIN(med,eq3[3]);
+								}
+							}
+						}
+						if ((max<min)||(med<0)) 
+						{
+							// 			  if(!hit) fprintf(stdout,"((max<min)||(med<0)) ((%f<%f)||(%f<0))",max,min,med);
+							if(!GetGlobalData()->hit)
+							{
+								dumpplanes(cvx1);
+								dumpplanes(cvx2);
+							}
+							//fprintf(stdout,"Planes %d %d\n",i,j);
+							return false;
+
+						}
+						// find the value for the solution in one dimension
+						solution[n] = (c1[n]>=0) ? max:min;
+						// lift to 2D
+						solution[m] = dMUL((eq2[3]-dMUL(eq2[n],solution[n])),eq2[m]);
+						// lift to 3D
+						solution[l] = dDIV((eq1[3]-(dMUL(eq1[m],solution[m])+
+							dMUL(eq1[n],solution[n]))),eq1[l]);
+					}
+				}
+			}
+		}
+	}
+	return true;
+}
+
+/*! \brief A Support mapping function for convex shapes
+  \param dir direction to find the Support Point for
+  \param cvx convex object to find the support point for
+  \param out the support mapping in dir.
+ */
+inline void Support(dVector3 dir,dxConvex& cvx,dVector3 out)
+{
+	dVector3 point;
+	dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);
+	point[0]+=cvx.final_posr->pos[0];
+	point[1]+=cvx.final_posr->pos[1];
+	point[2]+=cvx.final_posr->pos[2];
+
+	dReal max = dDOT(point,dir);
+	dReal tmp;
+	for (unsigned int i = 1; i < cvx.pointcount; ++i) 
+	{
+		dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+i*3);
+		point[0]+=cvx.final_posr->pos[0];
+		point[1]+=cvx.final_posr->pos[1];
+		point[2]+=cvx.final_posr->pos[2];      
+		tmp = dDOT(point, dir);
+		if (tmp > max) 
+		{ 
+			out[0]=point[0];
+			out[1]=point[1];
+			out[2]=point[2];
+			max = tmp; 
+		}
+	}
+}
+
+inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max)
+{
+	dVector3 point;
+	dReal value;
+	//fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]);
+	dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);
+	//fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]);
+	point[0]+=cvx.final_posr->pos[0];
+	point[1]+=cvx.final_posr->pos[1];
+	point[2]+=cvx.final_posr->pos[2];
+	max = min = dDOT(axis,cvx.points);
+	for (unsigned int i = 1; i < cvx.pointcount; ++i) 
+	{
+		dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+i*3);
+		point[0]+=cvx.final_posr->pos[0];
+		point[1]+=cvx.final_posr->pos[1];
+		point[2]+=cvx.final_posr->pos[2];
+		value=dDOT(axis,point);
+		if(value<min)
+		{
+			min=value;
+		}
+		else if(value>max)
+		{
+			max=value;
+		}
+	}
+	//fprintf(stdout,"Compute Interval Min Max %f,%f\n",min,max);
+}
+
+/*! \brief Does an axis separation test between the 2 convex shapes
+using faces and edges */
+int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,
+						   dContactGeom *contact, int skip)
+{
+	dVector4 plane,savedplane = {};
+	dReal min1,max1,min2,max2;
+	dVector3 e1,e2,t;
+	int maxc = flags & NUMC_MASK; // this is causing a segfault
+	//int maxc = 3;
+	int contacts=0;
+	dxConvex *g1=0,*g2=0;
+	unsigned int *pPoly;
+	dVector3 v;
+	// Test faces of cvx1 for separation
+	pPoly=cvx1.polygons;
+	for(unsigned int i=0;i<cvx1.planecount;++i)
+	{
+		// -- Apply Transforms --
+		// Rotate
+		dMULTIPLY0_331(plane,cvx1.final_posr->R,cvx1.planes+i*4);
+		dNormalize3(plane);
+		// Translate
+		plane[3]=
+			(cvx1.planes[(i*4)+3])+
+			(dMUL(plane[0],cvx1.final_posr->pos[0]) + 
+			dMUL(plane[1],cvx1.final_posr->pos[1]) + 
+			dMUL(plane[2],cvx1.final_posr->pos[2]));
+		ComputeInterval(cvx1,plane,min1,max1);
+		ComputeInterval(cvx2,plane,min2,max2);
+		//fprintf(stdout,"width %f\n",max1-min1);
+		if(max2<min1 || max1<min2) return 0;
+#if 1
+		// this one ON works
+		else if ((max1>min2)&&(max1<max2))
+		{
+			savedplane[0]=plane[0];
+			savedplane[1]=plane[1];
+			savedplane[2]=plane[2];
+			savedplane[3]=plane[3];
+			g1=&cvx2; // cvx2 moves
+			g2=&cvx1;
+		}
+#endif
+		pPoly+=pPoly[0]+1;
+	}
+	// Test faces of cvx2 for separation
+	pPoly=cvx2.polygons;
+	for(unsigned int i=0;i<cvx2.planecount;++i)
+	{
+		//       fprintf(stdout,"Poly verts %d\n",pPoly[0]);
+		// -- Apply Transforms --
+		// Rotate
+		dMULTIPLY0_331(plane,
+			cvx2.final_posr->R,
+			cvx2.planes+i*4);
+		dNormalize3(plane);
+		// Translate
+		plane[3]=
+			(cvx2.planes[(i*4)+3])+
+			(dMUL(plane[0],cvx2.final_posr->pos[0]) + 
+			dMUL(plane[1],cvx2.final_posr->pos[1]) + 
+			dMUL(plane[2],cvx2.final_posr->pos[2]));
+		ComputeInterval(cvx2,plane,min1,max1);
+		ComputeInterval(cvx1,plane,min2,max2);
+		//fprintf(stdout,"width %f\n",max1-min1);
+		if(max2<min1 || max1<min2) return 0;
+#if 1
+		// this one ON does not work
+		else if ((max1>min2)&&(max1<max2))
+		{
+			savedplane[0]=plane[0];
+			savedplane[1]=plane[1];
+			savedplane[2]=plane[2];
+			savedplane[3]=plane[3];
+			g1=&cvx1; // cvx1 moves
+			g2=&cvx2;
+		}
+#endif
+		pPoly+=pPoly[0]+1;
+	}
+	
+	// Test cross products of pairs of edges with new set structure 
+	iterator* it1=cvx1.edges->setIterator();
+	iterator* it2=cvx2.edges->setIterator();
+	
+	it1->setToFirst();
+	it2->setToFirst();
+	int i, j;
+	
+	for(i = 1; i <= cvx1.edges->length(); i++) {
+		pair* p1 = new pair();
+		*p1 = it1->getElem();
+		int first1 = p1->getNum1();
+		int second1 = p1->getNum2();
+		
+		dMULTIPLY0_331 (t,cvx1.final_posr->R,cvx1.points+first1*3);
+		dMULTIPLY0_331 (e1,cvx1.final_posr->R,cvx1.points+second1*3);
+		e1[0]-=t[0];
+		e1[1]-=t[1];
+		e1[2]-=t[2];
+		
+		for(j = 1; j <= cvx2.edges->length(); j++) {
+			pair* p2 = new pair();
+			*p2 = it2->getElem();
+			int first2 = p2->getNum1();
+			int second2 = p2->getNum2();
+		
+			dMULTIPLY0_331 (t,cvx2.final_posr->R,cvx2.points+first2*3);
+			dMULTIPLY0_331 (e2,cvx2.final_posr->R,cvx2.points+second2*3);
+			e2[0]-=t[0];
+			e2[1]-=t[1];
+			e2[2]-=t[2];
+			dCROSS(plane,=,e1,e2);
+			plane[3]=0;
+			ComputeInterval(cvx1,plane,min1,max1);
+			ComputeInterval(cvx2,plane,min2,max2);
+			if(max2<min1 || max1 < min2) return 0;
+			
+			it2->next();
+		}
+		
+		it1->next();
+		it2->setToFirst();
+	}
+	
+	// If we get here, there was a collision
+	static int  cvxhit=0;
+	contacts=0;
+	if(cvxhit<2)
+		fprintf(stdout,"Plane: %f,%f,%f,%f\n",
+		savedplane[0],
+		savedplane[1],
+		savedplane[2],
+		savedplane[3]);
+	for(unsigned int i=0;i<g1->pointcount;++i)
+	{
+		if(contacts==maxc) break;
+		dMULTIPLY0_331 (v,g1->final_posr->R,&g1->points[(i*3)]);
+		v[0]=g1->final_posr->pos[0]+v[0];
+		v[1]=g1->final_posr->pos[1]+v[1];
+		v[2]=g1->final_posr->pos[2]+v[2];
+		dReal distance = (dMUL(savedplane[0],v[0])  + // Ax +
+			dMUL(savedplane[1],v[1])  + // Bx +
+			dMUL(savedplane[2],v[2])) - savedplane[3]; // Cz + D
+
+		if((contacts<maxc)&&(distance<0))
+		{
+			CONTACT(contact,skip*contacts)->normal[0] = savedplane[0];
+			CONTACT(contact,skip*contacts)->normal[1] = savedplane[1];
+			CONTACT(contact,skip*contacts)->normal[2] = savedplane[2];
+			CONTACT(contact,skip*contacts)->pos[0]=v[0];
+			CONTACT(contact,skip*contacts)->pos[1]=v[1];
+			CONTACT(contact,skip*contacts)->pos[2]=v[2];
+			CONTACT(contact,skip*contacts)->depth = -distance;
+			CONTACT(contact,skip*contacts)->g1 = g1;
+			CONTACT(contact,skip*contacts)->g2 = g2;
+			if(cvxhit<2)
+				fprintf(stdout,"Contact: %f,%f,%f depth %f\n",
+				CONTACT(contact,skip*contacts)->pos[0],
+				CONTACT(contact,skip*contacts)->pos[1],
+				CONTACT(contact,skip*contacts)->pos[2],
+				CONTACT(contact,skip*contacts)->depth);
+			contacts++;
+		}
+	}
+	cvxhit++;
+	return contacts;
+}
+
+int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
+			  dContactGeom *contact, int skip)
+{
+
+//   if(!hit) fprintf(stdout,"dCollideConvexConvex\n");
+  dxConvex *Convex1 = (dxConvex*) o1;
+  dxConvex *Convex2 = (dxConvex*) o2;
+  int contacts = TestConvexIntersection(*Convex1,*Convex2,flags,contact,skip);
+  if(contacts)
+    {
+      //fprintf(stdout,"We have a Hit!\n");
+    }
+  return contacts;
+}
+
+
+
+// Ray - Convex collider by David Walters, June 2006
+int dCollideRayConvex( dxGeom *o1, dxGeom *o2,
+					   int /*flags*/, dContactGeom *contact, int /*skip*/ )
+{
+
+	dxRay* ray = (dxRay*) o1;
+	dxConvex* convex = (dxConvex*) o2;
+
+	contact->g1 = ray;
+	contact->g2 = convex;
+
+	dReal alpha, beta, nsign;
+	int flag;
+
+	//
+	// Compute some useful info
+	//
+
+	flag = 0;	// Assume start point is behind all planes.
+
+	for ( unsigned int i = 0; i < convex->planecount; ++i )
+	{
+		// Alias this plane.
+		dReal* plane = convex->planes + i * 4;
+
+		// If alpha >= 0 then start point is outside of plane.
+		alpha = dDOT( plane, ray->final_posr->pos ) - plane[3];
+
+		// If any alpha is positive, then
+		// the ray start is _outside_ of the hull
+		if ( alpha >= 0 )
+		{
+			flag = 1;
+			break;
+		}
+	}
+
+	// If the ray starts inside the convex hull, then everything is flipped.
+	nsign = ( flag ) ? REAL( 1.0 ) : REAL( -1.0 );
+
+
+	//
+	// Find closest contact point
+	//
+
+	// Assume no contacts.
+	contact->depth = dInfinity;
+
+	for ( unsigned int i = 0; i < convex->planecount; ++i )
+	{
+		// Alias this plane.
+		dReal* plane = convex->planes + i * 4;
+
+		// If alpha >= 0 then point is outside of plane.
+		alpha = dMUL(nsign,( dDOT( plane, ray->final_posr->pos ) - plane[3] ));
+
+		// Compute [ plane-normal DOT ray-normal ], (/flip)
+		beta = dMUL(dDOT13( plane, ray->final_posr->R+2 ),nsign);
+
+		// Ray is pointing at the plane? ( beta < 0 )
+		// Ray start to plane is within maximum ray length?
+		// Ray start to plane is closer than the current best distance?
+		if ( beta < -dEpsilon &&
+			 alpha >= REAL(0.0) && alpha <= ray->length &&
+			 alpha < contact->depth )
+		{
+			// Compute contact point on convex hull surface.
+			contact->pos[0] = ray->final_posr->pos[0] + dMUL(alpha,ray->final_posr->R[0*4+2]);
+			contact->pos[1] = ray->final_posr->pos[1] + dMUL(alpha,ray->final_posr->R[1*4+2]);
+			contact->pos[2] = ray->final_posr->pos[2] + dMUL(alpha,ray->final_posr->R[2*4+2]);
+
+			flag = 0;
+
+			// For all _other_ planes.
+			for ( unsigned int j = 0; j < convex->planecount; ++j )
+			{
+				if ( i == j )
+					continue;	// Skip self.
+
+				// Alias this plane.
+				dReal* planej = convex->planes + j * 4;
+
+				// If beta >= 0 then start is outside of plane.
+				beta = dDOT( planej, contact->pos ) - plane[3];
+
+				// If any beta is positive, then the contact point
+				// is not on the surface of the convex hull - it's just
+				// intersecting some part of its infinite extent.
+				if ( beta > dEpsilon )
+				{
+					flag = 1;
+					break;
+				}
+			}
+
+			// Contact point isn't outside hull's surface? then it's a good contact!
+			if ( flag == 0 )
+			{
+				// Store the contact normal, possibly flipped.
+				contact->normal[0] = dMUL(nsign,plane[0]);
+				contact->normal[1] = dMUL(nsign,plane[1]);
+				contact->normal[2] = dMUL(nsign,plane[2]);
+
+				// Store depth
+				contact->depth = alpha;
+			}
+		}
+	}
+
+	// Contact?
+	return ( contact->depth <= ray->length );
+}
+
+
+//<-- Convex Collision