29 #if defined(DEBUG) || defined(_DEBUG) 32 #include <spu_printf.h> 33 #define printf spu_printf 42 #define GJK_MAX_ITERATIONS 128 44 #ifdef BT_USE_DOUBLE_PRECISION 45 #define GJK_ACCURACY ((btScalar)1e-12) 46 #define GJK_MIN_DISTANCE ((btScalar)1e-12) 47 #define GJK_DUPLICATED_EPS ((btScalar)1e-12) 49 #define GJK_ACCURACY ((btScalar)0.0001) 50 #define GJK_MIN_DISTANCE ((btScalar)0.0001) 51 #define GJK_DUPLICATED_EPS ((btScalar)0.0001) 52 #endif //BT_USE_DOUBLE_PRECISION 54 #define GJK_SIMPLEX2_EPS ((btScalar)0.0) 55 #define GJK_SIMPLEX3_EPS ((btScalar)0.0) 56 #define GJK_SIMPLEX4_EPS ((btScalar)0.0) 59 #define EPA_MAX_VERTICES 128 60 #define EPA_MAX_ITERATIONS 255 62 #ifdef BT_USE_DOUBLE_PRECISION 63 #define EPA_ACCURACY ((btScalar)1e-12) 64 #define EPA_PLANE_EPS ((btScalar)1e-14) 65 #define EPA_INSIDE_EPS ((btScalar)1e-9) 67 #define EPA_ACCURACY ((btScalar)0.0001) 68 #define EPA_PLANE_EPS ((btScalar)0.00001) 69 #define EPA_INSIDE_EPS ((btScalar)0.01) 72 #define EPA_FALLBACK (10 * EPA_ACCURACY) 73 #define EPA_MAX_FACES (EPA_MAX_VERTICES * 2) 76 typedef unsigned int U;
77 typedef unsigned char U1;
97 m_enableMargin = enable;
247 for (
U i = 0; i < 4; ++i)
262 lastw[clastw = (clastw + 1) & 3] = w;
266 alpha =
btMax(omega, alpha);
301 for (
U i = 0, ni = cs.
rank; i < ni; ++i)
306 ns.
p[ns.
rank++] = weights[i];
307 m_ray += cs.
c[i]->
w * weights[i];
344 for (
U i = 0; i < 3; ++i)
360 for (
U i = 0; i < 3; ++i)
415 simplex.
p[simplex.
rank] = 0;
421 return (a.
y() * b.
z() * c.
x() + a.
z() * b.
x() * c.
y() -
422 a.
x() * b.
z() * c.
y() - a.
y() * b.
x() * c.
z() +
423 a.
x() * b.
y() * c.
z() - a.
z() * b.
y() * c.
x());
446 return (a.length2());
450 w[0] = 1 - (w[1] = t);
452 return ((a + d * t).length2());
462 static const U imd3[] = {1, 2, 0};
464 const btVector3 dl[] = {a - b, b - c, c - a};
472 for (
U i = 0; i < 3; ++i)
478 if ((mindist < 0) || (subd < mindist))
481 m =
static_cast<U>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0));
497 w[2] = 1 - (w[0] + w[1]);
509 static const U imd3[] = {1, 2, 0};
510 const btVector3* vt[] = {&a, &b, &c, &d};
511 const btVector3 dl[] = {a - d, b - d, c - d};
513 const bool ng = (vl *
btDot(a,
btCross(b - c, a - b))) <= 0;
519 for (
U i = 0; i < 3; ++i)
526 if ((mindist < 0) || (subd < mindist))
529 m =
static_cast<U>((subm & 1 ? 1 << i : 0) +
530 (subm & 2 ? 1 << j : 0) +
543 w[0] =
det(c, b, d) / vl;
544 w[1] =
det(a, c, d) / vl;
545 w[2] =
det(b, a, d) / vl;
546 w[3] = 1 - (w[0] + w[1] + w[2]);
624 face->
l[1] = list.
root;
631 if (face->l[1]) face->l[1]->l[0] = face->l[0];
632 if (face->l[0]) face->l[0]->l[1] = face->l[1];
633 if (face == list.root) list.root = face->l[1];
663 if (gjk.
det(simplex.
c[0]->
w - simplex.
c[3]->
w,
664 simplex.
c[1]->
w - simplex.
c[3]->
w,
665 simplex.
c[2]->
w - simplex.
c[3]->
w) < 0)
671 sFace* tetra[] = {
newface(simplex.
c[0], simplex.
c[1], simplex.
c[2],
true),
672 newface(simplex.
c[1], simplex.
c[0], simplex.
c[3],
true),
673 newface(simplex.
c[2], simplex.
c[1], simplex.
c[3],
true),
674 newface(simplex.
c[0], simplex.
c[2], simplex.
c[3],
true)};
681 bind(tetra[0], 0, tetra[1], 0);
682 bind(tetra[0], 1, tetra[2], 0);
683 bind(tetra[0], 2, tetra[3], 0);
684 bind(tetra[1], 1, tetra[3], 2);
685 bind(tetra[1], 2, tetra[2], 1);
686 bind(tetra[2], 2, tetra[3], 1);
695 best->
pass = (
U1)(++pass);
700 for (
U j = 0; (j < 3) && valid; ++j)
703 best->
f[j], best->
e[j],
706 if (valid && (horizon.
nf >= 3))
740 outer.
c[2]->
w - projection)
743 outer.
c[0]->
w - projection)
746 outer.
c[1]->
w - projection)
788 else if (b_dot_ba < 0)
853 for (
sFace* f = minf->
l[1]; f; f = f->
l[1])
866 static const U i1m3[] = {1, 2, 0};
867 static const U i2m3[] = {2, 0, 1};
870 const U e1 = i1m3[e];
878 bind(horizon.
cf, 1, nf, 2);
888 const U e2 = i2m3[e];
890 if (
expand(pass, w, f->
f[e1], f->
e[e1], horizon) &&
891 expand(pass, w, f->
f[e2], f->
e[e2], horizon))
933 return (
sizeof(
GJK) +
sizeof(
EPA));
945 Initialize(shape0, wtrs0, shape1, wtrs1, results, shape,
false);
982 Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, usemargins);
998 results.
status = sResults::Penetrating;
1006 results.
status = sResults::EPA_Failed;
1010 results.
status = sResults::GJK_Failed;
1030 Initialize(shape0, wtrs0, &shape1, wtrs1, results, shape,
false);
1052 return (
length - margin);
1058 if (Penetration(shape0, wtrs0, &shape1, wtrs1, gjk.
m_ray, results))
1080 if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results))
1081 return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results,
false));
1089 #undef GJK_MAX_ITERATIONS 1091 #undef GJK_MIN_DISTANCE 1092 #undef GJK_DUPLICATED_EPS 1093 #undef GJK_SIMPLEX2_EPS 1094 #undef GJK_SIMPLEX3_EPS 1095 #undef GJK_SIMPLEX4_EPS 1097 #undef EPA_MAX_VERTICES 1098 #undef EPA_MAX_FACES 1099 #undef EPA_MAX_ITERATIONS 1102 #undef EPA_PLANE_EPS 1103 #undef EPA_INSIDE_EPS btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
static T sum(const btAlignedObjectArray< T > &items)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
void appendvertice(sSimplex &simplex, const btVector3 &v)
btVector3 Support(const btVector3 &d) const
btScalar length2() const
Return the length of the vector squared.
static void bind(sFace *fa, U ea, sFace *fb, U eb)
btScalar btSqrt(btScalar y)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
sFace * newface(sSV *a, sSV *b, sSV *c, bool forced)
bool getedgedist(sFace *face, sSV *a, sSV *b, btScalar &dist)
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
eStatus::_ Evaluate(GJK &gjk, const btVector3 &guess)
static void Initialize(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, btGjkEpaSolver2::sResults &results, tShape &shape, bool withmargins)
sSV m_sv_store[EPA_MAX_VERTICES]
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
const btConvexShape * m_shapes[2]
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
btVector3 Support0(const btVector3 &d) const
const btScalar & x() const
Return the x value.
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
#define GJK_MAX_ITERATIONS
static int StackSizeRequirement()
const btScalar & y() const
Return the y value.
const btScalar & z() const
Return the z value.
static bool Penetration(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results, bool usemargins=true)
void removevertice(sSimplex &simplex)
btVector3 Support1(const btVector3 &d) const
void getsupport(const btVector3 &d, sSV &sv) const
#define EPA_MAX_ITERATIONS
sFace m_fc_store[EPA_MAX_FACES]
btVector3 can be used to represent 3D points and vectors.
btScalar getMarginNonVirtual() const
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
#define GJK_DUPLICATED_EPS
enum btGjkEpaSolver2::sResults::eStatus status
void EnableMargin(bool enable)
const T & btMax(const T &a, const T &b)
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
btVector3(btConvexShape::* Ls)(const btVector3 &) const
eStatus::_ Evaluate(const tShape &shapearg, const btVector3 &guess)
static void append(sList &list, sFace *face)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar length() const
Return the length of the vector.
btScalar btFabs(btScalar x)
btVector3 Support(const btVector3 &d, U index) const