public ConvexPolygon2d getConvexPolygon2d() { putYoValuesIntoFrameConvexPolygon2d(); return this.convexPolygon2dForReading.getConvexPolygon2d(); }
public void shrinkConstantDistanceInto(FrameConvexPolygon2d polygonQ, double distance, FrameConvexPolygon2d framePolygonToPack) { if (Math.abs(distance) < 1.0e-10) { framePolygonToPack.setIncludingFrameAndUpdate(polygonQ); return; } framePolygonToPack.clear(polygonQ.getReferenceFrame()); framePolygonToPack.update(); ConvexPolygon2d polygon2dToPack = framePolygonToPack.getConvexPolygon2d(); shrinkConstantDistanceInto(polygonQ.getConvexPolygon2dCopy(), distance, polygon2dToPack); // framePolygonToPack.updateFramePoints(); framePolygonToPack.update(); } }
/** * This function changes the polygon given, such that it has the desired number of vertices. It is conservative in * the sense, that the modified polygon will be contained in the original polygon completely. * * @param polygon: modified to have the desired number of vertices * @param desiredVertices: number of vertices that the polygon should have */ public static void limitVerticesConservative(FrameConvexPolygon2d polygon, int desiredVertices) { limitVerticesConservative(polygon.getConvexPolygon2d(), desiredVertices); }
public static int cutPolygonWithLine(FrameLine2d cuttingLine, FrameConvexPolygon2d polygonToCut, RobotSide sideOfLineToCut) { cuttingLine.checkReferenceFrameMatch(polygonToCut); return cutPolygonWithLine(cuttingLine.getLine2d(), polygonToCut.getConvexPolygon2d(), sideOfLineToCut); }
public void intersectWithLine(FrameConvexPolygon2d frameConvexPolygon2d, FrameLine2d frameLine2d) { checkAndSetFrames(frameConvexPolygon2d, frameLine2d); int intersectionTypeInt = ConvexPolygon2dCalculator.intersectionWithLine(frameLine2d.getLine2d(), intersectionPointOne.getPoint(), intersectionPointTwo.getPoint(), frameConvexPolygon2d.getConvexPolygon2d()); packIntersectionType(intersectionTypeInt); }
/** * There is actually no ray class at the moment, so we use a FrameLine2d. * * TODO: Make ray classes and use them. @dcalvert */ public void intersectWithRay(FrameConvexPolygon2d frameConvexPolygon2d, FrameLine2d frameRay2d) { checkAndSetFrames(frameConvexPolygon2d, frameRay2d); int intersectionTypeInt = ConvexPolygon2dCalculator.intersectionWithRay(frameRay2d.getLine2d(), intersectionPointOne.getPoint(), intersectionPointTwo.getPoint(), frameConvexPolygon2d.getConvexPolygon2d()); packIntersectionType(intersectionTypeInt); }
public void setNextFootstep(Footstep nextFootstep) { isUsingNextFootstep.set(nextFootstep != null); if (isUsingNextFootstep.getBooleanValue()) { ReferenceFrame footstepSoleFrame = nextFootstep.getSoleReferenceFrame(); ConvexPolygon2d footPolygon = footPolygons.get(nextFootstep.getRobotSide()).getConvexPolygon2d(); nextFootstepPolygon.setIncludingFrameAndUpdate(footstepSoleFrame, footPolygon); nextFootstepPolygon.changeFrameAndProjectToXYPlane(worldFrame); } }
public FootstepAdjustor(SideDependentList<? extends ContactablePlaneBody> contactableFeet, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { parentRegistry.addChild(registry); if (yoGraphicsListRegistry != null && VISUALIZE) { footstepAdjusterVisualizer = new FootstepAdjusterVisualizer(this, yoGraphicsListRegistry, registry); } defaultSupportPolygons = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { defaultSupportPolygons.put(robotSide, new FrameConvexPolygon2d(contactableFeet.get(robotSide).getContactPoints2d()).getConvexPolygon2d()); } }
/** * This function is called at beginning of every DoubleSupport, not frequent enough to preallocate eveything. */ public void updatePointAndPolygon(FrameConvexPolygon2d polygon, Point2d pointInPolygonFrame) { //copy external point back to polygon frame framePoint2d = new FramePoint2d(polygon.getReferenceFrame(), pointInPolygonFrame); //update polygon class point2dInConvexPolygon2d = new Point2dInConvexPolygon2d(polygon.getConvexPolygon2d(), framePoint2d.getX(), framePoint2d.getY()); angle.set(point2dInConvexPolygon2d.getAngle()); eccentricity.set(point2dInConvexPolygon2d.getEccentricity()); }
/** * All math in polygon frame. */ public boolean checkIfIntersectionExists(FrameSphere3d sphere, FrameConvexPolygon2d polygon) { ReferenceFrame originalSphereFrame = sphere.getReferenceFrame(); sphere.changeFrame(polygon.getReferenceFrame()); sphere.getCenter(closestPointOnPolygon); closestPointOnPolygon2d.setByProjectionOntoXYPlaneIncludingFrame(closestPointOnPolygon); Point2d pointUnsafe = closestPointOnPolygon2d.getPoint(); ConvexPolygon2dCalculator.orthogonalProjection(pointUnsafe, polygon.getConvexPolygon2d()); closestPointOnPolygon2d.set(pointUnsafe.getX(), pointUnsafe.getY()); closestPointOnPolygon.setXY(closestPointOnPolygon2d); boolean isInsideOrOnSurface = sphere.getSphere3d().isInsideOrOnSurface(closestPointOnPolygon.getPoint()); closestPointOnPolygon.changeFrame(ReferenceFrame.getWorldFrame()); sphere.changeFrame(originalSphereFrame); return isInsideOrOnSurface; }
defaultFootPolygons.put(robotSide, defaultFootPolygon.getConvexPolygon2d());
tempPolygon2.setIncludingFrameAndUpdate(nextFootstep.getSoleReferenceFrame(), tempPolygon1.getConvexPolygon2d()); tempPolygon2.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); polygonShrinker.shrinkConstantDistanceInto(tempPolygon2, -distanceToExtendUpcomingFoothold.getDoubleValue(), tempPolygon1);
ConvexPolygon2d footPolygon = footDefaultPolygons.get(nextFootstep.getRobotSide()).getConvexPolygon2d(); nextFootstepPolygon.setIncludingFrameAndUpdate(footstepSoleFrame, footPolygon);
RigidBodyTransform snapTransform = PlanarRegionsListPolygonSnapper.snapPolygonToPlanarRegionsList(footPolygon.getConvexPolygon2d(), planarRegionsList, regionToMoveTo); if (snapTransform == null)