private boolean pointsBelowPlane(List<Point3D> point3ds, Plane3D plane, double tolerance) { for (Point3D point3d : point3ds) { if (!pointBelowPlane(point3d, plane, tolerance)) { return false; } } return true; }
private CombinedTerrainObject3D createStepsGroundProfile() { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D("stairs"); AppearanceDefinition color = YoAppearance.DarkGray(); double courseAngle = 135; int numberOfSteps = 3; double rise = 0.2; double startDistance = 4.0; double run = 0.4; for (int i = 0; i < numberOfSteps; i++) { double[] newPoint = rotateAroundOrigin(new double[] {startDistance + (i * run), 0}, courseAngle); setUpWall(combinedTerrainObject, newPoint, 3.0, run, rise * (i + 1), courseAngle, color); } { double[] newPoint = rotateAroundOrigin(new double[] {startDistance + (numberOfSteps * run), 0}, courseAngle); setUpWall(combinedTerrainObject, newPoint, 3.0, run, rise * (numberOfSteps - 1 + 1), courseAngle, color); } for (int i = 1; i < numberOfSteps + 1; i++) { double offset = numberOfSteps * run; double[] newPoint = rotateAroundOrigin(new double[] {offset + startDistance + (i * run), 0}, courseAngle); setUpWall(combinedTerrainObject, newPoint, 3.0, run, rise * (-i + numberOfSteps + 1), courseAngle, color); } return combinedTerrainObject; }
boolean visualizeAndKeepUp = false; CombinedTerrainObject3D groundProfile = createOddTerrainProfile(); QuadTreeFootstepSnapper footstepSnapper = createConvexHullFootstepSnapper();
boolean visualizeAndKeepUp = false; CombinedTerrainObject3D groundProfile = createStepsGroundProfile(); SimpleFootstepSnapper footstepSnapper = createSimpleFootstepSnapper();
boolean visualizeAndKeepUp = false; CombinedTerrainObject3D groundProfile = createStepsGroundProfile(); QuadTreeFootstepSnapper footstepSnapper = createConvexHullFootstepSnapper();
boolean visualizeAndKeepUp = false; CombinedTerrainObject3D groundProfile = createOddTerrainProfile(); QuadTreeFootstepSnapper footstepSnapper = createAdjustingFootstepSnapper();
public void testSimpleFootstepSnapperOnBumpyGround() throws InsufficientDataException { boolean assertPositionConditions = true; boolean assertPointConditions = false; boolean visualizeAndKeepUp = false; GroundProfile3D groundProfile = createBumpyGroundProfile(); SimpleFootstepSnapper footstepSnapper = createSimpleFootstepSnapper(); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension); BoundingBox2D rangeOfPointsToTest = new BoundingBox2D(-1.0, -1.0, 1.0, 1.0); FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("Simple Bumpy Ground", footstepSnapper, null, visualizeAndKeepUp); double resolution = 0.02; double heightThreshold = 0.002; double quadTreeMaxMultiLevelZChangeToFilterNoise = 0.2; int maxSameHeightPointsPerNode = 20; double maxAllowableXYDistanceForAPointToBeConsideredClose = 0.2; int maxNodes = 1000000; helper.createHeightMap(groundProfile.getHeightMapIfAvailable(), rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes); int numberOfPointsToTest = 1000; helper.testRandomPoints(numberOfPointsToTest, rangeOfPointsToTest, assertPositionConditions, assertPointConditions); if (visualizeAndKeepUp) { ThreadTools.sleepForever(); } }
ConvexHullFootstepSnapper footstepSnapper = createConvexHullFootstepSnapper(); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3;