How to use

clear

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

/**
* This method does:
* 1- {@code clear()};
* 2- {@code addVertexByProjectionOntoXYPlane(vertices.get(i)};
* 3- {@code update()}.
* @param vertices {@code List<FramePoint>} the list of points that is used to creates the vertices.
*/
public void setAndUpdate(List<FramePoint> vertices)
{
 clear();
 for (int i = 0; i < vertices.size(); i++)
 {
   addVertexByProjectionOntoXYPlane(vertices.get(i));
 }
 update();
}

/**
* This method does:
* 1- {@code clear(referenceFrame)};
* 2- {@code addVertices(vertices)};
* 3- {@code update()}.
* @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon.
* @param vertices {@code List<Point2d>} the list of points that is used to creates the vertices.
*/
public void setIncludingFrameAndUpdate(ReferenceFrame referenceFrame, List<Point2d> vertices)
{
 clear(referenceFrame);
 this.convexPolygon.addVertices(vertices, vertices.size());
 update();
}

public void setIncludingFrameAndUpdate(ReferenceFrame referenceFrame, Point2d[] vertices)
{
 clear(referenceFrame);
 this.convexPolygon.addVertices(vertices, vertices.length);
 update();
}

/**
* This method does:
* 1- {@code clear(vertices.get(0).getReferenceFrame())};
* 2- {@code addVerticesByProjectionOntoXYPlane(vertices, vertices.size())};
* 3- {@code update()}.
* @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon.
* @param vertices {@code List<FramePoint>} the list of points that is used to creates the vertices.
* @throws ReferenceFrameMismatchException
*/
public void setIncludingFrameByProjectionOntoXYPlaneAndUpdate(ReferenceFrame referenceFrame, List<FramePoint> vertices)
{
 int numberOfVertices = vertices.size();
 clear(referenceFrame);
 addVerticesByProjectionOntoXYPlane(vertices, numberOfVertices);
 update();
}

/**
* This method does:
* 1- {@code clear(referenceFrame)};
* 2- {@code addVertices(vertices)};
* 3- {@code update()}.
* @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon.
* @param vertices {@code double[>=numberOfVertices][>=2]} the array of points that is used to creates the vertices.
*/
public void setIncludingFrameAndUpdate(ReferenceFrame referenceFrame, double[][] vertices)
{
 clear(referenceFrame);
 this.convexPolygon.addVertices(vertices, vertices.length);
 update();
}

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().");
 }
}

/**
* This method does:
* 1- {@code clear(referenceFrame)};
* 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 referenceFrame {@code ReferenceFrame} the new reference frame of this polygon.
* @param otherPolygon {@code ConvexPolygon2d}
*/
public void setIncludingFrameAndUpdate(ReferenceFrame referenceFrame, ConvexPolygon2d otherPolygon)
{
 clear(referenceFrame);
 this.convexPolygon.addVertices(otherPolygon);
 update();
}

/**
* This method does:
* 1- {@code clear(vertices.get(0).getReferenceFrame())};
* 2- {@code addVerticesByProjectionOntoXYPlane(vertices, numberOfVertices)};
* 3- {@code update()}.
* @param referenceFrame {@code ReferenceFrame} the new reference frame of this polygon.
* @param vertices {@code List<FramePoint>} the list of points that is used to creates the vertices.
* @param numberOfVertices {@code int}.
* @throws ReferenceFrameMismatchException
*/
public void setIncludingFrameByProjectionOntoXYPlaneAndUpdate(ReferenceFrame referenceFrame, List<FramePoint> vertices, int numberOfVertices)
{
 clear(referenceFrame);
 addVerticesByProjectionOntoXYPlane(vertices, numberOfVertices);
 update();
}

/**
* 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()};
* 2- {@code addVerticesByProjectionOntoXYPlane(vertices, numberOfVertices)};
* 3- {@code update()}.
* @param vertices {@code List<FramePoint>} 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.
*/
public void setByProjectionOntoXYPlaneAndUpdate(List<FramePoint> vertices, int numberOfVertices)
{
 clear();
 addVerticesByProjectionOntoXYPlane(vertices, numberOfVertices);
 update();
}

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

public boolean intersectionWith(FrameConvexPolygon2d secondConvexPolygon, FrameConvexPolygon2d intersectionToPack)
{
 checkReferenceFrameMatch(secondConvexPolygon);
 intersectionToPack.clear(secondConvexPolygon.getReferenceFrame());
 boolean success = convexPolygon.intersectionWith(secondConvexPolygon.convexPolygon, intersectionToPack.convexPolygon);
 return success;
}

public void computeSupportPolygon()
{
 contactPointsPolygon.clear(planeFrame);
 for (int i = 0; i < getTotalNumberOfContactPoints(); i++)
 {
   contactPoints.get(i).getPosition(tempContactPointPosition);
   contactPointsPolygon.addVertexByProjectionOntoXYPlane(tempContactPointPosition);
 }
 contactPointsPolygon.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();
}

private void computeFootstepCentroid(FramePoint2d centroidToPack, Footstep footstep)
{
 predictedSupportPolygon.clear(footstep.getSoleReferenceFrame());
 addPredictedContactPointsToPolygon(footstep, predictedSupportPolygon);
 predictedSupportPolygon.update();
 predictedSupportPolygon.getCentroid(centroidToPack);
}

private void computePredictedSupportCentroid(FramePoint2d centroidToPack, Footstep footstep, Footstep nextFootstep)
{
 predictedSupportPolygon.clear(worldFrame);
 addPredictedContactPointsToPolygon(footstep, predictedSupportPolygon);
 addPredictedContactPointsToPolygon(nextFootstep, predictedSupportPolygon);
 predictedSupportPolygon.update();
 predictedSupportPolygon.getCentroid(centroidToPack);
}

  public static boolean combineDisjointPolygons(FrameConvexPolygon2d polygon1, FrameConvexPolygon2d polygon2, FrameConvexPolygon2d combinedPolygonToPack,
     FrameLineSegment2d connectingEdge1ToPack, FrameLineSegment2d connectingEdge2ToPack)
  {
   combinedPolygonToPack.clear(polygon1.getReferenceFrame());
   combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge1ToPack);
   combinedPolygonToPack.checkReferenceFrameMatch(connectingEdge2ToPack);

   boolean success = combineDisjointPolygons(polygon1.convexPolygon, polygon2.convexPolygon, combinedPolygonToPack.convexPolygon,
      connectingEdge1ToPack.lineSegment, connectingEdge2ToPack.lineSegment);

   if (!success)
     return false;

//      combinedPolygonToPack.updateFramePoints();
   combinedPolygonToPack.update();

   return true;
  }

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

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