int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int /*flags*/,
dContactGeom *contact, int /*skip*/)
{
dxCapsule *cyl = (dxCapsule*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = o1;
contact->g2 = o2;
// get p1,p2 = cylinder axis endpoints, get radius
dVector3 p1,p2;
dReal clen = dMUL(cyl->lz,REAL(0.5));
p1[0] = o1->final_posr->pos[0] + dMUL(clen,o1->final_posr->R[2]);
p1[1] = o1->final_posr->pos[1] + dMUL(clen,o1->final_posr->R[6]);
p1[2] = o1->final_posr->pos[2] + dMUL(clen,o1->final_posr->R[10]);
p2[0] = o1->final_posr->pos[0] - dMUL(clen,o1->final_posr->R[2]);
p2[1] = o1->final_posr->pos[1] - dMUL(clen,o1->final_posr->R[6]);
p2[2] = o1->final_posr->pos[2] - dMUL(clen,o1->final_posr->R[10]);
dReal radius = cyl->radius;
// copy out box center, rotation matrix, and side array
dReal *c = o2->final_posr->pos;
dReal *R = o2->final_posr->R;
const dReal *side = box->side;
// get the closest point between the cylinder axis and the box
dVector3 pl,pb;
dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
// generate contact point
return dCollideSpheres (pl,radius,pb,0,contact);
}
int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int /*flags*/,
dContactGeom *contact, int /*skip*/)
{
//dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dBoxClass);
//dIASSERT ((flags & NUMC_MASK) >= 1);
dxCapsule *cyl = (dxCapsule*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
// get p1,p2 = cylinder axis endpoints, get radius
dVector3 p1,p2;
dReal clen = cyl->lz * REAL(0.5);
p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2];
p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6];
p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10];
p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2];
p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6];
p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10];
dReal radius = cyl->radius;
// copy out box center, rotation matrix, and side array
dReal *c = o2->final_posr->pos;
dReal *R = o2->final_posr->R;
const dReal *side = box->side;
// get the closest point between the cylinder axis and the box
dVector3 pl,pb;
dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
// generate contact point
return dCollideSpheres (pl,radius,pb,0,contact);
}
int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxCapsule *cyl = (dxCapsule*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
// get p1,p2 = cylinder axis endpoints, get radius
dVector3 p1,p2;
dReal clen = cyl->lz * REAL(0.5);
p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2];
p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6];
p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10];
p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2];
p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6];
p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10];
dReal radius = cyl->radius;
// copy out box center, rotation matrix, and side array
dReal *c = o2->final_posr->pos;
dReal *R = o2->final_posr->R;
const dReal *side = box->side;
// get the closest point between the cylinder axis and the box
dVector3 pl,pb;
dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
// if the capsule is penetrated further than radius
// then pl and pb are equal (up to eps) -> unknown normal
// we simply consider the capsule as box and use the box-box algorithm
#ifdef dSINGLE
dReal mindist = REAL(1e-6);
#else
dReal mindist = REAL(1e-15);
#endif
// if (dCalcPointsDistance3(pl, pb) < mindist) {
if (dDISTANCE(pl, pb) < mindist) {
dVector3 normal;
dReal depth;
int code;
// consider capsule as box
dReal rad2 = radius*REAL(2.0);
const dVector3 capboxside = {rad2, rad2, cyl->lz + rad2};
int num = dBoxBox (c, R, side,
o1->final_posr->pos, o1->final_posr->R, capboxside,
normal, &depth, &code, flags, contact, skip);
for (int i=0; i<num; i++) {
dContactGeom *currContact = CONTACT(contact,i*skip);
currContact->normal[0] = normal[0];
currContact->normal[1] = normal[1];
currContact->normal[2] = normal[2];
currContact->g1 = o1;
currContact->g2 = o2;
currContact->side1 = -1;
currContact->side2 = -1;
}
return num;
}else{
// generate contact point
return dCollideSpheres (pl,radius,pb,0,contact);
}
}
