private void dotest(final Record rec) {
PolyTree ptree1;
PolyTree ptree2;
ptree1 = (PolyTree) bodies.get(rec.poly1);
if (ptree1 == null) {
System.err.println("PolyTree " + rec.poly1 + " not found");
System.exit(1);
}
ptree2 = (PolyTree) bodies.get(rec.poly2);
if (ptree2 == null) {
System.err.println("PolyTree " + rec.poly2 + " not found");
System.exit(1);
}
double[] dlist;
final Vector3d offset = new Vector3d();
if (ptree1.numNodes() > 0 || ptree2.numNodes() > 0)
dlist = new double[] {-0.1, 0.1, rec.minDist};
else dlist = new double[] {-0.1, 0.1, 1.0, rec.minDist};
// dlist = new double[] { rec.minDist };
for (final double element : dlist) {
offset.scale(element - rec.minDist, rec.minNrml);
if (exhaustive) exhaustiveCheck(rec, offset, ptree1, ptree2);
else singleCheck(rec, offset, ptree1, ptree2, null, null);
}
}
private void exhaustiveCheck(
final Record rec, final Vector3d offset, final PolyTree ptree1, final PolyTree ptree2) {
final Matrix4dX X21 = new Matrix4dX(rec.X21);
final Matrix4dX X12 = new Matrix4dX();
X21.m03 += offset.x;
X21.m13 += offset.y;
X21.m23 += offset.z;
X12.invert(X21);
final Vector flist1 = getAllPolyhdreonFeatures(ptree1.getPolyhedron());
final Vector flist2 = getAllPolyhdreonFeatures(ptree2.getPolyhedron());
for (final Iterator it1 = flist1.iterator(); it1.hasNext(); ) {
final Feature f1 = (Feature) it1.next();
for (final Iterator it2 = flist2.iterator(); it2.hasNext(); ) {
final Feature f2 = (Feature) it2.next();
if (!(f1.type == Feature.FACE && f2.type == Feature.FACE)) {
final FeaturePair featurePair = new FeaturePair(f1, f2);
final PolyTreePair ptreePair = new PolyTreePair(ptree1, ptree2);
closestFeaturesHT.put(ptreePair, featurePair);
final DistanceReport rep = rec.createDistanceReport();
final double d = ptree1.vclip(rep, ptree2, X21, MAX_DIST, closestFeaturesHT);
rep.transformSecondPoints(X21);
checkVclipResults(rec, offset, rep, d, featurePair);
}
}
}
}
public void execute(PolyTree solution, double delta) {
solution.Clear();
fixOrientations();
doOffset(delta);
// now clean up 'corners' ...
final DefaultClipper clpr = new DefaultClipper(Clipper.REVERSE_SOLUTION);
clpr.addPaths(destPolys, PolyType.SUBJECT, true);
if (delta > 0) {
clpr.execute(ClipType.UNION, solution, PolyFillType.POSITIVE, PolyFillType.POSITIVE);
} else {
final LongRect r = destPolys.getBounds();
final Path outer = new Path(4);
outer.add(new LongPoint(r.left - 10, r.bottom + 10, 0));
outer.add(new LongPoint(r.right + 10, r.bottom + 10, 0));
outer.add(new LongPoint(r.right + 10, r.top - 10, 0));
outer.add(new LongPoint(r.left - 10, r.top - 10, 0));
clpr.addPath(outer, PolyType.SUBJECT, true);
clpr.execute(ClipType.UNION, solution, PolyFillType.NEGATIVE, PolyFillType.NEGATIVE);
// remove the outer PolyNode rectangle ...
if (solution.getChildCount() == 1 && solution.getChilds().get(0).getChildCount() > 0) {
final PolyNode outerNode = solution.getChilds().get(0);
solution.getChilds().set(0, outerNode.getChilds().get(0));
solution.getChilds().get(0).setParent(solution);
for (int i = 1; i < outerNode.getChildCount(); i++) {
solution.addChild(outerNode.getChilds().get(i));
}
} else {
solution.Clear();
}
}
}
private void dotime(final Record rec, final boolean dryrun) {
PolyTree ptree1;
PolyTree ptree2;
ptree1 = (PolyTree) bodies.get(rec.poly1);
if (ptree1 == null) {
System.err.println("PolyTree " + rec.poly1 + " not found");
System.exit(1);
}
ptree2 = (PolyTree) bodies.get(rec.poly2);
if (ptree2 == null) {
System.err.println("PolyTree " + rec.poly2 + " not found");
System.exit(1);
}
final DistanceReport rep = new DistanceReport();
if (ptree1.isAtomic() && ptree2.isAtomic() && !dryrun)
ptree1.vclip(rep, ptree2, rec.X21, MAX_DIST, closestFeaturesHT);
}
VclipTest(final Reader testDataReader, final StreamTokenizer polyTreeStream) {
testlist = new Vector(100);
library = new HashMap();
try {
PolyTree.scanLibrary(polyTreeStream, library, true);
} catch (final Exception e) {
System.out.println("Error polyTreeStream");
e.printStackTrace();
System.exit(1);
}
readTestList(testDataReader);
}
public static void main(String[] args) {
double[] vertexCoords = {
1.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0
};
int[][] faceIndices = {{0, 3, 2, 1}, {4, 0, 1}, {4, 1, 2}, {4, 2, 3}, {4, 3, 0}};
ConvexPolyhedron poly = new ConvexPolyhedron(vertexCoords, faceIndices);
PolyTree pyramid = new PolyTree("pyramid", poly);
PolyTree hourglass = new PolyTree("hourglass");
Matrix4d X = new Matrix4d();
X.setIdentity();
X.setTranslation(new Vector3d(0, 0, -1));
hourglass.addComponent("bottom", pyramid, X);
X.setTranslation(new Vector3d(0, 0, 1));
X.setRotation(new AxisAngle4d(1, 0, 0, Math.PI));
hourglass.addComponent("top", pyramid, X);
hourglass.buildBoundingHull(PolyTree.OBB_HULL);
}
private void checkPolyVertices(final Point3d[] pnts, final PolyTree ptree) throws Exception {
final Vertex[] verts = ptree.poly_.getVerts();
final boolean[] marked = new boolean[verts.length];
for (int i = 0; i < marked.length; i++) marked[i] = false;
for (final Point3d pnt : pnts) {
int j;
for (j = 0; j < verts.length; j++)
if (!marked[j] && verts[j].coords.epsilonEquals(pnt, 1e-8)) {
marked[j] = true;
break;
}
if (j == verts.length)
throw new Exception("vertex " + pnt + " missing in hull for " + ptree.getName());
}
}
private void singleCheck(
final Record rec,
final Vector3d offset,
final PolyTree ptree1,
final PolyTree ptree2,
final String fname1,
final String fname2) {
final Matrix4dX X21 = new Matrix4dX(rec.X21);
final Matrix4dX X12 = new Matrix4dX();
X21.m03 += offset.x;
X21.m13 += offset.y;
X21.m23 += offset.z;
X12.invert(X21);
FeaturePair featurePair;
if (fname1 != null) {
Feature f1, f2;
if (fname1.equals("")) f1 = ptree1.poly_.getVerts()[0];
else f1 = ptree1.poly_.findFeature(fname1);
if (fname1.equals("")) f2 = ptree2.poly_.getVerts()[0];
else f2 = ptree2.poly_.findFeature(fname2);
featurePair = new FeaturePair(f1, f2);
final PolyTreePair ptreePair = new PolyTreePair(ptree1, ptree2);
closestFeaturesHT.put(ptreePair, featurePair);
} else {
featurePair = (FeaturePair) closestFeaturesHT.get(new PolyTreePair(ptree1, ptree2));
if (featurePair == null)
featurePair = new FeaturePair(ptree1.poly_.getVerts()[0], ptree2.poly_.getVerts()[0]);
}
final DistanceReport rep = rec.createDistanceReport();
final double d = ptree1.vclip(rep, ptree2, X21, MAX_DIST, closestFeaturesHT);
rep.transformSecondPoints(X21);
checkVclipResults(rec, offset, rep, d, featurePair);
}
void polyTreeTests() throws Exception {
final PolyTree ptree = new PolyTree("compound");
final PolyTree unitCube = (PolyTree) library.get("unit-cube");
final Matrix4dX M = new Matrix4dX();
final double c = Math.cos(Math.toRadians(30));
M.setRpy(Math.toRadians(30), Math.toRadians(0), Math.toRadians(20));
final Vector3d p = new Vector3d(M.m01, M.m11, M.m21);
// Vector3d p = new Vector3d (M.m00, M.m10, M.m20);
M.setXyz(p.x, p.y, p.z);
// M.setXyz (-0.5, c, 0);
ptree.addComponent(null, unitCube, M);
M.setXyz(-p.x, -p.y, -p.z);
// M.setXyz ( 0.5,-c, 0);
ptree.addComponent(null, unitCube, M);
ptree.buildBoundingHull(PolyTree.CONVEX_HULL);
Vertex[] verts = ptree.getPolyhedron().getVerts();
final Point3dX[] hullPnts = new Point3dX[verts.length];
for (int i = 0; i < hullPnts.length; i++) hullPnts[i] = new Point3dX(verts[i].coords);
ptree.buildBoundingHull(PolyTree.OBB_HULL);
verts = ptree.getPolyhedron().getVerts();
final Point3dX px = new Point3dX();
for (final Vertex vert : verts) {
M.transform(vert.coords, px);
px.set(vert.coords);
}
for (int i = 0; i < verts.length; i++)
if (!locatePoint(verts[i].coords, hullPnts))
throw new Exception("Can't locate point " + verts[i].coords);
M.setIdentity();
final PolyTree cubepair = new PolyTree("cubepair");
final Point3d[] cubepairHullCheck =
new Point3d[] {
new Point3d(1.5, 0.5, 0.5),
new Point3d(1.5, 0.5, -0.5),
new Point3d(1.5, -0.5, 0.5),
new Point3d(1.5, -0.5, -0.5),
new Point3d(-1.5, 0.5, 0.5),
new Point3d(-1.5, 0.5, -0.5),
new Point3d(-1.5, -0.5, 0.5),
new Point3d(-1.5, -0.5, -0.5),
};
M.setXyz(1, 0, 0);
cubepair.addComponent(null, unitCube, M);
M.setXyz(-1, 0, 0);
cubepair.addComponent(null, unitCube, M);
cubepair.buildBoundingHull(PolyTree.OBB_HULL);
checkPolyVertices(cubepairHullCheck, cubepair);
cubepair.buildBoundingHull(PolyTree.CONVEX_HULL);
checkPolyVertices(cubepairHullCheck, cubepair);
Point3d[] jackHullCheck;
final PolyTree jack = new PolyTree("jack");
jack.addComponent("jack_0", cubepair);
M.setXyz(0, 0, 0);
M.setRpy(Math.toRadians(90), 0, 0);
jack.addComponent("jack_1", cubepair, M);
final Matrix4dX MI = new Matrix4dX();
MI.invertTrans(M);
jackHullCheck = appendPoints(cubepairHullCheck, transformPoints(MI, cubepairHullCheck));
M.setRpy(0, Math.toRadians(90), 0);
jack.addComponent("jack_2", cubepair, M);
MI.invertTrans(M);
jackHullCheck = appendPoints(jackHullCheck, transformPoints(MI, cubepairHullCheck));
jack.buildBoundingHull(PolyTree.CONVEX_HULL);
checkPolyVertices(jackHullCheck, jack);
jack.buildAllBoundingHulls(PolyTree.CONVEX_HULL);
checkPolyVertices(jackHullCheck, jack);
int i = 0;
for (final Iterator it = jack.getComponents(); it.hasNext(); ) {
final PolyTree part = (PolyTree) it.next();
part.setName("jack_" + i);
checkPolyVertices(cubepairHullCheck, part);
i++;
}
final Point3d[] jackOBBHullCheck =
new Point3d[] {
new Point3d(1.5, 1.5, 1.5),
new Point3d(1.5, 1.5, -1.5),
new Point3d(1.5, -1.5, 1.5),
new Point3d(1.5, -1.5, -1.5),
new Point3d(-1.5, 1.5, 1.5),
new Point3d(-1.5, 1.5, -1.5),
new Point3d(-1.5, -1.5, 1.5),
new Point3d(-1.5, -1.5, -1.5),
};
jack.buildAllBoundingHulls(PolyTree.OBB_HULL);
checkPolyVertices(jackOBBHullCheck, jack);
for (final Iterator it = jack.getComponents(); it.hasNext(); )
checkPolyVertices(cubepairHullCheck, (PolyTree) it.next());
final PolyTree cross1 = new PolyTree(null, library, "cross");
final PolyTree cross2 = new PolyTree(null, library, "cross");
final DistanceReport rep = new DistanceReport(10);
final double del = 0.1;
M.setRpy(0, 0, 0);
M.setXyz(1.5 + del, -1.0 - del, 0);
rep.setMaxPairDistance(2 * del);
cross1.vclip(rep, cross2, M, 10.0, null);
rep.transformSecondPoints(M);
checkDistanceReport(
rep,
new NormalDistPair[] {
new NormalDistPair(1, 0, 0, del),
new NormalDistPair(1, 0, 0, del),
new NormalDistPair(0, -1, 0, del)
});
}
