void TransformedGeometryExtensionLocalParams(dGeomID geom_transform,const dReal* axis,float center_prg,dReal* local_axis,dReal& local_center_prg) { R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform"); const dReal* rot=dGeomGetRotation(geom_transform); const dReal* pos=dGeomGetPosition(geom_transform); dVector3 local_pos; dMULTIPLY1_331(local_axis,rot,axis); dMULTIPLY1_331(local_pos,rot,pos); local_center_prg=center_prg-dDOT(local_pos,local_axis); }
//positive inside dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z) { dUASSERT (g && g->type == dConeClass,"argument not a cone"); dxCone *cone = (dxCone*) g; dVector3 tmp,q; tmp[0] = x - cone->pos[0]; tmp[1] = y - cone->pos[1]; tmp[2] = z - cone->pos[2]; dMULTIPLY1_331 (q,cone->R,tmp); dReal r = cone->radius; dReal h = cone->lz; dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]); dReal d1 = q[2]; dReal d2 = h-q[2]; if (d0 < d1) { if (d0 < d2) return d0; else return d2; } else { if (d1 < d2) return d1; else return d2; } }
EXPORT_C dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z) { g->recomputePosr(); dxBox *b = (dxBox*) g; // Set p = (x,y,z) relative to box center // // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2) dVector3 p,q; p[0] = x - b->final_posr->pos[0]; p[1] = y - b->final_posr->pos[1]; p[2] = z - b->final_posr->pos[2]; // Rotate p into box's coordinate frame, so we can // treat the OBB as an AABB dMULTIPLY1_331 (q,b->final_posr->R,p); // Record distance from point to each successive box side, and see // if the point is inside all six sides dReal dist[6]; int i; bool inside = true; for (i=0; i < 3; i++) { dReal side = dMUL(b->side[i],REAL(0.5)); dist[i ] = side - q[i]; dist[i+3] = side + q[i]; if ((dist[i] < 0) || (dist[i+3] < 0)) { inside = false; } } // If point is inside the box, the depth is the smallest positive distance // to any side if (inside) { dReal smallest_dist = (dReal) (unsigned) -1; for (i=0; i < 6; i++) { if (dist[i] < smallest_dist) smallest_dist = dist[i]; } return smallest_dist; } // Otherwise, if point is outside the box, the depth is the largest // distance to any side. This is an approximation to the 'proper' // solution (the proper solution may be larger in some cases). dReal largest_dist = 0; for (i=0; i < 6; i++) { if (dist[i] > largest_dist) largest_dist = dist[i]; } return -largest_dist; }
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 dBoxBox2 (const btVector3 p1, const dMatrix3 R1, const btVector3 side1, const btVector3 p2, const dMatrix3 R2, const btVector3 side2, BoxBoxResults *results) { const btScalar fudge_factor = 1.05; btVector3 p,pp,normalC; normalC[0] = 0.f; normalC[1] = 0.f; normalC[2] = 0.f; const btScalar *normalR = 0; btScalar 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,normal[3],depth; 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 (already specified as half lengths) 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 = 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 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(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 = btFabs(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] + 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 (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); TST (dDOT41(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 = btFabs(expr1) - (expr2); \ if (s2 > SIMD_EPSILON) return 0; \ l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ if (l > SIMD_EPSILON) { \ 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); \ } \ } btScalar fudge2 = 1.0e-5f; Q11 += fudge2; Q12 += fudge2; Q13 += fudge2; Q21 += fudge2; Q22 += fudge2; Q23 += fudge2; Q31 += fudge2; Q32 += fudge2; Q33 += fudge2; // 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; results->code = code; // 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 btVector3 pa; btScalar sign; for (i=0; i<3; i++) pa[i] = p1[i]; for (j=0; j<3; j++) { sign = (dDOT14(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 btVector3 pb; for (i=0; i<3; i++) pb[i] = p2[i]; for (j=0; j<3; j++) { sign = (dDOT14(normal,R2+j) > 0) ? -1.0 : 1.0; for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; } btScalar alpha,beta; btVector3 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; { btVector3 pointInWorld; addContactPoint(results,normal,pb,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). const btScalar *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. btVector3 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] = btFabs (nr[0]); anr[1] = btFabs (nr[1]); anr[2] = btFabs (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 btVector3 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 btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) btScalar 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); { btScalar k1 = m11*Sb[a1]; btScalar k2 = m21*Sb[a1]; btScalar k3 = m12*Sb[a2]; btScalar 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 btScalar rect[2]; rect[0] = Sa[code1]; rect[1] = Sa[code2]; // intersect the incident and reference faces btScalar ret[16]; int n = intersectRectQuad2 (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. btScalar point[3*8]; // penetrating contact points btScalar dep[8]; // depths for those points btScalar det1 = 1.f/(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++) { btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); btScalar 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] - 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 int maxc = 4; if (maxc > cnum) maxc = cnum; if (maxc < 1) maxc = 1; if (cnum <= maxc) { if (code<4) { // we have less contacts than we need, so we use them all for (j=0; j < cnum; j++) { btVector3 pointInWorld; for (i=0; i<3; i++) pointInWorld[i] = point[j*3+i] + pa[i]; addContactPoint(results,normal,pointInWorld,dep[j]); } } else { // we have less contacts than we need, so we use them all for (j=0; j < cnum; j++) { btVector3 pointInWorld; for (i=0; i<3; i++) pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j]; addContactPoint(results,normal,pointInWorld,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; btScalar maxdepth = dep[0]; for (i=1; i<cnum; i++) { if (dep[i] > maxdepth) { maxdepth = dep[i]; i1 = i; } } int iret[8]; cullPoints2 (cnum,ret,maxc,i1,iret); for (j=0; j < maxc; j++) { btVector3 posInWorld; for (i=0; i<3; i++) posInWorld[i] = point[iret[j]*3+i] + pa[i]; if (code<4) { addContactPoint(results, normal,posInWorld,dep[iret[j]]); } else { posInWorld[0] -= normal[0]*dep[iret[j]]; posInWorld[1] -= normal[1]*dep[iret[j]]; posInWorld[2] -= normal[2]*dep[iret[j]]; addContactPoint(results, normal,posInWorld,dep[iret[j]]); } } cnum = maxc; } return cnum; }
void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, const dVector3 c, const dMatrix3 R, const dVector3 side, dVector3 lret, dVector3 bret) { int i; // compute the start and delta of the line p1-p2 relative to the box. // we will do all subsequent computations in this box-relative coordinate // system. we have to do a translation and rotation for each point. dVector3 tmp,s,v; tmp[0] = p1[0] - c[0]; tmp[1] = p1[1] - c[1]; tmp[2] = p1[2] - c[2]; dMULTIPLY1_331 (s,R,tmp); tmp[0] = p2[0] - p1[0]; tmp[1] = p2[1] - p1[1]; tmp[2] = p2[2] - p1[2]; dMULTIPLY1_331 (v,R,tmp); // mirror the line so that v has all components >= 0 dVector3 sign; for (i=0; i<3; i++) { if (v[i] < 0) { s[i] = -s[i]; v[i] = -v[i]; sign[i] = -1; } else sign[i] = 1; } // compute v^2 dVector3 v2; v2[0] = v[0]*v[0]; v2[1] = v[1]*v[1]; v2[2] = v[2]*v[2]; // compute the half-sides of the box double h[3]; h[0] = side[0]; h[1] = side[1]; h[2] = side[2]; // region is -1,0,+1 depending on which side of the box planes each // coordinate is on. tanchor is the next t value at which there is a // transition, or the last one if there are no more. int region[3]; double tanchor[3]; // find the region and tanchor values for p1 for (i=0; i<3; i++) { if (v[i] > 0) { if (s[i] < -h[i]) { region[i] = -1; tanchor[i] = (-h[i]-s[i])/v[i]; } else { region[i] = (s[i] > h[i]); tanchor[i] = (h[i]-s[i])/v[i]; } } else { region[i] = 0; tanchor[i] = 2; // this will never be a valid tanchor } } // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point double t=0; double dd2dt = 0; for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i]; if (dd2dt >= 0) goto got_answer; do { // find the point on the line that is at the next clip plane boundary double next_t = 1; for (i=0; i<3; i++) { if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t) next_t = tanchor[i]; } // compute d|d|^2/dt for the next t double next_dd2dt = 0; for (i=0; i<3; i++) { next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]); } // if the sign of d|d|^2/dt has changed, solution = the crossover point if (next_dd2dt >= 0) { double m = (next_dd2dt-dd2dt)/(next_t - t); t -= dd2dt/m; goto got_answer; } // advance to the next anchor point / region for (i=0; i<3; i++) { if (tanchor[i] == next_t) { tanchor[i] = (h[i]-s[i])/v[i]; region[i]++; } } t = next_t; dd2dt = next_dd2dt; } while (t < 1); t = 1; got_answer: // compute closest point on the line for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1 // compute closest point on the box for (i=0; i<3; i++) { tmp[i] = sign[i] * (s[i] + t*v[i]); if (tmp[i] < -h[i]) tmp[i] = -h[i]; else if (tmp[i] > h[i]) tmp[i] = h[i]; } dMULTIPLY0_331 (s,R,tmp); for (i=0; i<3; i++) bret[i] = s[i] + c[i]; }
dReal doStuffAndGetError (int n) { switch (n) { // ********** fixed joint case 0: { // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); // check the orientations are the same const dReal *R1 = dBodyGetRotation (body[0]); const dReal *R2 = dBodyGetRotation (body[1]); dReal err1 = dMaxDifference (R1,R2,3,3); // check the body offset is correct dVector3 p,pp; const dReal *p1 = dBodyGetPosition (body[0]); const dReal *p2 = dBodyGetPosition (body[1]); for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; dMULTIPLY1_331 (pp,R1,p); pp[0] += 0.5; pp[1] += 0.5; return (err1 + length (pp)) * 300; } case 1: { // 1 body to static env addOscillatingTorque (0.1); // check the orientation is the identity dReal err1 = cmpIdentity (dBodyGetRotation (body[0])); // check the body offset is correct dVector3 p; const dReal *p1 = dBodyGetPosition (body[0]); for (int i=0; i<3; i++) p[i] = p1[i]; p[0] -= 0.25; p[1] -= 0.25; p[2] -= 1; return (err1 + length (p)) * 1e6; } case 2: { // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); // check the body offset is correct // Should really check body rotation too. Oh well. const dReal *R1 = dBodyGetRotation (body[0]); dVector3 p,pp; const dReal *p1 = dBodyGetPosition (body[0]); const dReal *p2 = dBodyGetPosition (body[1]); for (int i=0; i<3; i++) p[i] = p2[i] - p1[i]; dMULTIPLY1_331 (pp,R1,p); pp[0] += 0.5; pp[1] += 0.5; return length(pp) * 300; } case 3: { // 1 body to static env with relative rotation addOscillatingTorque (0.1); // check the body offset is correct dVector3 p; const dReal *p1 = dBodyGetPosition (body[0]); for (int i=0; i<3; i++) p[i] = p1[i]; p[0] -= 0.25; p[1] -= 0.25; p[2] -= 1; return length (p) * 1e6; } // ********** hinge joint case 200: // 2 body addOscillatingTorque (0.1); dampRotationalMotion (0.1); return dInfinity; case 220: // hinge angle polarity test dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); if (iteration == 40) { dReal a = dJointGetHingeAngle (joint); if (a > 0.5 && a < 1) return 0; else return 10; } return 0; case 221: { // hinge angle rate test static dReal last_angle = 0; dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); dReal a = dJointGetHingeAngle (joint); dReal r = dJointGetHingeAngleRate (joint); dReal er = (a-last_angle)/STEPSIZE; // estimated rate last_angle = a; return fabs(r-er) * 4e4; } case 230: // hinge motor rate (and polarity) test case 231: { // ...with stops static dReal a = 0; dReal r = dJointGetHingeAngleRate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHingeParam (joint,dParamVel,cos(a)); if (n==231) return dInfinity; return err * 1e6; } // ********** slider joint case 300: // 2 body addOscillatingTorque (0.05); dampRotationalMotion (0.1); addSpringForce (0.5); return dInfinity; case 320: // slider angle polarity test dBodyAddForce (body[0],0,0,0.1); dBodyAddForce (body[1],0,0,-0.1); if (iteration == 40) { dReal a = dJointGetSliderPosition (joint); if (a > 0.2 && a < 0.5) return 0; else return 10; return a; } return 0; case 321: { // slider angle rate test static dReal last_pos = 0; dBodyAddForce (body[0],0,0,0.1); dBodyAddForce (body[1],0,0,-0.1); dReal p = dJointGetSliderPosition (joint); dReal r = dJointGetSliderPositionRate (joint); dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact) last_pos = p; return fabs(r-er) * 1e9; } case 330: // slider motor rate (and polarity) test case 331: { // ...with stops static dReal a = 0; dReal r = dJointGetSliderPositionRate (joint); dReal err = fabs (0.7*cos(a) - r); if (a < 0.04) err = 0; a += 0.03; dJointSetSliderParam (joint,dParamVel,0.7*cos(a)); if (n==331) return dInfinity; return err * 1e6; } // ********** hinge-2 joint case 420: // hinge-2 steering angle polarity test dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); if (iteration == 40) { dReal a = dJointGetHinge2Angle1 (joint); if (a > 0.5 && a < 0.6) return 0; else return 10; } return 0; case 421: { // hinge-2 steering angle rate test static dReal last_angle = 0; dBodyAddTorque (body[0],0,0,0.01); dBodyAddTorque (body[1],0,0,-0.01); dReal a = dJointGetHinge2Angle1 (joint); dReal r = dJointGetHinge2Angle1Rate (joint); dReal er = (a-last_angle)/STEPSIZE; // estimated rate last_angle = a; return fabs(r-er)*2e4; } case 430: // hinge 2 steering motor rate (+polarity) test case 431: { // ...with stops static dReal a = 0; dReal r = dJointGetHinge2Angle1Rate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHinge2Param (joint,dParamVel,cos(a)); if (n==431) return dInfinity; return err * 1e6; } case 432: { // hinge 2 wheel motor rate (+polarity) test static dReal a = 0; dReal r = dJointGetHinge2Angle2Rate (joint); dReal err = fabs (cos(a) - r); if (a==0) err = 0; a += 0.03; dJointSetHinge2Param (joint,dParamVel2,cos(a)); return err * 1e6; } // ********** angular motor joint case 600: { // test euler angle calculations // desired euler angles from last iteration static dReal a1,a2,a3; // find actual euler angles dReal aa1 = dJointGetAMotorAngle (joint,0); dReal aa2 = dJointGetAMotorAngle (joint,1); dReal aa3 = dJointGetAMotorAngle (joint,2); // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3); dReal err = dInfinity; if (iteration > 0) { err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3); err *= 1e10; } // get random base rotation for both bodies dMatrix3 Rbase; dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), 3*(dRandReal()-0.5), 3*(dRandReal()-0.5)); dBodySetRotation (body[0],Rbase); // rotate body 2 by random euler angles w.r.t. body 1 a1 = 3.14 * 2 * (dRandReal()-0.5); a2 = 1.57 * 2 * (dRandReal()-0.5); a3 = 3.14 * 2 * (dRandReal()-0.5); dMatrix3 R1,R2,R3,Rtmp1,Rtmp2; dRFromAxisAndAngle (R1,0,0,1,-a1); dRFromAxisAndAngle (R2,0,1,0,a2); dRFromAxisAndAngle (R3,1,0,0,-a3); dMultiply0 (Rtmp1,R2,R3,3,3,3); dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3); dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3); dBodySetRotation (body[1],Rtmp1); // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3); return err; } // ********** universal joint case 700: { // 2 body: joint constraint dVector3 ax1, ax2; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); return fabs(10*dDOT(ax1, ax2)); } case 701: { // 2 body: angle 1 rate static dReal last_angle = 0; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; // I'm not sure why the error is so large here. return fabs(r - er) * 1e1; } case 702: { // 2 body: angle 2 rate static dReal last_angle = 0; addOscillatingTorque (0.1); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; // I'm not sure why the error is so large here. return fabs(r - er) * 1e1; } case 720: { // universal transmit torque test: constraint error dVector3 ax1, ax2; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); return fabs(10*dDOT(ax1, ax2)); } case 721: { // universal transmit torque test: angle1 rate static dReal last_angle = 0; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 722: { // universal transmit torque test: angle2 rate static dReal last_angle = 0; addOscillatingTorqueAbout (0.1, 1, 1, 0); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 730:{ dVector3 ax1, ax2; dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); return fabs(10*dDOT(ax1, ax2)); } case 731:{ dVector3 ax1; static dReal last_angle = 0; dJointGetUniversalAxis1(joint, ax1); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 2e3; } case 732:{ dVector3 ax1; static dReal last_angle = 0; dJointGetUniversalAxis1(joint, ax1); addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 740:{ dVector3 ax1, ax2; dJointGetUniversalAxis1(joint, ax1); dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); return fabs(10*dDOT(ax1, ax2)); } case 741:{ dVector3 ax2; static dReal last_angle = 0; dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle1(joint); dReal r = dJointGetUniversalAngle1Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e10; } case 742:{ dVector3 ax2; static dReal last_angle = 0; dJointGetUniversalAxis2(joint, ax2); addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]); dampRotationalMotion (0.1); dReal a = dJointGetUniversalAngle2(joint); dReal r = dJointGetUniversalAngle2Rate(joint); dReal diff = a - last_angle; if (diff > M_PI) diff -= 2*M_PI; if (diff < -M_PI) diff += 2*M_PI; dReal er = diff / STEPSIZE; // estimated rate last_angle = a; return fabs(r - er) * 1e4; } } return dInfinity; }
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 = 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. 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 (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); TST (dDOT41(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 = (dDOT14(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 = (dDOT14(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 = 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 = 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] - 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 | 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; }
int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dRayClass); dIASSERT (o2->type == dBoxClass); dIASSERT ((flags & NUMC_MASK) >= 1); dxRay *ray = (dxRay*) o1; dxBox *box = (dxBox*) o2; contact->g1 = ray; contact->g2 = box; contact->side1 = -1; contact->side2 = -1; int i; // compute the start and delta of the ray relative to the box. // we will do all subsequent computations in this box-relative coordinate // system. we have to do a translation and rotation for each point. dVector3 tmp,s,v; tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0]; tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1]; tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2]; dMULTIPLY1_331 (s,box->final_posr->R,tmp); tmp[0] = ray->final_posr->R[0*4+2]; tmp[1] = ray->final_posr->R[1*4+2]; tmp[2] = ray->final_posr->R[2*4+2]; dMULTIPLY1_331 (v,box->final_posr->R,tmp); // mirror the line so that v has all components >= 0 dVector3 sign; for (i=0; i<3; i++) { if (v[i] < 0) { s[i] = -s[i]; v[i] = -v[i]; sign[i] = 1; } else sign[i] = -1; } // compute the half-sides of the box dReal h[3]; h[0] = REAL(0.5) * box->side[0]; h[1] = REAL(0.5) * box->side[1]; h[2] = REAL(0.5) * box->side[2]; // do a few early exit tests if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] || (s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] || (s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] || (v[0] == 0 && v[1] == 0 && v[2] == 0)) { return 0; } // compute the t=[lo..hi] range for where s+v*t intersects the box dReal lo = -dInfinity; dReal hi = dInfinity; int nlo = 0, nhi = 0; for (i=0; i<3; i++) { if (v[i] != 0) { dReal k = (-h[i] - s[i])/v[i]; if (k > lo) { lo = k; nlo = i; } k = (h[i] - s[i])/v[i]; if (k < hi) { hi = k; nhi = i; } } } // check if the ray intersects if (lo > hi) return 0; dReal alpha; int n; if (lo >= 0) { alpha = lo; n = nlo; } else { alpha = hi; n = nhi; } if (alpha < 0 || alpha > ray->length) return 0; contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2]; contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2]; contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2]; contact->normal[0] = box->final_posr->R[0*4+n] * sign[n]; contact->normal[1] = box->final_posr->R[1*4+n] * sign[n]; contact->normal[2] = box->final_posr->R[2*4+n] * sign[n]; contact->depth = alpha; return 1; }