49 #define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)]) 
   50 #define dInfinity FLT_MAX 
   61 #define dMULTIPLYOP1_331(A, op, B, C)   \ 
   63                 (A)[0] op dDOT41((B), (C));     \ 
   64                 (A)[1] op dDOT41((B + 1), (C)); \ 
   65                 (A)[2] op dDOT41((B + 2), (C)); \ 
   68 #define dMULTIPLYOP0_331(A, op, B, C) \ 
   70                 (A)[0] op dDOT((B), (C));     \ 
   71                 (A)[1] op dDOT((B + 4), (C)); \ 
   72                 (A)[2] op dDOT((B + 8), (C)); \ 
   75 #define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C) 
   76 #define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C) 
  104                 *alpha = (q1 + uaub * q2) * d;
 
  105                 *beta = (uaub * q1 + q2) * d;
 
  125         for (
int dir = 0; dir <= 1; dir++)
 
  128                 for (
int sign = -1; sign <= 1; sign += 2)
 
  134                         for (
int i = nq; i > 0; i--)
 
  137                                 if (sign * pq[dir] < h[dir])
 
  150                                 btScalar* nextq = (i > 1) ? pq + 2 : q;
 
  151                                 if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
 
  154                                         pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) /
 
  155                                                                                                         (nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]);
 
  156                                         pr[dir] = sign * h[dir];
 
  168                         r = (q == ret) ? buffer : ret;
 
  173         if (q != ret) memcpy(ret, q, nr * 2 * 
sizeof(
btScalar));
 
  177 #define M__PI 3.14159265f 
  208                 for (i = 0; i < (n - 1); i++)
 
  210                         q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
 
  212                         cx += q * (p[i * 2] + p[i * 2 + 2]);
 
  213                         cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
 
  215                 q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
 
  218                         a = 1.f / (
btScalar(3.0) * (a + q));
 
  224                 cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
 
  225                 cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
 
  230         for (i = 0; i < n; i++) A[i] = 
btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
 
  234         for (i = 0; i < n; i++) avail[i] = 1;
 
  238         for (j = 1; j < m; j++)
 
  246                 for (i = 0; i < n; i++)
 
  259 #if defined(DEBUG) || defined(_DEBUG) 
  281         btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33,
 
  282                 Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l;
 
  283         int i, j, invert_normal, code;
 
  298         R11 = 
dDOT44(R1 + 0, R2 + 0);
 
  299         R12 = 
dDOT44(R1 + 0, R2 + 1);
 
  300         R13 = 
dDOT44(R1 + 0, R2 + 2);
 
  301         R21 = 
dDOT44(R1 + 1, R2 + 0);
 
  302         R22 = 
dDOT44(R1 + 1, R2 + 1);
 
  303         R23 = 
dDOT44(R1 + 1, R2 + 2);
 
  304         R31 = 
dDOT44(R1 + 2, R2 + 0);
 
  305         R32 = 
dDOT44(R1 + 2, R2 + 1);
 
  306         R33 = 
dDOT44(R1 + 2, R2 + 2);
 
  328 #define TST(expr1, expr2, norm, cc)    \ 
  329         s2 = btFabs(expr1) - (expr2);      \ 
  330         if (s2 > 0) return 0;              \ 
  335                 invert_normal = ((expr1) < 0); \ 
  344         TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
 
  345         TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
 
  346         TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);
 
  349         TST(
dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
 
  350         TST(
dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
 
  351         TST(
dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);
 
  356 #define TST(expr1, expr2, n1, n2, n3, cc)                \ 
  357         s2 = btFabs(expr1) - (expr2);                        \ 
  358         if (s2 > SIMD_EPSILON) return 0;                     \ 
  359         l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \ 
  360         if (l > SIMD_EPSILON)                                \ 
  363                 if (s2 * fudge_factor > s)                       \ 
  367                         normalC[0] = (n1) / l;                       \ 
  368                         normalC[1] = (n2) / l;                       \ 
  369                         normalC[2] = (n3) / l;                       \ 
  370                         invert_normal = ((expr1) < 0);               \ 
  390         TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
 
  391         TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
 
  392         TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);
 
  395         TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
 
  396         TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
 
  397         TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);
 
  400         TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
 
  401         TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
 
  402         TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);
 
  412                 normal[0] = normalR[0];
 
  413                 normal[1] = normalR[4];
 
  414                 normal[2] = normalR[8];
 
  422                 normal[0] = -normal[0];
 
  423                 normal[1] = -normal[1];
 
  424                 normal[2] = -normal[2];
 
  436                 for (i = 0; i < 3; i++) pa[i] = p1[i];
 
  437                 for (j = 0; j < 3; j++)
 
  440                         for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j];
 
  445                 for (i = 0; i < 3; i++) pb[i] = p2[i];
 
  446                 for (j = 0; j < 3; j++)
 
  449                         for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j];
 
  454                 for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4];
 
  455                 for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4];
 
  458                 for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha;
 
  459                 for (i = 0; i < 3; i++) pb[i] += ub[i] * beta;
 
  466 #ifdef USE_CENTER_POINT 
  467                         for (i = 0; i < 3; i++)
 
  468                                 pointInWorld[i] = (pa[i] + pb[i]) * 
btScalar(0.5);
 
  469                         output.addContactPoint(-normal, pointInWorld, -*depth);
 
  471                         output.addContactPoint(-normal, pb, -*depth);
 
  484         const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb;
 
  509                 normal2[0] = normal[0];
 
  510                 normal2[1] = normal[1];
 
  511                 normal2[2] = normal[2];
 
  515                 normal2[0] = -normal[0];
 
  516                 normal2[1] = -normal[1];
 
  517                 normal2[2] = -normal[2];
 
  563                 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
 
  567                 for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
 
  571         int codeN, code1, code2;
 
  594         btScalar c1, c2, m11, m12, m21, m22;
 
  595         c1 = 
dDOT14(center, Ra + code1);
 
  596         c2 = 
dDOT14(center, Ra + code2);
 
  600         m11 = 
dDOT44(Ra + code1, Rb + a1);
 
  601         m12 = 
dDOT44(Ra + code1, Rb + a2);
 
  602         m21 = 
dDOT44(Ra + code2, Rb + a1);
 
  603         m22 = 
dDOT44(Ra + code2, Rb + a2);
 
  609                 quad[0] = c1 - k1 - k3;
 
  610                 quad[1] = c2 - k2 - k4;
 
  611                 quad[2] = c1 - k1 + k3;
 
  612                 quad[3] = c2 - k2 + k4;
 
  613                 quad[4] = c1 + k1 + k3;
 
  614                 quad[5] = c2 + k2 + k4;
 
  615                 quad[6] = c1 + k1 - k3;
 
  616                 quad[7] = c2 + k2 - k4;
 
  635         btScalar det1 = 1.f / (m11 * m22 - m12 * m21);
 
  641         for (j = 0; j < n; j++)
 
  643                 btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
 
  644                 btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
 
  645                 for (i = 0; i < 3; i++) point[cnum * 3 + i] =
 
  646                                                                         center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
 
  647                 dep[cnum] = Sa[codeN] - 
dDOT(normal2, point + cnum * 3);
 
  650                         ret[cnum * 2] = ret[j * 2];
 
  651                         ret[cnum * 2 + 1] = ret[j * 2 + 1];
 
  655         if (cnum < 1) 
return 0;  
 
  658         if (maxc > cnum) maxc = cnum;
 
  659         if (maxc < 1) maxc = 1;
 
  666                         for (j = 0; j < cnum; j++)
 
  669                                 for (i = 0; i < 3; i++)
 
  670                                         pointInWorld[i] = point[j * 3 + i] + pa[i];
 
  671                                 output.addContactPoint(-normal, pointInWorld, -dep[j]);
 
  677                         for (j = 0; j < cnum; j++)
 
  680                                 for (i = 0; i < 3; i++)
 
  681                                         pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
 
  683                                 output.addContactPoint(-normal, pointInWorld, -dep[j]);
 
  693                 for (i = 1; i < cnum; i++)
 
  695                         if (dep[i] > maxdepth)
 
  705                 for (j = 0; j < maxc; j++)
 
  712                         for (i = 0; i < 3; i++)
 
  713                                 posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
 
  716                                 output.addContactPoint(-normal, posInWorld, -dep[iret[j]]);
 
  720                                 output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
 
  736         dContactGeom* contact = 0;
 
  741         for (
int j = 0; j < 3; j++)
 
  743                 R1[0 + 4 * j] = transformA.
getBasis()[j].x();
 
  744                 R2[0 + 4 * j] = transformB.
getBasis()[j].x();
 
  746                 R1[1 + 4 * j] = transformA.
getBasis()[j].y();
 
  747                 R2[1 + 4 * j] = transformB.
getBasis()[j].y();
 
  749                 R1[2 + 4 * j] = transformA.
getBasis()[j].z();
 
  750                 R2[2 + 4 * j] = transformB.
getBasis()[j].z();
 
  764                          normal, &depth, &return_code,