int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dCapsuleClass); dIASSERT (o2->type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); dxCapsule *ccyl = (dxCapsule*) o1; dxSphere *sphere = (dxSphere*) o2; contact->g1 = o1; contact->g2 = o2; contact->side1 = -1; contact->side2 = -1; // find the point on the cylinder axis that is closest to the sphere dReal alpha = o1->final_posr->R[2] * (o2->final_posr->pos[0] - o1->final_posr->pos[0]) + o1->final_posr->R[6] * (o2->final_posr->pos[1] - o1->final_posr->pos[1]) + o1->final_posr->R[10] * (o2->final_posr->pos[2] - o1->final_posr->pos[2]); dReal lz2 = ccyl->lz * REAL(0.5); if (alpha > lz2) alpha = lz2; if (alpha < -lz2) alpha = -lz2; // collide the spheres dVector3 p; p[0] = o1->final_posr->pos[0] + alpha * o1->final_posr->R[2]; p[1] = o1->final_posr->pos[1] + alpha * o1->final_posr->R[6]; p[2] = o1->final_posr->pos[2] + alpha * o1->final_posr->R[10]; return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact); }
int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dSphereClass); dIASSERT (o2->type == dSphereClass); dIASSERT ((flags & NUMC_MASK) >= 1); dxSphere *sphere1 = (dxSphere*) o1; dxSphere *sphere2 = (dxSphere*) o2; contact->g1 = o1; contact->g2 = o2; return dCollideSpheres (o1->final_posr->pos,sphere1->radius, o2->final_posr->pos,sphere2->radius,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 dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dCapsuleClass); dIASSERT (o2->type == dCapsuleClass); dIASSERT ((flags & NUMC_MASK) >= 1); int i; const dReal tolerance = REAL(1e-5); dxCapsule *cyl1 = (dxCapsule*) o1; dxCapsule *cyl2 = (dxCapsule*) o2; contact->g1 = o1; contact->g2 = o2; // copy out some variables, for convenience dReal lz1 = cyl1->lz * REAL(0.5); dReal lz2 = cyl2->lz * REAL(0.5); dReal *pos1 = o1->final_posr->pos; dReal *pos2 = o2->final_posr->pos; dReal axis1[3],axis2[3]; axis1[0] = o1->final_posr->R[2]; axis1[1] = o1->final_posr->R[6]; axis1[2] = o1->final_posr->R[10]; axis2[0] = o2->final_posr->R[2]; axis2[1] = o2->final_posr->R[6]; axis2[2] = o2->final_posr->R[10]; // if the cylinder axes are close to parallel, we'll try to detect up to // two contact points along the body of the cylinder. if we can't find any // points then we'll fall back to the closest-points algorithm. note that // we are not treating this special case for reasons of degeneracy, but // because we want two contact points in some situations. the closet-points // algorithm is robust in all casts, but it can return only one contact. dVector3 sphere1,sphere2; dReal a1a2 = dDOT (axis1,axis2); dReal det = REAL(1.0)-a1a2*a1a2; if (det < tolerance) { // the cylinder axes (almost) parallel, so we will generate up to two // contacts. alpha1 and alpha2 (line position parameters) are related by: // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) // first compute where the two cylinders overlap in alpha1 space: if (a1a2 < 0) { axis2[0] = -axis2[0]; axis2[1] = -axis2[1]; axis2[2] = -axis2[2]; } dReal q[3]; for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; dReal k = dDOT (axis1,q); dReal a1lo = -lz1; dReal a1hi = lz1; dReal a2lo = -lz2 - k; dReal a2hi = lz2 - k; dReal lo = (a1lo > a2lo) ? a1lo : a2lo; dReal hi = (a1hi < a2hi) ? a1hi : a2hi; if (lo <= hi) { int num_contacts = flags & NUMC_MASK; if (num_contacts >= 2 && lo < hi) { // generate up to two contacts. if one of those contacts is // not made, fall back on the one-contact strategy. for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i]; for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i]; int n1 = dCollideSpheres (sphere1,cyl1->radius, sphere2,cyl2->radius,contact); if (n1) { for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i]; for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i]; dContactGeom *c2 = CONTACT(contact,skip); int n2 = dCollideSpheres (sphere1,cyl1->radius, sphere2,cyl2->radius, c2); if (n2) { c2->g1 = o1; c2->g2 = o2; return 2; } } } // just one contact to generate, so put it in the middle of // the range dReal alpha1 = (lo + hi) * REAL(0.5); dReal alpha2 = alpha1 + k; for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; return dCollideSpheres (sphere1,cyl1->radius, sphere2,cyl2->radius,contact); } } // use the closest point algorithm dVector3 a1,a2,b1,b2; a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1; a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1; a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1; a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1; a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1; a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1; b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2; b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2; b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2; b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2; b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2; b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2; dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2); return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,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); } }