/** * Retrieves and returns a copy of the normal in world frame of this planar region. */ public Vector3D getNormal() { Vector3D normal = new Vector3D(); getNormal(normal); return normal; }
@Override public boolean isPlanarRegionSteppable(PlanarRegion query) { double angle = query.getNormal().angle(vertical); if (angle > getMinimumSurfaceInclineRadians() + 1e-5) return false; return true; } };
@Override public boolean isPlanarRegionNavigable(PlanarRegion query, List<PlanarRegion> allOtherRegions) { return query.getNormal().getZ() >= getNormalZThresholdForAccessibleRegions(); } };
public static List<PlanarRegion> filterRegionsByTruncatingVerticesBeneathHomeRegion(List<PlanarRegion> regionsToCheck, PlanarRegion homeRegion, double depthThresholdForConvexDecomposition, PlanarRegionFilter filter) { List<PlanarRegion> filteredList = new ArrayList<>(); Point3D pointOnPlane = new Point3D(); Vector3D planeNormal = new Vector3D(); homeRegion.getPointInRegion(pointOnPlane); homeRegion.getNormal(planeNormal); for (PlanarRegion regionToCheck : regionsToCheck) { PlanarRegion truncatedPlanarRegion = truncatePlanarRegionIfIntersectingWithPlane(pointOnPlane, planeNormal, regionToCheck, depthThresholdForConvexDecomposition, filter); if (truncatedPlanarRegion != null) filteredList.add(truncatedPlanarRegion); } return filteredList; }
@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; } }
closestContainer.getHomeRegion().getNormal(regionNormal); closestContainer.getHomeRegion().getPointInRegion(pointOnRegion); double minDistance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point, pointOnRegion, regionNormal); candidate.getHomeRegion().getNormal(regionNormal); candidate.getHomeRegion().getPointInRegion(pointOnRegion); double distance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point, pointOnRegion, regionNormal);
private static void writePlanarRegionsData(Path folderPath, PlanarRegionsList planarRegionData) throws IOException { File header = new File(folderPath.toFile(), "header.txt"); FileWriter fileWriter = new FileWriter(header); Map<Integer, MutableInt> regionIdToIndex = new HashMap<>(); for (PlanarRegion region : planarRegionData.getPlanarRegionsAsList()) { Point3D origin = new Point3D(); Vector3D normal = new Vector3D(); region.getPointInRegion(origin); region.getNormal(normal); int numberOfConvexPolygons = region.getNumberOfConvexPolygons(); int[] convexPolygonsSizes = new int[numberOfConvexPolygons]; for (int i = 0; i < numberOfConvexPolygons; i++) convexPolygonsSizes[i] = region.getConvexPolygon(i).getNumberOfVertices(); int regionId = region.getRegionId(); MutableInt regionIndex = regionIdToIndex.getOrDefault(regionId, new MutableInt(0)); regionIdToIndex.put(regionId, regionIndex); regionIndex.increment(); fileWriter.write("regionId: " + Integer.toString(regionId)); fileWriter.write(", index: " + Integer.toString(regionIndex.getValue().intValue())); fileWriter.write(", origin: " + origin.getX() + ", " + origin.getY() + ", " + origin.getZ()); fileWriter.write(", normal: " + normal.getX() + ", " + normal.getY() + ", " + normal.getZ()); fileWriter.write(", concave hull size: " + region.getConcaveHullSize()); fileWriter.write(", number of convex polygons: " + numberOfConvexPolygons + ", " + Arrays.toString(convexPolygonsSizes)); fileWriter.write("\n"); writePlanarRegionVertices(folderPath, region, regionIndex.intValue()); } fileWriter.close(); }
@Override public boolean checkIfInside(double x, double y, double z, Point3D intersectionToPack, Vector3D normalToPack) { tempPoint3dForCheckInside.setX(x); tempPoint3dForCheckInside.setY(y); tempPoint3dForCheckInside.setZ(z); boolean isPointInside = planarRegion.isPointOnOrSlightlyBelow(tempPoint3dForCheckInside, allowablePenetrationThickness); if (isPointInside) { if (intersectionToPack != null) { intersectionToPack.set(tempPoint3dForCheckInside); } if (normalToPack != null) { planarRegion.getNormal(normalToPack); } } return isPointInside; }
private static void checkPlanarRegionsEqual(int regionId, PlanarRegion planarRegionA, PlanarRegion planarRegionB) { Point3D centerA = new Point3D(); Point3D centerB = new Point3D(); planarRegionA.getPointInRegion(centerA); planarRegionB.getPointInRegion(centerB); Vector3D normalA = new Vector3D(); Vector3D normalB = new Vector3D(); planarRegionA.getNormal(normalA); planarRegionB.getNormal(normalB); EuclidCoreTestTools.assertPoint3DGeometricallyEquals("Center of regions " + regionId + " are not equal.", centerA, centerB, epsilon); EuclidCoreTestTools.assertVector3DGeometricallyEquals("Normal of regions " + regionId + " are not equal.", normalA, normalB, epsilon); assertEquals("Number of convex polygons of " + regionId + " not equal. ", planarRegionA.getNumberOfConvexPolygons(), planarRegionB.getNumberOfConvexPolygons()); for (int i = 0; i < planarRegionA.getNumberOfConvexPolygons(); i++) { assertTrue("Convex polygon " + i + " of planar region " + regionId + " is not equal", planarRegionA.getConvexPolygon(i).epsilonEquals(planarRegionB.getConvexPolygon(i), epsilon)); } }
planarRegion.getNormal(regionNormal);
planarRegionToSnapTo.getNormal(surfaceNormal);
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; }
planarRegionToSnapTo.getNormal(surfaceNormal);
planarRegion.getNormal(planarRegionNormalToPack);
planarRegion.getNormal(planarRegionSurfaceNormal);
@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)); } }