51 #define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])    52 #define dInfinity FLT_MAX    64 #define dMULTIPLYOP1_331(A,op,B,C) \    66   (A)[0] op dDOT41((B),(C)); \    67   (A)[1] op dDOT41((B+1),(C)); \    68   (A)[2] op dDOT41((B+2),(C)); \    71 #define dMULTIPLYOP0_331(A,op,B,C) \    73   (A)[0] op dDOT((B),(C)); \    74   (A)[1] op dDOT((B+4),(C)); \    75   (A)[2] op dDOT((B+8),(C)); \    78 #define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)    79 #define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)   105     *alpha = (q1 + uaub*q2)*d;
   106     *beta  = (uaub*q1 + q2)*d;
   128   for (
int dir=0; dir <= 1; dir++) {
   130     for (
int sign=-1; sign <= 1; sign += 2) {
   135       for (
int i=nq; i > 0; i--) {
   137         if (sign*pq[dir] < h[dir]) {
   148         btScalar *nextq = (i > 1) ? pq+2 : q;
   149         if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
   151           pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
   152             (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
   153           pr[dir] = sign*h[dir];
   164       r = (q==ret) ? buffer : ret;
   169   if (q != ret) memcpy (ret,q,nr*2*
sizeof(
btScalar));
   174 #define M__PI 3.14159265f   202     for (i=0; i<(n-1); i++) {
   203       q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
   205       cx += q*(p[i*2]+p[i*2+2]);
   206       cy += q*(p[i*2+1]+p[i*2+3]);
   208     q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
   216     cx = a*(cx + q*(p[n*2-2]+p[0]));
   217     cy = a*(cy + q*(p[n*2-1]+p[1]));
   222   for (i=0; i<n; i++) A[i] = 
btAtan2(p[i*2+1]-cy,p[i*2]-cx);
   226   for (i=0; i<n; i++) avail[i] = 1;
   230   for (j=1; j<m; j++) {
   237     for (i=0; i<n; i++) {
   241         if (diff < maxdiff) {
   247 #if defined(DEBUG) || defined (_DEBUG)   271   btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
   272     Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
   273   int i,j,invert_normal,code;
   306 #define TST(expr1,expr2,norm,cc) \   307   s2 = btFabs(expr1) - (expr2); \   308   if (s2 > 0) return 0; \   312     invert_normal = ((expr1) < 0); \   321   TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
   322   TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
   323   TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
   326   TST (
dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
   327   TST (
dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
   328   TST (
dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
   333 #define TST(expr1,expr2,n1,n2,n3,cc) \   334   s2 = btFabs(expr1) - (expr2); \   335   if (s2 > SIMD_EPSILON) return 0; \   336   l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \   337   if (l > SIMD_EPSILON) { \   339     if (s2*fudge_factor > s) { \   342       normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \   343       invert_normal = ((expr1) < 0); \   348   btScalar fudge2 (1.0e-5f);
   363   TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
   364   TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
   365   TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
   368   TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
   369   TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
   370   TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
   373   TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
   374   TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
   375   TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
   384     normal[0] = normalR[0];
   385     normal[1] = normalR[4];
   386     normal[2] = normalR[8];
   392     normal[0] = -normal[0];
   393     normal[1] = -normal[1];
   394     normal[2] = -normal[2];
   405     for (i=0; i<3; i++) pa[i] = p1[i];
   406     for (j=0; j<3; j++) {
   408       for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
   413     for (i=0; i<3; i++) pb[i] = p2[i];
   414     for (j=0; j<3; j++) {
   416       for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
   421     for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
   422     for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
   425     for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
   426     for (i=0; i<3; i++) pb[i] += ub[i]*beta;
   434 #ifdef USE_CENTER_POINT   436                         pointInWorld[i] = (pa[i]+pb[i])*
btScalar(0.5);
   452   const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
   474     normal2[0] = normal[0];
   475     normal2[1] = normal[1];
   476     normal2[2] = normal[2];
   479     normal2[0] = -normal[0];
   480     normal2[1] = -normal[1];
   481     normal2[2] = -normal[2];
   492   if (anr[1] > anr[0]) {
   493     if (anr[1] > anr[2]) {
   505     if (anr[0] > anr[2]) {
   520     for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
   523     for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
   527   int codeN,code1,code2;
   528   if (code <= 3) codeN = code-1; 
else codeN = code-4;
   544   btScalar c1,c2,m11,m12,m21,m22;
   545   c1 = 
dDOT14 (center,Ra+code1);
   546   c2 = 
dDOT14 (center,Ra+code2);
   550   m11 = 
dDOT44 (Ra+code1,Rb+a1);
   551   m12 = 
dDOT44 (Ra+code1,Rb+a2);
   552   m21 = 
dDOT44 (Ra+code2,Rb+a1);
   553   m22 = 
dDOT44 (Ra+code2,Rb+a2);
   555     btScalar k1 = m11*Sb[a1];
   556     btScalar k2 = m21*Sb[a1];
   557     btScalar k3 = m12*Sb[a2];
   558     btScalar k4 = m22*Sb[a2];
   559     quad[0] = c1 - k1 - k3;
   560     quad[1] = c2 - k2 - k4;
   561     quad[2] = c1 - k1 + k3;
   562     quad[3] = c2 - k2 + k4;
   563     quad[4] = c1 + k1 + k3;
   564     quad[5] = c2 + k2 + k4;
   565     quad[6] = c1 + k1 - k3;
   566     quad[7] = c2 + k2 - k4;
   585   btScalar det1 = 1.f/(m11*m22 - m12*m21);
   591   for (j=0; j < n; j++) {
   592     btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
   593     btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
   594     for (i=0; i<3; i++) point[cnum*3+i] =
   595                           center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
   596     dep[cnum] = Sa[codeN] - 
dDOT(normal2,point+cnum*3);
   597     if (dep[cnum] >= 0) {
   598       ret[cnum*2] = ret[j*2];
   599       ret[cnum*2+1] = ret[j*2+1];
   603   if (cnum < 1) 
return 0;       
   606   if (maxc > cnum) maxc = cnum;
   607   if (maxc < 1) maxc = 1;
   614     for (j=0; j < cnum; j++) 
   618                         pointInWorld[i] = point[j*3+i] + pa[i];
   625                 for (j=0; j < cnum; j++) 
   629                                 pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
   639     btScalar maxdepth = dep[0];
   640     for (i=1; i<cnum; i++) {
   641       if (dep[i] > maxdepth) {
   650     for (j=0; j < maxc; j++) {
   657                         posInWorld[i] = point[iret[j]*3+i] + pa[i];
   663                         output.
addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
   680         dContactGeom *contact = 0;
   685         for (
int j=0;j<3;j++)
   687                 R1[0+4*j] = transformA.
getBasis()[j].x();
   688                 R2[0+4*j] = transformB.
getBasis()[j].x();
   690                 R1[1+4*j] = transformA.
getBasis()[j].y();
   691                 R2[1+4*j] = transformB.
getBasis()[j].y();
   694                 R1[2+4*j] = transformA.
getBasis()[j].z();
   695                 R2[2+4*j] = transformB.
getBasis()[j].z();
   713         normal, &depth, &return_code,
 
static btScalar dDOT44(const btScalar *a, const btScalar *b)
btVector3 getHalfExtentsWithMargin() const
btBoxBoxDetector(const btBoxShape *box1, const btBoxShape *box2)
ODE box-box collision detection is adapted to work with Bullet. 
static btScalar dDOT14(const btScalar *a, const btScalar *b)
static btScalar dDOT41(const btScalar *a, const btScalar *b)
void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
btScalar btAtan2(btScalar x, btScalar y)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
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)
#define dMULTIPLY0_331(A, B, C)
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
btVector3 can be used to represent 3D points and vectors. 
static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
#define dMULTIPLY1_331(A, B, C)
void dLineClosestApproach(const btVector3 &pa, const btVector3 &ua, const btVector3 &pb, const btVector3 &ub, btScalar *alpha, btScalar *beta)
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)=0
const btBoxShape * m_box2
#define TST(expr1, expr2, norm, cc)
const btBoxShape * m_box1
#define dDOTpq(a, b, p, q)
static btScalar dDOT(const btScalar *a, const btScalar *b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btFabs(btScalar x)