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