collide.c

[Game.Programming].Academic - Graphics Gems (6 books source code)

/*
 * C code from the article
 * "Fast Collision Detection of Moving Convex Polyhedra"
 * by Rich Rabbitz, [e-mail]
 * in "Graphics Gems IV", Academic Press, 1994
 */

#include <math.h>

typedef long	       Boolean;

#define FUZZ	       (0.00001)
#define TRUE	       (1)
#define FALSE	       (0)
#define MAX_VERTS      (1026)

#define ABS(x)	       ( (x) > 0 ? (x) : -(x) )
#define EQZ(x)	       ( ABS((x)) < FUZZ ? TRUE : FALSE )

#define DOT3(u,v)      ( u[0]*v[0] + u[1]*v[1] + u[2]*v[2])
#define VECADD3(r,u,v) { r[0]=u[0]+v[0]; r[1]=u[1]+v[1]; r[2]=u[2]+v[2]; }
#define VECADDS3(r,a,u,v){r[0]=a*u[0]+v[0]; r[1]=a*u[1]+v[1]; r[2]=a*u[2]+v[2];}
#define VECSMULT3(a,u) { u[0]= a * u[0]; u[1]= a * u[1]; u[2]= a * u[2]; }
#define VECSUB3(r,u,v) { r[0]=u[0]-v[0]; r[1]=u[1]-v[1]; r[2]=u[2]-v[2]; }
#define CPVECTOR3(u,v) { u[0]=v[0];	 u[1]=v[1];	 u[2]=v[2]; }
#define VECNEGATE3(u)  { u[0]=(-u[0]);	 u[1]=(-u[1]);	 u[2]=(-u[2]); }

#define GET(u,i,j,s) (*(u+i*s+j))
#define GET3(u,i,j,k,s) (*(u+i*(s*2)+(j*2)+k))

/**************************************************************************
 *
 * The structure polyhedron is used to store the geometry of the primitives
 * used in this collision detection example.  Since the collision detection
 * algorithm only needs to operate on the vertex set of a polyhedron, and
 * no rendering is done in this example, the faces and edges of a
 * polyhedron are not stored.  Adding faces and edges to the structure for
 * rendering purposes should be straight forward and will have no effect on
 * the collision detection computations.
 *
 **************************************************************************/

typedef struct polyhedron {
   double   verts[MAX_VERTS][3]; /* 3-D vertices of polyhedron. */
   int	    m;			 /* number of 3-D vertices.  */
   double   trn[3];		 /* translational position in world coords. */
   double   itrn[3];		 /* inverse of translational position. */
} *Polyhedron;

/**************************************************************************
 *
 * The structure couple is used to store the information required to
 * repeatedly test a pair of polyhedra for collision.  This information
 * includes : a reference to each polyhedron,  a flag indicating if there
 * is a cached prospective proper separating plane, two points for
 * constructing the proper separating plane, and possibly a cached set
 * of points from each polyhedron for speeding up the distance algorithm.
 *
 **************************************************************************/

typedef struct couple {
   Polyhedron  polyhdrn1;	/* First polyhedron of collision test. */
   Polyhedron  polyhdrn2;	/* Second polyhedron of collision test. */
   Boolean     plane_exists;	/* prospective separating plane flag */
   double      pln_pnt1[3];	/* 1st point used to form separating plane. */
   double      pln_pnt2[3];	/* 2nd point used to form separating plane. */
   int	       vert_indx[4][2]; /* cached points for distance algorithm. */
   int	       n;		/* number of cached points, if any. */
} *Couple;


/*** Arrays for vertex sets of three primitives ***/

double box[24];
double cyl[108];
double sphere[1026];

/*** RJR 08/20/93 ***********************************************************
 *
 *   Function to create vertex set of a polyhedron used to represent a
 *   box primitive.
 *
 *   On Entry:
 *	box_verts - an empty array of type double of size 24 or more.
 *
 *   On Exit:
 *	box_verts  - vertices of a polyhedron representing a box with
 *		     dimensions length = 5.0, width = 5.0, and height = 5.0.
 *
 *   Function Return : none.
 *
 ****************************************************************************/

void mak_box(box_verts)
double		  box_verts[];
{
   int		  i;
   static  double verts[24] =
	  {-5.0,  5.0, 5.0,  -5.0,  5.0, -5.0,	5.0,  5.0, -5.0,
	  5.0,	5.0, 5.0,  -5.0, -5.0, 5.0,  -5.0, -5.0, -5.0,
	  5.0, -5.0, -5.0,  5.0, -5.0, 5.0};

   for (i = 0; i < 24; i++)
      box_verts[i] = verts[i];
}


/*** RJR 08/20/93 ***********************************************************
 *
 *   Function to create vertex set of a polyhedron used to approximate
 *   a cylinder primitive.
 *
 *   On Entry:
 *	cyl_verts - an empty array of type double of size 108 or more.
 *
 *   On Exit:
 *	cyl_verts  - vertices of a polyhedron approximating a cylinder with
 *		     a base radius = 5.0, and height = 5.0.
 *   Function Return : none.
 *
 ****************************************************************************/

void mak_cyl(cyl_verts)
double	      cyl_verts[];
{
   int	      i;
   double     *pD_1, *pD_2, rads, stp, radius;

   pD_1 = cyl_verts;	  pD_2 = cyl_verts + 54;
   stp = 0.34906585;	  rads = 0.0;		    radius = 5.0;

   for (i = 0; i < 18; i++) {
      pD_1[0] = pD_2[0] = radius * cos(rads);	   /* X for top and bot. */
      pD_1[1] = 5.0;				   /* Y for top */
      pD_2[1] = 0.0;				   /* Y for bot. */
      pD_1[2] = pD_2[2] = -(radius * sin(rads));   /* Z for top and bot. */
      rads += stp;   pD_1 += 3;	  pD_2 += 3;
   }
}


/*** RJR 08/20/93 ***********************************************************
 *
 *   Function to create vertex set of a polyhedron used to approximate
 *   a sphere primitive.
 *
 *   On Entry:
 *	sph_verts - an empty array of type double of size 1026 or more.
 *
 *   On Exit:
 *	sph_verts  - vertices of a polyhedron approximating a sphere with
 *		     a radius = 5.25.
 *   Function Return : none.
 *
 ****************************************************************************/

void mak_sph(sph_verts)
double	    sph_verts[];
{
   int	    i, j;
   double   rads_1, rads_2, stp_1, stp_2, *pD_1, *pD_2, radius;

   rads_1 = 1.570796327;   stp_1  = 0.174532935;
   rads_2 = 0.0;	   stp_2  = 0.34906585;
   pD_1	  = sph_verts;	   radius = 5.25;

   for (i = 0; i < 19; i++) {
      pD_1[0] = radius * cos(rads_1);	  pD_1[1] = radius * sin(rads_1);
      pD_1[2] = 0.0;
      if (EQZ(pD_1[0]))
	 pD_1[0] = 0.01;
      rads_2 = 0.0;	stp_2 = 0.34906585;

      for (j = 0; j < 18; j++) {
	 pD_2 = pD_1 + j * 3;
	 pD_2[0] =   pD_1[0] * cos(rads_2) - pD_1[2] * sin(rads_2);  /* X */
	 pD_2[1] =   pD_1[1];					     /* Y */
	 pD_2[2] = -(pD_1[0] * sin(rads_2) + pD_1[2] * cos(rads_2)); /* Z */
	 rads_2 += stp_2;
      }
      pD_1 += 54;   rads_1 -= stp_1;
   }
}


/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to evaluate the support and contact functions at A for a given
 *   polytope. See equations (6) & (7).
 *
 *   On Entry:
 *	P   - table of 3-element points containing polytope vertices.
 *	r   - number of points in table.
 *	A   - vector at which support and contact functions will be evaluated.
 *	Cp  - empty 3-element array.
 *	P_i - pointer to an int.
 *
 *   On Exit:
 *	Cp  - contact point of P w.r.t. A.
 *	P_i - index into P of contact point.
 *
 *   Function Return :
 *	the result of the evaluation of eq. (6) for P and A.
 *
 ****************************************************************************/

double Hp(P, r, A, Cp, P_i)
double		 P[][3], A[], Cp[];
int		 r, *P_i;
{
   int		 i;
   double	 max_val, val;

   max_val = DOT3(P[0], A);	 *P_i = 0;

   for (i = 1; i < r; i++) {
      val = DOT3(P[i], A);
      if (val > max_val) {
	 *P_i = i;
	 max_val = val;
      }
   }
   CPVECTOR3(Cp, P[*P_i]);

   return max_val;
}


/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to evaluate the support and contact functions at A for the
 *   set difference of two polytopes. See equations (8) & (9).
 *
 *   On Entry:
 *	P1   - table of 3-element points containing first polytope's vertices.
 *	m1   - number of points in P1.
 *	P2   - table of 3-element points containing second polytope's vertices.
 *	m2   - number of points in P2.
 *	A    - vector at which to evaluate support and contact functions.
 *	Cs   - an empty array of size 3.
 *	P1_i - a pointer to an int.
 *	P2_i - a pointer to an int.
 *
 *   On Exit:
 *	Cs   - solution to equation 9.
 *	P1_i - index into P1 for solution to equation 9.
 *	P2_i - index into P2 for solution to equation 9.
 *
 *   Function Return :
 *	the result of the evaluation of eq. (8) for P1 and P2 at A.
 *
 ****************************************************************************/

double Hs(P1, m1, P2, m2, A, Cs, P1_i, P2_i)
double	     P1[][3], P2[][3], A[], Cs[];
int	     m1, m2, *P1_i, *P2_i;
{
   double    Cp_1[3], Cp_2[3], neg_A[3], Hp_1, Hp_2;

   Hp_1 = Hp(P1, m1, A, Cp_1, P1_i);

   CPVECTOR3(neg_A, A);
   VECNEGATE3(neg_A);
   Hp_2 = Hp(P2, m2, neg_A, Cp_2, P2_i);

   VECSUB3(Cs ,Cp_1, Cp_2);

   return (Hp_1 + Hp_2);
}

/*** RJR 05/26/93 ***********************************************************
 *
 *   Alternate function to compute the point in a polytope closest to the
 *   origin in 3-space. The polytope size m is restricted to 1 < m <= 4.
 *   This function is called only when comp_sub_dist fails.
 *
 *   On Entry:
 *	stop_index - number of sets to test.
 *	D_P	   - array of determinants for each set.
 *	Di_P	   - cofactors for each set.
 *	Is	   - indices for each set.
 *	c2	   - row size offset.
 *
 *   On Exit:
 *
 *   Function Return :
 *	the index of the set that is numerically closest to eq. (14).
 *
 ****************************************************************************/

int sub_dist_back(P, stop_index, D_P, Di_P, Is, c2)
double		  P[][3], Di_P[][4], *D_P;
int		  stop_index, *Is, c2;
{
   Boolean	  first, pass;
   int		  i, k, s, is, best_s;
   float	  sum, v_aff, best_v_aff;

   first = TRUE;  best_s = -1;
   for (s = 0; s < stop_index; s++) {
      pass = TRUE;
      if (D_P[s] > 0.0) {
	 for (i = 1; i <= GET(Is,s,0,c2); i++) {
	    is = GET(Is,s,i,c2);
	    if (Di_P[s][is] <= 0.0)
	       pass = FALSE;
	 }
      }
      else
	 pass = FALSE;

      if (pass) {

	 /*** Compute equation (33) in Gilbert ***/

	 k = GET(Is,s,1,c2);
	 sum = 0;
	 for (i = 1; i <= GET(Is, s, 0, c2); i++) {
	    is = GET(Is,s,i,c2);
	    sum += Di_P[s][is] * DOT3(P[is],P[k]);
	 }
	 v_aff = sqrt(sum / D_P[s]);
	 if (first) {
	    best_s = s;
	    best_v_aff = v_aff;
	    first = FALSE;
	 }
	 else {
	    if (v_aff < best_v_aff) {
	       best_s = s;
	       best_v_aff = v_aff;
	    }
	 }
      }
   }
   if (best_s == -1) {
      printf("backup failed\n");
      exit(0);
   }
   return best_s;
}

/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to compute the point in a polytope closest to the origin in
 *   3-space. The polytope size m is restricted to 1 < m <= 4.
 *
 *   On Entry:
 *	   P  - table of 3-element points containing polytope's vertices.
 *	   m  - number of points in P.
 *	  jo  - table of indices for storing Dj_P cofactors in Di_P.
 *	  Is  - indices into P for all sets of subsets of P.
 *	  IsC - indices into P for complement sets of Is.
 *   near_pnt - an empty array of size 3.
 *  near_indx - an empty array of size 4.
 *     lambda - an empty array of size 4.
 *
 *   On Exit:
 *   near_pnt - the point in P closest to the origin.
 *  near_indx - indices for a subset of P which is affinely independent.
 *		See eq. (14).
 *     lambda - the lambda as in eq. (14).
 *
 *   Function Return :
 *	the number of entries in near_indx and lambda.
 *
 ****************************************************************************/

int comp_sub_dist(P, m, jo, Is, IsC, near_pnt, near_indx, lambda)
double		    P[][3], near_pnt[], lambda[];
int		    m, *jo, *Is, *IsC, near_indx[];
{
   Boolean	    pass, fail;
   int		    i, j, k, isp, is, s, row, col, stop_index, c1, c2;
   double	    D_P[15], x[3], Dj_P, Di_P[15][4];
   static int	    combinations[5] = {0,0,3,7,15};

   stop_index = combinations[m];    /** how many subsets in P **/
   c1 = m;  c2 = m + 1;		    /** row offsets for IsC and Is **/

   /** Initialize Di_P for singletons **/

   Di_P[0][0] = Di_P[1][1] = Di_P[2][2] = Di_P[3][3] = 1.0;
   s = 0;   pass = FALSE;

   while ((s < stop_index) && (!pass)) {	/* loop through each subset */
      D_P[s] = 0.0;  fail = FALSE;
      for (i = 1; i <= GET(Is,s,0,c2); i++) {	/** loop through all Is **/
	 is = GET(Is,s,i,c2);
	 if (Di_P[s][is] > 0.0)			/** Condition 2 Theorem 2 **/
	    D_P[s] += Di_P[s][is];		/** sum from eq. (16)	  **/
	 else
	    fail = TRUE;
      }

      for (j = 1; j <= GET(IsC,s,0,c1); j++) {	/** loop through all IsC **/
	 Dj_P = 0;  k = GET(Is,s,1,c2);
	 isp = GET(IsC,s,j,c1);

	 for (i = 1; i <= GET(Is,s,0,c2); i++) {
	    is = GET(Is,s,i,c2);
	    VECSUB3(x, P[k], P[isp]);		  /** Wk - Wj  eq. (18) **/
	    Dj_P += Di_P[s][is] * DOT3(P[is], x); /** sum from eq. (18) **/
	 }
	 row = GET3(jo,s,isp,0,c1);
	 col = GET3(jo,s,isp,1,c1);
	 Di_P[row][col] = Dj_P;			 /** add new cofactors	**/

	 if (Dj_P > 0.00001)			 /** Condition 3 Theorem 2 **/
	    fail = TRUE;
      }
      if ((!fail) && (D_P[s] > 0.0))  /** Conditions 2 && 3 && 1 Theorem 2  **/
	 pass = TRUE;
      else
	 s++;
   }
   if (!pass) {
      printf("*** using backup procedure in sub_dist\n");
      s = sub_dist_back(P, stop_index, D_P, Di_P, Is, c2);
   }

   near_pnt[0] = near_pnt[1] = near_pnt[2] = 0.0;
   j = 0;
   for (i = 1; i <= GET(Is,s,0,c2); i++) {	 /** loop through all Is **/
      is = GET(Is,s,i,c2);
      near_indx[j] = is;
      lambda[j] = Di_P[s][is] / D_P[s];		       /** eq. (17)  **/
      VECADDS3(near_pnt, lambda[j], P[is], near_pnt);  /** eq. (17)  **/
      j++;
   }

   return (i-1);
}

/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to compute the point in a polytope closest to the origin in
 *   3-space.  The polytope size m is restricted to 1 < m <= 4.
 *
 *   On Entry:
 *	   P  - table of 3-element points containing polytope's vertices.
 *	   m  - number of points in P.
 *   near_pnt - an empty array of size 3.
 *  near_indx - an empty array of size 4.
 *     lambda - an empty array of size 4.
 *
 *   On Exit:
 *   near_pnt - the point in P closest to the origin.
 *  near_indx - indices for a subset of P which is affinely independent.
 *		See eq. (14).
 *     lambda - the lambda as in eq. (14).
 *
 *   Function Return :
 *	the number of entries in near_indx and lambda.
 *
 ****************************************************************************/

int sub_dist(P, m, near_pnt, near_indx, lambda)
double	      P[][3], near_pnt[], lambda[];
int	      near_indx[], m;
{
   int	      size;

/*
 *
 *  Tables to index the Di_P cofactor table in comp_sub_dist.  The s,i
 *  entry indicates where to store the cofactors computed with Is_C.
 *
 */

   static int	   jo_2[2][2][2]  = { {{0,0}, {2,1}},
				      {{2,0}, {0,0}}};

   static int	   jo_3[6][3][2]  = { {{0,0}, {3,1}, {4,2}},