How to use

addVertices

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

/**
* This method does:
* 1- {@code clear()};
* 2- {@code addVertices(vertices, numberOfVertices)};
* 3- {@code update()}.
* @param vertices {@code List<FramePoint2d>} the list of points that is used to creates the vertices.
* @param numberOfVertices {@code int} that is used to determine the number of vertices of the polygon. Note the: {@code vertices.size()} can be greater or equal to numberOfVertices.
* @throws ReferenceFrameMismatchException
*/
public void setAndUpdate(List<FramePoint2d> vertices, int numberOfVertices)
{
 clear();
 addVertices(vertices, numberOfVertices);
 update();
}

/**
* This method does:
* 1- {@code clear()};
* 2- {@code addVertices(otherPolygon)};
* 3- {@code update()}.
* <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/>
* @param otherPolygon {@code FrameConvexPolygon2d}
* @throws ReferenceFrameMismatchException
*/
public void setAndUpdate(ConvexPolygon2d otherPolygon)
{
 clear();
 addVertices(otherPolygon);
 update();
}

/**
* This method does:
* 1- {@code clear(otherPolygon.getReferenceFrame())};
* 2- {@code addVertices(otherPolygon)};
* 3- {@code update()}.
* <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/>
* @param otherPolygon {@code FrameConvexPolygon2d}
*/
public void setIncludingFrameAndUpdate(FrameConvexPolygon2d otherPolygon)
{
 clear(otherPolygon.getReferenceFrame());
 addVertices(otherPolygon);
 update();
}

/**
* This method does:
* 1- {@code clear()};
* 2- {@code addVertices(firstPolygon)};
* 2- {@code addVertices(secondPolygon)};
* 3- {@code update()}.
* <p/> TODO: Make this more efficient by finding the rotating calipers, as in the intersection method.<p/>
* @param firstPolygon {@code FrameConvexPolygon2d}
* @param secondPolygon {@code FrameConvexPolygon2d}
* @throws ReferenceFrameMismatchException
*/
public void setAndUpdate(FrameConvexPolygon2d firstPolygon, FrameConvexPolygon2d secondPolygon)
{
 referenceFrame.checkReferenceFrameMatch(firstPolygon.getReferenceFrame());
 referenceFrame.checkReferenceFrameMatch(secondPolygon.getReferenceFrame());
 clear();
 addVertices(firstPolygon);
 addVertices(secondPolygon);
 update();
}

/**
* If the list of vertices is empty, this polygon will be empty and its reference frame won't be changed.
* If the list of vertices is not empty, this method does:
* 1- {@code clear(vertices.get(0).getReferenceFrame())};
* 2- {@code addVertices(vertices)};
* 3- {@code update()}.
* @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon.
* @param vertices {@code List<FramePoint2d>} the list of points that is used to creates the vertices.
* @throws ReferenceFrameMismatchException
*/
public void setIncludingFrameAndUpdate(List<FramePoint2d> vertices)
{
 int numberOfVertices = vertices.size();
 if (numberOfVertices < 1 )
 {
   clear();
   update();
 }
 else
 {
   clear(vertices.get(0).getReferenceFrame());
   addVertices(vertices, numberOfVertices);
   update();
 }
}

/**
* This method does:
* 1- {@code clear()};
* 2- {@code addVertices(otherPolygon)};
* 3- {@code update()}.
* <p/> TODO There is no need to call update() there, instead update everything from the other polygon to make it faster.<p/>
* @param otherPolygon {@code FrameConvexPolygon2d}
* @throws ReferenceFrameMismatchException
*/
public void setAndUpdate(FrameConvexPolygon2d otherPolygon)
{
 referenceFrame.checkReferenceFrameMatch(otherPolygon.getReferenceFrame());
 clear();
 addVertices(otherPolygon);
 update();
}

private void fitLine()
{
 if (!footCoPOccupancyGrid.fitLineToData(line))
   return;
 lineL.setIncludingFrame(line);
 lineL.shiftToLeft(width/2.0);
 lineR.setIncludingFrame(line);
 lineR.shiftToRight(width/2.0);
 backupFootPolygon.set(shrunkFootPolygon);
 shrunkFootPolygon.clear();
 shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineL));
 shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineR));
 shrunkFootPolygon.update();
}

private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB)
{
 footA.getCentroid(tempCentroid);
 firstCMP.setXYIncludingFrame(tempCentroid);
 firstCMP.changeFrame(worldFrame);
 footB.getCentroid(tempCentroid);
 secondCMP.setXYIncludingFrame(tempCentroid);
 secondCMP.changeFrame(worldFrame);
 upcomingSupport.clear(worldFrame);
 tempFootPolygon.setIncludingFrame(footA);
 tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame);
 upcomingSupport.addVertices(tempFootPolygon);
 tempFootPolygon.setIncludingFrame(footB);
 tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame);
 upcomingSupport.addVertices(tempFootPolygon);
 upcomingSupport.update();
 entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame);
 exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame);
 upcomingSupport.getCentroid(tempCentroid);
 tempCentroid3d.setXYIncludingFrame(tempCentroid);
 double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0);
 if (chicken <= 0.5)
   entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0);
 else
   entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0);
 exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex));
}

/**
* If the list of vertices is empty, this polygon will be empty and its reference frame won't be changed.
* If the list of vertices is not empty, this method does:
* 1- {@code clear(vertices.get(0).getReferenceFrame())};
* 2- {@code addVertices(vertices)};
* 3- {@code update()}.
* @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon.
* @param vertices {@code FrameTuple2dArrayList<FramePoint2d>} the list of points that is used to creates the vertices.
* @throws ReferenceFrameMismatchException
*/
public void setIncludingFrameAndUpdate(FrameTuple2dArrayList<FramePoint2d> vertices)
{
 int numberOfVertices = vertices.size();
 if (numberOfVertices < 1 )
 {
   clear();
   update();
 }
 else
 {
   clear(vertices.get(0).getReferenceFrame());
   addVertices(vertices, numberOfVertices);
   update();
 }
}

fullSupportAfterShrinking.setIncludingFrameAndUpdate(oppositeFootPolygon);
fullSupportAfterShrinking.changeFrameAndProjectToXYPlane(worldFrame);
fullSupportAfterShrinking.addVertices(controllerFootPolygonInWorld);
fullSupportAfterShrinking.update();
momentumBasedController.getCapturePoint(capturePoint);

tempPolygon2.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame());
polygonShrinker.shrinkConstantDistanceInto(tempPolygon2, -distanceToExtendUpcomingFoothold.getDoubleValue(), tempPolygon1);
safeArea.addVertices(tempPolygon1);
safeArea.update();