int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dBoxClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dVector3 normal;
dReal depth;
int code;
dxBox *b1 = (dxBox*) o1;
dxBox *b2 = (dxBox*) o2;
int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
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;
}
int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dVector3 normal;
dReal depth;
int code;
dxBox *b1 = (dxBox*) o1;
dxBox *b2 = (dxBox*) o2;
int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
normal,&depth,&code,flags & NUMC_MASK,contact,skip);
for (int i=0; i<num; i++) {
CONTACT(contact,i*skip)->normal[0] = -normal[0];
CONTACT(contact,i*skip)->normal[1] = -normal[1];
CONTACT(contact,i*skip)->normal[2] = -normal[2];
CONTACT(contact,i*skip)->g1 = o1;
CONTACT(contact,i*skip)->g2 = o2;
}
return num;
}
s32 xColBoxBox(iCollisionObject* o1,iCollisionObject* o2)
{
X_Assert(o1->muClass==iCollisionObject::Col_BBox);
X_Assert(o2->muClass==iCollisionObject::Col_BBox);
vec4_t normal;
cBBox *box1 = (cBBox*)o1;
cBBox *box2 = (cBBox*)o2;
vec4_t side1,side2;
xQ4ScaleS(side1,box1->mkHalfSide,2.0f);
xQ4ScaleS(side2,box2->mkHalfSide,2.0f);
// this is goin to consume a lot of time, calculating the
// collision points, though simd will help a little
cContact* g = gpkContactHolder;
s32 numc = dBoxBox(
box1->mpkWorld->pos,box1->mpkWorld->rot,side1,
box2->mpkWorld->pos,box2->mpkWorld->rot,side2,
g,normal);
// s32 numc = cBoxCollide(
// box1->mpkWorld->pos,box1->mpkWorld->rot,box1->mkHalfSide,
// box2->mpkWorld->pos,box2->mpkWorld->rot,box2->mkHalfSide,
// g,normal);
// I'm not sure if the normal needs negetion
// this step might be unnecessary
if( numc )
{
normal[0] =-normal[0];
normal[1] =-normal[1];
normal[2] =-normal[2];
normal[3] = 0;
s32 num = numc;
while( num )
{
--num;
xQ4CpyMac(gpkContactHolder[num].norm,normal);
}
}
return numc;
}
int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
std::vector<Contact>* result)
{
dVector3 halfSize0;
dVector3 halfSize1;
convVector(0.5 * size0, halfSize0);
convVector(0.5 * size1, halfSize1);
dMatrix3 R0, R1;
convMatrix(T0, R0);
convMatrix(T1, R1);
dVector3 p0;
dVector3 p1;
convVector(T0.translation(), p0);
convVector(T1.translation(), p1);
return dBoxBox(p1, R1, halfSize1, p0, R0, halfSize0, *result);
}
int collideBoxBox(CollisionObject* o1, CollisionObject* o2,
const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
CollisionResult& result)
{
dVector3 halfSize0;
dVector3 halfSize1;
convVector(0.5 * size0, halfSize0);
convVector(0.5 * size1, halfSize1);
dMatrix3 R0, R1;
convMatrix(T0, R0);
convMatrix(T1, R1);
dVector3 p0;
dVector3 p1;
convVector(T0.translation(), p0);
convVector(T1.translation(), p1);
return dBoxBox(o1, o2, p1, R1, halfSize1, p0, R0, halfSize0, result);
}
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);
}
}
int test_dBoxBox()
{
int k,bt;
dVector3 p1,p2,side1,side2,normal,normal2;
dMatrix3 R1,R2;
dReal depth,depth2;
int code;
dContactGeom contact[48];
dSimpleSpace space(0);
dGeomID box1 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box1);
dGeomID box2 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box2);
dMakeRandomVector (p1,3,0.5);
dMakeRandomVector (p2,3,0.5);
for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
// dRSetIdentity (R1); // we can also try this
// dRSetIdentity (R2);
dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
dGeomSetRotation (box1,R1);
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
dGeomSetRotation (box2,R2);
code = 0;
depth = 0;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact,
sizeof(dContactGeom));
if (bt==1) {
p2[0] += normal[0] * 0.96 * depth;
p2[1] += normal[1] * 0.96 * depth;
p2[2] += normal[2] * 0.96 * depth;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
sizeof(dContactGeom));
/*
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
draw_all_objects (space);
*/
if (bt != 1) {
FAILED();
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
draw_all_objects (space);
}
p2[0] += normal[0] * 0.08 * depth;
p2[1] += normal[1] * 0.08 * depth;
p2[2] += normal[2] * 0.08 * depth;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
sizeof(dContactGeom));
if (bt != 0) FAILED();
// dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
// draw_all_objects (space);
}
// printf ("code=%2d depth=%.4f ",code,depth);
PASSED();
}
