How to use

getConvexPolygon2d

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

/**
* 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 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();
  }
}

public ConvexPolygon2d getConvexPolygon2d()
{
 putYoValuesIntoFrameConvexPolygon2d();
 return this.convexPolygon2dForReading.getConvexPolygon2d();
}

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)