public boolean collide(
ConvexShape shape0,
Transform wtrs0,
ConvexShape shape1,
Transform wtrs1,
float radialmargin /*,
btStackAlloc* stackAlloc*/,
Results results) {
// Initialize
results.witnesses[0].set(0f, 0f, 0f);
results.witnesses[1].set(0f, 0f, 0f);
results.normal.set(0f, 0f, 0f);
results.depth = 0;
results.status = ResultsStatus.Separated;
results.epa_iterations = 0;
results.gjk_iterations = 0;
/* Use GJK to locate origin */
gjk.init(
/*stackAlloc,*/
wtrs0.basis,
wtrs0.origin,
shape0,
wtrs1.basis,
wtrs1.origin,
shape1,
radialmargin + EPA_accuracy);
try {
boolean collide = gjk.SearchOrigin();
results.gjk_iterations = gjk.iterations + 1;
if (collide) {
/* Then EPA for penetration depth */
EPA epa = new EPA(gjk);
float pd = epa.EvaluatePD();
results.epa_iterations = epa.iterations + 1;
if (pd > 0) {
results.status = ResultsStatus.Penetrating;
results.normal.set(epa.normal);
results.depth = pd;
results.witnesses[0].set(epa.nearest[0]);
results.witnesses[1].set(epa.nearest[1]);
return (true);
} else {
if (epa.failed) {
results.status = ResultsStatus.EPA_Failed;
}
}
} else {
if (gjk.failed) {
results.status = ResultsStatus.GJK_Failed;
}
}
return (false);
} finally {
gjk.destroy();
}
}
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 Mkv Support(Vector3f w) {
// Mkv v = new Mkv();
Mkv v = stackMkv.get();
gjk.Support(w, v);
return v;
}