846 lines
23 KiB
C++
846 lines
23 KiB
C++
/*************************************************************************/
|
|
/* gjk_epa.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* http://www.godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
|
/* a copy of this software and associated documentation files (the */
|
|
/* "Software"), to deal in the Software without restriction, including */
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
|
/* the following conditions: */
|
|
/* */
|
|
/* The above copyright notice and this permission notice shall be */
|
|
/* included in all copies or substantial portions of the Software. */
|
|
/* */
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
|
/*************************************************************************/
|
|
#include "gjk_epa.h"
|
|
|
|
/*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/
|
|
|
|
// Config
|
|
|
|
/* GJK */
|
|
#define GJK_MAX_ITERATIONS 128
|
|
#define GJK_ACCURARY ((real_t)0.0001)
|
|
#define GJK_MIN_DISTANCE ((real_t)0.0001)
|
|
#define GJK_DUPLICATED_EPS ((real_t)0.0001)
|
|
#define GJK_SIMPLEX2_EPS ((real_t)0.0)
|
|
#define GJK_SIMPLEX3_EPS ((real_t)0.0)
|
|
#define GJK_SIMPLEX4_EPS ((real_t)0.0)
|
|
|
|
/* EPA */
|
|
#define EPA_MAX_VERTICES 64
|
|
#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2)
|
|
#define EPA_MAX_ITERATIONS 255
|
|
#define EPA_ACCURACY ((real_t)0.0001)
|
|
#define EPA_FALLBACK (10 * EPA_ACCURACY)
|
|
#define EPA_PLANE_EPS ((real_t)0.00001)
|
|
#define EPA_INSIDE_EPS ((real_t)0.01)
|
|
|
|
namespace GjkEpa2 {
|
|
|
|
struct sResults {
|
|
enum eStatus {
|
|
Separated, /* Shapes doesnt penetrate */
|
|
Penetrating, /* Shapes are penetrating */
|
|
GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
|
|
EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
|
|
} status;
|
|
|
|
Vector3 witnesses[2];
|
|
Vector3 normal;
|
|
real_t distance;
|
|
};
|
|
|
|
// Shorthands
|
|
typedef unsigned int U;
|
|
typedef unsigned char U1;
|
|
|
|
// MinkowskiDiff
|
|
struct MinkowskiDiff {
|
|
|
|
const ShapeSW *m_shapes[2];
|
|
|
|
Transform transform_A;
|
|
Transform transform_B;
|
|
|
|
// i wonder how this could be sped up... if it can
|
|
_FORCE_INLINE_ Vector3 Support0(const Vector3 &d) const {
|
|
return transform_A.xform(m_shapes[0]->get_support(transform_A.basis.xform_inv(d).normalized()));
|
|
}
|
|
|
|
_FORCE_INLINE_ Vector3 Support1(const Vector3 &d) const {
|
|
return transform_B.xform(m_shapes[1]->get_support(transform_B.basis.xform_inv(d).normalized()));
|
|
}
|
|
|
|
_FORCE_INLINE_ Vector3 Support(const Vector3 &d) const {
|
|
return (Support0(d) - Support1(-d));
|
|
}
|
|
|
|
_FORCE_INLINE_ Vector3 Support(const Vector3 &d, U index) const {
|
|
if (index)
|
|
return (Support1(d));
|
|
else
|
|
return (Support0(d));
|
|
}
|
|
};
|
|
|
|
typedef MinkowskiDiff tShape;
|
|
|
|
// GJK
|
|
struct GJK {
|
|
/* Types */
|
|
struct sSV {
|
|
Vector3 d, w;
|
|
};
|
|
struct sSimplex {
|
|
sSV *c[4];
|
|
real_t p[4];
|
|
U rank;
|
|
};
|
|
struct eStatus {
|
|
enum _ {
|
|
Valid,
|
|
Inside,
|
|
Failed
|
|
};
|
|
};
|
|
/* Fields */
|
|
tShape m_shape;
|
|
Vector3 m_ray;
|
|
real_t m_distance;
|
|
sSimplex m_simplices[2];
|
|
sSV m_store[4];
|
|
sSV *m_free[4];
|
|
U m_nfree;
|
|
U m_current;
|
|
sSimplex *m_simplex;
|
|
eStatus::_ m_status;
|
|
/* Methods */
|
|
GJK() {
|
|
Initialize();
|
|
}
|
|
void Initialize() {
|
|
m_ray = Vector3(0, 0, 0);
|
|
m_nfree = 0;
|
|
m_status = eStatus::Failed;
|
|
m_current = 0;
|
|
m_distance = 0;
|
|
}
|
|
eStatus::_ Evaluate(const tShape &shapearg, const Vector3 &guess) {
|
|
U iterations = 0;
|
|
real_t sqdist = 0;
|
|
real_t alpha = 0;
|
|
Vector3 lastw[4];
|
|
U 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 real_t sqrl = m_ray.length_squared();
|
|
appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : Vector3(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 U next = 1 - m_current;
|
|
sSimplex &cs = m_simplices[m_current];
|
|
sSimplex &ns = m_simplices[next];
|
|
/* Check zero */
|
|
const real_t 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 Vector3 &w = cs.c[cs.rank - 1]->w;
|
|
bool found = false;
|
|
for (U i = 0; i < 4; ++i) {
|
|
if ((w - lastw[i]).length_squared() < 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 real_t omega = vec3_dot(m_ray, w) / rl;
|
|
alpha = MAX(omega, alpha);
|
|
if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0) { /* Return old simplex */
|
|
removevertice(m_simplices[m_current]);
|
|
break;
|
|
}
|
|
/* Reduce simplex */
|
|
real_t weights[4];
|
|
U 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 = Vector3(0, 0, 0);
|
|
m_current = next;
|
|
for (U 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 (U i = 0; i < 3; ++i) {
|
|
Vector3 axis = Vector3(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 Vector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w;
|
|
for (U i = 0; i < 3; ++i) {
|
|
Vector3 axis = Vector3(0, 0, 0);
|
|
axis[i] = 1;
|
|
const Vector3 p = vec3_cross(d, axis);
|
|
if (p.length_squared() > 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 Vector3 n = vec3_cross(m_simplex->c[1]->w - m_simplex->c[0]->w,
|
|
m_simplex->c[2]->w - m_simplex->c[0]->w);
|
|
if (n.length_squared() > 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 (Math::abs(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 Vector3 &d, sSV &sv) const {
|
|
sv.d = d / d.length();
|
|
sv.w = m_shape.Support(sv.d);
|
|
}
|
|
void removevertice(sSimplex &simplex) {
|
|
m_free[m_nfree++] = simplex.c[--simplex.rank];
|
|
}
|
|
void appendvertice(sSimplex &simplex, const Vector3 &v) {
|
|
simplex.p[simplex.rank] = 0;
|
|
simplex.c[simplex.rank] = m_free[--m_nfree];
|
|
getsupport(v, *simplex.c[simplex.rank++]);
|
|
}
|
|
static real_t det(const Vector3 &a, const Vector3 &b, const Vector3 &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 real_t projectorigin(const Vector3 &a,
|
|
const Vector3 &b,
|
|
real_t *w, U &m) {
|
|
const Vector3 d = b - a;
|
|
const real_t l = d.length_squared();
|
|
if (l > GJK_SIMPLEX2_EPS) {
|
|
const real_t t(l > 0 ? -vec3_dot(a, d) / l : 0);
|
|
if (t >= 1) {
|
|
w[0] = 0;
|
|
w[1] = 1;
|
|
m = 2;
|
|
return (b.length_squared());
|
|
} else if (t <= 0) {
|
|
w[0] = 1;
|
|
w[1] = 0;
|
|
m = 1;
|
|
return (a.length_squared());
|
|
} else {
|
|
w[0] = 1 - (w[1] = t);
|
|
m = 3;
|
|
return ((a + d * t).length_squared());
|
|
}
|
|
}
|
|
return (-1);
|
|
}
|
|
static real_t projectorigin(const Vector3 &a,
|
|
const Vector3 &b,
|
|
const Vector3 &c,
|
|
real_t *w, U &m) {
|
|
static const U imd3[] = { 1, 2, 0 };
|
|
const Vector3 *vt[] = { &a, &b, &c };
|
|
const Vector3 dl[] = { a - b, b - c, c - a };
|
|
const Vector3 n = vec3_cross(dl[0], dl[1]);
|
|
const real_t l = n.length_squared();
|
|
if (l > GJK_SIMPLEX3_EPS) {
|
|
real_t mindist = -1;
|
|
real_t subw[2];
|
|
U subm;
|
|
for (U i = 0; i < 3; ++i) {
|
|
if (vec3_dot(*vt[i], vec3_cross(dl[i], n)) > 0) {
|
|
const U j = imd3[i];
|
|
const real_t subd(projectorigin(*vt[i], *vt[j], subw, subm));
|
|
if ((mindist < 0) || (subd < mindist)) {
|
|
mindist = subd;
|
|
m = static_cast<U>(((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 real_t d = vec3_dot(a, n);
|
|
const real_t s = Math::sqrt(l);
|
|
const Vector3 p = n * (d / l);
|
|
mindist = p.length_squared();
|
|
m = 7;
|
|
w[0] = (vec3_cross(dl[1], b - p)).length() / s;
|
|
w[1] = (vec3_cross(dl[2], c - p)).length() / s;
|
|
w[2] = 1 - (w[0] + w[1]);
|
|
}
|
|
return (mindist);
|
|
}
|
|
return (-1);
|
|
}
|
|
static real_t projectorigin(const Vector3 &a,
|
|
const Vector3 &b,
|
|
const Vector3 &c,
|
|
const Vector3 &d,
|
|
real_t *w, U &m) {
|
|
static const U imd3[] = { 1, 2, 0 };
|
|
const Vector3 *vt[] = { &a, &b, &c, &d };
|
|
const Vector3 dl[] = { a - d, b - d, c - d };
|
|
const real_t vl = det(dl[0], dl[1], dl[2]);
|
|
const bool ng = (vl * vec3_dot(a, vec3_cross(b - c, a - b))) <= 0;
|
|
if (ng && (Math::abs(vl) > GJK_SIMPLEX4_EPS)) {
|
|
real_t mindist = -1;
|
|
real_t subw[3];
|
|
U subm;
|
|
for (U i = 0; i < 3; ++i) {
|
|
const U j = imd3[i];
|
|
const real_t s = vl * vec3_dot(d, vec3_cross(dl[i], dl[j]));
|
|
if (s > 0) {
|
|
const real_t subd = projectorigin(*vt[i], *vt[j], d, subw, subm);
|
|
if ((mindist < 0) || (subd < mindist)) {
|
|
mindist = subd;
|
|
m = static_cast<U>((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 EPA {
|
|
/* Types */
|
|
typedef GJK::sSV sSV;
|
|
struct sFace {
|
|
Vector3 n;
|
|
real_t d;
|
|
real_t p;
|
|
sSV *c[3];
|
|
sFace *f[3];
|
|
sFace *l[2];
|
|
U1 e[3];
|
|
U1 pass;
|
|
};
|
|
struct sList {
|
|
sFace *root;
|
|
U count;
|
|
sList()
|
|
: root(0), count(0) {}
|
|
};
|
|
struct sHorizon {
|
|
sFace *cf;
|
|
sFace *ff;
|
|
U 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;
|
|
GJK::sSimplex m_result;
|
|
Vector3 m_normal;
|
|
real_t m_depth;
|
|
sSV m_sv_store[EPA_MAX_VERTICES];
|
|
sFace m_fc_store[EPA_MAX_FACES];
|
|
U m_nextsv;
|
|
sList m_hull;
|
|
sList m_stock;
|
|
/* Methods */
|
|
EPA() {
|
|
Initialize();
|
|
}
|
|
|
|
static inline void bind(sFace *fa, U ea, sFace *fb, U eb) {
|
|
fa->e[ea] = (U1)eb;
|
|
fa->f[ea] = fb;
|
|
fb->e[eb] = (U1)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 = Vector3(0, 0, 0);
|
|
m_depth = 0;
|
|
m_nextsv = 0;
|
|
for (U i = 0; i < EPA_MAX_FACES; ++i) {
|
|
append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]);
|
|
}
|
|
}
|
|
eStatus::_ Evaluate(GJK &gjk, const Vector3 &guess) {
|
|
GJK::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) {
|
|
SWAP(simplex.c[0], simplex.c[1]);
|
|
SWAP(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;
|
|
U pass = 0;
|
|
U 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 = (U1)(++pass);
|
|
gjk.getsupport(best->n, *w);
|
|
const real_t wdist = vec3_dot(best->n, w->w) - best->d;
|
|
if (wdist > EPA_ACCURACY) {
|
|
for (U 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();
|
|
if (best->p >= outer.p) outer = *best;
|
|
} else {
|
|
m_status = eStatus::InvalidHull;
|
|
break;
|
|
}
|
|
} else {
|
|
m_status = eStatus::AccuraryReached;
|
|
break;
|
|
}
|
|
} else {
|
|
m_status = eStatus::OutOfVertices;
|
|
break;
|
|
}
|
|
}
|
|
const Vector3 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] = vec3_cross(outer.c[1]->w - projection,
|
|
outer.c[2]->w - projection)
|
|
.length();
|
|
m_result.p[1] = vec3_cross(outer.c[2]->w - projection,
|
|
outer.c[0]->w - projection)
|
|
.length();
|
|
m_result.p[2] = vec3_cross(outer.c[0]->w - projection,
|
|
outer.c[1]->w - projection)
|
|
.length();
|
|
const real_t 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 real_t nl = m_normal.length();
|
|
if (nl > 0)
|
|
m_normal = m_normal / nl;
|
|
else
|
|
m_normal = Vector3(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);
|
|
}
|
|
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 = vec3_cross(b->w - a->w, c->w - a->w);
|
|
const real_t l = face->n.length();
|
|
const bool v = l > EPA_ACCURACY;
|
|
face->p = MIN(MIN(
|
|
vec3_dot(a->w, vec3_cross(face->n, a->w - b->w)),
|
|
vec3_dot(b->w, vec3_cross(face->n, b->w - c->w))),
|
|
vec3_dot(c->w, vec3_cross(face->n, c->w - a->w))) /
|
|
(v ? l : 1);
|
|
face->p = face->p >= -EPA_INSIDE_EPS ? 0 : face->p;
|
|
if (v) {
|
|
face->d = vec3_dot(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;
|
|
real_t mind = minf->d * minf->d;
|
|
real_t maxp = minf->p;
|
|
for (sFace *f = minf->l[1]; f; f = f->l[1]) {
|
|
const real_t sqd = f->d * f->d;
|
|
if ((f->p >= maxp) && (sqd < mind)) {
|
|
minf = f;
|
|
mind = sqd;
|
|
maxp = f->p;
|
|
}
|
|
}
|
|
return (minf);
|
|
}
|
|
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon) {
|
|
static const U i1m3[] = { 1, 2, 0 };
|
|
static const U i2m3[] = { 2, 0, 1 };
|
|
if (f->pass != pass) {
|
|
const U e1 = i1m3[e];
|
|
if ((vec3_dot(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 U e2 = i2m3[e];
|
|
f->pass = (U1)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 ShapeSW *shape0, const Transform &wtrs0,
|
|
const ShapeSW *shape1, const Transform &wtrs1,
|
|
sResults &results,
|
|
tShape &shape,
|
|
bool withmargins) {
|
|
/* Results */
|
|
results.witnesses[0] =
|
|
results.witnesses[1] = Vector3(0, 0, 0);
|
|
results.status = sResults::Separated;
|
|
/* Shape */
|
|
shape.m_shapes[0] = shape0;
|
|
shape.m_shapes[1] = shape1;
|
|
shape.transform_A = wtrs0;
|
|
shape.transform_B = wtrs1;
|
|
}
|
|
|
|
//
|
|
// Api
|
|
//
|
|
|
|
//
|
|
|
|
//
|
|
bool Distance(const ShapeSW *shape0,
|
|
const Transform &wtrs0,
|
|
const ShapeSW *shape1,
|
|
const Transform &wtrs1,
|
|
const Vector3 &guess,
|
|
sResults &results) {
|
|
tShape shape;
|
|
Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false);
|
|
GJK gjk;
|
|
GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess);
|
|
if (gjk_status == GJK::eStatus::Valid) {
|
|
Vector3 w0 = Vector3(0, 0, 0);
|
|
Vector3 w1 = Vector3(0, 0, 0);
|
|
for (U i = 0; i < gjk.m_simplex->rank; ++i) {
|
|
const real_t 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] = w0;
|
|
results.witnesses[1] = 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 == GJK::eStatus::Inside ?
|
|
sResults::Penetrating :
|
|
sResults::GJK_Failed;
|
|
return (false);
|
|
}
|
|
}
|
|
|
|
//
|
|
bool Penetration(const ShapeSW *shape0,
|
|
const Transform &wtrs0,
|
|
const ShapeSW *shape1,
|
|
const Transform &wtrs1,
|
|
const Vector3 &guess,
|
|
sResults &results) {
|
|
tShape shape;
|
|
Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false);
|
|
GJK gjk;
|
|
GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, -guess);
|
|
switch (gjk_status) {
|
|
case GJK::eStatus::Inside: {
|
|
EPA epa;
|
|
EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess);
|
|
if (epa_status != EPA::eStatus::Failed) {
|
|
Vector3 w0 = Vector3(0, 0, 0);
|
|
for (U i = 0; i < epa.m_result.rank; ++i) {
|
|
w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i];
|
|
}
|
|
results.status = sResults::Penetrating;
|
|
results.witnesses[0] = w0;
|
|
results.witnesses[1] = 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 GJK::eStatus::Failed:
|
|
results.status = sResults::GJK_Failed;
|
|
break;
|
|
default: {}
|
|
}
|
|
return (false);
|
|
}
|
|
|
|
/* Symbols cleanup */
|
|
|
|
#undef GJK_MAX_ITERATIONS
|
|
#undef GJK_ACCURARY
|
|
#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
|
|
|
|
} // end of namespace
|
|
|
|
bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
|
|
|
|
GjkEpa2::sResults res;
|
|
|
|
if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
|
|
|
|
r_result_A = res.witnesses[0];
|
|
r_result_B = res.witnesses[1];
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) {
|
|
|
|
GjkEpa2::sResults res;
|
|
|
|
if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
|
|
if (p_result_callback) {
|
|
if (p_swap)
|
|
p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
|
|
else
|
|
p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|