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); } } } }
DistanceReport createDistanceReport() { final DistanceReport rep = new DistanceReport(); if (promote) rep.setFeaturePromotion(true, -1); if (maxPairDist != 0) { rep.setMaxClosePairs(100); rep.setMaxPairDistance(maxPairDist); } return rep; }
private void printError( final String msg, final Record rec, final DistanceReport rep, final FeaturePair fpair) { System.out.println("Error, line " + rec.lineNumber); if (msg != null) System.out.println(msg); System.out.println("Wanted:"); for (final ClosestPointPair testPair : rec.testPairs) System.out.println(pairToStr(testPair)); System.out.println("Got:"); if (rep.numClosePairs() > 0) { final ClosestPointPair[] pairs = rep.getClosePairs(); for (int i = 0; i < rep.numClosePairs(); i++) System.out.println(pairToStr(pairs[i])); } else System.out.println(pairToStr(rep.getClosestPair())); System.out.println("Initial Features: " + fpair.first.getName() + " " + fpair.second.getName()); System.exit(1); }
private void printResults(final DistanceReport rep, final FeaturePair fpair) { final Vector3dX del = new Vector3dX(); final ClosestPointPair cpp = rep.getClosestPair(); del.sub(cpp.pnt2, cpp.pnt1); System.out.println("Initial Features: " + fpair.first.getName() + " " + fpair.second.getName()); System.out.println("dist=" + rep.getClosestDistance()); System.out.println("del=" + del); System.out.println("pnt1_1=" + cpp.pnt1); System.out.println("pnt2_1=" + cpp.pnt2); System.out.println("nrml_1=" + cpp.nrml); if (cpp.feat1 != null) System.out.println("feature1=" + cpp.feat1.getClass().getName() + " " + cpp.feat1.name); else System.out.println("feature1=null"); if (cpp.feat2 != null) System.out.println("feature2=" + cpp.feat2.getClass().getName() + " " + cpp.feat2.name); else System.out.println("feature2=null"); }
/* Evaluate the "selected" candidates in the corpus using its "gold" * candidates. */ public DistanceReport evaluate() { DistanceReport dreport = new DistanceReport(); for (Document<Token> doc : corpus) { if (!doc.isTrain()) { Coordinate systemCoord = doc.getSystemCoord(); Coordinate goldCoord = doc.getGoldCoord(); if (systemCoord != null && goldCoord != null) { dreport.addDistance(systemCoord.distanceInKm(goldCoord)); } } } return dreport; }
private void checkDistanceReport(final DistanceReport rep, final NormalDistPair[] checkpairs) throws Exception { if (rep.numClosePairs() != checkpairs.length) throw new Exception( "Distance report has " + rep.numClosePairs() + " close pairs vs. " + checkpairs.length); final ClosestPointPair[] list = new ClosestPointPair[checkpairs.length]; for (int i = 0; i < list.length; i++) list[i] = rep.getClosePairs()[i]; for (final NormalDistPair checkpair : checkpairs) { int j; for (j = 0; j < list.length; j++) if (list[j] != null && checkpair.check(list[j])) { list[j] = null; break; } if (j == list.length) throw new Exception("close pair " + checkpair.vec + ", d=" + checkpair.d + " not found"); } }
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); }
private void checkVclipResults( final Record rec, final Vector3d offset, final DistanceReport rep, final double retDist, final FeaturePair fpair) { for (int i = 0; i < rec.testPairs.length; i++) { rec.testPairs[i].pnt2.add(rec.basePairs[i].pnt2, offset); rec.testPairs[i].dist = rec.basePairs[i].dist + offset.dot(rec.minNrml); } if (rep.getClosestDistance() != retDist) { final String msg = "dist returned=" + retDist + " closestDistance=" + rep.getClosestDistance(); printError(msg, rec, rep, fpair); } if (rep.numClosePairs() == 0) { final ClosestPointPair cpair = rep.getClosestPair(); boolean expectedCollision = false; for (final ClosestPointPair testPair : rec.testPairs) if (testPair.dist <= 0) { expectedCollision = true; break; } if (expectedCollision) { if (cpair.dist > 0) printError("Expected collision", rec, rep, fpair); } else if (rec.testPairs.length != 1 || !matchPair(rep.getClosestPair(), rec.testPairs[0])) printError(null, rec, rep, fpair); } else { final int n = rep.numClosePairs(); if (n != rec.testPairs.length) printError(null, rec, rep, fpair); final ClosestPointPair[] pairs = new ClosestPointPair[n]; for (int i = 0; i < n; i++) pairs[i] = rep.getClosePairs()[i]; for (int i = 0; i < n; i++) { int j; for (j = 0; j < n; j++) if (pairs[j] != null && matchPair(rec.testPairs[i], pairs[j])) { pairs[j] = null; break; } if (j == n) printError(null, rec, rep, fpair); } } }
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) }); }