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());
if(isOnBoundaryOfPlanarRegions(planarRegionsList, footPolygon))
{
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;
}