private QuadTreeTestHelper testOnAStaircase(Point3D center, Vector3D normal, double halfWidth, double resolution, double stairSeparation, double oneStairLandingHeight, boolean visualize) { normal.normalize(); BoundingBox2D boundingBox = new BoundingBox2D(center.getX() - halfWidth, center.getY() - halfWidth, center.getX() + halfWidth, center.getY() + halfWidth); Plane3D plane3d = new Plane3D(center, normal); ArrayList<Point3D> points = generatePointsForStairs(plane3d, halfWidth, resolution, stairSeparation, oneStairLandingHeight); // Collections.shuffle(points); int pointsPerBallUpdate = 1; QuadTreeTestHelper testHelper = testOnAListOfPoints(points, pointsPerBallUpdate, boundingBox, resolution, visualize); if (visualize) { testHelper.drawPoints(points, resolution/2.0, YoAppearance.Blue()); ArrayList<Point3D> allPointsInQuadTree = testHelper.getAllPointsInQuadTree(); testHelper. drawPoints(allPointsInQuadTree, resolution*0.6, YoAppearance.Chartreuse()); } // testHelper. drawPointsWithinAreaInSCS(pointsPerBallUpdate, YoAppearance.Chartreuse()); // testHelper.drawHeightOfOriginalPointsInPurple(points, pointsPerBallUpdate); // testHelper.drawHeightMap(boundingBox, resolution); return testHelper; }
staticLinkGraphics.addCube(cubeSize, cubeSize, cubeSize, YoAppearance.Chartreuse()); scs.addStaticLinkGraphics(staticLinkGraphics);
pointSFViz = new YoGraphicPosition("pointSF", "", registry, pointSize, YoAppearance.Chartreuse()); pointD0Viz = new YoGraphicPosition("pointD0", "", registry, pointSize, YoAppearance.BlueViolet()); pointDFViz = new YoGraphicPosition("pointDF", "", registry, pointSize, YoAppearance.Azure()); pointSFMinViz = new YoGraphicPosition("pointSFMin", "", registry, 0.8 * pointSize, YoAppearance.Chartreuse()); pointD0MinViz = new YoGraphicPosition("pointD0Min", "", registry, 0.8 * pointSize, YoAppearance.BlueViolet()); pointDFMinViz = new YoGraphicPosition("pointDFMin", "", registry, 0.8 * pointSize, YoAppearance.Azure()); pointSFMaxViz = new YoGraphicPosition("pointSFMax", "", registry, 0.9 * pointSize, YoAppearance.Chartreuse()); pointD0MaxViz = new YoGraphicPosition("pointD0Max", "", registry, 0.9 * pointSize, YoAppearance.BlueViolet()); pointDFMaxViz = new YoGraphicPosition("pointDFMax", "", registry, 0.9 * pointSize, YoAppearance.Azure());
polygonSnapperVisualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.Gold(), YoAppearance.Purple(), YoAppearance.Brown(), YoAppearance.Blue(), YoAppearance.Chartreuse());
final YoGraphicVector currentVelocityViz = new YoGraphicVector(namePrefix + "CurrentVelocity", currentPosition.buildUpdatedYoFramePointForVisualizationOnly(), currentVelocity.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.Chartreuse()); final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicVector currentAngularVelocityViz = new YoGraphicVector(namePrefix + "CurrentAngularVelocity",