EXPORT_C int dBoxBox (const dVector3 p1, const dMatrix3 R1,
const dVector3 side1, const dVector3 p2,
const dMatrix3 R2, const dVector3 side2,
dVector3 normal, dReal *depth, int *return_code,
int maxc, dContactGeom *contact, int skip)
{
const dReal fudge_factor = REAL(1.05);
dVector3 p,pp,normalC;
const dReal *normalR = 0;
dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = dMUL(side1[0],REAL(0.5));
A[1] = dMUL(side1[1],REAL(0.5));
A[2] = dMUL(side1[2],REAL(0.5));
B[0] = dMUL(side2[0],REAL(0.5));
B[1] = dMUL(side2[1],REAL(0.5));
B[2] = dMUL(side2[2],REAL(0.5));
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr1,expr2,norm,cc) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + dMUL(B[0],Q11) + dMUL(B[1],Q12) + dMUL(B[2],Q13)),R1+0,1);
TST (pp[1],(A[1] + dMUL(B[0],Q21) + dMUL(B[1],Q22) + dMUL(B[2],Q23)),R1+1,2);
TST (pp[2],(A[2] + dMUL(B[0],Q31) + dMUL(B[1],Q32) + dMUL(B[2],Q33)),R1+2,3);
// separating axis = v1,v2,v3
TST (dDOT41(R2+0,p),(dMUL(A[0],Q11) + dMUL(A[1],Q21) + dMUL(A[2],Q31) + B[0]),R2+0,4);
TST (dDOT41(R2+1,p),(dMUL(A[0],Q12) + dMUL(A[1],Q22) + dMUL(A[2],Q32) + B[1]),R2+1,5);
TST (dDOT41(R2+2,p),(dMUL(A[0],Q13) + dMUL(A[1],Q23) + dMUL(A[2],Q33) + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
l = dSqrt (dMUL((n1),(n1)) + dMUL((n2),(n2)) + dMUL((n3),(n3))); \
if (l > 0) { \
s2 = dDIV(s2,l); \
if (dMUL(s2,fudge_factor) > s) { \
s = s2; \
normalR = 0; \
normalC[0] = dDIV((n1),l); normalC[1] = dDIV((n2),l); normalC[2] = dDIV((n3),l); \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST((dMUL(pp[2],R21)-dMUL(pp[1],R31)),(dMUL(A[1],Q31)+dMUL(A[2],Q21)+dMUL(B[1],Q13)+dMUL(B[2],Q12)),0,-R31,R21,7);
TST((dMUL(pp[2],R22)-dMUL(pp[1],R32)),(dMUL(A[1],Q32)+dMUL(A[2],Q22)+dMUL(B[0],Q13)+dMUL(B[2],Q11)),0,-R32,R22,8);
TST((dMUL(pp[2],R23)-dMUL(pp[1],R33)),(dMUL(A[1],Q33)+dMUL(A[2],Q23)+dMUL(B[0],Q12)+dMUL(B[1],Q11)),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST((dMUL(pp[0],R31)-dMUL(pp[2],R11)),(dMUL(A[0],Q31)+dMUL(A[2],Q11)+dMUL(B[1],Q23)+dMUL(B[2],Q22)),R31,0,-R11,10);
TST((dMUL(pp[0],R32)-dMUL(pp[2],R12)),(dMUL(A[0],Q32)+dMUL(A[2],Q12)+dMUL(B[0],Q23)+dMUL(B[2],Q21)),R32,0,-R12,11);
TST((dMUL(pp[0],R33)-dMUL(pp[2],R13)),(dMUL(A[0],Q33)+dMUL(A[2],Q13)+dMUL(B[0],Q22)+dMUL(B[1],Q21)),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST((dMUL(pp[1],R11)-dMUL(pp[0],R21)),(dMUL(A[0],Q21)+dMUL(A[1],Q11)+dMUL(B[1],Q33)+dMUL(B[2],Q32)),-R21,R11,0,13);
TST((dMUL(pp[1],R12)-dMUL(pp[0],R22)),(dMUL(A[0],Q22)+dMUL(A[1],Q12)+dMUL(B[0],Q33)+dMUL(B[2],Q31)),-R22,R12,0,14);
TST((dMUL(pp[1],R13)-dMUL(pp[0],R23)),(dMUL(A[0],Q23)+dMUL(A[1],Q13)+dMUL(B[0],Q32)+dMUL(B[1],Q31)),-R23,R13,0,15);
#undef TST
if (!code) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR) {
normal[0] = normalR[0];
normal[1] = normalR[4];
normal[2] = normalR[8];
}
else {
dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
*depth = -s;
// compute contact point(s)
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
dVector3 pa;
dReal sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=0; i<3; i++) pa[i] += dMUL(sign,dMUL(A[j],R1[i*4+j]));
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=0; i<3; i++) pb[i] += dMUL(sign,dMUL(B[j],R2[i*4+j]));
}
dReal alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += dMUL(ua[i],alpha);
for (i=0; i<3; i++) pb[i] += dMUL(ub[i],beta);
for (i=0; i<3; i++) contact[0].pos[i] = dMUL(REAL(0.5),(pa[i]+pb[i]));
contact[0].depth = *depth;
*return_code = code;
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
dVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = dFabs (nr[0]);
anr[1] = dFabs (nr[1]);
anr[2] = dFabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
dVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + dMUL(Sb[lanr],Rb[i*4+lanr]);
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - dMUL(Sb[lanr],Rb[i*4+lanr]);
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
dReal quad[8]; // 2D coordinate of incident face (x,y pairs)
dReal c1,c2,m11,m12,m21,m22;
c1 = dDOT14 (center,Ra+code1);
c2 = dDOT14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dDOT44 (Ra+code1,Rb+a1);
m12 = dDOT44 (Ra+code1,Rb+a2);
m21 = dDOT44 (Ra+code2,Rb+a1);
m22 = dDOT44 (Ra+code2,Rb+a2);
{
dReal k1 = dMUL(m11,Sb[a1]);
dReal k2 = dMUL(m21,Sb[a1]);
dReal k3 = dMUL(m12,Sb[a2]);
dReal k4 = dMUL(m22,Sb[a2]);
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
dReal rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
dReal ret[16];
int n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
dReal point[3*8]; // penetrating contact points
dReal dep[8]; // depths for those points
dReal det1 = dRecip(dMUL(m11,m22) - dMUL(m12,m21));
m11 = dMUL(m11,det1);
m12 = dMUL(m12,det1);
m21 = dMUL(m21,det1);
m22 = dMUL(m22,det1);
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
dReal k1 = dMUL(m22,(ret[j*2]-c1)) - dMUL(m12,(ret[j*2+1]-c2));
dReal k2 = -dMUL(m21,(ret[j*2]-c1)) + dMUL(m11,(ret[j*2+1]-c2));
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + dMUL(k1,Rb[i*4+a1]) + dMUL(k2,Rb[i*4+a2]);
dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
con->depth = dep[j];
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
dReal maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints (cnum,ret,maxc,i1,iret);
for (j=0; j < maxc; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
con->depth = dep[iret[j]];
}
cnum = maxc;
}
*return_code = code;
return cnum;
}
int dBoxBox (const dVector3 p1, const dMatrix3 R1,
const dVector3 side1, const dVector3 p2,
const dMatrix3 R2, const dVector3 side2,
dVector3 normal, dReal *depth, int *return_code,
int flags, dContactGeom *contact, int skip)
{
const dReal fudge_factor = REAL(1.05);
dVector3 p,pp,normalC={0,0,0};
const dReal *normalR = 0;
dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMultiply1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0]*REAL(0.5);
A[1] = side1[1]*REAL(0.5);
A[2] = side1[2]*REAL(0.5);
B[0] = side2[0]*REAL(0.5);
B[1] = side2[1]*REAL(0.5);
B[2] = side2[2]*REAL(0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2);
Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
do {
#define TST(expr1,expr2,norm,cc) \
expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
s2 = dFabs(expr1_val) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1_val) < 0); \
code = (cc); \
if (flags & CONTACTS_UNIMPORTANT) break; \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (dCalcVectorDot3_41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
s2 = dFabs(expr1_val) - (expr2); \
if (s2 > 0) return 0; \
l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1_val) < 0); \
code = (cc); \
if (flags & CONTACTS_UNIMPORTANT) break; \
} \
}
// We only need to check 3 edges per box
// since parallel edges are equivalent.
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
} while (0);
if (!code) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR) {
normal[0] = normalR[0];
normal[1] = normalR[4];
normal[2] = normalR[8];
}
else {
dMultiply0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
*depth = -s;
// compute contact point(s)
if (code > 6) {
// An edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
dVector3 pa;
dReal sign;
// Copy p1 into pa
for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy?
// Get world position of p2 into pa
for (j=0; j<3; j++) {
sign = (dCalcVectorDot3_14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
// Copy p2 into pb
for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy?
// Get world position of p2 into pb
for (j=0; j<3; j++) {
sign = (dCalcVectorDot3_14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
dReal alpha,beta;
dVector3 ua,ub;
// Get direction of first edge
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
// Get direction of second edge
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
// Get closest points between edges (one at each)
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
// Set the contact point as halfway between the 2 closest points
for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
contact[0].depth = *depth;
*return_code = code;
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
// Note: Unmodified parameter values are being used here
const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) { // One of the faces of box 1 is the reference face
Ra = R1; // Rotation of 'a'
Rb = R2; // Rotation of 'b'
pa = p1; // Center (location) of 'a'
pb = p2; // Center (location) of 'b'
Sa = A; // Side Lenght of 'a'
Sb = B; // Side Lenght of 'b'
}
else { // One of the faces of box 2 is the reference face
Ra = R2; // Rotation of 'a'
Rb = R1; // Rotation of 'b'
pa = p2; // Center (location) of 'a'
pb = p1; // Center (location) of 'b'
Sa = B; // Side Lenght of 'a'
Sb = A; // Side Lenght of 'b'
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
/*
The normal is flipped if necessary so it always points outward from box 'a',
box 'b' is thus always the incident box
*/
dVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
// Rotate normal2 in incident box opposite direction
dMultiply1_331 (nr,Rb,normal2);
anr[0] = dFabs (nr[0]);
anr[1] = dFabs (nr[1]);
anr[2] = dFabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the incident face. the other axis numbers of the incident face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
dVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
dReal quad[8]; // 2D coordinate of incident face (x,y pairs)
dReal c1,c2,m11,m12,m21,m22;
c1 = dCalcVectorDot3_14 (center,Ra+code1);
c2 = dCalcVectorDot3_14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dCalcVectorDot3_44 (Ra+code1,Rb+a1);
m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2);
m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1);
m22 = dCalcVectorDot3_44 (Ra+code2,Rb+a2);
{
dReal k1 = m11*Sb[a1];
dReal k2 = m21*Sb[a1];
dReal k3 = m12*Sb[a2];
dReal k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
dReal rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
dReal ret[16];
int n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
dReal point[3*8]; // penetrating contact points
dReal dep[8]; // depths for those points
dReal det1 = dRecip(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
dep[cnum] = Sa[codeN] - dCalcVectorDot3(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
break;
}
}
}
if (cnum < 1) {
return 0; // this should not happen, yet does at times (demo_plane2d single precision).
}
// we can't generate more contacts than we actually have
int maxc = flags & NUMC_MASK;
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1; // Even though max count must not be zero this check is kept for backward compatibility as this is a public function
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
con->depth = dep[j];
}
}
else {
dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
dReal maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints (cnum,ret,maxc,i1,iret);
for (j=0; j < maxc; j++) {
dContactGeom *con = CONTACT(contact,skip*j);
for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
con->depth = dep[iret[j]];
}
cnum = maxc;
}
*return_code = code;
return cnum;
}
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
// generate contact points. this returns 0 if there is no contact otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `return_code' returns a number indicating the type of contact that was
// detected:
// 1,2,3 = box 2 intersects with a face of box 1
// 4,5,6 = box 1 intersects with a face of box 2
// 7..15 = edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to the
// collision functions. this function only fills in the position and depth
// fields.
int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1,
const dVector3 p2, const dMatrix3 R2, const dVector3 side2,
std::vector<Contact>& result)
{
const double fudge_factor = 1.05;
dVector3 p,pp,normalC = {0.0, 0.0, 0.0, 0.0};
const double *normalR = 0;
double A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0];
A[1] = side1[1];
A[2] = side1[2];
B[0] = side2[0];
B[1] = side2[1];
B[2] = side2[2];
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = Inner44(R1+0,R2+0); R12 = Inner44(R1+0,R2+1); R13 = Inner44(R1+0,R2+2);
R21 = Inner44(R1+1,R2+0); R22 = Inner44(R1+1,R2+1); R23 = Inner44(R1+1,R2+2);
R31 = Inner44(R1+2,R2+0); R32 = Inner44(R1+2,R2+1); R33 = Inner44(R1+2,R2+2);
Q11 = std::abs(R11); Q12 = std::abs(R12); Q13 = std::abs(R13);
Q21 = std::abs(R21); Q22 = std::abs(R22); Q23 = std::abs(R23);
Q31 = std::abs(R31); Q32 = std::abs(R32); Q33 = std::abs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr1,expr2,norm,cc) \
s2 = std::abs(expr1) - (expr2); \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -1E12;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (Inner41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (Inner41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (Inner41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = std::abs(expr1) - (expr2); \
l = sqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
if (!code) return 0;
if (s > 0.0) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
Eigen::Vector3d normal;
Eigen::Vector3d point_vec;
double penetration;
if (normalR) {
normal << normalR[0],normalR[4],normalR[8];
}
else {
normal << Inner((R1),(normalC)), Inner((R1+4),(normalC)), Inner((R1+8),(normalC));
//dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal *= -1.0;
}
// compute contact point(s)
// single point
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
dVector3 pa;
double sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++)
{
#define TEMP_INNER14(a,b) (a[0]*(b)[0] + a[1]*(b)[4] + a[2]*(b)[8])
sign = (TEMP_INNER14(normal,R1+j) > 0) ? 1.0 : -1.0;
//sign = (Inner14(normal,R1+j) > 0) ? 1.0 : -1.0;
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (TEMP_INNER14(normal,R2+j) > 0) ? -1.0 : 1.0;
#undef TEMP_INNER14
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
double alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
{
point_vec << 0.5*(pa[0]+pb[0]), 0.5*(pa[1]+pb[1]), 0.5*(pa[2]+pb[2]);
penetration = -s;
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.push_back(contact);
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
const double *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
dVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = fabs (nr[0]);
anr[1] = fabs (nr[1]);
anr[2] = fabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
dVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
double quad[8]; // 2D coordinate of incident face (x,y pairs)
double c1,c2,m11,m12,m21,m22;
c1 = Inner14 (center,Ra+code1);
c2 = Inner14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = Inner44 (Ra+code1,Rb+a1);
m12 = Inner44 (Ra+code1,Rb+a2);
m21 = Inner44 (Ra+code2,Rb+a1);
m22 = Inner44 (Ra+code2,Rb+a2);
{
double k1 = m11*Sb[a1];
double k2 = m21*Sb[a1];
double k3 = m12*Sb[a2];
double k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
double rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
double ret[16];
int n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
//real point[3*8]; // penetrating contact points
double point[24]; // penetrating contact points
double dep[8]; // depths for those points
double det1 = dRecip(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
double k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
double k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++)
{
point[cnum*3+i] = center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
}
dep[cnum] = Sa[codeN] - Inner(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
int maxc = 4;
if (maxc > cnum) maxc = cnum;
//if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++)
{
point_vec << point[j*3+0] + pa[0], point[j*3+1] + pa[1], point[j*3+2] + pa[2];
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[j];
result.push_back(contact);
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
double maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints (cnum,ret,maxc,i1,iret);
cnum = maxc;
for (j=0; j < cnum; j++)
{
point_vec << point[iret[j]*3+0] + pa[0], point[iret[j]*3+1] + pa[1], point[iret[j]*3+2] + pa[2];
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[iret[j]];
result.push_back(contact);
}
}
return cnum;
}
s32 dBoxBox (const f32* p1, const f32* R1, const f32* side1,
const f32* p2, const f32* R2, const f32* side2,
cContact* contact,f32* normal)
{
s32 maxc = 4;
const f32 fudge_factor = (f32)(1.05);
vec4_t p,pp,normalC;
const f32 *normalR = 0;
f32 depth;
f32 A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
s32 i,j,invert_normal,code;
const f32 *Ra,*Rb,*pa,*pb,*Sa,*Sb;
vec4_t normal2,nr,anr;vec4_t center;
s32 lanr,a1,a2;
s32 codeN,code1,code2;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
p[3] = 0;
xMul0_344 (pp,p,R1); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0]*(f32)(0.5);
A[1] = side1[1]*(f32)(0.5);
A[2] = side1[2]*(f32)(0.5);
B[0] = side2[0]*(f32)(0.5);
B[1] = side2[1]*(f32)(0.5);
B[2] = side2[2]*(f32)(0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT(R1+0,R2+0); R12 = dDOT(R1+0,R2+4); R13 = dDOT(R1+0,R2+8);
R21 = dDOT(R1+4,R2+0); R22 = dDOT(R1+4,R2+4); R23 = dDOT(R1+4,R2+8);
R31 = dDOT(R1+8,R2+0); R32 = dDOT(R1+8,R2+4); R33 = dDOT(R1+8,R2+8);
Q11 = fabsr(R11); Q12 = fabsr(R12); Q13 = fabsr(R13);
Q21 = fabsr(R21); Q22 = fabsr(R22); Q23 = fabsr(R23);
Q31 = fabsr(R31); Q32 = fabsr(R32); Q33 = fabsr(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr_1,expr2,norm,cc) \
expr1 = expr_1;\
s2 = fabsr(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
f32 expr1;
s = -N3DInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+4,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+8,3);
// separating axis = v1,v2,v3
TST (dDOT(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (dDOT(R2+4,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+4,5);
TST (dDOT(R2+8,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+8,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = fabsr(expr1) - (expr2); \
if (s2 > 0) return 0; \
l = fsqrtr((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
if (!code) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR) {
normal[0] = normalR[0];
normal[1] = normalR[1];
normal[2] = normalR[2];
}
else {
xMul1_344 (normal,normalC,R1);
}
if (invert_normal) {
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
depth = -s;
// compute contact point(s)
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
vec4_t pa,pb;
f32 sign;f32 alpha,beta;
vec4_t ua,ub;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++) {
sign = (dDOT(normal,(R1+j*4)) > 0) ? (f32)(1.0) : (f32)(-1.0);
xQ4ScaleNAdd(pa,&R1[j*4],sign * A[j]);
}
// find a point pb on the intersecting edge of box 2
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (dDOT(normal,(R2+j*4)) > 0) ? (f32)(-1.0) : (f32)(1.0);
xQ4ScaleNAdd(pb,&R2[j*4],sign * B[j]);
}
for (i=0; i<3; i++) ua[i] = R1[(((code)-7)/3)*4 + i];
for (i=0; i<3; i++) ub[i] = R2[(((code)-7)%3)*4 + i];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
for (i=0; i<3; i++) contact[0].pos[i] = (f32)(0.5)*(pa[i]+pb[i]);
contact[0].depth = depth;
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
xMul0_344 (nr,normal2,Rb);
anr[0] = fabsr (nr[0]);
anr[1] = fabsr (nr[1]);
anr[2] = fabsr (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i+lanr*4];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i+lanr*4];
}
// find the normal and non-normal axis numbers of the reference box
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
f32 quad[8]; // 2D coordinate of incident face (x,y pairs)
f32 c1,c2,m11,m12,m21,m22;
c1 = dDOT (center,(Ra+code1*4));
c2 = dDOT (center,(Ra+code2*4));
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dDOT (Ra+code1*4,Rb+a1*4);
m12 = dDOT (Ra+code1*4,Rb+a2*4);
m21 = dDOT (Ra+code2*4,Rb+a1*4);
m22 = dDOT (Ra+code2*4,Rb+a2*4);
{
f32 k1 = m11*Sb[a1];
f32 k2 = m21*Sb[a1];
f32 k3 = m12*Sb[a2];
f32 k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
f32 rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
f32 ret[16];
s32 n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
f32 point[3*8]; // penetrating contact points
f32 dep[8]; // depths for those points
f32 det1 = 1.f/(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
s32 cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
f32 k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
f32 k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + k1*Rb[i+a1*4] + k2*Rb[i+a2*4];
dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++) {
cContact *con = &contact[j];
for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
con->depth = dep[j];
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
s32 i1 = 0;
f32 maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
//s32 iret[8];
//cullPoints (cnum,ret,maxc,i1,iret);
for (j=0; j < maxc; j++) {
cContact *con = &contact[j];
for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
con->depth = dep[j];
}
cnum = maxc;
}
return cnum;
}
