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 int mergeAdjacentFace(HalfEdge hedgeAdj, Face[] discarded) {
Face oppFace = hedgeAdj.oppositeFace();
int numDiscarded = 0;
discarded[numDiscarded++] = oppFace;
oppFace.mark = DELETED;
HalfEdge hedgeOpp = hedgeAdj.getOpposite();
HalfEdge hedgeAdjPrev = hedgeAdj.prev;
HalfEdge hedgeAdjNext = hedgeAdj.next;
HalfEdge hedgeOppPrev = hedgeOpp.prev;
HalfEdge hedgeOppNext = hedgeOpp.next;
while (hedgeAdjPrev.oppositeFace() == oppFace) {
hedgeAdjPrev = hedgeAdjPrev.prev;
hedgeOppNext = hedgeOppNext.next;
}
while (hedgeAdjNext.oppositeFace() == oppFace) {
hedgeOppPrev = hedgeOppPrev.prev;
hedgeAdjNext = hedgeAdjNext.next;
}
HalfEdge hedge;
for (hedge = hedgeOppNext; hedge != hedgeOppPrev.next; hedge = hedge.next) {
hedge.face = this;
}
if (hedgeAdj == he0) {
he0 = hedgeAdjNext;
}
// handle the half edges at the head
Face discardedFace;
discardedFace = connectHalfEdges(hedgeOppPrev, hedgeAdjNext);
if (discardedFace != null) {
discarded[numDiscarded++] = discardedFace;
}
// handle the half edges at the tail
discardedFace = connectHalfEdges(hedgeAdjPrev, hedgeOppNext);
if (discardedFace != null) {
discarded[numDiscarded++] = discardedFace;
}
computeNormalAndCentroid();
checkConsistency();
return numDiscarded;
}
private Face connectHalfEdges(HalfEdge hedgePrev, HalfEdge hedge) {
Face discardedFace = null;
if (hedgePrev.oppositeFace()
== hedge.oppositeFace()) { // then there is a redundant edge that we can get rid off
Face oppFace = hedge.oppositeFace();
HalfEdge hedgeOpp;
if (hedgePrev == he0) {
he0 = hedge;
}
if (oppFace.numVertices() == 3) { // then we can get rid of the opposite face altogether
hedgeOpp = hedge.getOpposite().prev.getOpposite();
oppFace.mark = DELETED;
discardedFace = oppFace;
} else {
hedgeOpp = hedge.getOpposite().next;
if (oppFace.he0 == hedgeOpp.prev) {
oppFace.he0 = hedgeOpp;
}
hedgeOpp.prev = hedgeOpp.prev.prev;
hedgeOpp.prev.next = hedgeOpp;
}
hedge.prev = hedgePrev.prev;
hedge.prev.next = hedge;
hedge.opposite = hedgeOpp;
hedgeOpp.opposite = hedge;
// oppFace was modified, so need to recompute
oppFace.computeNormalAndCentroid();
} else {
hedgePrev.next = hedge;
hedge.prev = hedgePrev;
}
return discardedFace;
}
public int BuildHorizon(int markid, Mkv w, Face f, int e, Face[] cf, Face[] ff) {
int ne = 0;
if (f.mark != markid) {
int e1 = mod3[e + 1];
if ((f.n.dot(w.w) + f.d) > 0) {
Face nf = NewFace(f.v[e1], f.v[e], w);
Link(nf, 0, f, e);
if (cf[0] != null) {
Link(cf[0], 1, nf, 2);
} else {
ff[0] = nf;
}
cf[0] = nf;
ne = 1;
} else {
int e2 = mod3[e + 2];
Detach(f);
f.mark = markid;
ne += BuildHorizon(markid, w, f.f[e1], f.e[e1], cf, ff);
ne += BuildHorizon(markid, w, f.f[e2], f.e[e2], cf, ff);
}
}
return (ne);
}
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();
}
}
