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)
128 for (
int sign = -1; sign <= 1; sign += 2)
134 for (
int i =
nq; i > 0; i--)
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];
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;
328#define TST(expr1, expr2, norm, cc) \
329 s2 = btFabs(expr1) - (expr2); \
330 if (s2 > 0) return 0; \
335 invert_normal = ((expr1) < 0); \
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); \
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++)
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];
641 for (
j = 0;
j < n;
j++)
645 for (i = 0; i < 3; i++) point[
cnum * 3 + i] =
655 if (
cnum < 1)
return 0;
669 for (i = 0; i < 3; i++)
680 for (i = 0; i < 3; i++)
693 for (i = 1; i <
cnum; i++)
695 if (
dep[i] > maxdepth)
712 for (i = 0; i < 3; i++)
741 for (
int j = 0;
j < 3;
j++)
static btScalar dDOT14(const btScalar *a, const btScalar *b)
#define dMULTIPLY0_331(A, B, C)
void dLineClosestApproach(const btVector3 &pa, const btVector3 &ua, const btVector3 &pb, const btVector3 &ub, btScalar *alpha, btScalar *beta)
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)
static btScalar dDOT(const btScalar *a, const btScalar *b)
#define dMULTIPLY1_331(A, B, C)
static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
#define dDOTpq(a, b, p, q)
void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
#define TST(expr1, expr2, norm, cc)
static btScalar dDOT41(const btScalar *a, const btScalar *b)
static btScalar dDOT44(const btScalar *a, const btScalar *b)
const T & btMax(const T &a, const T &b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btAtan2(btScalar x, btScalar y)
btScalar btFabs(btScalar x)
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
btVector3 getHalfExtentsWithMargin() const
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
btVector3 can be used to represent 3D points and vectors.
const btBoxShape * m_box1
const btBoxShape * m_box2
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
btBoxBoxDetector(const btBoxShape *box1, const btBoxShape *box2)
ODE box-box collision detection is adapted to work with Bullet.