private Record readTestRecord(final StreamTokenizer stok) throws IOException {
final Record rec = new Record();
final double[] vals;
try {
rec.poly1 = scanString(stok);
} catch (final EOFException e) {
return null;
}
rec.lineNumber = stok.lineno();
rec.poly2 = scanString(stok);
rec.X21.scan("[%s]", stok);
rec.X12.invert(rec.X21);
final Vector pairList = new Vector(10);
String s;
rec.minDist = Double.POSITIVE_INFINITY;
while (true) {
s = scanString(stok);
if (s.equals("P")) rec.promote = true;
else if (s.equals("D")) rec.maxPairDist = TokenScanner.scanDouble(stok);
else break;
}
while (!s.equals("*")) {
final ClosestPointPair pair = new ClosestPointPair();
if (!s.equals("N")) {
pair.feat1 = getDummyFeature(s, stok);
pair.feat2 = getDummyFeature(scanString(stok), stok);
final Point3dX p1 = new Point3dX();
p1.scan("[%v]", stok);
pair.pnt1.set(p1);
}
final Vector3dX dir = new Vector3dX();
dir.scan("[%v]", stok);
dir.normalize();
pair.nrml.set(dir);
pair.dist = TokenScanner.scanDouble(stok);
if (!s.equals("N")) pair.pnt2.scaleAdd(pair.dist, pair.nrml, pair.pnt1);
if (pair.dist < rec.minDist) {
rec.minDist = pair.dist;
rec.minNrml = new Vector3d(pair.nrml);
}
pairList.add(pair);
s = scanString(stok);
}
rec.basePairs = (ClosestPointPair[]) pairList.toArray(new ClosestPointPair[0]);
rec.testPairs = new ClosestPointPair[rec.basePairs.length];
for (int i = 0; i < rec.testPairs.length; i++)
rec.testPairs[i] = new ClosestPointPair(rec.basePairs[i]);
return rec;
}
private boolean locatePoint(final Point3dX pnt, final Point3dX[] list) {
for (int i = 0; i < list.length; i++)
if (list[i] != null && pnt.epsilonEquals(list[i], 1e-6)) {
list[i] = null;
return true;
}
return false;
}
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)
});
}
