/** * @return a full depth copy of this region. The copy can be entirely modified without interfering with this region. */ public PlanarRegion copy() { RigidBodyTransform transformToWorldCopy = new RigidBodyTransform(fromLocalToWorldTransform); List<Point2d[]> concaveHullsCopy = new ArrayList<>(); for (int hullIndex = 0; hullIndex < concaveHullsVertices.size(); hullIndex++) { Point2d[] hullVertices = new Point2d[concaveHullsVertices.get(hullIndex).length]; for (int i = 0; i < hullVertices.length; i++) hullVertices[i] = new Point2d(concaveHullsVertices.get(hullIndex)[i]); concaveHullsCopy.add(hullVertices); } List<ConvexPolygon2d> convexPolygonsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfConvexPolygons(); i++) convexPolygonsCopy.add(new ConvexPolygon2d(convexPolygons.get(i))); PlanarRegion planarRegion = new PlanarRegion(transformToWorldCopy, concaveHullsCopy, convexPolygonsCopy); planarRegion.setRegionId(regionId); return planarRegion; }
public void addPolygon(ConvexPolygon2d polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void addPolygon(ConvexPolygon2D polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void addPolygons(ArrayList<ConvexPolygon2D> polygons) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygons); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
private static PlanarRegion nextPlanarRegion(Random random) { RigidBodyTransform transformToWorld = nextRegionTransform(random); Point2D[] concaveHullVertices = nextPoint2DArray(random); List<ConvexPolygon2D> convexPolygons = nextConvexPolygon2Ds(random); PlanarRegion next = new PlanarRegion(transformToWorld, concaveHullVertices, convexPolygons); next.setRegionId(random.nextInt()); return next; }
public void addPolygons(ArrayList<ConvexPolygon2d> polygons) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygons); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public void addPolygon(RigidBodyTransformGenerator transformGenerator, ConvexPolygon2D polygon) { PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), polygon); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
public static PlanarRegion generatePlanarRegionFromRandomPolygonsWithRandomTransform(Random random, int numberOfRandomlyGeneratedPolygons, double maxAbsoluteXYForPolygons, int numberOfPossiblePointsForPolygons) { List<ConvexPolygon2d> regionConvexPolygons = new ArrayList<>(); for (int i = 0; i < numberOfRandomlyGeneratedPolygons; i++) { ConvexPolygon2d randomPolygon = ConvexPolygon2d.generateRandomConvexPolygon2d(random, maxAbsoluteXYForPolygons, numberOfPossiblePointsForPolygons); regionConvexPolygons.add(randomPolygon); } for (ConvexPolygon2d convexPolygon : regionConvexPolygons) convexPolygon.update(); Vector3d randomTranslation = RandomTools.generateRandomVector(random, 10.0); Quat4d randomOrientation = RandomTools.generateRandomQuaternion(random, Math.toRadians(45.0)); RigidBodyTransform regionTransform = new RigidBodyTransform(randomOrientation, randomTranslation); return new PlanarRegion(regionTransform, regionConvexPolygons); }
public static PlanarRegion generatePlanarRegionFromRandomPolygonsWithRandomTransform(Random random, int numberOfRandomlyGeneratedPolygons, double maxAbsoluteXYForPolygons, int numberOfPossiblePointsForPolygons) { List<ConvexPolygon2D> regionConvexPolygons = new ArrayList<>(); for (int i = 0; i < numberOfRandomlyGeneratedPolygons; i++) { ConvexPolygon2D randomPolygon = EuclidGeometryRandomTools.nextConvexPolygon2D(random, maxAbsoluteXYForPolygons, numberOfPossiblePointsForPolygons); regionConvexPolygons.add(randomPolygon); } for (ConvexPolygon2D convexPolygon : regionConvexPolygons) convexPolygon.update(); Vector3D randomTranslation = RandomGeometry.nextVector3D(random, 10.0); Quaternion randomOrientation = RandomGeometry.nextQuaternion(random, Math.toRadians(45.0)); RigidBodyTransform regionTransform = new RigidBodyTransform(randomOrientation, randomTranslation); return new PlanarRegion(regionTransform, regionConvexPolygons); }
/** * @return a full depth copy of this region. The copy can be entirely modified without * interfering with this region. */ public PlanarRegion copy() { RigidBodyTransform transformToWorldCopy = new RigidBodyTransform(fromLocalToWorldTransform); Point2D[] concaveHullCopy = new Point2D[concaveHullsVertices.length]; for (int i = 0; i < concaveHullsVertices.length; i++) concaveHullCopy[i] = new Point2D(concaveHullsVertices[i]); List<ConvexPolygon2D> convexPolygonsCopy = new ArrayList<>(); for (int i = 0; i < getNumberOfConvexPolygons(); i++) convexPolygonsCopy.add(new ConvexPolygon2D(convexPolygons.get(i))); PlanarRegion planarRegion = new PlanarRegion(transformToWorldCopy, concaveHullCopy, convexPolygonsCopy); planarRegion.setRegionId(regionId); return planarRegion; }
private static PlanarRegion createRandomPlanarRegion(Random random, int idNumber) { int numberOfVertices = RandomNumbers.nextInt(random, 3, 50); int numberOfPolygons = RandomNumbers.nextInt(random, 1, 5); List<ConvexPolygon2D> polygons = new ArrayList<>(); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); for (int i = 0; i < numberOfPolygons; i++) { polygons.add(EuclidGeometryRandomTools.nextConvexPolygon2D(random, 10.0, numberOfVertices)); } PlanarRegion planarRegion = new PlanarRegion(transform, polygons); planarRegion.setRegionId(idNumber); return planarRegion; }
private void addRectangle(RigidBodyTransformGenerator transformGenerator, double lengthX, double widthY) { ConvexPolygon2d rectangle = createRectanglePolygon(lengthX, widthY); PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), rectangle); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
private void addRectangle(RigidBodyTransformGenerator transformGenerator, double lengthX, double widthY) { ConvexPolygon2D rectangle = createRectanglePolygon(lengthX, widthY); PlanarRegion planarRegion = new PlanarRegion(transformGenerator.getRigidBodyTransformCopy(), rectangle); planarRegion.setRegionId(id++); planarRegionsList.addPlanarRegion(planarRegion); }
private static PlanarRegion constructPlanarRegion(RigidBodyTransform transformToWorld, double width, double length) { ConvexPolygon2D polygon = new ConvexPolygon2D(); polygon.addVertex(0.0, -0.5 * length); polygon.addVertex(0.0, 0.5 * length); polygon.addVertex(width, -0.5 * length); polygon.addVertex(width, 0.5 * length); polygon.update(); return new PlanarRegion(transformToWorld, polygon); } }
private boolean snapToPlanarRegionAndCheckIfGoodSnap(BipedalFootstepPlannerNode nodeToExpand) { if (planarRegionsList != null) { PlanarRegion planarRegion = new PlanarRegion(); RigidBodyTransform nodeToExpandSnapTransform = getSnapAndWiggleTransform(parameters.getWiggleInsideDelta(), nodeToExpand, planarRegion); if (nodeToExpandSnapTransform == null) { return false; } nodeToExpand.transformSoleTransformWithSnapTransformFromZeroZ(nodeToExpandSnapTransform, planarRegion); baseOfCliffAvoider.shiftAwayFromCliffBottoms(parameters, planarRegionsList, nodeToExpand); boolean isEnoughArea = checkIfEnoughArea(nodeToExpand, planarRegion); if (!isEnoughArea) return false; } return true; }
private PlanarRegion createEndPlanarRegion(Point3D centered) { ConvexPolygon2D convexPolygon2D = new ConvexPolygon2D(); List<Point2D> points = createPlatformFace(); for (Point2D point : points) { point.add(centered.getX(), centered.getY()); convexPolygon2D.addVertex(point); } convexPolygon2D.update(); TranslationReferenceFrame planarRegionFrame = new TranslationReferenceFrame("planarRegionFrame", ReferenceFrame.getWorldFrame()); planarRegionFrame.updateTranslation(new Vector3D(0.0, 0.0, 0.3)); PlanarRegion planarRegion = new PlanarRegion(planarRegionFrame.getTransformToWorldFrame(), convexPolygon2D); return planarRegion; }
private PlanarRegion createSteppingStonePlanarRegion(Point3D centered) { ConvexPolygon2D convexPolygon2D = new ConvexPolygon2D(); List<Point2D> points = createSteppingStoneFace(); for (Point2D point : points) { point.add(centered.getX(), centered.getY()); convexPolygon2D.addVertex(point); } convexPolygon2D.update(); TranslationReferenceFrame planarRegionFrame = new TranslationReferenceFrame("planarRegionFrame", ReferenceFrame.getWorldFrame()); planarRegionFrame.updateTranslation(new Vector3D(0.0, 0.0, 0.3)); PlanarRegion planarRegion = new PlanarRegion(planarRegionFrame.getTransformToWorldFrame(), convexPolygon2D); return planarRegion; }
private void addPlanarRegionAtZeroHeight(double xLocation, double yLocation) { ConvexPolygon2D polygon = new ConvexPolygon2D(); polygon.addVertex(0.3, 0.3); polygon.addVertex(-0.3, 0.3); polygon.addVertex(0.3, -0.3); polygon.addVertex(-0.3, -0.25); polygon.update(); PlanarRegion planarRegion = new PlanarRegion(new RigidBodyTransform(new AxisAngle(), new Vector3D(xLocation, yLocation, 0.0)), polygon); planarRegionsList.addPlanarRegion(planarRegion); }
private void addPlanarRegionAtZeroHeight(double xLocation, double yLocation) { ConvexPolygon2D polygon = new ConvexPolygon2D(); polygon.addVertex(0.3, 0.3); polygon.addVertex(-0.3, 0.3); polygon.addVertex(0.3, -0.3); polygon.addVertex(-0.3, -0.25); polygon.update(); PlanarRegion planarRegion = new PlanarRegion(new RigidBodyTransform(new AxisAngle(), new Vector3D(xLocation, yLocation, 0.0)), polygon); planarRegionsList.addPlanarRegion(planarRegion); }
private PlanarRegion createPlanarRegion(RigidBodyTransform regionToWorldFrameTransform, ConvexPolygon2D desiredFootholdInWorldXYPlane) { RigidBodyTransform worldToRegionTransform = new RigidBodyTransform(regionToWorldFrameTransform); worldToRegionTransform.invert(); ConvexPolygon2D planarRegionPolygon = new ConvexPolygon2D(); for (int i = 0; i < desiredFootholdInWorldXYPlane.getNumberOfVertices(); i++) { Point2DReadOnly vertex = desiredFootholdInWorldXYPlane.getVertex(i); double zHeight = getPlaneZGivenXY(regionToWorldFrameTransform, vertex.getX(), vertex.getY()); Vector4D transformPoint = new Vector4D(vertex.getX(), vertex.getY(), zHeight, 1.0); worldToRegionTransform.transform(transformPoint); planarRegionPolygon.addVertex(transformPoint.getX(), transformPoint.getY()); } planarRegionPolygon.update(); return new PlanarRegion(regionToWorldFrameTransform, planarRegionPolygon); }