/** * Returns a transform that will move the given polygon into the convex hull of a planar region. * * @param polygonToWiggleInRegionFrame * @param regionToWiggleInto * @param wiggleParameters * @return */ public static RigidBodyTransform wigglePolygonIntoConvexHullOfRegion(ConvexPolygon2d polygonToWiggleInRegionFrame, PlanarRegion regionToWiggleInto, WiggleParameters parameters) { return findWiggleTransform(polygonToWiggleInRegionFrame, regionToWiggleInto.getConvexHull(), parameters); }
/** * Returns a transform that will move the given polygon into the convex hull of a planar region. * * @param polygonToWiggleInRegionFrame * @param regionToWiggleInto * @param wiggleParameters * @return */ public static RigidBodyTransform wigglePolygonIntoConvexHullOfRegion(ConvexPolygon2D polygonToWiggleInRegionFrame, PlanarRegion regionToWiggleInto, WiggleParameters parameters) { return findWiggleTransform(polygonToWiggleInRegionFrame, regionToWiggleInto.getConvexHull(), parameters); }
ConvexPolygon2D cp2d = planarRegionsList.getPlanarRegion(j).getConvexHull(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); planarRegionsList.getPlanarRegion(j).getTransformToWorld(transformToWorld);
ConvexPolygon2D cp2d = planarRegionsList.getPlanarRegion(j).getConvexHull(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); planarRegionsList.getPlanarRegion(j).getTransformToWorld(transformToWorld);
public static boolean isPlanarRegionIntersectingWithCircle(Point2DReadOnly circleOriginInWorld, double circleRadius, PlanarRegion query) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); query.getTransformToWorld(transformToWorld); Point2D originInLocal = new Point2D(circleOriginInWorld); originInLocal.applyInverseTransform(transformToWorld, false); return query.getConvexHull().signedDistance(originInLocal) <= circleRadius; }
transformFromAToB.preMultiplyInvertOther(transformFromBToWorld); ConvexPolygon2D convexHullB = regionB.getConvexHull(); for (int i = 0; i < regionA.getConvexHull().getNumberOfVertices(); i++) Point3D vertexA3D = new Point3D(regionA.getConvexHull().getVertex(i)); transformFromAToB.transform(vertexA3D); Point2D vertexA2D = new Point2D(vertexA3D);
public static boolean isPointInLocalInsidePlanarRegion(PlanarRegion planarRegion, Point2DReadOnly pointInLocalToCheck, double epsilon) { ConvexPolygon2D convexHull = planarRegion.getConvexHull(); BoundingBox2D boundingBox = convexHull.getBoundingBox(); if (planarRegion.getConcaveHullSize() < convexHull.getNumberOfVertices()) throw new IllegalArgumentException("The concave hull of this polygon is not valid."); if (!boundingBox.isInsideEpsilon(pointInLocalToCheck, epsilon)) return false; if (!convexHull.isPointInside(pointInLocalToCheck, epsilon)) return false; if (MathTools.epsilonEquals(0.0, epsilon, 1.0e-10)) { return isPointInsidePolygon(planarRegion.getConcaveHull(), pointInLocalToCheck); } else { double[] epsilons = new double[planarRegion.getConcaveHullSize()]; Arrays.fill(epsilons, epsilon); List<Point2D> concaveHull = ClusterTools.extrudePolygon(true, Arrays.asList(planarRegion.getConcaveHull()), epsilons); return isPointInsidePolygon(concaveHull, pointInLocalToCheck); } }
transformFromOtherToHome.preMultiplyInvertOther(transformFromHomeToWorld); for (int i = 0; i < obstacleRegion.getConvexHull().getNumberOfVertices(); i++) Point3D concaveHullVertexHome = new Point3D(obstacleRegion.getConvexHull().getVertex(i)); concaveHullVertexHome.applyTransform(transformFromOtherToHome); rawPointsInLocal.add(concaveHullVertexHome);
/** * From the local coordinates of the {@code regionB}, this method computes and return the minimum * z-coordinate among the vertices of {@code regionA}'s concave hull. * * @param regionA the query. Not modified. * @param regionB the reference. Not modified. * @return the height of the lowest vertex of {@code regionA} above {@code regionB}. The returned * value is negative if the lowest vertex is below {@code regionB}. */ public static double computeMinHeightOfRegionAAboveRegionB(PlanarRegion regionA, PlanarRegion regionB) { RigidBodyTransform transformFromBToWorld = new RigidBodyTransform(); regionB.getTransformToWorld(transformFromBToWorld); RigidBodyTransform transformFromAToB = new RigidBodyTransform(); regionA.getTransformToWorld(transformFromAToB); transformFromAToB.preMultiplyInvertOther(transformFromBToWorld); double minZ = Double.POSITIVE_INFINITY; for (int i = 0; i < regionA.getConvexHull().getNumberOfVertices(); i++) { Point3D vertexA3D = new Point3D(regionA.getConcaveHull()[i]); transformFromAToB.transform(vertexA3D); minZ = Math.min(minZ, vertexA3D.getZ()); } return minZ; }
@Override public void setPlanarRegions(PlanarRegionsList planarRegions) { super.setPlanarRegions(planarRegions); if (!hasPlanarRegions()) return; planarRegionPolytopes.clear(); for (PlanarRegion planarRegion : planarRegions.getPlanarRegionsAsList()) { ConvexPolytope planarRegionPolytope = new ConvexPolytope(); List<? extends Point2DReadOnly> pointsInPlanarRegion = planarRegion.getConvexHull().getVertexBufferView(); planarRegion.getTransformToWorld(tempTransform); for (Point2DReadOnly point : pointsInPlanarRegion) { tempPoint.setToZero(); tempPoint.set(point.getX(), point.getY(), 0.0); tempPoint.applyTransform(tempTransform); planarRegionPolytope.addVertex(tempPoint.getX(), tempPoint.getY(), tempPoint.getZ()); } planarRegionPolytopes.put(planarRegion, planarRegionPolytope); } }
if (transformA.epsilonEquals(transformB, 0.01)) ConvexPolygon2D newHull = new ConvexPolygon2D(regionA.getConvexHull(), regionB.getConvexHull()); ret.addPlanarRegion(new PlanarRegion(transformA, newHull)); foundMatchingRegion = true; ret.addPlanarRegion(new PlanarRegion(transformA, new ConvexPolygon2D(regionA.getConvexHull())));
if (transformA.epsilonEquals(transformB, 0.01)) ConvexPolygon2D newHull = new ConvexPolygon2D(regionA.getConvexHull(), regionB.getConvexHull()); ret.addPlanarRegion(new PlanarRegion(transformA, newHull)); foundMatchingRegion = true; ret.addPlanarRegion(new PlanarRegion(transformA, new ConvexPolygon2D(regionA.getConvexHull())));
if (transformA.epsilonEquals(transformB, 0.01)) ConvexPolygon2D newHull = new ConvexPolygon2D(regionA.getConvexHull(), regionB.getConvexHull()); PlanarRegion combinedRegion = new PlanarRegion(transformA, newHull); combinedRegion.setRegionId(regionA.getRegionId()); PlanarRegion region = new PlanarRegion(transformA, new ConvexPolygon2D(regionA.getConvexHull())); region.setRegionId(regionA.getRegionId()); ret.addPlanarRegion(region);
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; }
if (planarRegion.getConvexHull().getNumberOfVertices() > visiblePolygons.get(polygonIdx).getMaxNumberOfVertices()) pose.set(transformToWorld); visiblePolygonPoses.get(polygonIdx).set(pose); visiblePolygons.get(polygonIdx).set(planarRegion.getConvexHull());
private boolean isOnBoundaryOfPlanarRegions(PlanarRegionsList planarRegionsList, FrameConvexPolygon2D footPolygonInWorld) { PoseReferenceFrame planarRegionFrame = new PoseReferenceFrame("PlanarRegionFrame", ReferenceFrame.getWorldFrame()); FrameConvexPolygon2D planarRegionPolygon = new FrameConvexPolygon2D(); ConvexPolygon2D planarRegionsBoundingPolygon = new ConvexPolygon2D(); planarRegionsBoundingPolygon.clear(); for(PlanarRegion region : planarRegionsList.getPlanarRegionsAsList()) { RigidBodyTransform transform = new RigidBodyTransform(); region.getTransformToWorld(transform); planarRegionFrame.setPoseAndUpdate(transform); planarRegionPolygon.set(region.getConvexHull()); planarRegionPolygon.setReferenceFrame(planarRegionFrame); planarRegionPolygon.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); planarRegionsBoundingPolygon.addVertices(planarRegionPolygon); } planarRegionsBoundingPolygon.update(); for (int i = 0; i < footPolygonInWorld.getNumberOfVertices(); i++) { if (!planarRegionsBoundingPolygon.isPointInside(footPolygonInWorld.getVertex(i))) { return true; } } return false; }
public static Point3D intersectRegionWithRay(PlanarRegion region, Point3D rayStart, Vector3D rayDirection) { RigidBodyTransform regionToWorld = new RigidBodyTransform(); region.getTransformToWorld(regionToWorld); Vector3D planeNormal = new Vector3D(0.0, 0.0, 1.0); planeNormal.applyTransform(regionToWorld); Point3D pointOnPlane = new Point3D(); pointOnPlane.set(region.getConvexPolygon(0).getVertex(0)); pointOnPlane.applyTransform(regionToWorld); Point3D intersectionWithPlane = EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(pointOnPlane, planeNormal, rayStart, rayDirection); if (intersectionWithPlane == null) { return null; } Point3D intersectionInPlaneFrame = new Point3D(intersectionWithPlane); intersectionInPlaneFrame.applyInverseTransform(regionToWorld); // checking convex hull here - might be better to check all polygons to avoid false positive if (!region.getConvexHull().isPointInside(intersectionInPlaneFrame.getX(), intersectionInPlaneFrame.getY())) { return null; } Vector3D rayToIntersection = new Vector3D(); rayToIntersection.sub(intersectionWithPlane, rayStart); if (rayToIntersection.dot(rayDirection) < 0.0) { return null; } return intersectionWithPlane; }
ConvexPolygon2D convexHull = region.getConvexHull(); if (!convexHull.isPointInside(intersectionInPlaneFrame2D))
footPolygonInRegion.set(footStepPolygon); footPolygonInRegion.applyTransform(soleToRegion, false); convexPolygonTools.computeIntersectionOfPolygons(regionToMoveTo.getConvexHull(), footPolygonInRegion, foothold); soleToRegion.invert(); foothold.applyTransform(soleToRegion, false);
footPolygonInRegion.setAndUpdate(footStepPolygon); footPolygonInRegion.applyTransformAndProjectToXYPlane(soleToRegion); ConvexPolygon2d foothold = regionToMoveTo.getConvexHull().intersectionWith(footPolygonInRegion); soleToRegion.invert(); foothold.applyTransformAndProjectToXYPlane(soleToRegion);