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