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]]);