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