public void Support(Vector3f d, Mkv v) {
v.r.set(d);
Vector3f tmp1 = LocalSupport(d, 0, Stack.alloc(Vector3f.class));
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.set(d);
tmp.negate();
Vector3f tmp2 = LocalSupport(tmp, 1, Stack.alloc(Vector3f.class));
v.w.sub(tmp1, tmp2);
v.w.scaleAdd(margin, d, v.w);
}
public Vector3f LocalSupport(Vector3f d, /*unsigned*/ int i, Vector3f out) {
Vector3f tmp = Stack.alloc(Vector3f.class);
MatrixUtil.transposeTransform(tmp, d, wrotations[i]);
shapes[i].localGetSupportingVertex(tmp, out);
wrotations[i].transform(out);
out.add(positions[i]);
return out;
}
public boolean SolveSimplex3a(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f cabc) {
// TODO: optimize
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.cross(cabc, ab);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
tmp2.cross(cabc, ac);
if (tmp.dot(ao) < -GJK_insimplex_eps) {
order = 1;
simplex[0].set(simplex[1]);
simplex[1].set(simplex[2]);
return SolveSimplex2(ao, ab);
} else if (tmp2.dot(ao) > +GJK_insimplex_eps) {
order = 1;
simplex[1].set(simplex[2]);
return SolveSimplex2(ao, ac);
} else {
float d = cabc.dot(ao);
if (Math.abs(d) > GJK_insimplex_eps) {
if (d > 0) {
ray.set(cabc);
} else {
ray.negate(cabc);
Mkv swapTmp = new Mkv();
swapTmp.set(simplex[0]);
simplex[0].set(simplex[1]);
simplex[1].set(swapTmp);
}
return false;
} else {
return true;
}
}
}
public boolean FetchSupport() {
int h = Hash(ray);
He e = table[h];
while (e != null) {
if (e.v.equals(ray)) {
--order;
return false;
} else {
e = e.n;
}
}
// e = (He*)sa->allocate(sizeof(He));
// e = new He();
e = stackHe.get();
e.v.set(ray);
e.n = table[h];
table[h] = e;
Support(ray, simplex[++order]);
return (ray.dot(simplex[order].w) > 0);
}
public boolean SolveSimplex2(Vector3f ao, Vector3f ab) {
if (ab.dot(ao) >= 0) {
Vector3f cabo = Stack.alloc(Vector3f.class);
cabo.cross(ab, ao);
if (cabo.lengthSquared() > GJK_sqinsimplex_eps) {
ray.cross(cabo, ab);
} else {
return true;
}
} else {
order = 0;
simplex[0].set(simplex[1]);
ray.set(ao);
}
return (false);
}
public float EvaluatePD(float accuracy) {
pushStack();
try {
Vector3f tmp = Stack.alloc(Vector3f.class);
// btBlock* sablock = sa->beginBlock();
Face bestface = null;
int markid = 1;
depth = -cstInf;
normal.set(0f, 0f, 0f);
root = null;
nfaces = 0;
iterations = 0;
failed = false;
/* Prepare hull */
if (gjk.EncloseOrigin()) {
// const U* pfidx = 0;
int[][] pfidx_ptr = null;
int pfidx_index = 0;
int nfidx = 0;
// const U* peidx = 0;
int[][] peidx_ptr = null;
int peidx_index = 0;
int neidx = 0;
Mkv[] basemkv = new Mkv[5];
Face[] basefaces = new Face[6];
switch (gjk.order) {
// Tetrahedron
case 3:
{
// pfidx=(const U*)fidx;
pfidx_ptr = tetrahedron_fidx;
pfidx_index = 0;
nfidx = 4;
// peidx=(const U*)eidx;
peidx_ptr = tetrahedron_eidx;
peidx_index = 0;
neidx = 6;
}
break;
// Hexahedron
case 4:
{
// pfidx=(const U*)fidx;
pfidx_ptr = hexahedron_fidx;
pfidx_index = 0;
nfidx = 6;
// peidx=(const U*)eidx;
peidx_ptr = hexahedron_eidx;
peidx_index = 0;
neidx = 9;
}
break;
}
int i;
for (i = 0; i <= gjk.order; ++i) {
basemkv[i] = new Mkv();
basemkv[i].set(gjk.simplex[i]);
}
for (i = 0; i < nfidx; ++i, pfidx_index++) {
basefaces[i] =
NewFace(
basemkv[pfidx_ptr[pfidx_index][0]],
basemkv[pfidx_ptr[pfidx_index][1]],
basemkv[pfidx_ptr[pfidx_index][2]]);
}
for (i = 0; i < neidx; ++i, peidx_index++) {
Link(
basefaces[peidx_ptr[peidx_index][0]],
peidx_ptr[peidx_index][1],
basefaces[peidx_ptr[peidx_index][2]],
peidx_ptr[peidx_index][3]);
}
}
if (0 == nfaces) {
// sa->endBlock(sablock);
return (depth);
}
/* Expand hull */
for (; iterations < EPA_maxiterations; ++iterations) {
Face bf = FindBest();
if (bf != null) {
tmp.negate(bf.n);
Mkv w = Support(tmp);
float d = bf.n.dot(w.w) + bf.d;
bestface = bf;
if (d < -accuracy) {
Face[] cf = new Face[] {null};
Face[] ff = new Face[] {null};
int nf = 0;
Detach(bf);
bf.mark = ++markid;
for (int i = 0; i < 3; ++i) {
nf += BuildHorizon(markid, w, bf.f[i], bf.e[i], cf, ff);
}
if (nf <= 2) {
break;
}
Link(cf[0], 1, ff[0], 2);
} else {
break;
}
} else {
break;
}
}
/* Extract contact */
if (bestface != null) {
Vector3f b = GetCoordinates(bestface, Stack.alloc(Vector3f.class));
normal.set(bestface.n);
depth = Math.max(0, bestface.d);
for (int i = 0; i < 2; ++i) {
float s = i != 0 ? -1f : 1f;
for (int j = 0; j < 3; ++j) {
tmp.scale(s, bestface.v[j].r);
gjk.LocalSupport(tmp, i, features[i][j]);
}
}
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
tmp1.scale(b.x, features[0][0]);
tmp2.scale(b.y, features[0][1]);
tmp3.scale(b.z, features[0][2]);
VectorUtil.add(nearest[0], tmp1, tmp2, tmp3);
tmp1.scale(b.x, features[1][0]);
tmp2.scale(b.y, features[1][1]);
tmp3.scale(b.z, features[1][2]);
VectorUtil.add(nearest[1], tmp1, tmp2, tmp3);
} else {
failed = true;
}
// sa->endBlock(sablock);
return (depth);
} finally {
popStack();
}
}
public boolean Set(Face f, Mkv a, Mkv b, Mkv c) {
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
Vector3f nrm = Stack.alloc(Vector3f.class);
tmp1.sub(b.w, a.w);
tmp2.sub(c.w, a.w);
nrm.cross(tmp1, tmp2);
float len = nrm.length();
tmp1.cross(a.w, b.w);
tmp2.cross(b.w, c.w);
tmp3.cross(c.w, a.w);
boolean valid =
(tmp1.dot(nrm) >= -EPA_inface_eps)
&& (tmp2.dot(nrm) >= -EPA_inface_eps)
&& (tmp3.dot(nrm) >= -EPA_inface_eps);
f.v[0] = a;
f.v[1] = b;
f.v[2] = c;
f.mark = 0;
f.n.scale(1f / (len > 0f ? len : cstInf), nrm);
f.d = Math.max(0, -f.n.dot(a.w));
return valid;
}
public Vector3f GetCoordinates(Face face, Vector3f out) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f o = Stack.alloc(Vector3f.class);
o.scale(-face.d, face.n);
float[] a = floatArrays.getFixed(3);
tmp1.sub(face.v[0].w, o);
tmp2.sub(face.v[1].w, o);
tmp.cross(tmp1, tmp2);
a[0] = tmp.length();
tmp1.sub(face.v[1].w, o);
tmp2.sub(face.v[2].w, o);
tmp.cross(tmp1, tmp2);
a[1] = tmp.length();
tmp1.sub(face.v[2].w, o);
tmp2.sub(face.v[0].w, o);
tmp.cross(tmp1, tmp2);
a[2] = tmp.length();
float sm = a[0] + a[1] + a[2];
out.set(a[1], a[2], a[0]);
out.scale(1f / (sm > 0f ? sm : 1f));
floatArrays.release(a);
return out;
}
public boolean EncloseOrigin() {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
switch (order) {
// Point
case 0:
break;
// Line
case 1:
{
Vector3f ab = Stack.alloc(Vector3f.class);
ab.sub(simplex[1].w, simplex[0].w);
Vector3f[] b =
new Vector3f[] {
Stack.alloc(Vector3f.class),
Stack.alloc(Vector3f.class),
Stack.alloc(Vector3f.class)
};
b[0].set(1f, 0f, 0f);
b[1].set(0f, 1f, 0f);
b[2].set(0f, 0f, 1f);
b[0].cross(ab, b[0]);
b[1].cross(ab, b[1]);
b[2].cross(ab, b[2]);
float m[] =
new float[] {b[0].lengthSquared(), b[1].lengthSquared(), b[2].lengthSquared()};
Quat4f tmpQuat = Stack.alloc(Quat4f.class);
tmp.normalize(ab);
QuaternionUtil.setRotation(tmpQuat, tmp, cst2Pi / 3f);
Matrix3f r = Stack.alloc(Matrix3f.class);
MatrixUtil.setRotation(r, tmpQuat);
Vector3f w = Stack.alloc(Vector3f.class);
w.set(b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]);
tmp.normalize(w);
Support(tmp, simplex[4]);
r.transform(w);
tmp.normalize(w);
Support(tmp, simplex[2]);
r.transform(w);
tmp.normalize(w);
Support(tmp, simplex[3]);
r.transform(w);
order = 4;
return (true);
}
// Triangle
case 2:
{
tmp1.sub(simplex[1].w, simplex[0].w);
tmp2.sub(simplex[2].w, simplex[0].w);
Vector3f n = Stack.alloc(Vector3f.class);
n.cross(tmp1, tmp2);
n.normalize();
Support(n, simplex[3]);
tmp.negate(n);
Support(tmp, simplex[4]);
order = 4;
return (true);
}
// Tetrahedron
case 3:
return (true);
// Hexahedron
case 4:
return (true);
}
return (false);
}
public boolean SearchOrigin(Vector3f initray) {
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
Vector3f tmp4 = Stack.alloc(Vector3f.class);
iterations = 0;
order = -1;
failed = false;
ray.set(initray);
ray.normalize();
Arrays.fill(table, null);
FetchSupport();
ray.negate(simplex[0].w);
for (; iterations < GJK_maxiterations; ++iterations) {
float rl = ray.length();
ray.scale(1f / (rl > 0f ? rl : 1f));
if (FetchSupport()) {
boolean found = false;
switch (order) {
case 1:
{
tmp1.negate(simplex[1].w);
tmp2.sub(simplex[0].w, simplex[1].w);
found = SolveSimplex2(tmp1, tmp2);
break;
}
case 2:
{
tmp1.negate(simplex[2].w);
tmp2.sub(simplex[1].w, simplex[2].w);
tmp3.sub(simplex[0].w, simplex[2].w);
found = SolveSimplex3(tmp1, tmp2, tmp3);
break;
}
case 3:
{
tmp1.negate(simplex[3].w);
tmp2.sub(simplex[2].w, simplex[3].w);
tmp3.sub(simplex[1].w, simplex[3].w);
tmp4.sub(simplex[0].w, simplex[3].w);
found = SolveSimplex4(tmp1, tmp2, tmp3, tmp4);
break;
}
}
if (found) {
return true;
}
} else {
return false;
}
}
failed = true;
return false;
}
public boolean SearchOrigin() {
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.set(1f, 0f, 0f);
return SearchOrigin(tmp);
}
public boolean SolveSimplex4(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f ad) {
// TODO: optimize
Vector3f crs = Stack.alloc(Vector3f.class);
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.cross(ab, ac);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
tmp2.cross(ac, ad);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
tmp3.cross(ad, ab);
if (tmp.dot(ao) > GJK_insimplex_eps) {
crs.set(tmp);
order = 2;
simplex[0].set(simplex[1]);
simplex[1].set(simplex[2]);
simplex[2].set(simplex[3]);
return SolveSimplex3a(ao, ab, ac, crs);
} else if (tmp2.dot(ao) > GJK_insimplex_eps) {
crs.set(tmp2);
order = 2;
simplex[2].set(simplex[3]);
return SolveSimplex3a(ao, ac, ad, crs);
} else if (tmp3.dot(ao) > GJK_insimplex_eps) {
crs.set(tmp3);
order = 2;
simplex[1].set(simplex[0]);
simplex[0].set(simplex[2]);
simplex[2].set(simplex[3]);
return SolveSimplex3a(ao, ad, ab, crs);
} else {
return (true);
}
}
public boolean SolveSimplex3(Vector3f ao, Vector3f ab, Vector3f ac) {
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.cross(ab, ac);
return (SolveSimplex3a(ao, ab, ac, tmp));
}
public void set(Mkv m) {
w.set(m.w);
r.set(m.r);
}
