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