from the start... checking in ode-0.9

1 files changed, 1294 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/src/convex.cpp b/libraries/ode-0.9/ode/src/convex.cpp
new file mode 100644
index 0000000..db93beb
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/convex.cpp
@@ -0,0 +1,1294 @@
+/*************************************************************************
+ *                                                                       *
+ * 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"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+#if _MSC_VER <= 1200
+#define dMIN(A,B)  ((A)>(B) ? B : A)
+#define dMAX(A,B)  ((A)>(B) ? A : B)
+#else
+#define dMIN(A,B)  std::min(A,B)
+#define dMAX(A,B)  std::max(A,B)
+#endif
+
+//****************************************************************************
+// Convex public API
+dxConvex::dxConvex (dSpaceID space,
+                    dReal *_planes,
+                    unsigned int _planecount,
+                    dReal *_points,
+                    unsigned int _pointcount,
+                    unsigned int *_polygons) : 
+  dxGeom (space,1)
+{
+  dAASSERT (_planes != NULL);
+  dAASSERT (_points != NULL);
+  dAASSERT (_polygons != NULL);
+  //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)
+        {
+                //fprintf(stdout,"Points in Poly: %d\n",*points_in_poly);
+                for(unsigned int j=0;j<*points_in_poly;++j)
+                {
+                        edges.insert(edge(dMIN(index[j],index[(j+1)%*points_in_poly]),
+                                dMAX(index[j],index[(j+1)%*points_in_poly])));
+                        //fprintf(stdout,"Insert %d-%d\n",index[j],index[(j+1)%*points_in_poly]);
+                }
+                points_in_poly+=(*points_in_poly+1);
+                index=points_in_poly+1;
+        }
+        /*
+        fprintf(stdout,"Edge Count: %d\n",edges.size());
+        for(std::set<edge>::iterator it=edges.begin();
+        it!=edges.end();
+        ++it)
+        {
+        fprintf(stdout,"Edge: %d-%d\n",it->first,it->second);
+        }
+        */
+}
+
+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);
+}
+
+void dGeomSetConvex (dGeomID g,dReal *_planes,unsigned int _planecount,
+                     dReal *_points,
+                     unsigned int _pointcount,
+                     unsigned int *_polygons)
+{
+  //fprintf(stdout,"dxConvex dGeomSetConvex\n");
+  dUASSERT (g && g->type == dConvexClass,"argument not a convex shape");
+  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 = (p[3] - dDOT(p,a)) / dDOT(p,ab);
+  
+  // If t in [0..1] compute and return intersection point
+  if (t >= 0.0 && t <= 1.0) 
+    {
+      q[0] = a[0] + t * ab[0];
+      q[1] = a[1] + t * ab[1];
+      q[2] = a[2] + 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 = (a*c)-(b*b);
+  if(denominator==0.0f)
+    {
+      return false;
+    }
+  t = ((a*e)-(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]=p1[3]*p2[0] - p2[3]*p1[0];
+  n[1]=p1[3]*p2[1] - p2[3]*p1[1];
+  n[2]=p1[3]*p2[2] - p2[3]*p1[2];
+  // Compute point on intersection line
+  //Cross(n, d,p);
+  dCROSS(p,=,n,d);
+  p[0]/=denom;
+  p[1]/=denom;
+  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 <= 0.0f && d2 <= 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 >= 0.0f && d4 <= d3)
+        {
+          out[0]=b[0];
+          out[1]=b[1];
+          out[2]=b[2];
+          return false;
+        }      
+      vc = d1*d4 - d3*d2;
+      if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) 
+        {
+          dReal v = d1 / (d1 - d3);
+          out[0] = a[0] + (ab[0]*v);
+          out[1] = a[1] + (ab[1]*v);
+          out[2] = a[2] + (ab[2]*v);
+          return false;
+        }
+    }
+  return true;
+}
+
+int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
+                                                 dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));

+        dIASSERT (o1->type == dConvexClass);

+        dIASSERT (o2->type == dPlaneClass);

+        dIASSERT ((flags & NUMC_MASK) >= 1);

+        

+        dxConvex *Convex = (dxConvex*) o1;
+        dxPlane *Plane = (dxPlane*) o2;
+        unsigned int contacts=0;
+        unsigned int maxc = flags & NUMC_MASK;
+        dVector3 v2;

+

+#define LTEQ_ZERO       0x10000000

+#define GTEQ_ZERO       0x20000000

+#define BOTH_SIGNS      (LTEQ_ZERO | GTEQ_ZERO)

+        dIASSERT((BOTH_SIGNS & NUMC_MASK) == 0); // used in conditional operator later

+

+        unsigned int totalsign = 0;

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

+        {

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

+                dVector3Add(Convex->final_posr->pos, v2, v2);

+                

+                unsigned int distance2sign = GTEQ_ZERO;

+                dReal distance2 = dVector3Dot(Plane->p, v2) - Plane->p[3]; // Ax + By + Cz - D

+                if((distance2 <= REAL(0.0)))

+                {

+                        distance2sign = distance2 != REAL(0.0) ? LTEQ_ZERO : BOTH_SIGNS;

+

+                        if (contacts != maxc)

+                        {

+                                dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);

+                                dVector3Copy(Plane->p, target->normal);

+                                dVector3Copy(v2, target->pos);

+                                target->depth = -distance2;

+                                target->g1 = Convex;

+                                target->g2 = Plane;

+                                contacts++;

+                        }

+                }

+

+                // Take new sign into account

+                totalsign |= distance2sign;

+                // Check if contacts are full and both signs have been already found

+                if ((contacts ^ maxc | totalsign) == BOTH_SIGNS) // harder to comprehend but requires one register less

+                {

+                        break; // Nothing can be changed any more

+                }

+        }

+        if (totalsign == BOTH_SIGNS) return contacts;

+        return 0;

+#undef BOTH_SIGNS

+#undef GTEQ_ZERO

+#undef LTEQ_ZERO

+}
+
+int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
+                          dContactGeom *contact, int skip)
+{
+  dIASSERT (skip >= (int)sizeof(dContactGeom));

+  dIASSERT (o1->type == dSphereClass);

+  dIASSERT (o2->type == dConvexClass);

+  dIASSERT ((flags & NUMC_MASK) >= 1);

+
+  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;
+  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 = dVector3Dot(plane, offsetpos) - plane[3]; // Ax + By + Cz - D
+      if(dist>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]+
+                    (-contact->normal[0]*Sphere->radius);
+                  contact->pos[1] = Sphere->final_posr->pos[1]+
+                    (-contact->normal[1]*Sphere->radius);
+                  contact->pos[2] = Sphere->final_posr->pos[2]+
+                    (-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=(temp[0]*temp[0])+(temp[1]*temp[1])+(temp[2]*temp[2]);
+                  // avoid the sqrt unless really necesary
+                  if(dist<(Sphere->radius*Sphere->radius))
+                    {
+                      // We got an indirect hit
+                      dist=dSqrt(dist);
+                      contact->normal[0] = temp[0]/dist;
+                      contact->normal[1] = temp[1]/dist;
+                      contact->normal[2] = temp[2]/dist;
+                      contact->pos[0] = Sphere->final_posr->pos[0]+
+                        (-contact->normal[0]*Sphere->radius);
+                      contact->pos[1] = Sphere->final_posr->pos[1]+
+                        (-contact->normal[1]*Sphere->radius);
+                      contact->pos[2] = Sphere->final_posr->pos[2]+
+                        (-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)
+{
+  dIASSERT (skip >= (int)sizeof(dContactGeom));

+  dIASSERT (o1->type == dConvexClass);

+  dIASSERT (o2->type == dBoxClass);

+  dIASSERT ((flags & NUMC_MASK) >= 1);

+  

+  dxConvex *Convex = (dxConvex*) o1;
+  dxBox *Box = (dxBox*) o2;

+  
+  return 0;
+}
+
+int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
+                             int flags, dContactGeom *contact, int skip)
+{
+  dIASSERT (skip >= (int)sizeof(dContactGeom));

+  dIASSERT (o1->type == dConvexClass);

+  dIASSERT (o2->type == dCapsuleClass);

+  dIASSERT ((flags & NUMC_MASK) >= 1);

+
+  dxConvex *Convex = (dxConvex*) o1;
+  dxCapsule *Capsule = (dxCapsule*) o2;

+  
+  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])+
+                        ((plane[0] * cvx.final_posr->pos[0]) + 
+                        (plane[1] * cvx.final_posr->pos[1]) + 
+                        (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
+static bool hit=false;
+
+/*
+  \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])+
+                        ((eq1[0] * cvx->final_posr->pos[0]) + 
+                        (eq1[1] * cvx->final_posr->pos[1]) + 
+                        (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=
+                        ((eq1[0]*solution[0])+
+                        (eq1[1]*solution[1])+
+                        (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(fabs(eq1[j])>fabs(eq1[l]))
+                                {
+                                        l=j;
+                                }
+                        }
+                        if(eq1[l]==0)
+                        {
+                                if(!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]=eq1[0]/eq1[l];
+                        aoveral[1]=eq1[1]/eq1[l];
+                        aoveral[2]=eq1[2]/eq1[l];
+                        aoveral[3]=eq1[3]/eq1[l];
+                        c1[0]=c[0]-((c[l]/eq1[l])*eq1[0]);
+                        c1[1]=c[1]-((c[l]/eq1[l])*eq1[1]);
+                        c1[2]=c[2]-((c[l]/eq1[l])*eq1[2]);
+                        solution[0]=solution[0]-((solution[l]/eq1[l])*eq1[0]);
+                        solution[1]=solution[1]-((solution[l]/eq1[l])*eq1[1]);
+                        solution[2]=solution[2]-((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])+
+                                                ((eq2[0] * cvx->final_posr->pos[0]) + 
+                                                (eq2[1] * cvx->final_posr->pos[1]) + 
+                                                (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]-=(cvx->planes[(index*4)+l]*aoveral[0]);
+                                        eq2[1]-=(cvx->planes[(index*4)+l]*aoveral[1]);
+                                        eq2[2]-=(cvx->planes[(index*4)+l]*aoveral[2]);
+                                        eq2[3]-=(cvx->planes[(index*4)+l]*aoveral[3]);
+                                        sum=
+                                                ((eq2[0]*solution[0])+
+                                                (eq2[1]*solution[1])+
+                                                (eq2[2]*solution[2]))-eq2[3];
+                                        if(sum>0)
+                                        {
+                                                m=0;
+                                                for( k=0;k<3;++k)
+                                                {
+                                                        if(fabs(eq2[k])>fabs(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]=eq2[0]/eq2[m];
+                                                aoveram[1]=eq2[1]/eq2[m];
+                                                aoveram[2]=eq2[2]/eq2[m];
+                                                aoveram[3]=eq2[3]/eq2[m];
+                                                c1[0]=c[0]-((c[m]/eq2[m])*eq2[0]);
+                                                c1[1]=c[1]-((c[m]/eq2[m])*eq2[1]);
+                                                c1[2]=c[2]-((c[m]/eq2[m])*eq2[2]);
+                                                solution[0]=solution[0]-((solution[m]/eq2[m])*eq2[0]);
+                                                solution[1]=solution[1]-((solution[m]/eq2[m])*eq2[1]);
+                                                solution[2]=solution[2]-((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])+
+                                                                        ((eq3[0] * cvx->final_posr->pos[0]) + 
+                                                                        (eq3[1] * cvx->final_posr->pos[1]) + 
+                                                                        (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]-=(cvx->planes[(index*4)+m]*aoveram[0]);
+                                                                eq3[1]-=(cvx->planes[(index*4)+m]*aoveram[1]);
+                                                                eq3[2]-=(cvx->planes[(index*4)+m]*aoveram[2]);
+                                                                eq3[3]-=(cvx->planes[(index*4)+m]*aoveram[3]);
+                                                                if(eq3[n]>0)
+                                                                {
+                                                                        max=dMIN(max,eq3[3]/eq3[n]);
+                                                                }
+                                                                else if(eq3[n]<0)
+                                                                {
+                                                                        min=dMAX(min,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(!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] = (eq2[3]-(eq2[n]*solution[n]))/eq2[m];
+                                                // lift to 3D
+                                                solution[l] = (eq1[3]-(eq1[m]*solution[m]+
+                                                        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)
+{
+        unsigned int index = 0;
+        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,min_depth=-dInfinity;
+        dVector3 e1,e2,t;
+        int maxc = flags & NUMC_MASK; // this is causing a segfault
+        dIASSERT(maxc != 0);
+        dxConvex *g1,*g2;
+        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])+
+                        ((plane[0] * cvx1.final_posr->pos[0]) + 
+                        (plane[1] * cvx1.final_posr->pos[1]) + 
+                        (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))
+                {
+                        min_depth=max1-min2;
+                        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])+
+                        ((plane[0] * cvx2.final_posr->pos[0]) + 
+                        (plane[1] * cvx2.final_posr->pos[1]) + 
+                        (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))
+                {
+                        min_depth=max1-min2;
+                        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
+        for(std::set<edge>::iterator i = cvx1.edges.begin();
+                i!= cvx1.edges.end();
+                ++i)
+        {
+                // we only need to apply rotation here
+                dMULTIPLY0_331 (t,cvx1.final_posr->R,cvx1.points+(i->first*3));
+                dMULTIPLY0_331 (e1,cvx1.final_posr->R,cvx1.points+(i->second*3));
+                e1[0]-=t[0];
+                e1[1]-=t[1];
+                e1[2]-=t[2];
+                for(std::set<edge>::iterator j = cvx2.edges.begin();
+                        j!= cvx2.edges.end();
+                        ++j)
+                {
+                        // we only need to apply rotation here
+                        dMULTIPLY0_331 (t,cvx2.final_posr->R,cvx2.points+(j->first*3));
+                        dMULTIPLY0_331 (e2,cvx2.final_posr->R,cvx2.points+(j->second*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;
+                }      
+        }
+/* -- uncomment if you are debugging

+        static int  cvxhit=0;

+        if(cvxhit<2)

+                fprintf(stdout,"Plane: %f,%f,%f,%f\n",

+                (double)savedplane[0],

+                (double)savedplane[1],

+                (double)savedplane[2],

+                (double)savedplane[3]);

+*/

+        // If we get here, there was a collision

+        int contacts=0;

+        for(unsigned int i=0;i<g1->pointcount;++i)
+        {
+                dMULTIPLY0_331 (v,g1->final_posr->R,&g1->points[(i*3)]);
+                dVector3Add(g1->final_posr->pos, v, v);

+
+                dReal distance = dVector3Dot(savedplane, v) - savedplane[3]; // Ax + By + Cz - D

+                if(distance<0)
+                {
+                        dContactGeom *target = SAFECONTACT(flags, contact, contacts, skip);

+                        dVector3Copy(savedplane, target->normal);

+                        dVector3Copy(v, target->pos);

+                        target->depth = -distance;

+                        target->g1 = g1;

+                        target->g2 = g2;

+/* -- uncomment if you are debugging

+                        if(cvxhit<2)
+                                fprintf(stdout,"Contact: %f,%f,%f depth %f\n",
+                                (double)target->pos[0],

+                                (double)target->pos[1],

+                                (double)target->pos[2],

+                                (double)target->depth);

+*/

+                        contacts++;

+                        if (contacts==maxc) break;
+                }
+        }
+/* -- uncomment if you are debugging

+        cvxhit++;

+*/
+

+        return contacts;
+}
+
+int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,
+                          dContactGeom *contact, int skip)
+{
+  dIASSERT (skip >= (int)sizeof(dContactGeom));

+  dIASSERT (o1->type == dConvexClass);

+  dIASSERT (o2->type == dConvexClass);

+  dIASSERT ((flags & NUMC_MASK) >= 1);

+
+//   if(!hit) fprintf(stdout,"dCollideConvexConvex\n");
+  dxConvex *Convex1 = (dxConvex*) o1;
+  dxConvex *Convex2 = (dxConvex*) o2;
+  int contacts;
+  if(contacts=TestConvexIntersection(*Convex1,*Convex2,flags,
+                                     contact,skip))
+    {
+      //fprintf(stdout,"We have a Hit!\n");
+    }
+  return contacts;
+}
+
+#if 0
+
+int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags, 
+                       dContactGeom *contact, int skip)
+{
+  dIASSERT (skip >= (int)sizeof(dContactGeom));

+  dIASSERT( o1->type == dRayClass );

+  dIASSERT( o2->type == dConvexClass );

+  dIASSERT ((flags & NUMC_MASK) >= 1);

+
+  dxRay* ray = (dxRay*) o1;
+  dxConvex* convex = (dxConvex*) o2;
+  dVector3 origin,destination,contactpoint,out;
+  dReal depth;
+  dVector4 plane;
+  unsigned int *pPoly=convex->polygons;
+  // Calculate ray origin and destination
+  destination[0]=0;
+  destination[1]=0;
+  destination[2]= ray->length;
+  // -- Rotate --
+  dMULTIPLY0_331(destination,ray->final_posr->R,destination);
+  origin[0]=ray->final_posr->pos[0];
+  origin[1]=ray->final_posr->pos[1];
+  origin[2]=ray->final_posr->pos[2];
+  destination[0]+=origin[0];
+  destination[1]+=origin[1];
+  destination[2]+=origin[2];
+  for(int i=0;i<convex->planecount;++i)
+    {
+      // Rotate
+      dMULTIPLY0_331(plane,convex->final_posr->R,convex->planes+(i*4));
+      // Translate
+      plane[3]=
+        (convex->planes[(i*4)+3])+
+        ((plane[0] * convex->final_posr->pos[0]) + 
+         (plane[1] * convex->final_posr->pos[1]) + 
+         (plane[2] * convex->final_posr->pos[2]));
+      if(IntersectSegmentPlane(origin, 
+                               destination, 
+                               plane, 
+                               depth, 
+                               contactpoint))
+        {
+          if(IsPointInPolygon(contactpoint,pPoly,convex,out))
+            {
+              contact->pos[0]=contactpoint[0];
+              contact->pos[1]=contactpoint[1];
+              contact->pos[2]=contactpoint[2];
+              contact->normal[0]=plane[0];
+              contact->normal[1]=plane[1];
+              contact->normal[2]=plane[2];
+              contact->depth=depth;
+              contact->g1 = ray;
+              contact->g2 = convex;
+              return 1;
+            }
+        }
+      pPoly+=pPoly[0]+1;
+    }
+  return 0;
+}
+
+#else
+
+// Ray - Convex collider by David Walters, June 2006
+int dCollideRayConvex( dxGeom *o1, dxGeom *o2,
+                                           int flags, dContactGeom *contact, int skip )
+{
+        dIASSERT( skip >= (int)sizeof(dContactGeom) );
+        dIASSERT( o1->type == dRayClass );
+        dIASSERT( o2->type == dConvexClass );

+        dIASSERT ((flags & NUMC_MASK) >= 1);

+
+        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 = nsign * ( dDOT( plane, ray->final_posr->pos ) - plane[3] );
+
+                // Compute [ plane-normal DOT ray-normal ], (/flip)
+                beta = 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 >= 0 && alpha <= ray->length &&
+                         alpha < contact->depth )
+                {
+                        // Compute contact point on convex hull surface.
+                        contact->pos[0] = ray->final_posr->pos[0] + alpha * ray->final_posr->R[0*4+2];
+                        contact->pos[1] = ray->final_posr->pos[1] + alpha * ray->final_posr->R[1*4+2];
+                        contact->pos[2] = ray->final_posr->pos[2] + 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] = nsign * plane[0];
+                                contact->normal[1] = nsign * plane[1];
+                                contact->normal[2] = nsign * plane[2];
+
+                                // Store depth
+                                contact->depth = alpha;

+                                

+                                if ((flags & CONTACTS_UNIMPORTANT) && contact->depth <= ray->length )

+                                {

+                                        // Break on any contact if contacts are not important

+                                        break; 

+                                }
+                        }
+                }
+        }
+
+        // Contact?
+        return ( contact->depth <= ray->length );
+}
+
+#endif
+
+//<-- Convex Collision