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 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 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 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 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 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 SolveSimplex3(Vector3f ao, Vector3f ab, Vector3f ac) {
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.cross(ab, ac);
return (SolveSimplex3a(ao, ab, ac, tmp));
}