How to use

getNumberOfVertices

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

private void getYoValuesFromFrameConvexPolygon2d()
{
 numVertices.set(convexPolygon2dForWriting.getNumberOfVertices());
 try
 {
   for (int i = 0; i < numVertices.getIntegerValue(); i++)
   {
    yoFramePoints.get(i).checkReferenceFrameMatch(convexPolygon2dForWriting);
    yoFramePoints.get(i).set(convexPolygon2dForWriting.getVertex(i));
   }
 }
 catch (Exception e)
 {
   System.err.println("In YoFrameConvexPolygon2d.java: " + e.getClass().getSimpleName() + " while calling getYoValuesFromFrameConvexPolygon2d().");
   e.printStackTrace();
 }
}

@Override
public String toString()
{
 FramePoint2d vertex = new FramePoint2d();
 String ret = "";
 for (int i = 0; i < getNumberOfVertices(); i++)
 {
   this.getFrameVertex(i, vertex);
   ret = ret + vertex.toString() + "\n";
 }
 return ret;
}

public void setSupportPolygon(RobotSide robotSide, FrameConvexPolygon2d footPolygon)
{
 int numberOfVertices = footPolygon.getNumberOfVertices();
 if (numberOfVertices > MAXIMUM_NUMBER_OF_VERTICES)
 {
   numberOfVertices = MAXIMUM_NUMBER_OF_VERTICES;
 }
 if (robotSide == RobotSide.LEFT)
 {
   leftFootSupportPolygonLength = numberOfVertices;
 }
 else
 {
   rightFootSupportPolygonLength = numberOfVertices;
 }
 for (int i = 0; i < numberOfVertices; i++)
 {
   if (robotSide == RobotSide.LEFT)
   {
    footPolygon.getVertex(i, leftFootSupportPolygonStore[i]);
   }
   else
   {
    footPolygon.getVertex(i, rightFootSupportPolygonStore[i]);
   }
 }
}

public void updateCMPConstraintForSingleSupport(RobotSide supportSide, ICPOptimizationSolver solver)
{
 FrameConvexPolygon2d supportPolygon = bipedSupportPolygons.getFootPolygonInSoleFrame(supportSide);
 solver.setNumberOfCMPVertices(supportPolygon.getNumberOfVertices());
 for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++)
 {
   supportPolygon.getFrameVertex(i, tempVertex);
   solver.setSupportPolygonVertex(i, tempVertex, supportPolygon.getReferenceFrame(), maxCMPSingleSupportExitForward.getDoubleValue(),
      maxCMPSingleSupportExitSideways.getDoubleValue());
 }
}

public void updateCMPConstraintForDoubleSupport(ICPOptimizationSolver solver)
{
 int numberOfVertices = 0;
 for (RobotSide robotSide : RobotSide.values)
   numberOfVertices += bipedSupportPolygons.getFootPolygonInMidFeetZUp(robotSide).getNumberOfVertices();
 solver.setNumberOfCMPVertices(numberOfVertices);
 numberOfVertices = 0;
 for (RobotSide robotSide : RobotSide.values)
 {
   FrameConvexPolygon2d supportPolygon = bipedSupportPolygons.getFootPolygonInMidFeetZUp(robotSide);
   for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++)
   {
    supportPolygon.getFrameVertex(i, tempVertex);
    solver.setSupportPolygonVertex(numberOfVertices + i, tempVertex, supportPolygon.getReferenceFrame(), maxCMPDoubleSupportExitForward.getDoubleValue(),
       maxCMPDoubleSupportExitSideways.getDoubleValue());
   }
   numberOfVertices += supportPolygon.getNumberOfVertices();
 }
}

public void resetFootSupportPolygon(RobotSide robotSide)
{
 YoPlaneContactState contactState = footContactStates.get(robotSide);
 List<YoContactPoint> contactPoints = contactState.getContactPoints();
 FrameConvexPolygon2d defaultSupportPolygon = defaultFootPolygons.get(robotSide);
 for (int i = 0; i < defaultSupportPolygon.getNumberOfVertices(); i++)
 {
   defaultSupportPolygon.getFrameVertexXY(i, tempPosition);
   contactPoints.get(i).setPosition(tempPosition);
 }
 contactState.notifyContactStateHasChanged();
}

/**
* Adds new vertices to this polygon from another convex polygon.
* Note that this method recycles memory.
* @param otherPolygon {@code FrameConvexPolygon2d} the other convex polygon that is used to add new vertices to this polygon.
* @throws ReferenceFrameMismatchException
*/
public void addVertices(FrameConvexPolygon2d otherPolygon)
{
 referenceFrame.checkReferenceFrameMatch(otherPolygon.getReferenceFrame());
 for (int i = 0; i < otherPolygon.getNumberOfVertices(); i++)
 {
   Point2d vertex = otherPolygon.convexPolygon.getVertex(i);
   convexPolygon.addVertex(vertex);
 }
}

private void drawPolygon(FrameConvexPolygon2d frameConvexPolygon2d, Graphics graphics)
{
 Polygon polygon = new Polygon();
 for (int i = 0; i < frameConvexPolygon2d.getNumberOfVertices(); i++)
 {
   Point2d vertex = frameConvexPolygon2d.getVertex(i);
   int xInt = doubleToInt(vertex.getX(), xCenter, scale, getWidth());
   int yInt = doubleToInt(vertex.getY(), yCenter, -scale, getHeight());
   polygon.addPoint(xInt, yInt);
 }
 graphics.drawPolygon(polygon);
}

for (int contactPointIndex = 0; contactPointIndex < footSupportPolygon.getNumberOfVertices(); contactPointIndex++)

public static void movePointInsidePolygonAlongVector(FramePoint2d pointToMove, FrameVector2d vector, FrameConvexPolygon2d polygon, double distanceToBeInside)
   if (polygon.getNumberOfVertices() < 2)

private void computeToePoints(RobotSide supportSide)
{
 FrameConvexPolygon2d footDefaultPolygon = footDefaultPolygons.get(supportSide);
 toePoints[0].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0);
 toePoints[1].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0);
 for (int i = 0; i < footDefaultPolygon.getNumberOfVertices(); i++)
 {
   footDefaultPolygon.getFrameVertex(i, footPoint);
   if (footPoint.getX() > toePoints[0].getX())
   {
    toePoints[1].set(toePoints[0]);
    toePoints[0].setXY(footPoint);
   }
   else if (footPoint.getX() > toePoints[1].getX())
   {
    toePoints[1].setXY(footPoint);
   }
 }
 middleToePoint.setToZero(footDefaultPolygon.getReferenceFrame());
 middleToePoint.interpolate(toePoints[0], toePoints[1], 0.5);
}

int corners = supportPolygon.getNumberOfVertices();
currentCornerIdx = (int) (timeExploring / timeToExploreCorner);
int corner = currentCornerIdx % corners;

private void sequenceShiftWeight()
{
 FramePoint2d center = new FramePoint2d(midFeetZUpFrame);
 FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d());
 supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame);
 FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame);
 for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++)
 {
   supportPolygon.getFrameVertex(i, desiredPelvisOffset);
   desiredPelvisOffset.sub(center);
   submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(),
      pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0);
 }
 // Get back to the first vertex again
 supportPolygon.getFrameVertex(0, desiredPelvisOffset);
 desiredPelvisOffset.sub(center);
 submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(),
    pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0);
 submitPelvisHomeCommand(false);
}

private void sequenceMediumWarmup()
{
 FramePoint2d center = new FramePoint2d(midFeetZUpFrame);
 FrameVector2d shiftScaleVector = new FrameVector2d(midFeetZUpFrame, 0.1, 0.7);
 FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d());
 supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame);
 FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame);
 for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++)
 {
   supportPolygon.getFrameVertex(i, desiredPelvisOffset);
   desiredPelvisOffset.sub(center);
   submitDesiredPelvisPositionOffset(false, shiftScaleVector.getX() * desiredPelvisOffset.getX(), shiftScaleVector.getY() * desiredPelvisOffset.getY(),
      0.0);
   sequenceSquats();
   sequenceChestRotations(0.55); //TODO increase/decrease limit?
   sequencePelvisRotations(0.3); //TODO increase/decrease limit?
 }
 // Get back to the first vertex again
 supportPolygon.getFrameVertex(0, desiredPelvisOffset);
 desiredPelvisOffset.sub(center);
 submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(),
    pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0);
 submitChestHomeCommand(false);
 submitPelvisHomeCommand(false);
}

for (int i = 0; i < controllerFootPolygon.getNumberOfVertices(); i++)

int numberOfVertices = supportPolygon.getNumberOfVertices();
ArrayList<FramePoint2d> supportCornerPoints = new ArrayList<>();

int corners = supportPolygon.getNumberOfVertices();
currentCornerIdx = (int) (timeExploring / timeToExploreCorner);
currentCornerIdx = currentCornerIdx % corners;

for (int i = 0; i < projectionArea.getNumberOfVertices(); i++)