@Override public double heightAt(double x, double y, double z) { if (planarRegion.isPointInsideByProjectionOntoXYPlane(x, y)) { return planarRegion.getPlaneZGivenXY(x, y); } else { return 0.0; } }
@Override public double heightAndNormalAt(double x, double y, double z, Vector3D normalToPack) { if (planarRegion.isPointInsideByProjectionOntoXYPlane(x, y)) { if (normalToPack != null) { planarRegion.getNormal(normalToPack); } return planarRegion.getPlaneZGivenXY(x, y); } else { normalToPack.set(0.0, 0.0, 1.0); return 0.0; } }
/** * Computes the distance of the point to the region projected onto the world xy-plane. * * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return distance to this region. If 0.0, point is in the region. */ public double distanceToPointByProjectionOntoXYPlane(double x, double y) { localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); localPoint2D.set(localPoint); return distanceToPoint(localPoint2D); }
private void assertThatAllPolygonVerticesAreInBoundingBox(List<ConvexPolygon2D> regionConvexPolygons, PlanarRegion planarRegion, BoundingBox3D boundingBox3dInWorld) { for (ConvexPolygon2D convexPolygon2dInWorld : regionConvexPolygons) { for (int i = 0; i < convexPolygon2dInWorld.getNumberOfVertices(); i++) { Point2DReadOnly vertex = convexPolygon2dInWorld.getVertex(i); double planeZGivenXY = planarRegion.getPlaneZGivenXY(vertex.getX(), vertex.getY()); assertTrue("Polygon vertex is not inside computed bounding box.\nVertex: " + vertex + "\nPlane z at vertex: " + planeZGivenXY + "\nBounding Box: " + boundingBox3dInWorld, boundingBox3dInWorld.isInsideInclusive(vertex.getX(), vertex.getY(), planeZGivenXY)); } } }
/** * Computes if the point is in the region projected onto the world xy-plane. * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInsideByProjectionOntoXYPlane(double x, double y) { Point3d localPoint = new Point3d(); localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); return isPointInside(localPoint.getX(), localPoint.getY()); }
/** * Computes if the point is in the region projected onto the world xy-plane. * * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInsideByProjectionOntoXYPlane(double x, double y) { Point3D localPoint = new Point3D(); localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); return isPointInside(localPoint.getX(), localPoint.getY()); }
transformToWorldFromIntersectingPlanarRegion.transform(vertex3dInWorld); double planeZGivenXY = highestElevationPlanarRegion.getPlaneZGivenXY(vertex3dInWorld.getX(), vertex3dInWorld.getY());
@ContinuousIntegrationTest(estimatedDuration = 5.0) @Test(timeout = 30000) public void testHeightAt() throws Exception { Random random = new Random(1776L); for (int i = 0; i < 100000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, random.nextInt(10), RandomNumbers.nextDouble(random, 0.0, 30.0), random.nextInt(10)); BoundingBox3D boundingBox3dInWorld = planarRegion.getBoundingBox3dInWorld(); double randomXCoord = RandomNumbers.nextDouble(random, boundingBox3dInWorld.getMinX() - 10.0, boundingBox3dInWorld.getMaxX() + 10.0); double randomYCoord = RandomNumbers.nextDouble(random, boundingBox3dInWorld.getMinY() - 10.0, boundingBox3dInWorld.getMaxY() + 10.0); double randomZCoord = RandomNumbers.nextDouble(random, boundingBox3dInWorld.getMinZ() - 10.0, boundingBox3dInWorld.getMaxZ() + 10.0); PlanarRegionTerrainObject terrainObject = new PlanarRegionTerrainObject(planarRegion, DEFAULT_ALLOWABLE_PENETRATION_THICKNESS); double planarRegionZAtXY = planarRegion.getPlaneZGivenXY(randomXCoord, randomYCoord); if (planarRegion.isPointInsideByProjectionOntoXYPlane(randomXCoord, randomYCoord)) { assertEquals(planarRegionZAtXY, terrainObject.heightAt(randomXCoord, randomYCoord, randomZCoord), 1e-10); } else { assertNotEquals(planarRegionZAtXY, terrainObject.heightAt(randomXCoord, randomYCoord, randomZCoord), 1e-10); } } }
public ConvexPolygon2D snapAndWiggle(FramePose3D solePose, ConvexPolygon2DReadOnly footStepPolygon, boolean walkingForward) throws SnappingFailedException { if (planarRegionsList == null) { return null; } planarRegionsList.getPlanarRegionsAsList().removeIf(region -> region.getConvexHull().getArea() < parameters.getMinPlanarRegionArea()); planarRegionsList.getPlanarRegionsAsList().removeIf(region -> region.getNormal().getZ() < Math.cos(parameters.getMaxPlanarRegionAngle())); FramePose3D solePoseBeforeSnapping = new FramePose3D(solePose); PoseReferenceFrame soleFrameBeforeSnapping = new PoseReferenceFrame("SoleFrameBeforeSnapping", solePose); FrameConvexPolygon2D footPolygon = new FrameConvexPolygon2D(soleFrameBeforeSnapping, footStepPolygon); footPolygon.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); // this works if the soleFrames are z up. if(isOnBoundaryOfPlanarRegions(planarRegionsList, footPolygon)) { /* * If foot is on the boundary of planar regions, don't snap/wiggle but * set it to the nearest plane's height */ FixedFramePoint3DBasics footPosition = solePose.getPosition(); PlanarRegion closestRegion = planarRegionsList.findClosestPlanarRegionToPointByProjectionOntoXYPlane(footPosition.getX(), footPosition.getY()); solePose.setZ(closestRegion.getPlaneZGivenXY(footPosition.getX(), footPosition.getY())); return new ConvexPolygon2D(footStepPolygon); } ConvexPolygon2D foothold = doSnapAndWiggle(solePose, footStepPolygon, footPolygon); checkAndHandleTopOfCliff(solePoseBeforeSnapping, solePose, walkingForward, footStepPolygon, footPolygon); checkAndHandleBottomOfCliff(solePose); return foothold; }
double planarRegionZAtXY = planarRegion.getPlaneZGivenXY(randomXCoord, randomYCoord); double heightAt = terrainObject.heightAndNormalAt(randomXCoord, randomYCoord, randomZCoord, terrainObjectNormalToPack); planarRegion.getNormal(planarRegionNormalToPack);
/** * Projects the input LineSegment2d to the plane defined by this PlanarRegion by translating each vertex in world z. * Then puts each vertex in local frame. In doing so, the length of the rotated lineSegment will actually increase on tilted PlanarRegions. * @param lineSegmentInWorld LineSegment2d to project * @return new projected LineSegment2d */ private LineSegment2d projectLineSegmentVerticallyToRegion(LineSegment2d lineSegmentInWorld) { Point2d[] snappedEndpoints = new Point2d[2]; Point2d originalVertex = lineSegmentInWorld.getFirstEndpoint(); Point3d snappedVertex3d = new Point3d(); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); snappedEndpoints[0] = new Point2d(snappedVertex3d.getX(), snappedVertex3d.getY()); originalVertex = lineSegmentInWorld.getSecondEndpoint(); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); snappedEndpoints[1] = new Point2d(snappedVertex3d.getX(), snappedVertex3d.getY()); LineSegment2d projectedLineSegment = new LineSegment2d(snappedEndpoints); return projectedLineSegment; }
/** * Projects the input LineSegment2d to the plane defined by this PlanarRegion by translating each * vertex in world z. Then puts each vertex in local frame. In doing so, the length of the * rotated lineSegment will actually increase on tilted PlanarRegions. * * @param lineSegmentInWorld LineSegment2d to project * @return new projected LineSegment2d */ private LineSegment2D projectLineSegmentVerticallyToRegion(LineSegment2D lineSegmentInWorld) { Point2DReadOnly originalVertex = lineSegmentInWorld.getFirstEndpoint(); Point3D snappedVertex3d = new Point3D(); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); Point2D snappedFirstEndpoint = new Point2D(snappedVertex3d.getX(), snappedVertex3d.getY()); originalVertex = lineSegmentInWorld.getSecondEndpoint(); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); Point2D snappedSecondEndpoint = new Point2D(snappedVertex3d.getX(), snappedVertex3d.getY()); LineSegment2D projectedLineSegment = new LineSegment2D(snappedFirstEndpoint, snappedSecondEndpoint); return projectedLineSegment; }
assertEquals(planarRegionToSnapTo.getPlaneZGivenXY(-1.0, -1.0), highVertexOne.getZ(), 1e-7); assertEquals(planarRegionToSnapTo.getPlaneZGivenXY(-1.0, 1.0), highVertexTwo.getZ(), 1e-7);
/** * Projects the input ConvexPolygon2d to the plane defined by this PlanarRegion by translating each vertex in world z. * Then puts each vertex in local frame. In doing so, the area of the rotated polygon will actually increase on tilted PlanarRegions. * @param convexPolygonInWorld Polygon to project * @return new projected ConvexPolygon2d */ private ConvexPolygon2d projectPolygonVerticallyToRegion(ConvexPolygon2d convexPolygonInWorld) { ConvexPolygon2d projectedPolygon = new ConvexPolygon2d(); Point3d snappedVertex3d = new Point3d(); for (int i = 0; i < convexPolygonInWorld.getNumberOfVertices(); i++) { Point2d originalVertex = convexPolygonInWorld.getVertex(i); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); // Add the snapped vertex to the snapped polygon projectedPolygon.addVertex(snappedVertex3d.getX(), snappedVertex3d.getY()); } projectedPolygon.update(); return projectedPolygon; }
/** * Projects the input ConvexPolygon2d to the plane defined by this PlanarRegion by translating * each vertex in world z. Then puts each vertex in local frame. In doing so, the area of the * rotated polygon will actually increase on tilted PlanarRegions. * * @param convexPolygonInWorld Polygon to project * @return new projected ConvexPolygon2d */ private ConvexPolygon2D projectPolygonVerticallyToRegion(ConvexPolygon2DReadOnly convexPolygonInWorld) { ConvexPolygon2D projectedPolygon = new ConvexPolygon2D(); Point3D snappedVertex3d = new Point3D(); for (int i = 0; i < convexPolygonInWorld.getNumberOfVertices(); i++) { Point2DReadOnly originalVertex = convexPolygonInWorld.getVertex(i); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); // Add the snapped vertex to the snapped polygon projectedPolygon.addVertex(snappedVertex3d.getX(), snappedVertex3d.getY()); } projectedPolygon.update(); return projectedPolygon; }
assertEquals(planarRegionToSnapTo.getPlaneZGivenXY(-1.0, -1.0), highVertexOne.getZ(), 1e-7);
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testIsPointOnOrSlightlyBelow() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointOnOrSlightlyBelow(pointBelow, 1e-5)); assertFalse(planarRegion.isPointOnOrSlightlyBelow(pointAbove, 1e-5)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testIsPointOn() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointOnOrSlightlyAbove(pointAbove, 1e-5)); assertFalse(planarRegion.isPointOnOrSlightlyAbove(pointBelow, 1e-5)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testIsPointInWorld2DInside() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointInWorld2DInside(pointAbove)); assertTrue(planarRegion.isPointInWorld2DInside(pointBelow)); } }
double planeZGivenXY = planarRegion.getPlaneZGivenXY(snappedVertex.getX(), snappedVertex.getY());