123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718 |
- #include "btBoxBoxDetector.h"
- #include "bullet/BulletCollision//CollisionShapes/btBoxShape.h"
- #include <float.h>
- #include <string.h>
- btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2)
- : m_box1(box1),
- m_box2(box2)
- {
- }
- struct dContactGeom;
- #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
- #define dInfinity FLT_MAX
- static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
- static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
- static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
- static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
- #define dMULTIPLYOP1_331(A,op,B,C) \
- {\
- (A)[0] op dDOT41((B),(C)); \
- (A)[1] op dDOT41((B+1),(C)); \
- (A)[2] op dDOT41((B+2),(C)); \
- }
- #define dMULTIPLYOP0_331(A,op,B,C) \
- { \
- (A)[0] op dDOT((B),(C)); \
- (A)[1] op dDOT((B+4),(C)); \
- (A)[2] op dDOT((B+8),(C)); \
- }
- #define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
- #define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
- typedef btScalar dMatrix3[4*3];
- void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
- const btVector3& pb, const btVector3& ub,
- btScalar *alpha, btScalar *beta);
- void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
- const btVector3& pb, const btVector3& ub,
- btScalar *alpha, btScalar *beta)
- {
- btVector3 p;
- p[0] = pb[0] - pa[0];
- p[1] = pb[1] - pa[1];
- p[2] = pb[2] - pa[2];
- btScalar uaub = dDOT(ua,ub);
- btScalar q1 = dDOT(ua,p);
- btScalar q2 = -dDOT(ub,p);
- btScalar d = 1-uaub*uaub;
- if (d <= btScalar(0.0001f)) {
-
- *alpha = 0;
- *beta = 0;
- }
- else {
- d = 1.f/d;
- *alpha = (q1 + uaub*q2)*d;
- *beta = (uaub*q1 + q2)*d;
- }
- }
- static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
- {
-
-
- int nq=4,nr=0;
- btScalar buffer[16];
- btScalar *q = p;
- btScalar *r = ret;
- for (int dir=0; dir <= 1; dir++) {
-
- for (int sign=-1; sign <= 1; sign += 2) {
-
- btScalar *pq = q;
- btScalar *pr = r;
- nr = 0;
- for (int i=nq; i > 0; i--) {
-
- if (sign*pq[dir] < h[dir]) {
-
- pr[0] = pq[0];
- pr[1] = pq[1];
- pr += 2;
- nr++;
- if (nr & 8) {
- q = r;
- goto done;
- }
- }
- btScalar *nextq = (i > 1) ? pq+2 : q;
- if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
-
- pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
- (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
- pr[dir] = sign*h[dir];
- pr += 2;
- nr++;
- if (nr & 8) {
- q = r;
- goto done;
- }
- }
- pq += 2;
- }
- q = r;
- r = (q==ret) ? buffer : ret;
- nq = nr;
- }
- }
- done:
- if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
- return nr;
- }
- #define M__PI 3.14159265f
- void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
- void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
- {
-
- int i,j;
- btScalar a,cx,cy,q;
- if (n==1) {
- cx = p[0];
- cy = p[1];
- }
- else if (n==2) {
- cx = btScalar(0.5)*(p[0] + p[2]);
- cy = btScalar(0.5)*(p[1] + p[3]);
- }
- else {
- a = 0;
- cx = 0;
- cy = 0;
- for (i=0; i<(n-1); i++) {
- q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
- a += q;
- cx += q*(p[i*2]+p[i*2+2]);
- cy += q*(p[i*2+1]+p[i*2+3]);
- }
- q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
- if (btFabs(a+q) > SIMD_EPSILON)
- {
- a = 1.f/(btScalar(3.0)*(a+q));
- } else
- {
- a=BT_LARGE_FLOAT;
- }
- cx = a*(cx + q*(p[n*2-2]+p[0]));
- cy = a*(cy + q*(p[n*2-1]+p[1]));
- }
-
- btScalar A[8];
- for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
-
- int avail[8];
- for (i=0; i<n; i++) avail[i] = 1;
- avail[i0] = 0;
- iret[0] = i0;
- iret++;
- for (j=1; j<m; j++) {
- a = btScalar(j)*(2*M__PI/m) + A[i0];
- if (a > M__PI) a -= 2*M__PI;
- btScalar maxdiff=1e9,diff;
- *iret = i0;
- for (i=0; i<n; i++) {
- if (avail[i]) {
- diff = btFabs (A[i]-a);
- if (diff > M__PI) diff = 2*M__PI - diff;
- if (diff < maxdiff) {
- maxdiff = diff;
- *iret = i;
- }
- }
- }
- #if defined(DEBUG) || defined (_DEBUG)
- btAssert (*iret != i0);
- #endif
- avail[*iret] = 0;
- iret++;
- }
- }
- int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
- const btVector3& side1, const btVector3& p2,
- const dMatrix3 R2, const btVector3& side2,
- btVector3& normal, btScalar *depth, int *return_code,
- int maxc, dContactGeom * , int ,btDiscreteCollisionDetectorInterface::Result& output);
- int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
- const btVector3& side1, const btVector3& p2,
- const dMatrix3 R2, const btVector3& side2,
- btVector3& normal, btScalar *depth, int *return_code,
- int maxc, dContactGeom * , int ,btDiscreteCollisionDetectorInterface::Result& output)
- {
- const btScalar fudge_factor = btScalar(1.05);
- btVector3 p,pp,normalC(0.f,0.f,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;
- int i,j,invert_normal,code;
-
- p = p2 - p1;
- dMULTIPLY1_331 (pp,R1,p);
-
- A[0] = side1[0]*btScalar(0.5);
- A[1] = side1[1]*btScalar(0.5);
- A[2] = side1[2]*btScalar(0.5);
- B[0] = side2[0]*btScalar(0.5);
- B[1] = side2[1]*btScalar(0.5);
- B[2] = side2[2]*btScalar(0.5);
-
- 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);
-
-
-
-
-
-
-
-
-
- #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;
-
- 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);
-
- 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);
-
-
- #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;
-
- 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);
-
- 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);
-
- 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 (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;
-
- if (code > 6) {
-
-
- 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) ? btScalar(1.0) : btScalar(-1.0);
- for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
- }
-
- btVector3 pb;
- for (i=0; i<3; i++) pb[i] = p2[i];
- for (j=0; j<3; j++) {
- sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(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;
- #ifdef USE_CENTER_POINT
- for (i=0; i<3; i++)
- pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
- output.addContactPoint(-normal,pointInWorld,-*depth);
- #else
- output.addContactPoint(-normal,pb,-*depth);
- #endif
- *return_code = code;
- }
- return 1;
- }
-
-
-
-
- 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;
- }
-
-
- 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]);
-
-
-
- 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;
- }
- }
-
- 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];
- }
-
- 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;
- }
-
- btScalar quad[8];
- btScalar c1,c2,m11,m12,m21,m22;
- c1 = dDOT14 (center,Ra+code1);
- c2 = dDOT14 (center,Ra+code2);
-
-
-
- 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;
- }
-
- btScalar rect[2];
- rect[0] = Sa[code1];
- rect[1] = Sa[code2];
-
- btScalar ret[16];
- int n = intersectRectQuad2 (rect,quad,ret);
- if (n < 1) return 0;
-
-
-
-
- btScalar point[3*8];
- btScalar dep[8];
- btScalar det1 = 1.f/(m11*m22 - m12*m21);
- m11 *= det1;
- m12 *= det1;
- m21 *= det1;
- m22 *= det1;
- int cnum = 0;
- 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;
-
- if (maxc > cnum) maxc = cnum;
- if (maxc < 1) maxc = 1;
- if (cnum <= maxc) {
- if (code<4)
- {
-
- for (j=0; j < cnum; j++)
- {
- btVector3 pointInWorld;
- for (i=0; i<3; i++)
- pointInWorld[i] = point[j*3+i] + pa[i];
- output.addContactPoint(-normal,pointInWorld,-dep[j]);
- }
- } else
- {
-
- 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];
-
- output.addContactPoint(-normal,pointInWorld,-dep[j]);
- }
- }
- }
- else {
-
-
- 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)
- {
- output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
- } else
- {
- output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
- }
- }
- cnum = maxc;
- }
- *return_code = code;
- return cnum;
- }
- void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* ,bool )
- {
-
- const btTransform& transformA = input.m_transformA;
- const btTransform& transformB = input.m_transformB;
-
- int skip = 0;
- dContactGeom *contact = 0;
- dMatrix3 R1;
- dMatrix3 R2;
- for (int j=0;j<3;j++)
- {
- R1[0+4*j] = transformA.getBasis()[j].x();
- R2[0+4*j] = transformB.getBasis()[j].x();
- R1[1+4*j] = transformA.getBasis()[j].y();
- R2[1+4*j] = transformB.getBasis()[j].y();
- R1[2+4*j] = transformA.getBasis()[j].z();
- R2[2+4*j] = transformB.getBasis()[j].z();
- }
-
- btVector3 normal;
- btScalar depth;
- int return_code;
- int maxc = 4;
- dBoxBox2 (transformA.getOrigin(),
- R1,
- 2.f*m_box1->getHalfExtentsWithMargin(),
- transformB.getOrigin(),
- R2,
- 2.f*m_box2->getHalfExtentsWithMargin(),
- normal, &depth, &return_code,
- maxc, contact, skip,
- output
- );
- }
|