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)
78 typedef unsigned int U;
79 typedef unsigned char U1;
82 template <
typename btConvexTemplate>
133 template <
typename btConvexTemplate>
162 GJK(
const btConvexTemplate&
a,
const btConvexTemplate&
b)
221 for (
U i = 0; i < 4; ++i)
236 lastw[clastw = (clastw + 1) & 3] =
w;
240 alpha =
btMax(omega, alpha);
275 for (
U i = 0, ni = cs.
rank; i < ni; ++i)
280 ns.
p[ns.
rank++] = weights[i];
281 m_ray += cs.
c[i]->
w * weights[i];
318 for (
U i = 0; i < 3; ++i)
334 for (
U i = 0; i < 3; ++i)
380 sv.
d = d / d.length();
389 simplex.
p[simplex.
rank] = 0;
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());
413 return (
b.length2());
420 return (
a.length2());
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)
452 if ((mindist < 0) || (subd < mindist))
455 m =
static_cast<U>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0));
467 mindist = p.length2();
471 w[2] = 1 - (
w[0] +
w[1]);
483 static const U imd3[] = {1, 2, 0};
493 for (
U i = 0; i < 3; ++i)
500 if ((mindist < 0) || (subd < mindist))
503 m =
static_cast<U>((subm & 1 ? 1 << i : 0) +
504 (subm & 2 ? 1 << j : 0) +
520 w[3] = 1 - (
w[0] +
w[1] +
w[2]);
543 template <
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];
637 if (gjk.
det(simplex.
c[0]->
w - simplex.
c[3]->
w,
638 simplex.
c[1]->
w - simplex.
c[3]->
w,
639 simplex.
c[2]->
w - simplex.
c[3]->
w) < 0)
645 sFace* tetra[] = {
newface(simplex.
c[0], simplex.
c[1], simplex.
c[2],
true),
646 newface(simplex.
c[1], simplex.
c[0], simplex.
c[3],
true),
647 newface(simplex.
c[2], simplex.
c[1], simplex.
c[3],
true),
648 newface(simplex.
c[0], simplex.
c[2], simplex.
c[3],
true)};
655 bind(tetra[0], 0, tetra[1], 0);
656 bind(tetra[0], 1, tetra[2], 0);
657 bind(tetra[0], 2, tetra[3], 0);
658 bind(tetra[1], 1, tetra[3], 2);
659 bind(tetra[1], 2, tetra[2], 1);
660 bind(tetra[2], 2, tetra[3], 1);
669 best->
pass = (
U1)(++pass);
674 for (
U j = 0; (j < 3) && valid; ++j)
677 best->
f[j], best->
e[j],
680 if (valid && (horizon.
nf >= 3))
714 outer.
c[2]->w - projection)
717 outer.
c[0]->w - projection)
720 outer.
c[1]->w - projection)
753 const btScalar ba_l2 = ba.length2();
760 dist =
a->w.length();
762 else if (b_dot_ba < 0)
765 dist =
b->w.length();
827 for (
sFace* f = minf->
l[1]; f; f = f->
l[1])
840 static const U i1m3[] = {1, 2, 0};
841 static const U i2m3[] = {2, 0, 1};
844 const U e1 = i1m3[
e];
852 bind(horizon.
cf, 1, nf, 2);
862 const U e2 = i2m3[
e];
864 if (
expand(pass,
w, f->
f[e1], f->
e[e1], horizon) &&
865 expand(pass,
w, f->
f[e2], f->
e[e2], horizon))
877 template <
typename btConvexTemplate>
878 static void Initialize(
const btConvexTemplate&
a,
const btConvexTemplate&
b,
888 shape.
m_toshape1 =
b.getWorldTransform().getBasis().transposeTimes(
a.getWorldTransform().getBasis());
889 shape.
m_toshape0 =
a.getWorldTransform().inverseTimes(
b.getWorldTransform());
897 template <
typename btConvexTemplate>
916 results.
witnesses[0] =
a.getWorldTransform() * w0;
917 results.
witnesses[1] =
a.getWorldTransform() * w1;
930 template <
typename btConvexTemplate>
932 const btConvexTemplate&
b,
949 for (
U i = 0; i < epa.
m_result.rank; ++i)
954 results.
witnesses[0] =
a.getWorldTransform() * w0;
975 int btComputeGjkEpaPenetration2(
const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
980 bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
981 colDesc.m_transformA,colDesc.m_transformB,
982 colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
991 distInfo->m_distance = results.
distance;
992 distInfo->m_normalBtoA = results.
normal;
994 btScalar lenSqr = tmpNormalInB.length2();
997 tmpNormalInB = results.
normal;
998 lenSqr = results.
normal.length2();
1003 tmpNormalInB /=
btSqrt(lenSqr);
1009 distInfo->m_pointOnA= results.
witnesses[0];
1010 distInfo->m_pointOnB= results.
witnesses[1];
1011 distInfo->m_normalBtoA= tmpNormalInB;
1023 template <
typename btConvexTemplate,
typename btDistanceInfoTemplate>
1035 distInfo->m_distance = results.
distance;
1036 distInfo->m_pointOnA = results.
witnesses[0];
1037 distInfo->m_pointOnB = results.
witnesses[1];
1038 distInfo->m_normalBtoA = results.
normal;
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
1056 #undef EPA_MAX_FACES
1057 #undef EPA_MAX_ITERATIONS
1060 #undef EPA_PLANE_EPS
1061 #undef EPA_INSIDE_EPS
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
ATTR_WARN_UNUSED_RESULT const BMVert * v
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
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
SIMD_FORCE_INLINE void btSwap(T &a, T &b)
static T sum(const btAlignedObjectArray< T > &items)
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
SIMD_FORCE_INLINE btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE btScalar distance2(const btVector3 &v) const
Return the distance squared between the ends of this and another vector This is symantically treating...
SIMD_FORCE_INLINE btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
BLI_INLINE float fb(float length, float L)
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
T length(const vec_base< T, Size > &a)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
GJK< btConvexTemplate >::sSV * c[3]
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
static void append(sList &list, sFace *face)
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