private PlanarRegionsList combine(PlanarRegionsList regionsA, PlanarRegionsList regionsB) PlanarRegionsList ret = new PlanarRegionsList(); boolean[] added = new boolean[regionsB.getNumberOfPlanarRegions()]; for (int regionBIdx = 0; regionBIdx < regionsB.getNumberOfPlanarRegions(); regionBIdx++) for (PlanarRegion regionA : regionsA.getPlanarRegionsAsList()) boolean foundMatchingRegion = false; for (int regionBIdx = 0; regionBIdx < regionsB.getNumberOfPlanarRegions(); regionBIdx++) PlanarRegion regionB = regionsB.getPlanarRegion(regionBIdx); RigidBodyTransform transformB = new RigidBodyTransform(); regionB.getTransformToWorld(transformB); ret.addPlanarRegion(new PlanarRegion(transformA, newHull)); foundMatchingRegion = true; added[regionBIdx] = true; ret.addPlanarRegion(new PlanarRegion(transformA, new ConvexPolygon2D(regionA.getConvexHull()))); for (int regionBIdx = 0; regionBIdx < regionsB.getNumberOfPlanarRegions(); regionBIdx++) ret.addPlanarRegion(regionsB.getPlanarRegion(regionBIdx));
/** * 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()); }
/** * Filter all the polygons and regions such that only polygon and regions with data remain. * Simplifies the algorithm for updating the YoVariables in {@link #processPlanarRegionsListQueue()}. * @param planarRegionsList the list of planar regions with non-empty regions and non-empty polygons. * @return */ private PlanarRegionsList filterEmptyRegionsAndEmptyPolygons(PlanarRegionsList planarRegionsList) { for (int regionIndex = planarRegionsList.getNumberOfPlanarRegions() - 1; regionIndex >= 0; regionIndex--) { PlanarRegion planarRegion = planarRegionsList.getPlanarRegion(regionIndex); for (int polygonIndex = planarRegion.getNumberOfConvexPolygons() - 1; polygonIndex >= 0; polygonIndex--) { if (planarRegion.getConvexPolygon(polygonIndex).isEmpty()) planarRegion.pollConvexPolygon(polygonIndex); } if (planarRegion.isEmpty()) planarRegionsList.pollPlanarRegion(regionIndex); } if (planarRegionsList.isEmpty()) return null; return planarRegionsList; }
/** * Retrieves the last planar region of this list. Special case: returns null when the list is * empty. */ public PlanarRegion getLastPlanarRegion() { if (isEmpty()) return null; else return getPlanarRegion(getNumberOfPlanarRegions() - 1); }
/** * Retrieves the last planar region of this list and removes it from this list. * Special case: returns null when the list is empty. */ public PlanarRegion pollLastPlanarRegion() { if (isEmpty()) return null; else return pollPlanarRegion(getNumberOfPlanarRegions() - 1); }
/** Adds the PlanarRegionsList transforming from the current coordinate system. * Uses the given appearances in order, one for each PlanarRegion. Then loops on the appearances. * * @param planarRegions */ public void addPlanarRegionsList(PlanarRegionsList planarRegions, AppearanceDefinition... appearances) { int numberOfPlanarRegions = planarRegions.getNumberOfPlanarRegions(); for (int i = 0; i < numberOfPlanarRegions; i++) { addPlanarRegion(planarRegions.getPlanarRegion(i), appearances[i % appearances.length]); } }
visiblePlanarRegions = new PlanarRegionsList(); break; case OCCLUSION_PLUS_GROUND: visiblePlanarRegions = new PlanarRegionsList(regions.getPlanarRegion(0)); break; case NO_OCCLUSION: int polygons = Math.min(maxPolygonsToVisualize, visiblePlanarRegions.getNumberOfPlanarRegions()); RigidBodyTransform transformToWorld = new RigidBodyTransform(); FramePose3D pose = new FramePose3D(); for (int polygonIdx = 0; polygonIdx < polygons; polygonIdx++) PlanarRegion planarRegion = visiblePlanarRegions.getPlanarRegion(polygonIdx); if (planarRegion.getConvexHull().getNumberOfVertices() > visiblePolygons.get(polygonIdx).getMaxNumberOfVertices()) vizGraphs.setPlanarRegions(visiblePlanarRegions.getPlanarRegionsAsList()); currentPosition.set(travelAlongBodyPath(marchingSpeedInMetersPerTick, currentPosition, bodyPath)); if (regions.findPlanarRegionsContainingPoint(currentPosition, maximumFlyingDistance) == null)
PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegions); assertEquals("Unexpected number of planar regions.", 2, planarRegionsList.getNumberOfPlanarRegions()); for (int i = 0; i < 2; i++) assertTrue(planarRegionsList.getPlanarRegion(i).epsilonEquals(planarRegions.get(i), 1.0e-10)); 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);
public static PlanarRegionsList generatePlanarRegionsListFromRandomPolygonsWithRandomTransform(Random random, int numberOfRandomlyGeneratedPolygons, double maxAbsoluteXYForPolygons, int numberOfPossiblePointsForPolygons, int numberOfPossiblePlanarRegions) { PlanarRegionsList planarRegionsList = new PlanarRegionsList(); int numberOfPlanarRegions = random.nextInt(numberOfPossiblePlanarRegions) + 1; while (planarRegionsList.getNumberOfPlanarRegions() < numberOfPlanarRegions) { planarRegionsList.addPlanarRegion(PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, numberOfRandomlyGeneratedPolygons, maxAbsoluteXYForPolygons, numberOfPossiblePointsForPolygons)); } return planarRegionsList; } }
PlanarRegionsList visiblePlanarRegions = new PlanarRegionsList(regions.getPlanarRegion(0)); int polygons = Math.min(maxPolygonsToVisualize, visiblePlanarRegions.getNumberOfPlanarRegions()); RigidBodyTransform transformToWorld = new RigidBodyTransform(); FramePose3D pose = new FramePose3D(); for (int polygonIdx = 0; polygonIdx < polygons; polygonIdx++) PlanarRegion planarRegion = visiblePlanarRegions.getPlanarRegion(polygonIdx); if (planarRegion.getConvexHull().getNumberOfVertices() > visiblePolygons.get(polygonIdx).getMaxNumberOfVertices())
public static PlanarRegionsList convertToPlanarRegionsList(PlanarRegionsListMessage planarRegionsListMessage) { List<PlanarRegion> planarRegions = new ArrayList<>(); List<PlanarRegionMessage> planarRegionMessages = planarRegionsListMessage.getPlanarRegions(); for (int regionIndex = 0; regionIndex < planarRegionMessages.size(); regionIndex++) planarRegions.add(convertToPlanarRegion(planarRegionMessages.get(regionIndex))); return new PlanarRegionsList(planarRegions); } }
/** * Projects the given point onto a planar region from the list. The projection is done along the * z axis in world frame and if there is multiple regions that the point can be projected onto, * the highest intersection point will be returned. * <p> * Will return null if the is no planar region above or below the point. */ public static Point3DReadOnly projectPointToPlanesVertically(Point3DReadOnly pointInWorld, PlanarRegionsList regions) { return projectPointToPlanesVertically(pointInWorld, regions.getPlanarRegionsAsList()); }
for (int i = 0; i < cinderBlockField.getNumberOfPlanarRegions(); i++) obstacleCourse.addPlanarRegion(cinderBlockField.getPlanarRegion(i));
/** * @return a full depth copy of this list of planar regions. The copy can be entirely modified * without interfering with this. */ public PlanarRegionsList copy() { List<PlanarRegion> planarRegionsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfPlanarRegions(); i++) planarRegionsCopy.add(regions.get(i).copy()); return new PlanarRegionsList(planarRegionsCopy); }
public PlanarRegionsList getPlanarRegionsList() { ensureHasBeenGenerated(); PlanarRegionsList planarRegionsList = new PlanarRegionsList(); for (int i = 0; i < planarRegionsLists.size(); i++) { planarRegionsList.getPlanarRegionsAsList().addAll(planarRegionsLists.get(i).getPlanarRegionsAsList()); } return planarRegionsList; }
private static PlanarRegionsList createRandomPlanarRegionList(Random random) { PlanarRegionsList planarRegionsList = new PlanarRegionsList(); for (int i = 0; i < RandomNumbers.nextInt(random, 1, 50); i++) planarRegionsList.addPlanarRegion(createRandomPlanarRegion(random, i)); return planarRegionsList; }
/** * Submit a new list of planar regions to eventually render. * This method does NOT update any YoVariables and does not update any graphics. * Once a list of planar regions is submitted, the method {@link #processPlanarRegionsListQueue()} has to be called every update tick in the caller. * @param planarRegionsList the list of planar regions to be eventually rendered. * @param requestedAppearance appearance to render the planar regions */ public void submitPlanarRegionsListToRender(PlanarRegionsList planarRegionsList, Color requestedColor) { for(int i = 0; i < planarRegionsList.getNumberOfPlanarRegions(); i++) { int index = i + currentMeshIndex.getIntegerValue(); if(index < 0 || index >= planarRegionsColorBuffer.length) break; planarRegionsColorBuffer[index].set(requestedColor.getRGB()); } submitPlanarRegionsListToRender(planarRegionsList); }
public void addPolygon(ConvexPolygon2d polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
if (planarRegionsListToProcess == null || planarRegionsListToProcess.isEmpty()) planarRegionToProcess = planarRegionsListToProcess.getLastPlanarRegion(); planarRegionsListToProcess.pollLastPlanarRegion(); planarRegionsListToProcess.pollLastPlanarRegion(); isPlanarRegionsListComplete.set(planarRegionsListToProcess.isEmpty());
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; }