diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp')
-rw-r--r-- | extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp | 57 |
1 files changed, 43 insertions, 14 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 31353f1b2c4..a7c8cf140ce 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -1,4 +1,3 @@ - /* * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. @@ -212,7 +211,7 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) a = 1.f/(btScalar(3.0)*(a+q)); } else { - a=1e30f; + a=BT_LARGE_FLOAT; } cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); @@ -267,7 +266,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) { const btScalar fudge_factor = btScalar(1.05); - btVector3 p,pp,normalC; + 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; @@ -333,9 +332,9 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ + if (s2 > SIMD_EPSILON) return 0; \ l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > 0) { \ + if (l > SIMD_EPSILON) { \ s2 /= l; \ if (s2*fudge_factor > s) { \ s = s2; \ @@ -346,6 +345,20 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, } \ } + 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); @@ -424,6 +437,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, output.addContactPoint(-normal,pointInWorld,-*depth); #else output.addContactPoint(-normal,pb,-*depth); + #endif // *return_code = code; } @@ -593,21 +607,30 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, 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++) { - - //AddContactPoint... - - //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]; + 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]; output.addContactPoint(-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]; + //pointInWorld[i] = point[j*3+i] + pa[i]; + output.addContactPoint(-normal,pointInWorld,-dep[j]); + } + } } else { // we have more contacts than are wanted, some of them must be culled. @@ -632,7 +655,13 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, btVector3 posInWorld; for (i=0; i<3; i++) posInWorld[i] = point[iret[j]*3+i] + pa[i]; - output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + if (code<4) + { + output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + } else + { + output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); + } } cnum = maxc; } |