[ODE] (flat) cylinder-plane collide generates more contacts than specified

[Patrik Stellmann]

This is a multi-part message in MIME format.
--------------080506090508010404050707
Content-Type: text/plain; charset=us-ascii; format=flowed
Content-Transfer-Encoding: 7bit

I just noticed that the cylinder-plane collider doesn't care about the 
maximal number or contacts it may create. No surprise that this results 
in writing to memory not meant to be used for contacts...
However, I added some checks for that without knowing if the result is 
really as it shold be, but for my cases, either maximal 1 contact (to 
check if there is collision) or up to 20 contacts (for actually creating 
contact geoms) it works...


--------------080506090508010404050707
Content-Type: text/plain;
 name="dCollideCylPlane.cpp"
Content-Transfer-Encoding: 7bit
Content-Disposition: inline;
 filename="dCollideCylPlane.cpp"

int dCollideCylPlane 
  (
  dxGeom *o1, dxGeom *o2, int flags,
        dContactGeom *contact, int skip){
  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
  dIASSERT (dGeomGetClass(o2) == dPlaneClass);
  contact->g1 = const_cast<dxGeom*> (o1);
  contact->g2 = const_cast<dxGeom*> (o2);
 const unsigned int maxret = (flags & NUMC_MASK);  //  <<<< MODIFIED
 unsigned int ret = 0;

 dReal radius;
 dReal hlz;
 dGeomCylinderGetParams(o1,&radius,&hlz);
 hlz /= 2;
 
 const dReal *R  =  dGeomGetRotation(o1);// rotation of cylinder
 const dReal* p  =  dGeomGetPosition(o1);
 dVector4 n;    // normal vector
 dReal pp;
 dGeomPlaneGetParams (o2, n);
 pp=n[3];
 dReal cos1,sin1;
  cos1=dFabs(dDOT14(n,R+1));

cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
sin1=sqrtf(REAL(1.)-cos1*cos1);
//////////////////////////////

dReal sidePr=cos1*hlz+sin1*radius;

dReal dist=-pp+dDOT(n,p);
dReal outDepth=sidePr-dist;

if(outDepth<0.f) return 0;

dVector3 pos;


/////////////////////////////////////////// from geom.cpp dCollideBP
  dReal Q1 = dDOT14(n,R+0);
  dReal Q2 = dDOT14(n,R+1);
  dReal Q3 = dDOT14(n,R+2);
  dReal factor =sqrtf(Q1*Q1+Q3*Q3);
  factor= factor ? factor :1.f;
  dReal A1 = radius *    Q1/factor;
  dReal A2 = hlz*Q2;
  dReal A3 = radius *    Q3/factor;

  pos[0]=p[0];
  pos[1]=p[1];
  pos[2]=p[2];

  pos[0]-= A1*R[0];
  pos[1]-= A1*R[4];
  pos[2]-= A1*R[8];

  pos[0]-= A3*R[2];
  pos[1]-= A3*R[6];
  pos[2]-= A3*R[10];

  pos[0]-= A2>0 ? hlz*R[1]:-hlz*R[1];
  pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5];
  pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9];

  contact->pos[0] = pos[0];
  contact->pos[1] = pos[1];
  contact->pos[2] = pos[2];
   contact->depth = outDepth;
  
  if (ret < maxret - 1)  //  <<<< MODIFIED
  {
    ret=1;
   
    if(dFabs(Q2)>M_SQRT1_2)
    {
      CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0];
      CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4];
      CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8];
      CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1);

      if ((ret < maxret - 1) &&  //  <<<< MODIFIED
        (CONTACT(contact,ret*skip)->depth>0.f))
        ret++;
        
        
      CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2];
      CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6];
      CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10];
      CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3);
      if ((ret < maxret - 1) &&  //  <<<< MODIFIED
        (CONTACT(contact,ret*skip)->depth>0.f)) 
        ret++;
    } 
    else 
    {
      CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]);
      CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]);
      CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]);
      CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2);

      if ((ret < maxret - 1) &&  //  <<<< MODIFIED
        (CONTACT(contact,ret*skip)->depth>0.f))
        ret++;
    }
  }


 for (unsigned int i=0; i<ret; i++) {
    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
  CONTACT(contact,i*skip)->normal[0] =n[0];
  CONTACT(contact,i*skip)->normal[1] =n[1];
  CONTACT(contact,i*skip)->normal[2] =n[2];
  }
  return ret;  
}
--------------080506090508010404050707--