[ODE] cylinder collisions

[Ivo Kwee]

Hi,

I needed the cylinder primitive, so started to implement collision 
functions for the (flat ended) cylinder. The cylinder/plane kind of 
works, but I got stuck with the cylinder/cylinder collisions function.

I got this far:

- Collisions on the sides are detected if the minimum distance between 
the cylinder axes are less than (cyl1->radius+cyl2->radius). The 
collision point is then defined in the center but the collision point 
must be within the length of both cylinders.
- Collisions on the top/bottom sides are checked by creating an 
auxiliary plane coinciding the side, and performing a cylinder/plane 
collision. If all the contact points are inside the radius of the disk 
everything is OK. If they are all out, no collision is reported. The 
problem is if the other cylinder collides on the rim.

So how should I determine the contact points on the rim? Should I do 
something like the CappedCylinder but moving "disks" instead of spheres? 
Can I maybe reuse Box-routines with appropriate rotated boxes (which are 
the projections of the cylinder)?

There is very little on cylinder collissions I could find on the www. 
Any ideas?

More generally, are Vortex/Havok using analyic collision detection for 
primitives? Or do they triangulate bodies first?

Ivo


==================== CODE SNIPPET ========================


// Given a disk centered at c, radius r, with rotation matrix R and
// a plane with normal vector n, and n.x = z, this computes the point p
// on the disk circumference closest to the plane. Knowing the equation
// of the disk and plane,
//    disk:      p = rx.cos(phi) + ry.sin(phi)
//    plane:   p.n = z
// phi can be solved:
//     phi = asin ( (z-n.c) / d ) - arctan ( n.rx / n.ry )
// where
//       d = (n.rx)^2 + (n.ry)^2

static
void collideDiskPlane ( dVector3 c, dReal r, dMatrix3 R,
             dVector3 n, dReal z,
             dReal *phi)
{
     dReal dx, dy, rr;
     // dVector3 nx, ny;
     //     nx[0] = r*R[0];   nx[1] = r*R[4];   nx[2] = r*R[8];
     //     ny[0] = r*R[1];   ny[1] = r*R[5];   ny[2] = r*R[9];
     dx = r * dDOT14(n,R);
     dy = r * dDOT14(n,R+1);
     rr = sqrt(dx*dx + dy*dy);

     // printf("collideDiskPlane::dx = %f, dy = %f, d = %f\n",dx, dy, rr);

     // The collision candidates are where the sinus is +1/-1.
     double z1 =  rr + dDOT(n,c);
     double z2 = -rr + dDOT(n,c);
     dReal phi1 =  0.5*M_PI - atan2(dx,dy);
     dReal phi2 = -0.5*M_PI - atan2(dx,dy);

     /*
     printf("collideDiskPlane::z=%f \t z1= %f z2=%f \n",z,z1,z2);
     printf("collideDiskPlane::phi1= %f phi2=%f\n",phi1,phi2);
     */

     /*     if ( rr < 0.001*r ) {
       printf("collideDiskPlane::flat on side??\n");
     }
     */

     // Return deepest point
     //     *phi = fabs(z1-z) < fabs(z2-z) ? phi1 : phi2;
     *phi = (z1-z) < (z2-z) ? phi1 : phi2;

     return;
}


int dCollideYP (const dxGeom *o1, const dxGeom *o2, int flags,
        dContactGeom *contact, int skip)
{
  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (o1->_class->num == dCylinderClass);
  dIASSERT (o2->_class->num == dPlaneClass);
  dxCylinder *cyl = (dxCylinder*) CLASSDATA(o1);
  dxPlane  *plane = (dxPlane*) CLASSDATA(o2);

  //@@@ need early return check?
  /*
  if ( dDOT(plane->p,o1->pos)-plane->p[3] > (cyl->lz+cyl->radius) )
    return 0;
  */

  // collide the deepest disk with the plane
  dReal sign = (dDOT14 (plane->p,o1->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
  dVector3 p, cp;
  p[0] = o1->pos[0] + o1->R[2]  * cyl->lz * REAL(0.5) * sign;
  p[1] = o1->pos[1] + o1->R[6]  * cyl->lz * REAL(0.5) * sign;
  p[2] = o1->pos[2] + o1->R[10] * cyl->lz * REAL(0.5) * sign;
  dReal phi;
  collideDiskPlane( p, cyl->radius, o1->R, plane->p, plane->p[3], &phi );
  cp[0] = p[0] + cyl->radius * ( o1->R[0]*cos(phi) + o1->R[1]*sin(phi) );
  cp[1] = p[1] + cyl->radius * ( o1->R[4]*cos(phi) + o1->R[5]*sin(phi) );
  cp[2] = p[2] + cyl->radius * ( o1->R[8]*cos(phi) + o1->R[9]*sin(phi) );

  // check depth
  dReal k = dDOT (cp, plane->p);
  dReal depth = plane->p[3] - k;
  if (depth < 0) return 0;
  //  printf("dCollideYP:: c= %f %f %f \td= %f\n",cp[0],cp[1],cp[2],depth);
     
  contact->normal[0] = plane->p[0];
  contact->normal[1] = plane->p[1];
  contact->normal[2] = plane->p[2];
  contact->pos[0] = cp[0];
  contact->pos[1] = cp[1];
  contact->pos[2] = cp[2];
  contact->depth = depth;
  int ncontacts = 1;

  // check opposite disk in case cylinder is almost parallel to
  // the plane. And add a second contact point on opposite side.
  // @@@ any faster check?
  if ((flags & NUMC_MASK) >= 2) {
    // collide the other disk with the plane
    cp[0] -= o1->R[2]  * cyl->lz * sign;
    cp[1] -= o1->R[6]  * cyl->lz * sign;
    cp[2] -= o1->R[10] * cyl->lz * sign;
    k = dDOT (cp, plane->p);
    depth = plane->p[3] - k;
    if (depth >= 0) {
      //      printf("dCollideYP:: cylinder parallel! depth = %f\n",depth);
      //      printf("dCollideYP:: cp2 = %f %f %f\n",cp[0],cp[1],cp[2]);
      dContactGeom *c2 = CONTACT(contact,skip);
      c2->normal[0] = plane->p[0];
      c2->normal[1] = plane->p[1];
      c2->normal[2] = plane->p[2];
      c2->pos[0] = cp[0];
      c2->pos[1] = cp[1];
      c2->pos[2] = cp[2];
      c2->depth = depth;
      ncontacts = 2;
    }
  }

  // Create extra contacts if the cylinder stands up
  int maxc = flags & NUMC_MASK;
  if (maxc < 1) maxc = 1;
  if (maxc > 3) maxc = 3;    // no more than 3 contacts per box allowed
  int nc = maxc - ncontacts;
  if ( ncontacts < maxc && (flags & NUMC_MASK) >= 2 ) {
    for ( int i = 0; i < nc; i++) {
      phi += 2 * M_PI / maxc;
      cp[0] = p[0] + cyl->radius*( o1->R[0]*cos(phi) + o1->R[1]*sin(phi) );
      cp[1] = p[1] + cyl->radius*( o1->R[4]*cos(phi) + o1->R[5]*sin(phi) );
      cp[2] = p[2] + cyl->radius*( o1->R[8]*cos(phi) + o1->R[9]*sin(phi) );
     
      // check depth
      dReal k = dDOT (cp, plane->p);
      dReal depth = plane->p[3] - k;
      if (depth >= 0) {
    //    printf("dCollideYP:: cylinder on side! depth = %f\n",depth);
    //    printf("dCollideYP:: cp2 = %f %f %f\n",cp[0],cp[1],cp[2]);
    dContactGeom *c2 = CONTACT(contact, ncontacts*skip);
    c2->normal[0] = plane->p[0];
    c2->normal[1] = plane->p[1];
    c2->normal[2] = plane->p[2];
    c2->pos[0] = cp[0];
    c2->pos[1] = cp[1];
    c2->pos[2] = cp[2];
    c2->depth = depth;
    ++ncontacts;
      }
    }
  }

  //  printf("dCollideYP:: ncontacts = %d\n",ncontacts);
  for (int i=0; i < ncontacts; i++) {
    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
  }

  return ncontacts;
}

// Collide two cylinders

int dCollideYY (const dxGeom *o1, const dxGeom *o2,
        int flags, dContactGeom *contact, int skip)
{
     int i;
     const dReal tolerance = REAL(1e-5);

     dIASSERT (skip >= (int)sizeof(dContactGeom));
     dIASSERT (o1->_class->num == dCylinderClass);
     dIASSERT (o2->_class->num == dCylinderClass);
     contact->g1 = const_cast<dxGeom*> (o1);
     contact->g2 = const_cast<dxGeom*> (o2);
     dxCylinder *cyl1 = (dxCylinder*) CLASSDATA(o1);
     dxCylinder *cyl2 = (dxCylinder*) CLASSDATA(o2);

     // copy out some variables, for convenience
     dReal lz1 = cyl1->lz * REAL(0.5);
     dReal lz2 = cyl2->lz * REAL(0.5);
     dReal *pos1 = o1->pos;
     dReal *pos2 = o2->pos;
     dReal axis1[3],axis2[3];
     axis1[0] = o1->R[2];
     axis1[1] = o1->R[6];
     axis1[2] = o1->R[10];
     axis2[0] = o2->R[2];
     axis2[1] = o2->R[6];
     axis2[2] = o2->R[10];

     // first do a quick check
     dReal alpha1,alpha2;
     lineClosestApproach( o1->pos, axis1, o2->pos, axis2, &alpha1, 
&alpha2 );
     dReal dx = pos1[0] + alpha1*axis1[0];
     dReal dy = pos1[1] + alpha1*axis1[1];
     dReal dz = pos1[2] + alpha1*axis1[2];
     dx -= (pos2[0] + alpha2*axis2[0]);
     dy -= (pos2[1] + alpha2*axis2[1]);
     dz -= (pos2[2] + alpha2*axis2[2]);
     dReal r = sqrt(dx*dx + dy*dy + dz*dz);
     if ( r > ( cyl1->radius + cyl2->radius ) )
       return 0;

     // First case is easy: if the closest points are within the
     // length of the cylinders, there is only one collision point
     // just in the middle.
     // @@@ IVO: what if they are parallel??
     if ( alpha1 >= -lz1 && alpha1 <= lz1 &&
      alpha2 >= -lz2 && alpha2 <= lz2 ) {
       contact->pos[0] = 0.5*(pos1[0]+alpha1*axis1[0] + 
pos2[0]+alpha2*axis2[0]);
       contact->pos[1] = 0.5*(pos1[1]+alpha1*axis1[1] + 
pos2[1]+alpha2*axis2[1]);
       contact->pos[2] = 0.5*(pos1[2]+alpha1*axis1[2] + 
pos2[2]+alpha2*axis2[2]);
       contact->normal[0] = dx / r;
       contact->normal[1] = dy / r;
       contact->normal[2] = dz / r;
       contact->depth = cyl1->radius + cyl2->radius - r;
       return 1;
     }
    
     // Else, the check is more elaborate. We have to check top and bottom
     // disk of each cylinder against other cylinder. We do as follows:
     // create a auxiliary plane at top/bottom disk, collide other cylinder
     // with this plane, check if collision point(s) are inside the radius,
     // clip CP within circle. Repeat with other cylinder.
     dxGeom  *g  = dCreatePlane(0,0,0,0,0);
     dxPlane *pl = (dxPlane*) CLASSDATA(g);
     dVector3 ca;
     int ntouch = 0;

     pl->p[0] = axis2[0];
     pl->p[1] = axis2[1];
     pl->p[2] = axis2[2];
     ca[0] = pos2[0] + lz2 * axis2[0];
     ca[1] = pos2[1] + lz2 * axis2[1];
     ca[2] = pos2[2] + lz2 * axis2[2];
     pl->p[3] = dDOT( pl->p, ca);
     ntouch = dCollideYP( o1, g, flags, contact, skip);
     if ( ntouch ) {
       int inside = 0;
       for (int i = 0; i < ntouch; ++i) {
     if ( dDISTANCE(contact->pos[i], ca) < cyl2->radius )
       ++inside;
       }
       // if all contactpoints are inside: easy
       if (inside==ntouch) {
     printf("dCollideYY:: cylinder A collision on top B\n");
     printf("dCollideYY:: all contacts inside\n");
     return ntouch;
       }
       else {
     printf("dCollideYY:: ntouch = %d   inside = %d\n",ntouch,inside);
     // clip_contacts( ntouch, contact, ca, cyl2->radius );
       }
     }

     // Need to check other side, and other cylinder too...
     /*
     dCollideYP( o1, g2b, flags, contact, skip);
     dCollideYP( o2, g1b, flags, contact, skip);
     dCollideYP( o2, g1t, flags, contact, skip);
     */
    

    
     return 0;
}