/** * Adds N new vertices to this polygon using a list of {@code FramePoint2d} after having changed their frame projected them on this polygon frame. * 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. */ public void addVerticesByProjectionOntoXYPlane(List<FramePoint> vertices, int numberOfVertices) { for (int i = 0; i < numberOfVertices; i++) { FramePoint vertex = vertices.get(i); addVertexByProjectionOntoXYPlane(vertex); } }
/** * 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()}; * 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 void computeSupportPolygon() { contactPointsPolygon.clear(planeFrame); for (int i = 0; i < getTotalNumberOfContactPoints(); i++) { contactPoints.get(i).getPosition(tempContactPointPosition); contactPointsPolygon.addVertexByProjectionOntoXYPlane(tempContactPointPosition); } contactPointsPolygon.update(); }
private void addPredictedContactPointsToPolygon(Footstep footstep, FrameConvexPolygon2d convexPolygonToExtend) { List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); if (predictedContactPoints != null && !predictedContactPoints.isEmpty()) { int numberOfContactPoints = predictedContactPoints.size(); for (int i = 0; i < numberOfContactPoints; i++) { tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), predictedContactPoints.get(i)); convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint); } } else { ConvexPolygon2d defaultPolygon = defaultFootPolygons.get(footstep.getRobotSide()); for (int i = 0; i < defaultPolygon.getNumberOfVertices(); i++) { tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), defaultPolygon.getVertex(i)); convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint); } } }
private void updateOnToesSupportPolygon(RobotSide trailingSide, FrameConvexPolygon2d leadingFootSupportPolygon) { computeToePoints(trailingSide); middleToePoint.changeFrame(worldFrame); onToesSupportPolygon.setIncludingFrameAndUpdate(leadingFootSupportPolygon); onToesSupportPolygon.changeFrameAndProjectToXYPlane(worldFrame); onToesSupportPolygon.addVertexByProjectionOntoXYPlane(middleToePoint); onToesSupportPolygon.update(); } }
footPolygonInWorldFrame.addVertexByProjectionOntoXYPlane(tempFramePoint); footPolygonInSoleFrame.addVertexByProjectionOntoXYPlane(tempFramePoint); footPolygonInSoleZUpFrame.addVertexByProjectionOntoXYPlane(tempFramePoint); footPolygonInAnkleZUp.addVertexByProjectionOntoXYPlane(tempFramePoint); footPolygonInMidFeetZUp.addVertexByProjectionOntoXYPlane(tempFramePoint);