/** * Find the closest planar region to the given point. The algorithm is equivalent to * projecting all the regions onto the XY-plane and then finding the closest one to the point. * * @param point the query coordinates. * @return the planar regions closest to the query. */ public PlanarRegion findClosestPlanarRegionToPointByProjectionOntoXYPlane(Point2DReadOnly point) { return findClosestPlanarRegionToPointByProjectionOntoXYPlane(point.getX(), point.getY()); }
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; }