collide.c

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

				      {{3,0}, {0,0}, {5,2}},
				      {{4,0}, {5,1}, {0,0}},
				      {{0,0}, {0,0}, {6,2}},
				      {{0,0}, {6,1}, {0,0}},
				      {{6,0}, {0,0}, {0,0}}};

   static int	  jo_4[14][4][2] = { { {0,0}, {4,1}, {5,2}, {6,3}},
				     { {4,0}, {0,0}, {7,2}, {8,3}},
				     { {5,0}, {7,1}, {0,0}, {9,3}},
				     { {6,0}, {8,1}, {9,2}, {0,0}},
				     { {0,0}, {0,0},{10,2},{11,3}},
				     { {0,0},{10,1}, {0,0},{12,3}},
				     { {0,0},{11,1},{12,2}, {0,0}},
				     {{10,0}, {0,0}, {0,0},{13,3}},
				     {{11,0}, {0,0},{13,2}, {0,0}},
				     {{12,0},{13,1}, {0,0}, {0,0}},
				     { {0,0}, {0,0}, {0,0},{14,3}},
				     { {0,0}, {0,0},{14,2}, {0,0}},
				     { {0,0},{14,1}, {0,0}, {0,0}},
				     {{14,0}, {0,0}, {0,0}, {0,0}}};


/*
 *  These tables represent each Is.  The first column of each row indicates
 *  the size of the set.
 *
 */
   static int	  Is_2[3][3] = { {1,0,0}, {1,1,0}, {2,0,1}};

   static int	  Is_3[7][4] = { {1,0,0,0}, {1,1,0,0}, {1,2,0,0}, {2,0,1,0},
				 {2,0,2,0}, {2,1,2,0}, {3,0,1,2}};

   static int	 Is_4[15][5] = { {1,0,0,0,0}, {1,1,0,0,0}, {1,2,0,0,0},
				 {1,3,0,0,0}, {2,0,1,0,0}, {2,0,2,0,0},
				 {2,0,3,0,0}, {2,1,2,0,0}, {2,1,3,0,0},
				 {2,2,3,0,0}, {3,0,1,2,0}, {3,0,1,3,0},
				 {3,0,2,3,0}, {3,1,2,3,0}, {4,0,1,2,3}};

/*
 *  These tables represent each Is complement. The first column of each row
 *  indicates the size of the set.
 *
 */
   static int	 IsC_2[3][2] = { {1,1}, {1,0}, {0,0}};

   static int	 IsC_3[7][3] = { {2,1,2}, {2,0,2}, {2,0,1}, {1,2,0}, {1,1,0},
				 {1,0,0}, {0,0,0}};

   static int	IsC_4[15][4] = { {3,1,2,3}, {3,0,2,3}, {3,0,1,3}, {3,0,1,2},
				 {2,2,3,0}, {2,1,3,0}, {2,1,2,0}, {2,0,3,0},
				 {2,0,2,0}, {2,0,1,0}, {1,3,0,0}, {1,2,0,0},
				 {1,1,0,0}, {1,0,0,0}, {0,0,0,0}};


   /** Call comp_sub_dist with appropriate tables according to size of P **/

   switch (m) {
      case 2:
	 size = comp_sub_dist(P, m, jo_2[0][0], Is_2[0], IsC_2[0], near_pnt,
							   near_indx, lambda);
      break;
      case 3:
	 size = comp_sub_dist(P, m, jo_3[0][0], Is_3[0], IsC_3[0], near_pnt,
							   near_indx, lambda);
      break;
      case 4:
	 size = comp_sub_dist(P, m, jo_4[0][0], Is_4[0], IsC_4[0], near_pnt,
							    near_indx, lambda);
      break;
   }

   return size;
}


/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to compute the minimum distance between two convex polytopes in
 *   3-space.
 *
 *   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.
 *	   VP - an empty array of size 3.
 *  near_indx - a 4x2 matrix possibly containing indices of initialization
 *		points. The first column are indices into P1, and the second
 *		column are indices into P2.
 *     lambda - an empty array of size 4.
 *	   m3 - a pointer to an int, which indicates how many initial points
 *		to extract from near_indx. If 0, near_indx is ignored.
 *
 *   On Exit:
 *	 Vp   - vector difference of the two near points in P1 and P2.
 *		The length of this vector is the minimum distance between P1
 *		and P2.
 *  near_indx - updated indices into P1 and P2 which indicate the affinely
 *		independent point sets from each polytope which can be used
 *		to compute along with lambda the near points in P1 and P2
 *		as in eq. (12). These indices can be used to re-initialize
 *		dist3d in the next iteration.
 *     lambda - the lambda as in eqs. (11) & (12).
 *	   m3 - the updated number of indices for P1 and P2 in near_indx.
 *
 *   Function Return : none.
 *
 ****************************************************************************/

void dist3d(P1, m1, P2, m2, VP, near_indx, lambda, m3)
double		    P1[][3], P2[][3], VP[], lambda[];
int		    m1, m2, near_indx[][2], *m3;
{
   Boolean	    pass;
   int		    set_size, I[4], i, j, i_tab[4], j_tab[4], P1_i, P2_i, k;
   double	    Hs(), Pk[4][3], Pk_subset[4][3], Vk[3], neg_Vk[3], Cp[3],
		    Gp;

   if ((*m3) == 0) {	     /** if *m3 == 0 use single point initialization **/
      set_size = 1;
      VECSUB3(Pk[0], P1[0], P2[0]);	 /** first elementary polytope **/
      i_tab[0] = j_tab[0] = 0;
   }
   else {				 /** else use indices from near_indx **/
      for (k = 0; k < (*m3); k++) {
	 i = i_tab[k] = near_indx[k][0];
	 j = j_tab[k] = near_indx[k][1];
	 VECSUB3(Pk[k], P1[i], P2[j]);	 /** first elementary polytope **/
      }
      set_size = *m3;
   }

   pass = FALSE;
   while (!pass) {

      /** compute Vk **/

      if (set_size == 1) {
	 CPVECTOR3(Vk, Pk[0]);
	 I[0] = 0;
      }
      else
	 set_size = sub_dist(Pk, set_size, Vk, I, lambda);

      /** eq. (13) **/

      CPVECTOR3(neg_Vk, Vk);	  VECNEGATE3(neg_Vk);
      Gp = DOT3(Vk, Vk) + Hs(P1, m1, P2, m2, neg_Vk, Cp, &P1_i, &P2_i);

      /** keep track of indices for P1 and P2 **/

      for (i = 0; i < set_size; i++) {
	 j = I[i];
	 i_tab[i] = i_tab[j];	  /** j is value from member of some Is **/
	 j_tab[i] = j_tab[j];	  /** j is value from member of some Is **/
      }

      if (EQZ(Gp))		  /** Do we have a solution **/
	 pass = TRUE;
      else {
	 for (i = 0; i < set_size; i++) {
	    j = I[i];
	    CPVECTOR3(Pk_subset[i], Pk[j]);  /** extract affine subset of Pk **/
	 }
	 for (i = 0; i < set_size; i++)
	    CPVECTOR3(Pk[i], Pk_subset[i]);  /** load into Pk+1 **/

	 CPVECTOR3(Pk[i], Cp);		     /** Union of Pk+1 with Cp **/
	 i_tab[i] = P1_i;  j_tab[i] = P2_i;
	 set_size++;
      }
   }

   CPVECTOR3(VP, Vk);			  /** load VP **/
   *m3 = set_size;
   for(i = 0; i < set_size; i++) {
      near_indx[i][0] = i_tab[i];	  /** set indices of near pnt. in P1 **/
      near_indx[i][1] = j_tab[i];	  /** set indices of near pnt. in P2 **/
   }
}

/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to compute a proper separating plane between a pair of
 *   polytopes.	 The plane will be a support plane for polytope 1.
 *
 *   On Entry:
 *	couple - couple structure for a pair of polytopes.
 *
 *   On Exit:
 *	couple - containing new proper separating plane, if one was
 *		 found.
 *
 *   Function Return :
 *	result of whether a separating plane exists, or not.
 *
 ****************************************************************************/

Boolean get_new_plane(couple)
Couple		  couple;
{
   Polyhedron	  polyhedron1, polyhedron2;
   Boolean	  plane_exists;
   double	  pnts1[MAX_VERTS][3], pnts2[MAX_VERTS][3], dist,
		  u[3], v[3], lambda[4], VP[3];
   int		  i, k, m1, m2;

   plane_exists = FALSE;

   polyhedron1 = couple->polyhdrn1;    polyhedron2 = couple->polyhdrn2;

   /** Apply M1 to vertices of polytope 1 **/

   m1 = polyhedron1->m;
   for (i = 0; i < m1; i++) {
      CPVECTOR3(pnts1[i], polyhedron1->verts[i]);
      VECADD3(pnts1[i], pnts1[i], polyhedron1->trn);
   }

   /** Apply M2 to vertices of polytope 1 **/

   m2 = polyhedron2->m;
   for (i = 0; i < m2; i++) {
      CPVECTOR3(pnts2[i], polyhedron2->verts[i]);
      VECADD3(pnts2[i], pnts2[i], polyhedron2->trn);
   }

   /** solve eq. (1) for two polytopes **/

   dist3d(pnts1, m1, pnts2, m2, VP, couple->vert_indx, lambda, &couple->n);

   dist = sqrt(DOT3(VP,VP));   /** distance between polytopes **/

   if (!EQZ(dist)) {	       /** Does a separating plane exist **/
      plane_exists = TRUE;
      u[0] = u[1] = u[2] = v[0] = v[1] = v[2] = 0.0;
      for (i = 0; i < couple->n; i++) {
	 k = couple->vert_indx[i][0];
	 VECADDS3(u, lambda[i], pnts1[k], u);  /** point in P1 **/
	 k = couple->vert_indx[i][1];
	 VECADDS3(v, lambda[i], pnts2[k], v);  /** point in P2 **/
      }

      /** Store separating plane in P1's local coordinates **/

      VECADD3(u, u, polyhedron1->itrn);
      VECADD3(v, v, polyhedron1->itrn);

      /** Place separating plane in couple data structure **/

      CPVECTOR3(couple->pln_pnt1, u);
      CPVECTOR3(couple->pln_pnt2, v);
   }
   return plane_exists;
}

/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to detect if two polyhedra are intersecting.
 *
 *   On Entry:
 *	couple - couple structure for a pair of polytopes.
 *
 *   On Exit:
 *
 *   Function Return :
 *	result of whether polyhedra are intersecting or not.
 *
 ****************************************************************************/

Boolean Collision(couple)
Couple		  couple;
{
   Polyhedron	  polyhedron1, polyhedron2;
   Boolean	  collide, loop;
   double	  u[3], v[3], norm[3], d;
   int		  i, m;

   polyhedron1 = couple->polyhdrn1;	polyhedron2 = couple->polyhdrn2;
   collide = FALSE;

   if (couple->plane_exists) {

      /** Transform proper separating plane to P2 local coordinates.   **/
      /** This avoids the computational cost of applying the	       **/
      /** transformation matrix to all the vertices of P2.	       **/

      CPVECTOR3(u, couple->pln_pnt1);	CPVECTOR3(v, couple->pln_pnt2);
      VECADD3(u, u, polyhedron1->trn);	VECADD3(v, v, polyhedron1->trn);
      VECADD3(u, u, polyhedron2->itrn); VECADD3(v, v, polyhedron2->itrn);
      VECSUB3(norm, v, u);

      m = polyhedron2->m;   i = 0; loop = TRUE;
      while ((i < m) && (loop)) {

	 /** Evaluate plane equation **/

	 VECSUB3(v, polyhedron2->verts[i], u);
	 d = DOT3(v, norm);

	 if (d <= 0.0) {		 /** is P2 in opposite half-space **/
	    loop = FALSE;
	    if (!get_new_plane(couple)) {
	       collide = TRUE;		 /** Collision **/
	       couple->plane_exists = FALSE;
	    }
	 }
	 i++;
      }
   }
   else
      if (get_new_plane(couple)) {
	 couple->plane_exists = TRUE;	 /** No Collision **/
      }
      else
	 collide = TRUE;		 /** Collision **/

   return collide;
}


/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to initialize a polyhedron.
 *
 *   On Entry:
 *   polyhedron - pointer to a polyhedron structure.
 *	  verts - verts to load.
 *	      m - number of verts.
 *	     tx - x translation.
 *	     ty - y translation.
 *	     tz - z translation.
 *
 *   On Exit:
 *	polyhedron - an initialized polyhedron.
 *
 *   Function Return : none.
 *
 ****************************************************************************/

void init_polyhedron(polyhedron, verts, m, tx, ty, tz)
Polyhedron    polyhedron;
double	      *verts, tx, ty, tz;
int	      m;
{
   int	      i;
   double     *p;

   polyhedron->trn[0]  =  tx;  polyhedron->trn[1]  =  ty;
   polyhedron->trn[2]  =  tz;

   polyhedron->itrn[0] = -tx;  polyhedron->itrn[1] = -ty;
   polyhedron->itrn[2] = -tz;

   polyhedron->m = m;

   p = verts;
   for (i = 0; i < m; i++) {
      CPVECTOR3(polyhedron->verts[i], p);
      p += 3;
   }
}

/*** RJR 05/26/93 ***********************************************************
 *
 *   Function to move a polyhedron.
 *
 *   On Entry:
 *    polyhedron - pointer to a polyhedron.
 *	      tx - x translation.
 *	      ty - y translation.
 *	      tz - z translation.
 *
 *   On Exit:
 *	polyhedron - an updated polyhedron.
 *
 *   Function Return : none.
 *
 ****************************************************************************/

void move_polyhedron(polyhedron, tx, ty, tz)
Polyhedron   polyhedron;
double	     tx, ty, tz;
{
   polyhedron->trn[0]  += tx;  polyhedron->trn[1]  +=  ty;
   polyhedron->trn[2]  += tz;

   polyhedron->itrn[0] -= tx;  polyhedron->itrn[1] -=  ty;
   polyhedron->itrn[2] -= tz;
}

/*** RJR 05/26/93 ***********************************************************
 *
 *   This is the Main Program for the Collision Detection example. This test
 *   program creates the vertices of three polyhedra: a sphere, a box, and a
 *   cylinder. The three polyhedra oscillate back and forth along the x-axis.
 *   A collision test is done after each movement on each pair of polyhedra.
 *   This test program was run on an SGI Onyx/4 and an SGI 4D/80.  A total of
 *   30,000 collision detection tests were performed.  There were 3,160
 *   collisions detected. The dist3d function was called in 14% of the
 *   collision tests.  The average number of iterations in dist3d was 1.7.
 *   The above functions are designed to compute accurate solutions when
 *   the polyhedra are simple and convex.  The functions will work on
 *   concave polyhedra, but the solutions are computed using the convex hulls
 *   of the concave polyhedra.	In this case when the algorithm returns a
 *   disjoint result it is exact, but when it returns an intersection result
 *   it is approximate.
 *
 ****************************************************************************/
main()
{
   Polyhedron	  Polyhedron1, Polyhedron2, Polyhedron3;
   Couple	  Couple1, Couple2, Couple3;
   double	  xstp1, xstp2, xstp3;
   int		  i, steps;
   long		  hits = 0;

   /*** Initialize the 3 test polyhedra ***/

   mak_box(box);
   mak_cyl(cyl);
   mak_sph(sphere);

   Polyhedron1 = (Polyhedron)malloc(sizeof(struct polyhedron));
   init_polyhedron(Polyhedron1, sphere, 342,  0.0, 0.0, 0.0);

   Polyhedron2 = (Polyhedron)malloc(sizeof(struct polyhedron));
   init_polyhedron(Polyhedron2, box, 8, 50.0, 0.0, 0.0);

   Polyhedron3 = (Polyhedron)malloc(sizeof(struct polyhedron));
   init_polyhedron(Polyhedron3, cyl, 36, -50.0, 0.0, 0.0);

   Couple1 = (Couple)malloc(sizeof(struct couple));
   Couple1->polyhdrn1 = Polyhedron1;   Couple1->polyhdrn2 = Polyhedron2;
   Couple1->n = 0;
   Couple1->plane_exists = FALSE;

   Couple2 = (Couple)malloc(sizeof(struct couple));
   Couple2->polyhdrn1 = Polyhedron1;   Couple2->polyhdrn2 = Polyhedron3;
   Couple2->n = 0;
   Couple2->plane_exists = FALSE;

   Couple3 = (Couple)malloc(sizeof(struct couple));
   Couple3->polyhdrn1 = Polyhedron3;   Couple3->polyhdrn2 = Polyhedron2;
   Couple3->n = 0;
   Couple3->plane_exists = FALSE;

   /** Perform Collision Tests **/

   xstp1 = 1.0;	 xstp2 = 5.0; xstp3 = 10.0;  steps = 10000;

   for (i = 0; i < steps; i++) {
      move_polyhedron(Polyhedron1, xstp1, 0.0, 0.0);
      move_polyhedron(Polyhedron2, xstp2, 0.0, 0.0);
      move_polyhedron(Polyhedron3, xstp3, 0.0, 0.0);

      if (Collision(Couple1))
	 hits++;
      if (Collision(Couple2))
	 hits++;
      if (Collision(Couple3))
	 hits++;

      if (ABS(Polyhedron1->trn[0]) > 100.0)
	 xstp1 = -xstp1;
      if (ABS(Polyhedron2->trn[0]) > 100.0)
	 xstp2 = -xstp2;
      if (ABS(Polyhedron3->trn[0]) > 100.0)
	 xstp3 = -xstp3;
   }
   printf("number of tests = %d\n",(steps * 3));
   printf("number of hits = %ld\n", hits);
}