1 /*************************************************************************

     2  *                                                                       *

     3  * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *

     4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

     5  *                                                                       *

     6  * This library is free software; you can redistribute it and/or         *

     7  * modify it under the terms of EITHER:                                  *

     8  *   (1) The GNU Lesser General Public License as published by the Free  *

     9  *       Software Foundation; either version 2.1 of the License, or (at  *

    10  *       your option) any later version. The text of the GNU Lesser      *

    11  *       General Public License is included with this library in the     *

    12  *       file LICENSE.TXT.                                               *

    13  *   (2) The BSD-style license that is included with this library in     *

    14  *       the file LICENSE-BSD.TXT.                                       *

    15  *                                                                       *

    16  * This library is distributed in the hope that it will be useful,       *

    17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *

    18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *

    19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

    20  *                                                                       *

    21  *************************************************************************/

    22 

    23 /*

    24 Code for Convex Collision Detection

    25 By Rodrigo Hernandez

    26 */

    27 //#include <algorithm>

    28 #include <ode/common.h>

    29 #include <ode/collision.h>

    30 #include <ode/matrix.h>

    31 #include <ode/rotation.h>

    32 #include <ode/odemath.h>

    33 #include "collision_kernel.h"

    34 #include "collision_std.h"

    35 #include "collision_util.h"

    36 

    37 #include "set.h"

    38 #include <ode/ode.h>

    39 

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

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

    42 

    43 //****************************************************************************

    44 // Convex public API

    45 dxConvex::dxConvex (dSpaceID space,

    46 		    dReal *_planes,

    47 		    unsigned int _planecount,

    48 		    dReal *_points,

    49 		    unsigned int _pointcount,

    50 		    unsigned int *_polygons) : 

    51   dxGeom (space,1)

    52 {

    53 

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

    55   type = dConvexClass;

    56   planes = _planes;

    57   planecount = _planecount;

    58   // we need points as well

    59   points = _points;

    60   pointcount = _pointcount;

    61   polygons=_polygons;

    62   FillEdges();

    63 }

    64 

    65 

    66 void dxConvex::computeAABB()

    67 {

    68   // this can, and should be optimized

    69   dVector3 point;

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

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

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

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

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

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

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

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

    78     {

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

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

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

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

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

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

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

    86     }

    87 }

    88 

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

    90   the polygon array gets updated */

    91 void dxConvex::FillEdges()

    92 {

    93 	unsigned int *points_in_poly=polygons;

    94 	unsigned int *index=polygons+1;

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

    96 	{

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

    98 		{

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

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

   101 			edges->addElem(*p);

   102 		}

   103 		points_in_poly+=(*points_in_poly+1);

   104 		index=points_in_poly+1;

   105 	}

   106 	

   107 }

   108 

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

   110 		    dReal *_points,

   111 		       unsigned int _pointcount,

   112 		       unsigned int *_polygons)

   113 {

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

   115   return new dxConvex(space,_planes, _planecount,

   116 		      _points,

   117 		      _pointcount,

   118 		      _polygons);

   119 }

   120 

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

   122 		     dReal *_points,

   123 		     unsigned int _pointcount,

   124 		     unsigned int *_polygons)

   125 {

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

   127   dxConvex *s = (dxConvex*) g;

   128   s->planes = _planes;

   129   s->planecount = _planecount;

   130   s->points = _points;

   131   s->pointcount = _pointcount;

   132   s->polygons=_polygons;

   133 }

   134 

   135 //****************************************************************************

   136 // Helper Inlines

   137 //

   138 

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

   140   \param a origin of the segment

   141   \param b segment destination

   142   \param p plane to test for intersection

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

   144   point

   145   \param q returns the intersection point

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

   147 */

   148 bool IntersectSegmentPlane(dVector3 a, 

   149 			   dVector3 b, 

   150 			   dVector4 p, 

   151 			   dReal &t, 

   152 			   dVector3 q)

   153 {

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

   155   dVector3 ab;

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

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

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

   159   

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

   161   

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

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

   164     {

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

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

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

   168       return true;

   169     }

   170   // Else no intersection

   171   return false;

   172 }

   173 

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

   175   \param Origin1 The origin of Ray 1

   176   \param Direction1 The direction of Ray 1

   177   \param Origin1 The origin of Ray 2

   178   \param Direction1 The direction of Ray 3

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

   180   (closest_point=Origin1+(Direction*t).

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

   182 */

   183 inline bool ClosestPointInRay(const dVector3 Origin1,

   184 			      const dVector3 Direction1,

   185 			      const dVector3 Origin2,

   186 			      const dVector3 Direction2,

   187 			      dReal& t)

   188 {

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

   190 		Origin1[1]-Origin2[1],

   191 		Origin1[2]-Origin2[2]};

   192   dReal a = dDOT(Direction1 , Direction1);

   193   dReal b = dDOT(Direction1 , Direction2);

   194   dReal c = dDOT(Direction2 , Direction2);

   195   dReal d = dDOT(Direction1 , w);

   196   dReal e = dDOT(Direction2 , w);

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

   198   if(denominator==REAL(0.0f))

   199     {

   200       return false;

   201     }

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

   203   return true;

   204 }

   205 

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

   207   \param p1 Plane 1

   208   \param p2 Plane 2

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

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

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

   212 */

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

   214 {

   215   // Compute direction of intersection line

   216   //Cross(p1, p2,d);

   217   dCROSS(d,=,p1,p2);

   218   

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

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

   221   dReal denom = dDOT(d, d);

   222   if (denom < dEpsilon) return false;

   223 

   224   dVector3 n;

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

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

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

   228   // Compute point on intersection line

   229   //Cross(n, d,p);

   230   dCROSS(p,=,n,d);

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

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

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

   234   return true;

   235 }

   236 

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

   238   \param p Point to test

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

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

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

   242 */

   243 inline bool IsPointInPolygon(dVector3 p,

   244 			     unsigned int *polygon,

   245 			     dxConvex *convex,

   246 			     dVector3 out)

   247 {

   248   // p is the point we want to check,

   249   // polygon is a pointer to the polygon we

   250   // are checking against, remember it goes

   251   // number of vertices then that many indexes

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

   253   // polygon if the point is not inside it.

   254   size_t pointcount=polygon[0];

   255   dVector3 a;

   256   dVector3 b;

   257   dVector3 c;

   258   dVector3 ab;

   259   dVector3 ac;

   260   dVector3 ap;

   261   dVector3 bp;

   262   dReal d1;

   263   dReal d2;

   264   dReal d3;

   265   dReal d4;

   266   dReal vc;

   267   polygon++; // skip past pointcount

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

   269     {

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

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

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

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

   274 

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

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

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

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

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

   280 

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

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

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

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

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

   286 

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

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

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

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

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

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

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

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

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

   296       d1 = dDOT(ab,ap);

   297       d2 = dDOT(ac,ap);

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

   299 	{

   300 	  out[0]=a[0];

   301 	  out[1]=a[1];

   302 	  out[2]=a[2];

   303 	  return false;

   304 	}

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

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

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

   308       d3 = dDOT(ab,bp);

   309       d4 = dDOT(ac,bp);

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

   311 	{

   312 	  out[0]=b[0];

   313 	  out[1]=b[1];

   314 	  out[2]=b[2];

   315 	  return false;

   316 	}      

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

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

   319 	{

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

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

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

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

   324 	  return false;

   325 	}

   326     }

   327   return true;

   328 }

   329 

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

   331 						 dContactGeom *contact, int skip)

   332 {

   333 

   334 	dxConvex *Convex = (dxConvex*) o1;

   335 	dxPlane *Plane = (dxPlane*) o2;

   336 	unsigned int contacts=0;

   337 	unsigned int maxc = flags & NUMC_MASK;

   338 	dVector3 v1;

   339 	dVector3 v2;

   340 	bool Hit=false;

   341 

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

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

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

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

   346 

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

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

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

   350 	if(distance1<=0)

   351 	{

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

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

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

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

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

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

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

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

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

   361 		contacts++;

   362 	}

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

   364 	{

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

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

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

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

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

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

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

   372 		if(!Hit) 

   373 			/* 

   374 			Avoid multiplication 

   375 			if we have already determined there is a hit 

   376 			*/

   377 		{

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

   379 			{

   380 				// there is a hit.

   381 				Hit=true;

   382 			}

   383 		}

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

   385 		{

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

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

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

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

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

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

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

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

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

   395 			contacts++;

   396 		}

   397 	}

   398 	if(Hit) return contacts;

   399 	return 0;

   400 }

   401 

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

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

   404 {

   405 

   406   dxSphere *Sphere = (dxSphere*) o1;

   407   dxConvex *Convex = (dxConvex*) o2;

   408   dReal dist,closestdist=dInfinity;

   409   dVector4 plane;

   410   // dVector3 contactpoint;

   411   dVector3 offsetpos,out,temp;

   412   unsigned int *pPoly=Convex->polygons;

   413   int closestplane=0;

   414   bool sphereinside=true;

   415   /* 

   416      Do a good old sphere vs plane check first,

   417      if a collision is found then check if the contact point

   418      is within the polygon

   419   */

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

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

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

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

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

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

   426     {

   427       // apply rotation to the plane

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

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

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

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

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

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

   434       if(dist>REAL(0.0))

   435 	{

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

   437 	  // outside of the convex hull.

   438 	  if(dist<Sphere->radius)

   439 	    {

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

   441 	      // the plane

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

   443 		{

   444 		  // finally if we get here we know that the

   445 		  // sphere is directly touching the inside of the polyhedron

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

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

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

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

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

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

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

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

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

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

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

   457 		  contact->g1 = Sphere;

   458 		  contact->g2 = Convex;

   459 		  return 1;

   460 		}

   461 	      else

   462 		{

   463 		  // the sphere may not be directly touching

   464 		  // the polyhedron, but it may be touching

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

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

   467 		  // center of the sphere is less than the sphere 

   468 		  // radius we have a hit.

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

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

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

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

   473 		  // avoid the sqrt unless really necesary

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

   475 		    {

   476 		      // We got an indirect hit

   477 		      dist=dSqrt(dist);

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

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

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

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

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

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

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

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

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

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

   488 		      contact->g1 = Sphere;

   489 		      contact->g2 = Convex;

   490 		      return 1;

   491 		    }

   492 		}

   493 	    }

   494 	  sphereinside=false;

   495 	}

   496       if(sphereinside)

   497 	{

   498 	  if(closestdist>dFabs(dist))

   499 	    {

   500 	      closestdist=dFabs(dist);

   501 	      closestplane=i;

   502 	    }

   503 	}

   504       pPoly+=pPoly[0]+1;

   505     }

   506     if(sphereinside)

   507       {

   508 	// if the center of the sphere is inside 

   509 	// the Convex, we need to pop it out

   510 	dMULTIPLY0_331(contact->normal,

   511 		       Convex->final_posr->R,

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

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

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

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

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

   517 	contact->g1 = Sphere;

   518 	contact->g2 = Convex;

   519 	return 1;

   520       }

   521   return 0;

   522 }

   523 

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

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

   526 {

   527   return 0;

   528 }

   529 

   530 int dCollideConvexCapsule (dxGeom */*o1*/, dxGeom */*o2*/,

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

   532 {

   533   return 0;

   534 }

   535 

   536 /*!

   537   \brief Retrieves the proper convex and plane index between 2 convex objects.

   538 

   539   Seidel's Algorithm does not discriminate between 2 different Convex Hulls,

   540   it only cares about planes, so we feed it the planes of Convex 1 followed

   541   by the planes of Convex 2 as a single collection of planes, 

   542   given an index into the single collection, 

   543   this function determines the correct convex object and index to retrieve

   544   the current plane.

   545   

   546   \param c1 Convex 1

   547   \param c2 Convex 2

   548   \param i Plane Index to retrieve

   549   \param index contains the translated index uppon return

   550   \return a pointer to the convex object containing plane index "i"

   551 */

   552 inline dxConvex* GetPlaneIndex(dxConvex& c1, 

   553 			       dxConvex& c2,

   554 			       unsigned int i,unsigned int& index)

   555 {

   556   if(i>=c1.planecount)

   557     {

   558       index = i-c1.planecount;

   559       return &c2;

   560     }

   561   index=i;

   562   return &c1;

   563 }

   564 

   565 inline void dumpplanes(dxConvex& cvx)

   566 {

   567 	// This is just a dummy debug function

   568 	dVector4 plane;

   569 	fprintf(stdout,"DUMP PLANES\n");

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

   571 	{

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

   573 		// Translate

   574 		plane[3]=

   575 			(cvx.planes[(i*4)+3])+

   576 			(dMUL(plane[0],cvx.final_posr->pos[0]) + 

   577 			dMUL(plane[1],cvx.final_posr->pos[1]) + 

   578 			dMUL(plane[2],cvx.final_posr->pos[2]));

   579 		fprintf(stdout,"%f %f %f %f\n",plane[0],plane[1],plane[2],plane[3]);

   580 	}

   581 }

   582 

   583 // this variable is for debuggin purpuses only, should go once everything works

   584 /*

   585   \brief Tests whether 2 convex objects collide

   586 

   587   Seidel's algorithm is a method to solve linear programs, 

   588   it finds the optimum vertex "v" of a set of functions, 

   589   in our case, the set of functions are the plane functions

   590   for the 2 convex objects being tested.

   591   We don't really care about the value optimum vertex though,

   592   but the 2 convex objects only collide if this vertex exists,

   593   otherwise, the set of functions is said to be "empty" or "void".

   594 

   595   Seidel's Original algorithm is recursive and not bound to any number

   596   of dimensions, the one I present here is Iterative rather than recursive

   597   and bound to 3 dimensions, which is what we care about.

   598   

   599   If you're interested (and you should be!) on the algorithm, the paper

   600   by Raimund Seidel himself should explain a lot better than I did, you can

   601   find it here: http://www.cs.berkeley.edu/~jrs/meshpapers/SeidelLP.pdf

   602 

   603   If posible, read the section about Linear Programming in 

   604   Christer Ericson's RealTime Collision Detection book.

   605   

   606   \note currently there seem to be some issues with this function since

   607   it doesn't detect collisions except for the most simple tests :(. 

   608 */

   609 bool SeidelLP(dxConvex& cvx1,dxConvex& cvx2)

   610 {

   611 	dVector3 c={1,0,0}; // The Objective vector can be anything

   612 	dVector3 solution;    // We dont really need the solution so its local

   613 	dxConvex *cvx;

   614 	unsigned int index;

   615 	unsigned int planecount=cvx1.planecount+cvx2.planecount;

   616 	dReal sum,min,max,med;

   617 	dVector3 c1; // ,c2;

   618 	dVector4 aoveral,aoveram; // these will contain cached computations

   619 	unsigned int l,m,n; // l and m are the axes to the zerod dimensions, n is the axe for the last dimension

   620 	unsigned int i,j,k;

   621 	dVector4 eq1,eq2,eq3; // cached equations for 3d,2d and 1d respectivelly

   622 	// Get the support mapping for a HUGE bounding box in direction c

   623 	solution[0]= (c[0]>0) ? dInfinity : -dInfinity;

   624 	solution[1]= (c[1]>0) ? dInfinity : -dInfinity;

   625 	solution[2]= (c[2]>0) ? dInfinity : -dInfinity;

   626 	for( i=0;i<planecount;++i)

   627 	{

   628 		// Isolate plane equation

   629 		cvx=GetPlaneIndex(cvx1,cvx2,i,index);

   630 		// Rotate

   631 		dMULTIPLY0_331(eq1,cvx->final_posr->R,&cvx->planes[(index*4)]);

   632 

   633 		// Translate

   634 		eq1[3]=(cvx->planes[(index*4)+3])+

   635 			(dMUL(eq1[0],cvx->final_posr->pos[0]) + 

   636 			dMUL(eq1[1],cvx->final_posr->pos[1]) + 

   637 			dMUL(eq1[2],cvx->final_posr->pos[2]));

   638 		//       if(!hit)

   639 		// 	{

   640 		// 	  fprintf(stdout,"Plane I %d: %f %f %f %f\n",i,

   641 		// 		  cvx->planes[(index*4)+0],

   642 		// 		  cvx->planes[(index*4)+1],

   643 		// 		  cvx->planes[(index*4)+2],

   644 		// 		  cvx->planes[(index*4)+3]);

   645 		// 	  fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",i,

   646 		// 		  eq1[0],

   647 		// 		  eq1[1],eq1[2],eq1[3]);

   648 		// 	  fprintf(stdout,"POS %f,%f,%f\n",

   649 		// 		  cvx->final_posr->pos[0],

   650 		// 		  cvx->final_posr->pos[1],

   651 		// 		  cvx->final_posr->pos[2]);

   652 		// 	}

   653 		// find if the solution is behind the current plane

   654 		sum=

   655 			(dMUL(eq1[0],solution[0])+

   656 			dMUL(eq1[1],solution[1])+

   657 			dMUL(eq1[2],solution[2]))-eq1[3];

   658 		// if not we need to discard the current solution

   659 		if(sum>0)

   660 		{

   661 			// go down a dimension by eliminating a variable

   662 			// first find l

   663 			l=0;

   664 			for( j=0;j<3;++j)

   665 			{

   666 				if(dFabs(eq1[j])>dFabs(eq1[l]))

   667 				{

   668 					l=j;

   669 				}

   670 			}

   671 			if(eq1[l]==0)

   672 			{

   673 				if(!GetGlobalData()->hit) 

   674 				{

   675 					fprintf(stdout,"Plane %d: %f %f %f %f is invalid\n",i,

   676 						eq1[0],eq1[1],eq1[2],eq1[3]);

   677 				}

   678 				return false; 

   679 			}

   680 			// then compute a/a[l] c1 and solution

   681 			aoveral[0]=dDIV(eq1[0],eq1[l]);

   682 			aoveral[1]=dDIV(eq1[1],eq1[l]);

   683 			aoveral[2]=dDIV(eq1[2],eq1[l]);

   684 			aoveral[3]=dDIV(eq1[3],eq1[l]);

   685 			c1[0]=c[0]-dMUL(dDIV(c[l],eq1[l]),eq1[0]);

   686 			c1[1]=c[1]-dMUL(dDIV(c[l],eq1[l]),eq1[1]);

   687 			c1[2]=c[2]-dMUL(dDIV(c[l],eq1[l]),eq1[2]);

   688 			solution[0]=solution[0]-dMUL(dDIV(solution[l],eq1[l]),eq1[0]);

   689 			solution[1]=solution[1]-dMUL(dDIV(solution[l],eq1[l]),eq1[1]);

   690 			solution[2]=solution[2]-dMUL(dDIV(solution[l],eq1[l]),eq1[2]);

   691 			// iterate a to get the new equations with the help of a/a[l]

   692 			for( j=0;j<planecount;++j)

   693 			{

   694 				if(i!=j)

   695 				{

   696 					cvx=GetPlaneIndex(cvx1,cvx2,j,index);

   697 					// Rotate

   698 					dMULTIPLY0_331(eq2,cvx->final_posr->R,&cvx->planes[(index*4)]);

   699 					// Translate

   700 					eq2[3]=(cvx->planes[(index*4)+3])+

   701 						(dMUL(eq2[0],cvx->final_posr->pos[0]) + 

   702 						dMUL(eq2[1],cvx->final_posr->pos[1]) + 

   703 						dMUL(eq2[2],cvx->final_posr->pos[2]));

   704 

   705 					//       if(!hit)

   706 					// 	{

   707 					// 	  fprintf(stdout,"Plane J %d: %f %f %f %f\n",j,

   708 					// 		  cvx->planes[(index*4)+0],

   709 					// 		  cvx->planes[(index*4)+1],

   710 					// 		  cvx->planes[(index*4)+2],

   711 					// 		  cvx->planes[(index*4)+3]);

   712 					// 	  fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",j,

   713 					// 		  eq2[0],

   714 					// 		  eq2[1],

   715 					// 		  eq2[2],

   716 					// 		  eq2[3]);

   717 					// 	  fprintf(stdout,"POS %f,%f,%f\n",

   718 					// 		  cvx->final_posr->pos[0],

   719 					// 		  cvx->final_posr->pos[1],

   720 					// 		  cvx->final_posr->pos[2]);

   721 					// 	}

   722 

   723 					// Take The equation down a dimension

   724 					eq2[0]-=dMUL(cvx->planes[(index*4)+l],aoveral[0]);

   725 					eq2[1]-=dMUL(cvx->planes[(index*4)+l],aoveral[1]);

   726 					eq2[2]-=dMUL(cvx->planes[(index*4)+l],aoveral[2]);

   727 					eq2[3]-=dMUL(cvx->planes[(index*4)+l],aoveral[3]);

   728 					sum=

   729 						(dMUL(eq2[0],solution[0])+

   730 						dMUL(eq2[1],solution[1])+

   731 						dMUL(eq2[2],solution[2]))-eq2[3];

   732 					if(sum>0)

   733 					{

   734 						m=0;

   735 						for( k=0;k<3;++k)

   736 						{

   737 							if(dFabs(eq2[k])>dFabs(eq2[m]))

   738 							{

   739 								m=k;

   740 							}

   741 						}

   742 						if(eq2[m]==0)

   743 						{

   744 							/* 

   745 							if(!hit) fprintf(stdout,

   746 							"Plane %d: %f %f %f %f is invalid\n",j,

   747 							eq2[0],eq2[1],eq2[2],eq2[3]);

   748 							*/

   749 							return false; 

   750 						}

   751 						// then compute a/a[m] c1 and solution

   752 						aoveram[0]=dDIV(eq2[0],eq2[m]);

   753 						aoveram[1]=dDIV(eq2[1],eq2[m]);

   754 						aoveram[2]=dDIV(eq2[2],eq2[m]);

   755 						aoveram[3]=dDIV(eq2[3],eq2[m]);

   756 						c1[0]=c[0]-dMUL(dDIV(c[m],eq2[m]),eq2[0]);

   757 						c1[1]=c[1]-dMUL(dDIV(c[m],eq2[m]),eq2[1]);

   758 						c1[2]=c[2]-dMUL(dDIV(c[m],eq2[m]),eq2[2]);

   759 						solution[0]=solution[0]-dMUL(dDIV(solution[m],eq2[m]),eq2[0]);

   760 						solution[1]=solution[1]-dMUL(dDIV(solution[m],eq2[m]),eq2[1]);

   761 						solution[2]=solution[2]-dMUL(dDIV(solution[m],eq2[m]),eq2[2]);

   762 						// figure out the value for n by elimination

   763 						n = (l==0) ? ((m==1)? 2:1):((m==0)?((l==1)?2:1):0);

   764 						// iterate a to get the new equations with the help of a/a[l]

   765 						min=-dInfinity;

   766 						max=med=dInfinity;

   767 						for(k=0;k<planecount;++k)

   768 						{

   769 							if((i!=k)&&(j!=k))

   770 							{

   771 								cvx=GetPlaneIndex(cvx1,cvx2,k,index);

   772 								// Rotate

   773 								dMULTIPLY0_331(eq3,cvx->final_posr->R,&cvx->planes[(index*4)]);

   774 								// Translate

   775 								eq3[3]=(cvx->planes[(index*4)+3])+

   776 									(dMUL(eq3[0],cvx->final_posr->pos[0]) + 

   777 									dMUL(eq3[1],cvx->final_posr->pos[1]) + 

   778 									dMUL(eq3[2],cvx->final_posr->pos[2]));

   779 								//       if(!hit)

   780 								// 	{

   781 								// 	  fprintf(stdout,"Plane K %d: %f %f %f %f\n",k,

   782 								// 		  cvx->planes[(index*4)+0],

   783 								// 		  cvx->planes[(index*4)+1],

   784 								// 		  cvx->planes[(index*4)+2],

   785 								// 		  cvx->planes[(index*4)+3]);

   786 								// 	  fprintf(stdout,"Transformed Plane %d: %f %f %f %f\n",k,

   787 								// 		  eq3[0],

   788 								// 		  eq3[1],

   789 								// 		  eq3[2],

   790 								// 		  eq3[3]);

   791 								// 	  fprintf(stdout,"POS %f,%f,%f\n",

   792 								// 		  cvx->final_posr->pos[0],

   793 								// 		  cvx->final_posr->pos[1],

   794 								// 		  cvx->final_posr->pos[2]);

   795 								// 	}

   796 

   797 								// Take the equation Down to 1D

   798 								eq3[0]-=dMUL(cvx->planes[(index*4)+m],aoveram[0]);

   799 								eq3[1]-=dMUL(cvx->planes[(index*4)+m],aoveram[1]);

   800 								eq3[2]-=dMUL(cvx->planes[(index*4)+m],aoveram[2]);

   801 								eq3[3]-=dMUL(cvx->planes[(index*4)+m],aoveram[3]);

   802 								if(eq3[n]>REAL(0.0))

   803 								{

   804 									max=dMIN(max,dDIV(eq3[3],eq3[n]));

   805 								}

   806 								else if(eq3[n]<0)

   807 								{

   808 									min=dMAX(min,dDIV(eq3[3],eq3[n]));

   809 								}

   810 								else

   811 								{

   812 									med=dMIN(med,eq3[3]);

   813 								}

   814 							}

   815 						}

   816 						if ((max<min)||(med<0)) 

   817 						{

   818 							// 			  if(!hit) fprintf(stdout,"((max<min)||(med<0)) ((%f<%f)||(%f<0))",max,min,med);

   819 							if(!GetGlobalData()->hit)

   820 							{

   821 								dumpplanes(cvx1);

   822 								dumpplanes(cvx2);

   823 							}

   824 							//fprintf(stdout,"Planes %d %d\n",i,j);

   825 							return false;

   826 

   827 						}

   828 						// find the value for the solution in one dimension

   829 						solution[n] = (c1[n]>=0) ? max:min;

   830 						// lift to 2D

   831 						solution[m] = dMUL((eq2[3]-dMUL(eq2[n],solution[n])),eq2[m]);

   832 						// lift to 3D

   833 						solution[l] = dDIV((eq1[3]-(dMUL(eq1[m],solution[m])+

   834 							dMUL(eq1[n],solution[n]))),eq1[l]);

   835 					}

   836 				}

   837 			}

   838 		}

   839 	}

   840 	return true;

   841 }

   842 

   843 /*! \brief A Support mapping function for convex shapes

   844   \param dir direction to find the Support Point for

   845   \param cvx convex object to find the support point for

   846   \param out the support mapping in dir.

   847  */

   848 inline void Support(dVector3 dir,dxConvex& cvx,dVector3 out)

   849 {

   850 	dVector3 point;

   851 	dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);

   852 	point[0]+=cvx.final_posr->pos[0];

   853 	point[1]+=cvx.final_posr->pos[1];

   854 	point[2]+=cvx.final_posr->pos[2];

   855 

   856 	dReal max = dDOT(point,dir);

   857 	dReal tmp;

   858 	for (unsigned int i = 1; i < cvx.pointcount; ++i) 

   859 	{

   860 		dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+i*3);

   861 		point[0]+=cvx.final_posr->pos[0];

   862 		point[1]+=cvx.final_posr->pos[1];

   863 		point[2]+=cvx.final_posr->pos[2];      

   864 		tmp = dDOT(point, dir);

   865 		if (tmp > max) 

   866 		{ 

   867 			out[0]=point[0];

   868 			out[1]=point[1];

   869 			out[2]=point[2];

   870 			max = tmp; 

   871 		}

   872 	}

   873 }

   874 

   875 inline void ComputeInterval(dxConvex& cvx,dVector4 axis,dReal& min,dReal& max)

   876 {

   877 	dVector3 point;

   878 	dReal value;

   879 	//fprintf(stdout,"Compute Interval Axis %f,%f,%f\n",axis[0],axis[1],axis[2]);

   880 	dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points);

   881 	//fprintf(stdout,"initial point %f,%f,%f\n",point[0],point[1],point[2]);

   882 	point[0]+=cvx.final_posr->pos[0];

   883 	point[1]+=cvx.final_posr->pos[1];

   884 	point[2]+=cvx.final_posr->pos[2];

   885 	max = min = dDOT(axis,cvx.points);

   886 	for (unsigned int i = 1; i < cvx.pointcount; ++i) 

   887 	{

   888 		dMULTIPLY0_331 (point,cvx.final_posr->R,cvx.points+i*3);

   889 		point[0]+=cvx.final_posr->pos[0];

   890 		point[1]+=cvx.final_posr->pos[1];

   891 		point[2]+=cvx.final_posr->pos[2];

   892 		value=dDOT(axis,point);

   893 		if(value<min)

   894 		{

   895 			min=value;

   896 		}

   897 		else if(value>max)

   898 		{

   899 			max=value;

   900 		}

   901 	}

   902 	//fprintf(stdout,"Compute Interval Min Max %f,%f\n",min,max);

   903 }

   904 

   905 /*! \brief Does an axis separation test between the 2 convex shapes

   906 using faces and edges */

   907 int TestConvexIntersection(dxConvex& cvx1,dxConvex& cvx2, int flags,

   908 						   dContactGeom *contact, int skip)

   909 {

   910 	dVector4 plane,savedplane = {};

   911 	dReal min1,max1,min2,max2;

   912 	dVector3 e1,e2,t;

   913 	int maxc = flags & NUMC_MASK; // this is causing a segfault

   914 	//int maxc = 3;

   915 	int contacts=0;

   916 	dxConvex *g1=0,*g2=0;

   917 	unsigned int *pPoly;

   918 	dVector3 v;

   919 	// Test faces of cvx1 for separation

   920 	pPoly=cvx1.polygons;

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

   922 	{

   923 		// -- Apply Transforms --

   924 		// Rotate

   925 		dMULTIPLY0_331(plane,cvx1.final_posr->R,cvx1.planes+i*4);

   926 		dNormalize3(plane);

   927 		// Translate

   928 		plane[3]=

   929 			(cvx1.planes[(i*4)+3])+

   930 			(dMUL(plane[0],cvx1.final_posr->pos[0]) + 

   931 			dMUL(plane[1],cvx1.final_posr->pos[1]) + 

   932 			dMUL(plane[2],cvx1.final_posr->pos[2]));

   933 		ComputeInterval(cvx1,plane,min1,max1);

   934 		ComputeInterval(cvx2,plane,min2,max2);

   935 		//fprintf(stdout,"width %f\n",max1-min1);

   936 		if(max2<min1 || max1<min2) return 0;

   937 #if 1

   938 		// this one ON works

   939 		else if ((max1>min2)&&(max1<max2))

   940 		{

   941 			savedplane[0]=plane[0];

   942 			savedplane[1]=plane[1];

   943 			savedplane[2]=plane[2];

   944 			savedplane[3]=plane[3];

   945 			g1=&cvx2; // cvx2 moves

   946 			g2=&cvx1;

   947 		}

   948 #endif

   949 		pPoly+=pPoly[0]+1;

   950 	}

   951 	// Test faces of cvx2 for separation

   952 	pPoly=cvx2.polygons;

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

   954 	{

   955 		//       fprintf(stdout,"Poly verts %d\n",pPoly[0]);

   956 		// -- Apply Transforms --

   957 		// Rotate

   958 		dMULTIPLY0_331(plane,

   959 			cvx2.final_posr->R,

   960 			cvx2.planes+i*4);

   961 		dNormalize3(plane);

   962 		// Translate

   963 		plane[3]=

   964 			(cvx2.planes[(i*4)+3])+

   965 			(dMUL(plane[0],cvx2.final_posr->pos[0]) + 

   966 			dMUL(plane[1],cvx2.final_posr->pos[1]) + 

   967 			dMUL(plane[2],cvx2.final_posr->pos[2]));

   968 		ComputeInterval(cvx2,plane,min1,max1);

   969 		ComputeInterval(cvx1,plane,min2,max2);

   970 		//fprintf(stdout,"width %f\n",max1-min1);

   971 		if(max2<min1 || max1<min2) return 0;

   972 #if 1

   973 		// this one ON does not work

   974 		else if ((max1>min2)&&(max1<max2))

   975 		{

   976 			savedplane[0]=plane[0];

   977 			savedplane[1]=plane[1];

   978 			savedplane[2]=plane[2];

   979 			savedplane[3]=plane[3];

   980 			g1=&cvx1; // cvx1 moves

   981 			g2=&cvx2;

   982 		}

   983 #endif

   984 		pPoly+=pPoly[0]+1;

   985 	}

   986 	

   987 	// Test cross products of pairs of edges with new set structure 

   988 	iterator* it1=cvx1.edges->setIterator();

   989 	iterator* it2=cvx2.edges->setIterator();

   990 	

   991 	it1->setToFirst();

   992 	it2->setToFirst();

   993 	int i, j;

   994 	

   995 	for(i = 1; i <= cvx1.edges->length(); i++) {

   996 		pair* p1 = new pair();

   997 		*p1 = it1->getElem();

   998 		int first1 = p1->getNum1();

   999 		int second1 = p1->getNum2();

  1000 		

  1001 		dMULTIPLY0_331 (t,cvx1.final_posr->R,cvx1.points+first1*3);

  1002 		dMULTIPLY0_331 (e1,cvx1.final_posr->R,cvx1.points+second1*3);

  1003 		e1[0]-=t[0];

  1004 		e1[1]-=t[1];

  1005 		e1[2]-=t[2];

  1006 		

  1007 		for(j = 1; j <= cvx2.edges->length(); j++) {

  1008 			pair* p2 = new pair();

  1009 			*p2 = it2->getElem();

  1010 			int first2 = p2->getNum1();

  1011 			int second2 = p2->getNum2();

  1012 		

  1013 			dMULTIPLY0_331 (t,cvx2.final_posr->R,cvx2.points+first2*3);

  1014 			dMULTIPLY0_331 (e2,cvx2.final_posr->R,cvx2.points+second2*3);

  1015 			e2[0]-=t[0];

  1016 			e2[1]-=t[1];

  1017 			e2[2]-=t[2];

  1018 			dCROSS(plane,=,e1,e2);

  1019 			plane[3]=0;

  1020 			ComputeInterval(cvx1,plane,min1,max1);

  1021 			ComputeInterval(cvx2,plane,min2,max2);

  1022 			if(max2<min1 || max1 < min2) return 0;

  1023 			

  1024 			it2->next();

  1025 		}

  1026 		

  1027 		it1->next();

  1028 		it2->setToFirst();

  1029 	}

  1030 	

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

  1032 	static int  cvxhit=0;

  1033 	contacts=0;

  1034 	if(cvxhit<2)

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

  1036 		savedplane[0],

  1037 		savedplane[1],

  1038 		savedplane[2],

  1039 		savedplane[3]);

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

  1041 	{

  1042 		if(contacts==maxc) break;

  1043 		dMULTIPLY0_331 (v,g1->final_posr->R,&g1->points[(i*3)]);

  1044 		v[0]=g1->final_posr->pos[0]+v[0];

  1045 		v[1]=g1->final_posr->pos[1]+v[1];

  1046 		v[2]=g1->final_posr->pos[2]+v[2];

  1047 		dReal distance = (dMUL(savedplane[0],v[0])  + // Ax +

  1048 			dMUL(savedplane[1],v[1])  + // Bx +

  1049 			dMUL(savedplane[2],v[2])) - savedplane[3]; // Cz + D

  1050 

  1051 		if((contacts<maxc)&&(distance<0))

  1052 		{

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

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

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

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

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

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

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

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

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

  1062 			if(cvxhit<2)

  1063 				fprintf(stdout,"Contact: %f,%f,%f depth %f\n",

  1064 				CONTACT(contact,skip*contacts)->pos[0],

  1065 				CONTACT(contact,skip*contacts)->pos[1],

  1066 				CONTACT(contact,skip*contacts)->pos[2],

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

  1068 			contacts++;

  1069 		}

  1070 	}

  1071 	cvxhit++;

  1072 	return contacts;

  1073 }

  1074 

  1075 int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags,

  1076 			  dContactGeom *contact, int skip)

  1077 {

  1078 

  1079 //   if(!hit) fprintf(stdout,"dCollideConvexConvex\n");

  1080   dxConvex *Convex1 = (dxConvex*) o1;

  1081   dxConvex *Convex2 = (dxConvex*) o2;

  1082   int contacts = TestConvexIntersection(*Convex1,*Convex2,flags,contact,skip);

  1083   if(contacts)

  1084     {

  1085       //fprintf(stdout,"We have a Hit!\n");

  1086     }

  1087   return contacts;

  1088 }

  1089 

  1090 

  1091 

  1092 // Ray - Convex collider by David Walters, June 2006

  1093 int dCollideRayConvex( dxGeom *o1, dxGeom *o2,

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

  1095 {

  1096 

  1097 	dxRay* ray = (dxRay*) o1;

  1098 	dxConvex* convex = (dxConvex*) o2;

  1099 

  1100 	contact->g1 = ray;

  1101 	contact->g2 = convex;

  1102 

  1103 	dReal alpha, beta, nsign;

  1104 	int flag;

  1105 

  1106 	//

  1107 	// Compute some useful info

  1108 	//

  1109 

  1110 	flag = 0;	// Assume start point is behind all planes.

  1111 

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

  1113 	{

  1114 		// Alias this plane.

  1115 		dReal* plane = convex->planes + i * 4;

  1116 

  1117 		// If alpha >= 0 then start point is outside of plane.

  1118 		alpha = dDOT( plane, ray->final_posr->pos ) - plane[3];

  1119 

  1120 		// If any alpha is positive, then

  1121 		// the ray start is _outside_ of the hull

  1122 		if ( alpha >= 0 )

  1123 		{

  1124 			flag = 1;

  1125 			break;

  1126 		}

  1127 	}

  1128 

  1129 	// If the ray starts inside the convex hull, then everything is flipped.

  1130 	nsign = ( flag ) ? REAL( 1.0 ) : REAL( -1.0 );

  1131 

  1132 

  1133 	//

  1134 	// Find closest contact point

  1135 	//