mirror of
https://github.com/godotengine/godot.git
synced 2024-12-21 10:25:24 +08:00
1063 lines
26 KiB
C++
1063 lines
26 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the
|
|
use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it
|
|
freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not
|
|
claim that you wrote the original software. If you use this software in a
|
|
product, an acknowledgment in the product documentation would be appreciated
|
|
but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be
|
|
misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
/*
|
|
GJK-EPA collision solver by Nathanael Presson, 2008
|
|
*/
|
|
|
|
#include "b3GjkEpa.h"
|
|
|
|
#include "b3SupportMappings.h"
|
|
|
|
namespace gjkepa2_impl2
|
|
{
|
|
// Config
|
|
|
|
/* GJK */
|
|
#define GJK_MAX_ITERATIONS 128
|
|
#define GJK_ACCURACY ((b3Scalar)0.0001)
|
|
#define GJK_MIN_DISTANCE ((b3Scalar)0.0001)
|
|
#define GJK_DUPLICATED_EPS ((b3Scalar)0.0001)
|
|
#define GJK_SIMPLEX2_EPS ((b3Scalar)0.0)
|
|
#define GJK_SIMPLEX3_EPS ((b3Scalar)0.0)
|
|
#define GJK_SIMPLEX4_EPS ((b3Scalar)0.0)
|
|
|
|
/* EPA */
|
|
#define EPA_MAX_VERTICES 64
|
|
#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2)
|
|
#define EPA_MAX_ITERATIONS 255
|
|
#define EPA_ACCURACY ((b3Scalar)0.0001)
|
|
#define EPA_FALLBACK (10 * EPA_ACCURACY)
|
|
#define EPA_PLANE_EPS ((b3Scalar)0.00001)
|
|
#define EPA_INSIDE_EPS ((b3Scalar)0.01)
|
|
|
|
// Shorthands
|
|
|
|
// MinkowskiDiff
|
|
struct b3MinkowskiDiff
|
|
{
|
|
const b3ConvexPolyhedronData* m_shapes[2];
|
|
|
|
b3Matrix3x3 m_toshape1;
|
|
b3Transform m_toshape0;
|
|
|
|
bool m_enableMargin;
|
|
|
|
void EnableMargin(bool enable)
|
|
{
|
|
m_enableMargin = enable;
|
|
}
|
|
inline b3Vector3 Support0(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA) const
|
|
{
|
|
if (m_enableMargin)
|
|
{
|
|
return localGetSupportVertexWithMargin(d, m_shapes[0], verticesA, 0.f);
|
|
}
|
|
else
|
|
{
|
|
return localGetSupportVertexWithoutMargin(d, m_shapes[0], verticesA);
|
|
}
|
|
}
|
|
inline b3Vector3 Support1(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesB) const
|
|
{
|
|
if (m_enableMargin)
|
|
{
|
|
return m_toshape0 * (localGetSupportVertexWithMargin(m_toshape1 * d, m_shapes[1], verticesB, 0.f));
|
|
}
|
|
else
|
|
{
|
|
return m_toshape0 * (localGetSupportVertexWithoutMargin(m_toshape1 * d, m_shapes[1], verticesB));
|
|
}
|
|
}
|
|
|
|
inline b3Vector3 Support(const b3Vector3& d, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const
|
|
{
|
|
return (Support0(d, verticesA) - Support1(-d, verticesB));
|
|
}
|
|
b3Vector3 Support(const b3Vector3& d, unsigned int index, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB) const
|
|
{
|
|
if (index)
|
|
return (Support1(d, verticesA));
|
|
else
|
|
return (Support0(d, verticesB));
|
|
}
|
|
};
|
|
|
|
typedef b3MinkowskiDiff tShape;
|
|
|
|
// GJK
|
|
struct b3GJK
|
|
{
|
|
/* Types */
|
|
struct sSV
|
|
{
|
|
b3Vector3 d, w;
|
|
};
|
|
struct sSimplex
|
|
{
|
|
sSV* c[4];
|
|
b3Scalar p[4];
|
|
unsigned int rank;
|
|
};
|
|
struct eStatus
|
|
{
|
|
enum _
|
|
{
|
|
Valid,
|
|
Inside,
|
|
Failed
|
|
};
|
|
};
|
|
/* Fields */
|
|
tShape m_shape;
|
|
const b3AlignedObjectArray<b3Vector3>& m_verticesA;
|
|
const b3AlignedObjectArray<b3Vector3>& m_verticesB;
|
|
b3Vector3 m_ray;
|
|
b3Scalar m_distance;
|
|
sSimplex m_simplices[2];
|
|
sSV m_store[4];
|
|
sSV* m_free[4];
|
|
unsigned int m_nfree;
|
|
unsigned int m_current;
|
|
sSimplex* m_simplex;
|
|
eStatus::_ m_status;
|
|
/* Methods */
|
|
b3GJK(const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB)
|
|
: m_verticesA(verticesA), m_verticesB(verticesB)
|
|
{
|
|
Initialize();
|
|
}
|
|
void Initialize()
|
|
{
|
|
m_ray = b3MakeVector3(0, 0, 0);
|
|
m_nfree = 0;
|
|
m_status = eStatus::Failed;
|
|
m_current = 0;
|
|
m_distance = 0;
|
|
}
|
|
eStatus::_ Evaluate(const tShape& shapearg, const b3Vector3& guess)
|
|
{
|
|
unsigned int iterations = 0;
|
|
b3Scalar sqdist = 0;
|
|
b3Scalar alpha = 0;
|
|
b3Vector3 lastw[4];
|
|
unsigned int clastw = 0;
|
|
/* Initialize solver */
|
|
m_free[0] = &m_store[0];
|
|
m_free[1] = &m_store[1];
|
|
m_free[2] = &m_store[2];
|
|
m_free[3] = &m_store[3];
|
|
m_nfree = 4;
|
|
m_current = 0;
|
|
m_status = eStatus::Valid;
|
|
m_shape = shapearg;
|
|
m_distance = 0;
|
|
/* Initialize simplex */
|
|
m_simplices[0].rank = 0;
|
|
m_ray = guess;
|
|
const b3Scalar sqrl = m_ray.length2();
|
|
appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : b3MakeVector3(1, 0, 0));
|
|
m_simplices[0].p[0] = 1;
|
|
m_ray = m_simplices[0].c[0]->w;
|
|
sqdist = sqrl;
|
|
lastw[0] =
|
|
lastw[1] =
|
|
lastw[2] =
|
|
lastw[3] = m_ray;
|
|
/* Loop */
|
|
do
|
|
{
|
|
const unsigned int next = 1 - m_current;
|
|
sSimplex& cs = m_simplices[m_current];
|
|
sSimplex& ns = m_simplices[next];
|
|
/* Check zero */
|
|
const b3Scalar rl = m_ray.length();
|
|
if (rl < GJK_MIN_DISTANCE)
|
|
{ /* Touching or inside */
|
|
m_status = eStatus::Inside;
|
|
break;
|
|
}
|
|
/* Append new vertice in -'v' direction */
|
|
appendvertice(cs, -m_ray);
|
|
const b3Vector3& w = cs.c[cs.rank - 1]->w;
|
|
bool found = false;
|
|
for (unsigned int i = 0; i < 4; ++i)
|
|
{
|
|
if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS)
|
|
{
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (found)
|
|
{ /* Return old simplex */
|
|
removevertice(m_simplices[m_current]);
|
|
break;
|
|
}
|
|
else
|
|
{ /* Update lastw */
|
|
lastw[clastw = (clastw + 1) & 3] = w;
|
|
}
|
|
/* Check for termination */
|
|
const b3Scalar omega = b3Dot(m_ray, w) / rl;
|
|
alpha = b3Max(omega, alpha);
|
|
if (((rl - alpha) - (GJK_ACCURACY * rl)) <= 0)
|
|
{ /* Return old simplex */
|
|
removevertice(m_simplices[m_current]);
|
|
break;
|
|
}
|
|
/* Reduce simplex */
|
|
b3Scalar weights[4];
|
|
unsigned int mask = 0;
|
|
switch (cs.rank)
|
|
{
|
|
case 2:
|
|
sqdist = projectorigin(cs.c[0]->w,
|
|
cs.c[1]->w,
|
|
weights, mask);
|
|
break;
|
|
case 3:
|
|
sqdist = projectorigin(cs.c[0]->w,
|
|
cs.c[1]->w,
|
|
cs.c[2]->w,
|
|
weights, mask);
|
|
break;
|
|
case 4:
|
|
sqdist = projectorigin(cs.c[0]->w,
|
|
cs.c[1]->w,
|
|
cs.c[2]->w,
|
|
cs.c[3]->w,
|
|
weights, mask);
|
|
break;
|
|
}
|
|
if (sqdist >= 0)
|
|
{ /* Valid */
|
|
ns.rank = 0;
|
|
m_ray = b3MakeVector3(0, 0, 0);
|
|
m_current = next;
|
|
for (unsigned int i = 0, ni = cs.rank; i < ni; ++i)
|
|
{
|
|
if (mask & (1 << i))
|
|
{
|
|
ns.c[ns.rank] = cs.c[i];
|
|
ns.p[ns.rank++] = weights[i];
|
|
m_ray += cs.c[i]->w * weights[i];
|
|
}
|
|
else
|
|
{
|
|
m_free[m_nfree++] = cs.c[i];
|
|
}
|
|
}
|
|
if (mask == 15) m_status = eStatus::Inside;
|
|
}
|
|
else
|
|
{ /* Return old simplex */
|
|
removevertice(m_simplices[m_current]);
|
|
break;
|
|
}
|
|
m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed;
|
|
} while (m_status == eStatus::Valid);
|
|
m_simplex = &m_simplices[m_current];
|
|
switch (m_status)
|
|
{
|
|
case eStatus::Valid:
|
|
m_distance = m_ray.length();
|
|
break;
|
|
case eStatus::Inside:
|
|
m_distance = 0;
|
|
break;
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
return (m_status);
|
|
}
|
|
bool EncloseOrigin()
|
|
{
|
|
switch (m_simplex->rank)
|
|
{
|
|
case 1:
|
|
{
|
|
for (unsigned int i = 0; i < 3; ++i)
|
|
{
|
|
b3Vector3 axis = b3MakeVector3(0, 0, 0);
|
|
axis[i] = 1;
|
|
appendvertice(*m_simplex, axis);
|
|
if (EncloseOrigin()) return (true);
|
|
removevertice(*m_simplex);
|
|
appendvertice(*m_simplex, -axis);
|
|
if (EncloseOrigin()) return (true);
|
|
removevertice(*m_simplex);
|
|
}
|
|
}
|
|
break;
|
|
case 2:
|
|
{
|
|
const b3Vector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w;
|
|
for (unsigned int i = 0; i < 3; ++i)
|
|
{
|
|
b3Vector3 axis = b3MakeVector3(0, 0, 0);
|
|
axis[i] = 1;
|
|
const b3Vector3 p = b3Cross(d, axis);
|
|
if (p.length2() > 0)
|
|
{
|
|
appendvertice(*m_simplex, p);
|
|
if (EncloseOrigin()) return (true);
|
|
removevertice(*m_simplex);
|
|
appendvertice(*m_simplex, -p);
|
|
if (EncloseOrigin()) return (true);
|
|
removevertice(*m_simplex);
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
case 3:
|
|
{
|
|
const b3Vector3 n = b3Cross(m_simplex->c[1]->w - m_simplex->c[0]->w,
|
|
m_simplex->c[2]->w - m_simplex->c[0]->w);
|
|
if (n.length2() > 0)
|
|
{
|
|
appendvertice(*m_simplex, n);
|
|
if (EncloseOrigin()) return (true);
|
|
removevertice(*m_simplex);
|
|
appendvertice(*m_simplex, -n);
|
|
if (EncloseOrigin()) return (true);
|
|
removevertice(*m_simplex);
|
|
}
|
|
}
|
|
break;
|
|
case 4:
|
|
{
|
|
if (b3Fabs(det(m_simplex->c[0]->w - m_simplex->c[3]->w,
|
|
m_simplex->c[1]->w - m_simplex->c[3]->w,
|
|
m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0)
|
|
return (true);
|
|
}
|
|
break;
|
|
}
|
|
return (false);
|
|
}
|
|
/* Internals */
|
|
void getsupport(const b3Vector3& d, sSV& sv) const
|
|
{
|
|
sv.d = d / d.length();
|
|
sv.w = m_shape.Support(sv.d, m_verticesA, m_verticesB);
|
|
}
|
|
void removevertice(sSimplex& simplex)
|
|
{
|
|
m_free[m_nfree++] = simplex.c[--simplex.rank];
|
|
}
|
|
void appendvertice(sSimplex& simplex, const b3Vector3& v)
|
|
{
|
|
simplex.p[simplex.rank] = 0;
|
|
simplex.c[simplex.rank] = m_free[--m_nfree];
|
|
getsupport(v, *simplex.c[simplex.rank++]);
|
|
}
|
|
static b3Scalar det(const b3Vector3& a, const b3Vector3& b, const b3Vector3& c)
|
|
{
|
|
return (a.y * b.z * c.x + a.z * b.x * c.y -
|
|
a.x * b.z * c.y - a.y * b.x * c.z +
|
|
a.x * b.y * c.z - a.z * b.y * c.x);
|
|
}
|
|
static b3Scalar projectorigin(const b3Vector3& a,
|
|
const b3Vector3& b,
|
|
b3Scalar* w, unsigned int& m)
|
|
{
|
|
const b3Vector3 d = b - a;
|
|
const b3Scalar l = d.length2();
|
|
if (l > GJK_SIMPLEX2_EPS)
|
|
{
|
|
const b3Scalar t(l > 0 ? -b3Dot(a, d) / l : 0);
|
|
if (t >= 1)
|
|
{
|
|
w[0] = 0;
|
|
w[1] = 1;
|
|
m = 2;
|
|
return (b.length2());
|
|
}
|
|
else if (t <= 0)
|
|
{
|
|
w[0] = 1;
|
|
w[1] = 0;
|
|
m = 1;
|
|
return (a.length2());
|
|
}
|
|
else
|
|
{
|
|
w[0] = 1 - (w[1] = t);
|
|
m = 3;
|
|
return ((a + d * t).length2());
|
|
}
|
|
}
|
|
return (-1);
|
|
}
|
|
static b3Scalar projectorigin(const b3Vector3& a,
|
|
const b3Vector3& b,
|
|
const b3Vector3& c,
|
|
b3Scalar* w, unsigned int& m)
|
|
{
|
|
static const unsigned int imd3[] = {1, 2, 0};
|
|
const b3Vector3* vt[] = {&a, &b, &c};
|
|
const b3Vector3 dl[] = {a - b, b - c, c - a};
|
|
const b3Vector3 n = b3Cross(dl[0], dl[1]);
|
|
const b3Scalar l = n.length2();
|
|
if (l > GJK_SIMPLEX3_EPS)
|
|
{
|
|
b3Scalar mindist = -1;
|
|
b3Scalar subw[2] = {0.f, 0.f};
|
|
unsigned int subm(0);
|
|
for (unsigned int i = 0; i < 3; ++i)
|
|
{
|
|
if (b3Dot(*vt[i], b3Cross(dl[i], n)) > 0)
|
|
{
|
|
const unsigned int j = imd3[i];
|
|
const b3Scalar subd(projectorigin(*vt[i], *vt[j], subw, subm));
|
|
if ((mindist < 0) || (subd < mindist))
|
|
{
|
|
mindist = subd;
|
|
m = static_cast<unsigned int>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0));
|
|
w[i] = subw[0];
|
|
w[j] = subw[1];
|
|
w[imd3[j]] = 0;
|
|
}
|
|
}
|
|
}
|
|
if (mindist < 0)
|
|
{
|
|
const b3Scalar d = b3Dot(a, n);
|
|
const b3Scalar s = b3Sqrt(l);
|
|
const b3Vector3 p = n * (d / l);
|
|
mindist = p.length2();
|
|
m = 7;
|
|
w[0] = (b3Cross(dl[1], b - p)).length() / s;
|
|
w[1] = (b3Cross(dl[2], c - p)).length() / s;
|
|
w[2] = 1 - (w[0] + w[1]);
|
|
}
|
|
return (mindist);
|
|
}
|
|
return (-1);
|
|
}
|
|
static b3Scalar projectorigin(const b3Vector3& a,
|
|
const b3Vector3& b,
|
|
const b3Vector3& c,
|
|
const b3Vector3& d,
|
|
b3Scalar* w, unsigned int& m)
|
|
{
|
|
static const unsigned int imd3[] = {1, 2, 0};
|
|
const b3Vector3* vt[] = {&a, &b, &c, &d};
|
|
const b3Vector3 dl[] = {a - d, b - d, c - d};
|
|
const b3Scalar vl = det(dl[0], dl[1], dl[2]);
|
|
const bool ng = (vl * b3Dot(a, b3Cross(b - c, a - b))) <= 0;
|
|
if (ng && (b3Fabs(vl) > GJK_SIMPLEX4_EPS))
|
|
{
|
|
b3Scalar mindist = -1;
|
|
b3Scalar subw[3] = {0.f, 0.f, 0.f};
|
|
unsigned int subm(0);
|
|
for (unsigned int i = 0; i < 3; ++i)
|
|
{
|
|
const unsigned int j = imd3[i];
|
|
const b3Scalar s = vl * b3Dot(d, b3Cross(dl[i], dl[j]));
|
|
if (s > 0)
|
|
{
|
|
const b3Scalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm);
|
|
if ((mindist < 0) || (subd < mindist))
|
|
{
|
|
mindist = subd;
|
|
m = static_cast<unsigned int>((subm & 1 ? 1 << i : 0) +
|
|
(subm & 2 ? 1 << j : 0) +
|
|
(subm & 4 ? 8 : 0));
|
|
w[i] = subw[0];
|
|
w[j] = subw[1];
|
|
w[imd3[j]] = 0;
|
|
w[3] = subw[2];
|
|
}
|
|
}
|
|
}
|
|
if (mindist < 0)
|
|
{
|
|
mindist = 0;
|
|
m = 15;
|
|
w[0] = det(c, b, d) / vl;
|
|
w[1] = det(a, c, d) / vl;
|
|
w[2] = det(b, a, d) / vl;
|
|
w[3] = 1 - (w[0] + w[1] + w[2]);
|
|
}
|
|
return (mindist);
|
|
}
|
|
return (-1);
|
|
}
|
|
};
|
|
|
|
// EPA
|
|
struct b3EPA
|
|
{
|
|
/* Types */
|
|
typedef b3GJK::sSV sSV;
|
|
struct sFace
|
|
{
|
|
b3Vector3 n;
|
|
b3Scalar d;
|
|
sSV* c[3];
|
|
sFace* f[3];
|
|
sFace* l[2];
|
|
unsigned char e[3];
|
|
unsigned char pass;
|
|
};
|
|
struct sList
|
|
{
|
|
sFace* root;
|
|
unsigned int count;
|
|
sList() : root(0), count(0) {}
|
|
};
|
|
struct sHorizon
|
|
{
|
|
sFace* cf;
|
|
sFace* ff;
|
|
unsigned int nf;
|
|
sHorizon() : cf(0), ff(0), nf(0) {}
|
|
};
|
|
struct eStatus
|
|
{
|
|
enum _
|
|
{
|
|
Valid,
|
|
Touching,
|
|
Degenerated,
|
|
NonConvex,
|
|
InvalidHull,
|
|
OutOfFaces,
|
|
OutOfVertices,
|
|
AccuraryReached,
|
|
FallBack,
|
|
Failed
|
|
};
|
|
};
|
|
/* Fields */
|
|
eStatus::_ m_status;
|
|
b3GJK::sSimplex m_result;
|
|
b3Vector3 m_normal;
|
|
b3Scalar m_depth;
|
|
sSV m_sv_store[EPA_MAX_VERTICES];
|
|
sFace m_fc_store[EPA_MAX_FACES];
|
|
unsigned int m_nextsv;
|
|
sList m_hull;
|
|
sList m_stock;
|
|
/* Methods */
|
|
b3EPA()
|
|
{
|
|
Initialize();
|
|
}
|
|
|
|
static inline void bind(sFace* fa, unsigned int ea, sFace* fb, unsigned int eb)
|
|
{
|
|
fa->e[ea] = (unsigned char)eb;
|
|
fa->f[ea] = fb;
|
|
fb->e[eb] = (unsigned char)ea;
|
|
fb->f[eb] = fa;
|
|
}
|
|
static inline void append(sList& list, sFace* face)
|
|
{
|
|
face->l[0] = 0;
|
|
face->l[1] = list.root;
|
|
if (list.root) list.root->l[0] = face;
|
|
list.root = face;
|
|
++list.count;
|
|
}
|
|
static inline void remove(sList& list, sFace* face)
|
|
{
|
|
if (face->l[1]) face->l[1]->l[0] = face->l[0];
|
|
if (face->l[0]) face->l[0]->l[1] = face->l[1];
|
|
if (face == list.root) list.root = face->l[1];
|
|
--list.count;
|
|
}
|
|
|
|
void Initialize()
|
|
{
|
|
m_status = eStatus::Failed;
|
|
m_normal = b3MakeVector3(0, 0, 0);
|
|
m_depth = 0;
|
|
m_nextsv = 0;
|
|
for (unsigned int i = 0; i < EPA_MAX_FACES; ++i)
|
|
{
|
|
append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]);
|
|
}
|
|
}
|
|
eStatus::_ Evaluate(b3GJK& gjk, const b3Vector3& guess)
|
|
{
|
|
b3GJK::sSimplex& simplex = *gjk.m_simplex;
|
|
if ((simplex.rank > 1) && gjk.EncloseOrigin())
|
|
{
|
|
/* Clean up */
|
|
while (m_hull.root)
|
|
{
|
|
sFace* f = m_hull.root;
|
|
remove(m_hull, f);
|
|
append(m_stock, f);
|
|
}
|
|
m_status = eStatus::Valid;
|
|
m_nextsv = 0;
|
|
/* Orient simplex */
|
|
if (gjk.det(simplex.c[0]->w - simplex.c[3]->w,
|
|
simplex.c[1]->w - simplex.c[3]->w,
|
|
simplex.c[2]->w - simplex.c[3]->w) < 0)
|
|
{
|
|
b3Swap(simplex.c[0], simplex.c[1]);
|
|
b3Swap(simplex.p[0], simplex.p[1]);
|
|
}
|
|
/* Build initial hull */
|
|
sFace* tetra[] = {newface(simplex.c[0], simplex.c[1], simplex.c[2], true),
|
|
newface(simplex.c[1], simplex.c[0], simplex.c[3], true),
|
|
newface(simplex.c[2], simplex.c[1], simplex.c[3], true),
|
|
newface(simplex.c[0], simplex.c[2], simplex.c[3], true)};
|
|
if (m_hull.count == 4)
|
|
{
|
|
sFace* best = findbest();
|
|
sFace outer = *best;
|
|
unsigned int pass = 0;
|
|
unsigned int iterations = 0;
|
|
bind(tetra[0], 0, tetra[1], 0);
|
|
bind(tetra[0], 1, tetra[2], 0);
|
|
bind(tetra[0], 2, tetra[3], 0);
|
|
bind(tetra[1], 1, tetra[3], 2);
|
|
bind(tetra[1], 2, tetra[2], 1);
|
|
bind(tetra[2], 2, tetra[3], 1);
|
|
m_status = eStatus::Valid;
|
|
for (; iterations < EPA_MAX_ITERATIONS; ++iterations)
|
|
{
|
|
if (m_nextsv < EPA_MAX_VERTICES)
|
|
{
|
|
sHorizon horizon;
|
|
sSV* w = &m_sv_store[m_nextsv++];
|
|
bool valid = true;
|
|
best->pass = (unsigned char)(++pass);
|
|
gjk.getsupport(best->n, *w);
|
|
const b3Scalar wdist = b3Dot(best->n, w->w) - best->d;
|
|
if (wdist > EPA_ACCURACY)
|
|
{
|
|
for (unsigned int j = 0; (j < 3) && valid; ++j)
|
|
{
|
|
valid &= expand(pass, w,
|
|
best->f[j], best->e[j],
|
|
horizon);
|
|
}
|
|
if (valid && (horizon.nf >= 3))
|
|
{
|
|
bind(horizon.cf, 1, horizon.ff, 2);
|
|
remove(m_hull, best);
|
|
append(m_stock, best);
|
|
best = findbest();
|
|
outer = *best;
|
|
}
|
|
else
|
|
{
|
|
m_status = eStatus::Failed;
|
|
//m_status=eStatus::InvalidHull;
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_status = eStatus::AccuraryReached;
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_status = eStatus::OutOfVertices;
|
|
break;
|
|
}
|
|
}
|
|
const b3Vector3 projection = outer.n * outer.d;
|
|
m_normal = outer.n;
|
|
m_depth = outer.d;
|
|
m_result.rank = 3;
|
|
m_result.c[0] = outer.c[0];
|
|
m_result.c[1] = outer.c[1];
|
|
m_result.c[2] = outer.c[2];
|
|
m_result.p[0] = b3Cross(outer.c[1]->w - projection,
|
|
outer.c[2]->w - projection)
|
|
.length();
|
|
m_result.p[1] = b3Cross(outer.c[2]->w - projection,
|
|
outer.c[0]->w - projection)
|
|
.length();
|
|
m_result.p[2] = b3Cross(outer.c[0]->w - projection,
|
|
outer.c[1]->w - projection)
|
|
.length();
|
|
const b3Scalar sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
|
|
m_result.p[0] /= sum;
|
|
m_result.p[1] /= sum;
|
|
m_result.p[2] /= sum;
|
|
return (m_status);
|
|
}
|
|
}
|
|
/* Fallback */
|
|
m_status = eStatus::FallBack;
|
|
m_normal = -guess;
|
|
const b3Scalar nl = m_normal.length();
|
|
if (nl > 0)
|
|
m_normal = m_normal / nl;
|
|
else
|
|
m_normal = b3MakeVector3(1, 0, 0);
|
|
m_depth = 0;
|
|
m_result.rank = 1;
|
|
m_result.c[0] = simplex.c[0];
|
|
m_result.p[0] = 1;
|
|
return (m_status);
|
|
}
|
|
bool getedgedist(sFace* face, sSV* a, sSV* b, b3Scalar& dist)
|
|
{
|
|
const b3Vector3 ba = b->w - a->w;
|
|
const b3Vector3 n_ab = b3Cross(ba, face->n); // Outward facing edge normal direction, on triangle plane
|
|
const b3Scalar a_dot_nab = b3Dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
|
|
|
|
if (a_dot_nab < 0)
|
|
{
|
|
// Outside of edge a->b
|
|
|
|
const b3Scalar ba_l2 = ba.length2();
|
|
const b3Scalar a_dot_ba = b3Dot(a->w, ba);
|
|
const b3Scalar b_dot_ba = b3Dot(b->w, ba);
|
|
|
|
if (a_dot_ba > 0)
|
|
{
|
|
// Pick distance vertex a
|
|
dist = a->w.length();
|
|
}
|
|
else if (b_dot_ba < 0)
|
|
{
|
|
// Pick distance vertex b
|
|
dist = b->w.length();
|
|
}
|
|
else
|
|
{
|
|
// Pick distance to edge a->b
|
|
const b3Scalar a_dot_b = b3Dot(a->w, b->w);
|
|
dist = b3Sqrt(b3Max((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (b3Scalar)0));
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
sFace* newface(sSV* a, sSV* b, sSV* c, bool forced)
|
|
{
|
|
if (m_stock.root)
|
|
{
|
|
sFace* face = m_stock.root;
|
|
remove(m_stock, face);
|
|
append(m_hull, face);
|
|
face->pass = 0;
|
|
face->c[0] = a;
|
|
face->c[1] = b;
|
|
face->c[2] = c;
|
|
face->n = b3Cross(b->w - a->w, c->w - a->w);
|
|
const b3Scalar l = face->n.length();
|
|
const bool v = l > EPA_ACCURACY;
|
|
|
|
if (v)
|
|
{
|
|
if (!(getedgedist(face, a, b, face->d) ||
|
|
getedgedist(face, b, c, face->d) ||
|
|
getedgedist(face, c, a, face->d)))
|
|
{
|
|
// Origin projects to the interior of the triangle
|
|
// Use distance to triangle plane
|
|
face->d = b3Dot(a->w, face->n) / l;
|
|
}
|
|
|
|
face->n /= l;
|
|
if (forced || (face->d >= -EPA_PLANE_EPS))
|
|
{
|
|
return face;
|
|
}
|
|
else
|
|
m_status = eStatus::NonConvex;
|
|
}
|
|
else
|
|
m_status = eStatus::Degenerated;
|
|
|
|
remove(m_hull, face);
|
|
append(m_stock, face);
|
|
return 0;
|
|
}
|
|
m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
|
|
return 0;
|
|
}
|
|
sFace* findbest()
|
|
{
|
|
sFace* minf = m_hull.root;
|
|
b3Scalar mind = minf->d * minf->d;
|
|
for (sFace* f = minf->l[1]; f; f = f->l[1])
|
|
{
|
|
const b3Scalar sqd = f->d * f->d;
|
|
if (sqd < mind)
|
|
{
|
|
minf = f;
|
|
mind = sqd;
|
|
}
|
|
}
|
|
return (minf);
|
|
}
|
|
bool expand(unsigned int pass, sSV* w, sFace* f, unsigned int e, sHorizon& horizon)
|
|
{
|
|
static const unsigned int i1m3[] = {1, 2, 0};
|
|
static const unsigned int i2m3[] = {2, 0, 1};
|
|
if (f->pass != pass)
|
|
{
|
|
const unsigned int e1 = i1m3[e];
|
|
if ((b3Dot(f->n, w->w) - f->d) < -EPA_PLANE_EPS)
|
|
{
|
|
sFace* nf = newface(f->c[e1], f->c[e], w, false);
|
|
if (nf)
|
|
{
|
|
bind(nf, 0, f, e);
|
|
if (horizon.cf)
|
|
bind(horizon.cf, 1, nf, 2);
|
|
else
|
|
horizon.ff = nf;
|
|
horizon.cf = nf;
|
|
++horizon.nf;
|
|
return (true);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
const unsigned int e2 = i2m3[e];
|
|
f->pass = (unsigned char)pass;
|
|
if (expand(pass, w, f->f[e1], f->e[e1], horizon) &&
|
|
expand(pass, w, f->f[e2], f->e[e2], horizon))
|
|
{
|
|
remove(m_hull, f);
|
|
append(m_stock, f);
|
|
return (true);
|
|
}
|
|
}
|
|
}
|
|
return (false);
|
|
}
|
|
};
|
|
|
|
//
|
|
static void Initialize(const b3Transform& transA, const b3Transform& transB,
|
|
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
|
b3GjkEpaSolver2::sResults& results,
|
|
tShape& shape,
|
|
bool withmargins)
|
|
{
|
|
/* Results */
|
|
results.witnesses[0] =
|
|
results.witnesses[1] = b3MakeVector3(0, 0, 0);
|
|
results.status = b3GjkEpaSolver2::sResults::Separated;
|
|
/* Shape */
|
|
shape.m_shapes[0] = hullA;
|
|
shape.m_shapes[1] = hullB;
|
|
shape.m_toshape1 = transB.getBasis().transposeTimes(transA.getBasis());
|
|
shape.m_toshape0 = transA.inverseTimes(transB);
|
|
shape.EnableMargin(withmargins);
|
|
}
|
|
|
|
} // namespace gjkepa2_impl2
|
|
|
|
//
|
|
// Api
|
|
//
|
|
|
|
using namespace gjkepa2_impl2;
|
|
|
|
//
|
|
int b3GjkEpaSolver2::StackSizeRequirement()
|
|
{
|
|
return (sizeof(b3GJK) + sizeof(b3EPA));
|
|
}
|
|
|
|
//
|
|
bool b3GjkEpaSolver2::Distance(const b3Transform& transA, const b3Transform& transB,
|
|
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
|
const b3Vector3& guess,
|
|
sResults& results)
|
|
{
|
|
tShape shape;
|
|
Initialize(transA, transB, hullA, hullB, verticesA, verticesB, results, shape, false);
|
|
b3GJK gjk(verticesA, verticesB);
|
|
b3GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess);
|
|
if (gjk_status == b3GJK::eStatus::Valid)
|
|
{
|
|
b3Vector3 w0 = b3MakeVector3(0, 0, 0);
|
|
b3Vector3 w1 = b3MakeVector3(0, 0, 0);
|
|
for (unsigned int i = 0; i < gjk.m_simplex->rank; ++i)
|
|
{
|
|
const b3Scalar p = gjk.m_simplex->p[i];
|
|
w0 += shape.Support(gjk.m_simplex->c[i]->d, 0, verticesA, verticesB) * p;
|
|
w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1, verticesA, verticesB) * p;
|
|
}
|
|
results.witnesses[0] = transA * w0;
|
|
results.witnesses[1] = transA * w1;
|
|
results.normal = w0 - w1;
|
|
results.distance = results.normal.length();
|
|
results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
|
|
return (true);
|
|
}
|
|
else
|
|
{
|
|
results.status = gjk_status == b3GJK::eStatus::Inside ? sResults::Penetrating : sResults::GJK_Failed;
|
|
return (false);
|
|
}
|
|
}
|
|
|
|
//
|
|
bool b3GjkEpaSolver2::Penetration(const b3Transform& transA, const b3Transform& transB,
|
|
const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesB,
|
|
const b3Vector3& guess,
|
|
sResults& results,
|
|
bool usemargins)
|
|
{
|
|
tShape shape;
|
|
Initialize(transA, transB, hullA, hullB, verticesA, verticesB, results, shape, usemargins);
|
|
b3GJK gjk(verticesA, verticesB);
|
|
b3GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess);
|
|
switch (gjk_status)
|
|
{
|
|
case b3GJK::eStatus::Inside:
|
|
{
|
|
b3EPA epa;
|
|
b3EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess);
|
|
if (epa_status != b3EPA::eStatus::Failed)
|
|
{
|
|
b3Vector3 w0 = b3MakeVector3(0, 0, 0);
|
|
for (unsigned int i = 0; i < epa.m_result.rank; ++i)
|
|
{
|
|
w0 += shape.Support(epa.m_result.c[i]->d, 0, verticesA, verticesB) * epa.m_result.p[i];
|
|
}
|
|
results.status = sResults::Penetrating;
|
|
results.witnesses[0] = transA * w0;
|
|
results.witnesses[1] = transA * (w0 - epa.m_normal * epa.m_depth);
|
|
results.normal = -epa.m_normal;
|
|
results.distance = -epa.m_depth;
|
|
return (true);
|
|
}
|
|
else
|
|
results.status = sResults::EPA_Failed;
|
|
}
|
|
break;
|
|
case b3GJK::eStatus::Failed:
|
|
results.status = sResults::GJK_Failed;
|
|
break;
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
return (false);
|
|
}
|
|
|
|
#if 0
|
|
//
|
|
b3Scalar b3GjkEpaSolver2::SignedDistance(const b3Vector3& position,
|
|
b3Scalar margin,
|
|
const b3Transform& transA,
|
|
const b3ConvexPolyhedronData& hullA,
|
|
const b3AlignedObjectArray<b3Vector3>& verticesA,
|
|
sResults& results)
|
|
{
|
|
tShape shape;
|
|
btSphereShape shape1(margin);
|
|
b3Transform wtrs1(b3Quaternion(0,0,0,1),position);
|
|
Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
|
|
GJK gjk;
|
|
GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,b3Vector3(1,1,1));
|
|
if(gjk_status==GJK::eStatus::Valid)
|
|
{
|
|
b3Vector3 w0=b3Vector3(0,0,0);
|
|
b3Vector3 w1=b3Vector3(0,0,0);
|
|
for(unsigned int i=0;i<gjk.m_simplex->rank;++i)
|
|
{
|
|
const b3Scalar p=gjk.m_simplex->p[i];
|
|
w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
|
|
w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
|
|
}
|
|
results.witnesses[0] = wtrs0*w0;
|
|
results.witnesses[1] = wtrs0*w1;
|
|
const b3Vector3 delta= results.witnesses[1]-
|
|
results.witnesses[0];
|
|
const b3Scalar margin= shape0->getMarginNonVirtual()+
|
|
shape1.getMarginNonVirtual();
|
|
const b3Scalar length= delta.length();
|
|
results.normal = delta/length;
|
|
results.witnesses[0] += results.normal*margin;
|
|
return(length-margin);
|
|
}
|
|
else
|
|
{
|
|
if(gjk_status==GJK::eStatus::Inside)
|
|
{
|
|
if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
|
|
{
|
|
const b3Vector3 delta= results.witnesses[0]-
|
|
results.witnesses[1];
|
|
const b3Scalar length= delta.length();
|
|
if (length >= B3_EPSILON)
|
|
results.normal = delta/length;
|
|
return(-length);
|
|
}
|
|
}
|
|
}
|
|
return(B3_INFINITY);
|
|
}
|
|
|
|
//
|
|
bool b3GjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
|
|
const b3Transform& wtrs0,
|
|
const btConvexShape* shape1,
|
|
const b3Transform& wtrs1,
|
|
const b3Vector3& guess,
|
|
sResults& results)
|
|
{
|
|
if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
|
|
return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
|
|
else
|
|
return(true);
|
|
}
|
|
#endif
|
|
|
|
/* Symbols cleanup */
|
|
|
|
#undef GJK_MAX_ITERATIONS
|
|
#undef GJK_ACCURACY
|
|
#undef GJK_MIN_DISTANCE
|
|
#undef GJK_DUPLICATED_EPS
|
|
#undef GJK_SIMPLEX2_EPS
|
|
#undef GJK_SIMPLEX3_EPS
|
|
#undef GJK_SIMPLEX4_EPS
|
|
|
|
#undef EPA_MAX_VERTICES
|
|
#undef EPA_MAX_FACES
|
|
#undef EPA_MAX_ITERATIONS
|
|
#undef EPA_ACCURACY
|
|
#undef EPA_FALLBACK
|
|
#undef EPA_PLANE_EPS
|
|
#undef EPA_INSIDE_EPS
|