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 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;
}