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