/** * Find all the planar regions that contain the given point. * The algorithm is equivalent to projecting all the regions onto the XY-plane and then finding the regions containing the point. * @param point the query coordinates. * @return the list of planar regions containing the query. Returns null when no region contains the query. */ public List<PlanarRegion> findPlanarRegionsContainingPointByProjectionOntoXYPlane(Point2d point) { return findPlanarRegionsContainingPointByProjectionOntoXYPlane(point.getX(), point.getY()); }
/** * Find all the planar regions that contain the given point. The algorithm is equivalent to * projecting all the regions onto the XY-plane and then finding the regions containing the * point. * * @param point the query coordinates. * @return the list of planar regions containing the query. Returns null when no region contains * the query. */ public List<PlanarRegion> findPlanarRegionsContainingPointByProjectionOntoXYPlane(Point2DReadOnly point) { return findPlanarRegionsContainingPointByProjectionOntoXYPlane(point.getX(), point.getY()); }
@Override public boolean isNodeValid(FootstepNode node, FootstepNode previousNode) { if(!hasPlanarRegions()) return true; Point2D nodePosition = new Point2D(node.getX(), node.getY()); List<PlanarRegion> intersection = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(nodePosition); boolean intersectsPlanarRegions = intersection != null; if(!intersectsPlanarRegions) rejectNode(node, previousNode, BipedalFootstepPlannerNodeRejectionReason.COULD_NOT_SNAP); return intersectsPlanarRegions; }
result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(0.0, 0.0); assertEquals(2, result.size()); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(2.0, 0.0); assertEquals(1, result.size()); assertTrue(result.get(0).epsilonEquals(planarRegion1, 1.0e-10)); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(-2.0, 0.0); assertEquals(1, result.size()); assertTrue(result.get(0).epsilonEquals(planarRegion1, 1.0e-10)); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(0.0, 2.0); assertEquals(1, result.size()); assertTrue(result.get(0).epsilonEquals(planarRegion2, 1.0e-10)); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(0.0, -2.0); assertEquals(1, result.size()); assertTrue(result.get(0).epsilonEquals(planarRegion2, 1.0e-10)); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(2.0, 2.0); assertNull(result); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(2.0, -2.0); assertNull(result); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(-2.0, -2.0); assertNull(result); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(-2.0, 2.0); assertNull(result); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(point2d); assertEquals(2, result.size()); point2d.set(2.0, 0.0); result = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(point2d); assertEquals(1, result.size()); assertTrue(result.get(0).epsilonEquals(planarRegion1, 1.0e-10));
List<PlanarRegion> planarRegions = planarRegionsList.findPlanarRegionsContainingPointByProjectionOntoXYPlane(snappedVertex.getX(), snappedVertex.getY());