49#if defined(DEBUG) || defined(_DEBUG)
52#include <spu_printf.h>
53#define printf spu_printf
60#define GJK_MAX_ITERATIONS 128
61#define GJK_ACCURARY ((btScalar)0.0001)
62#define GJK_MIN_DISTANCE ((btScalar)0.0001)
63#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
64#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
65#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
66#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
69#define EPA_MAX_VERTICES 64
70#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2)
71#define EPA_MAX_ITERATIONS 255
72#define EPA_ACCURACY ((btScalar)0.0001)
73#define EPA_FALLBACK (10 * EPA_ACCURACY)
74#define EPA_PLANE_EPS ((btScalar)0.00001)
75#define EPA_INSIDE_EPS ((btScalar)0.01)
78typedef unsigned int U;
79typedef unsigned char U1;
82template <
typename btConvexTemplate>
133template <
typename btConvexTemplate>
221 for (
U i = 0; i < 4; ++i)
275 for (
U i = 0,
ni =
cs.rank; i <
ni; ++i)
318 for (
U i = 0; i < 3; ++i)
334 for (
U i = 0; i < 3; ++i)
395 return (a.
y() * b.
z() * c.
x() + a.
z() * b.
x() * c.
y() -
396 a.
x() * b.
z() * c.
y() - a.
y() * b.
x() * c.
z() +
397 a.
x() * b.
y() * c.
z() - a.
z() * b.
y() * c.
x());
424 w[0] = 1 - (w[1] =
t);
426 return ((a + d *
t).length2());
436 static const U imd3[] = {1, 2, 0};
446 for (
U i = 0; i < 3; ++i)
455 m =
static_cast<U>(((
subm & 1) ? 1 << i : 0) + ((
subm & 2) ? 1 <<
j : 0));
471 w[2] = 1 - (w[0] + w[1]);
483 static const U imd3[] = {1, 2, 0};
493 for (
U i = 0; i < 3; ++i)
503 m =
static_cast<U>((
subm & 1 ? 1 << i : 0) +
504 (
subm & 2 ? 1 <<
j : 0) +
517 w[0] =
det(c, b, d) /
vl;
518 w[1] =
det(a, c, d) /
vl;
519 w[2] =
det(b, a, d) /
vl;
520 w[3] = 1 - (w[0] + w[1] + w[2]);
543template <
typename btConvexTemplate>
598 face->
l[1] =
list.root;
605 if (face->
l[1]) face->
l[1]->
l[0] = face->
l[0];
606 if (face->
l[0]) face->
l[0]->
l[1] = face->
l[1];
607 if (face ==
list.root)
list.root = face->
l[1];
625 if ((
simplex.rank > 1) &&
gjk.EncloseOrigin())
669 best->pass = (
U1)(++pass);
840 static const U i1m3[] = {1, 2, 0};
841 static const U i2m3[] = {2, 0, 1};
877template <
typename btConvexTemplate>
897template <
typename btConvexTemplate>
910 for (
U i = 0; i <
gjk.m_simplex->rank; ++i)
916 results.witnesses[0] = a.getWorldTransform() *
w0;
917 results.witnesses[1] = a.getWorldTransform() *
w1;
930template <
typename btConvexTemplate>
949 for (
U i = 0; i <
epa.m_result.rank; ++i)
954 results.witnesses[0] = a.getWorldTransform() *
w0;
955 results.witnesses[1] = a.getWorldTransform() * (
w0 -
epa.m_normal *
epa.m_depth);
1023template <
typename btConvexTemplate,
typename btDistanceInfoTemplate>
1047#undef GJK_MAX_ITERATIONS
1049#undef GJK_MIN_DISTANCE
1050#undef GJK_DUPLICATED_EPS
1051#undef GJK_SIMPLEX2_EPS
1052#undef GJK_SIMPLEX3_EPS
1053#undef GJK_SIMPLEX4_EPS
1055#undef EPA_MAX_VERTICES
1057#undef EPA_MAX_ITERATIONS
1061#undef EPA_INSIDE_EPS
int btComputeGjkDistance(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btDistanceInfoTemplate *distInfo)
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
#define GJK_DUPLICATED_EPS
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
#define GJK_MAX_ITERATIONS
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
#define EPA_MAX_ITERATIONS
const T & btMax(const T &a, const T &b)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSqrt(btScalar y)
btScalar btFabs(btScalar x)
static T sum(const btAlignedObjectArray< T > &items)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
btVector3 can be used to represent 3D points and vectors.
const btScalar & w() const
Return the w value.
const btScalar & z() const
Return the z value.
btScalar length() const
Return the length of the vector.
btScalar distance(const btVector3 &v) const
Return the distance between the ends of this and another vector This is symantically treating the vec...
btScalar length2() const
Return the length of the vector squared.
const btScalar & x() const
Return the x value.
const btScalar & y() const
Return the y value.
GJK< btConvexTemplate >::sSV * c[3]
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
static void append(sList &list, sFace *face)
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
GJK< btConvexTemplate >::sSimplex m_result
GJK< btConvexTemplate >::sSV m_sv_store[EPA_MAX_VERTICES]
static void remove(sList &list, sFace *face)
eEpaStatus Evaluate(GJK< btConvexTemplate > &gjk, const btVector3 &guess)
static void bind(sFace *fa, U ea, sFace *fb, U eb)
sFace m_fc_store[EPA_MAX_FACES]
eGjkStatus Evaluate(const MinkowskiDiff< btConvexTemplate > &shapearg, const btVector3 &guess)
void getsupport(const btVector3 &d, sSV &sv) const
MinkowskiDiff< btConvexTemplate > m_shape
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
GJK(const btConvexTemplate &a, const btConvexTemplate &b)
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
void removevertice(sSimplex &simplex)
void appendvertice(sSimplex &simplex, const btVector3 &v)
btVector3 Support1(const btVector3 &d) const
btVector3 Support0(const btVector3 &d) const
void EnableMargin(bool enable)
btVector3 Support(const btVector3 &d) const
btVector3 Support(const btVector3 &d, U index) const
MinkowskiDiff(const btConvexTemplate &a, const btConvexTemplate &b)
const btConvexTemplate * m_convexBPtr
const btConvexTemplate * m_convexAPtr
enum btGjkEpaSolver3::sResults::eStatus status