How to use

addVertex

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

public void addVertices(FramePoint2d[] vertices)
{
 for (int i = 0; i < vertices.length; i++)
 {
   FramePoint2d vertex = vertices[i];
   addVertex(vertex);
 }
}

/**
* Adds N new vertices to this polygon using a list of {@code FramePoint2d}.
* Note that this method recycles memory.
* @param vertices {@code List<FramePoint2d>} the list of new vertices (it is not modified).
* @param numberOfVertices {@code int} that is used to determine the number of vertices to add to this polygon. Note the: {@code vertices.size()} can be greater or equal to numberOfVertices.
* @throws ReferenceFrameMismatchException
*/
public void addVertices(List<FramePoint2d> vertices, int numberOfVertices)
{
 for (int i = 0; i < numberOfVertices; i++)
 {
   FramePoint2d vertex = vertices.get(i);
   addVertex(vertex);
 }
}

/**
* Adds N new vertices to this polygon using a list of {@code FramePoint2d}.
* Note that this method recycles memory.
* @param vertices {@code FrameTuple2dArrayList<FramePoint2d>} the list of new vertices (it is not modified).
* @param numberOfVertices {@code int} that is used to determine the number of vertices to add to this polygon. Note the: {@code vertices.size()} can be greater or equal to numberOfVertices.
* @throws ReferenceFrameMismatchException
*/
public void addVertices(FrameTuple2dArrayList<FramePoint2d> vertices, int numberOfVertices)
{
 for (int i = 0; i < numberOfVertices; i++)
 {
   FramePoint2d vertex = vertices.get(i);
   addVertex(vertex);
 }
}

/**
* Add a vertex to this polygon.
* Note that this method recycles memory.
* @param newVertex {@code FramePoint2d} the new vertex (it is not modified).
* @throws ReferenceFrameMismatchException
*/
public void addVertex(FramePoint2d vertex)
{
 vertex.checkReferenceFrameMatch(referenceFrame);
 addVertex(vertex.getPoint());
}

private void putYoValuesIntoFrameConvexPolygon2d()
{
 try
 {
   convexPolygon2dForReading.clear(referenceFrame);
   for (int i = 0; i < numVertices.getIntegerValue(); i++)
    convexPolygon2dForReading.addVertex(yoFramePoints.get(i).getFrameTuple2d());
   convexPolygon2dForReading.update();
 }
 catch (Exception e)
 {
   System.err.println("In YoFrameConvexPolygon2d.java: " + e.getClass().getSimpleName() + " while calling putYoValuesIntoFrameConvexPolygon2d().");
 }
}

/**
* 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(ConvexPolygon2d otherPolygon)
{
 for (int i = 0; i < otherPolygon.getNumberOfVertices(); i++)
 {
   Point2d vertex = otherPolygon.getVertex(i);
   addVertex(vertex);
 }
}

/**
* This method does:
* 1- {@code clear()};
* 2- {@code addVertex(vertices.get(i)};
* 3- {@code update()}.
* @param vertices {@code FramePoint2d[]} the array of points that is used to creates the vertices.
*/
public void setAndUpdate(FramePoint2d[] vertices)
{
 clear();
 for (int i = 0; i < vertices.length; i++)
 {
   addVertex(vertices[i]);
 }
 update();
}

private void calculateReachableRegions()
{
 for (RobotSide side : RobotSide.values)
 {
   FrameConvexPolygon2d reachableRegion = new FrameConvexPolygon2d();
   reachableRegion.clear(ankleZUpFrames.get(side));
   double sign = side == RobotSide.RIGHT ? 1.0 : -1.0;
   for (int i = 0; i < MAX_CAPTURE_REGION_POLYGON_POINTS - 1; i++)
   {
    double angle = sign * reachableRegionCutoffAngle * Math.PI * ((double) i) / ((double) (MAX_CAPTURE_REGION_POLYGON_POINTS - 2));
    double x = kinematicStepRange * Math.cos(angle) + midFootAnkleXOffset;
    double y = kinematicStepRange * Math.sin(angle);
    if (Math.abs(y) < footWidth / 2.0)
      y = sign * footWidth / 2.0;
    reachableRegion.addVertex(ankleZUpFrames.get(side), x, y);
   }
   reachableRegion.addVertex(ankleZUpFrames.get(side), midFootAnkleXOffset, sign * footWidth / 2.0);
   reachableRegion.update();
   reachableRegions.set(side, reachableRegion);
 }
}

/**
* Add a vertex to this polygon after doing {@code newVertex2d.changeFrameAndProjectToXYPlane(this.referenceFrame)}.
* Note that this method recycles memory.
* @param newVertex {@code FramePoint} the new vertex (it is not modified).
*/
public void addVertexByProjectionOntoXYPlane(FramePoint newVertex)
{
 tempPoint.setIncludingFrame(newVertex);
 tempPoint.changeFrame(referenceFrame);
 addVertex(referenceFrame, tempPoint.getX(), tempPoint.getY());
}

public void computeConvexHull(FrameConvexPolygon2d convexHullToPack)
{
 convexHullToPack.clear(soleFrame);
 tempCellCenter.setToZero(soleFrame);
 for (int xIndex = 0; xIndex < nLengthSubdivisions.getIntegerValue(); xIndex++)
 {
   for (int yIndex = 0; yIndex < nWidthSubdivisions.getIntegerValue(); yIndex++)
   {
    if (isCellOccupied(xIndex, yIndex))
    {
      getCellCenter(tempCellCenter, xIndex, yIndex);
      convexHullToPack.addVertex(tempCellCenter);
    }
   }
 }
 convexHullToPack.update();
}

gridBoundaries.addVertex(new FramePoint2d(soleFrame, -footLength / 2.0, -footWidth / 2.0));
gridBoundaries.addVertex(new FramePoint2d(soleFrame, -footLength / 2.0, footWidth / 2.0));
gridBoundaries.addVertex(new FramePoint2d(soleFrame, footLength / 2.0, -footWidth / 2.0));
gridBoundaries.addVertex(new FramePoint2d(soleFrame, footLength / 2.0, footWidth / 2.0));
gridBoundaries.update();

@Override
public void doTransitionIntoAction()
{
 super.doTransitionIntoAction();
 footPolygon.clear(soleFrame);
 for (int i = 0; i < contactPoints.size(); i++)
 {
   contactPoints.get(i).getPosition2d(toeOffContactPoint2d);
   footPolygon.addVertex(toeOffContactPoint2d);
 }
 footPolygon.update();
 FramePoint2d rayOrigin;
 if (!exitCMP2d.containsNaN() && footPolygon.isPointInside(exitCMP2d))
   rayOrigin = exitCMP2d;
 else
   rayOrigin = footPolygon.getCentroid();
 rayThroughExitCMP.set(rayOrigin, exitCMPRayDirection2d);
 frameConvexPolygonWithRayIntersector2d.intersectWithRay(footPolygon, rayThroughExitCMP);
 toeOffContactPoint2d.set(frameConvexPolygonWithRayIntersector2d.getIntersectionPointOne());
 contactPointPosition.setXYIncludingFrame(toeOffContactPoint2d);
 contactPointPosition.changeFrame(contactableFoot.getRigidBody().getBodyFixedFrame());
 pointFeedbackControlCommand.setBodyFixedPointToControl(contactPointPosition);
 desiredContactPointPosition.setXYIncludingFrame(toeOffContactPoint2d);
 desiredContactPointPosition.changeFrame(worldFrame);
 desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint());
 desiredOrientation.changeFrame(worldFrame);
 desiredYawToHold = desiredOrientation.getYaw();
 desiredRollToHold = desiredOrientation.getRoll();
}