/** * Calls {@code ConvexPolygon2d.update()} and updates the {@code FramePoint}s of this {@code FrameConvecPolygon2d}. */ public void update() { convexPolygon.update(); }
/** * Creates an empty convex polygon. */ public ConvexPolygon2d() { numberOfVertices = 0; update(); }
@Override public void setToNaN() { numberOfVertices = 1; if (clockwiseOrderedListOfPoints.isEmpty()) clockwiseOrderedListOfPoints.add(new Point2d()); Point2d point = clockwiseOrderedListOfPoints.get(0); point.set(Double.NaN, Double.NaN); isUpToDate = false; update(); }
@Override public void setToZero() { numberOfVertices = 1; if (clockwiseOrderedListOfPoints.isEmpty()) clockwiseOrderedListOfPoints.add(new Point2d()); Point2d point = clockwiseOrderedListOfPoints.get(0); point.set(0.0, 0.0); isUpToDate = false; update(); }
@Override public boolean containsNaN() { update(); for (int i = 0; i < numberOfVertices; i++) { Point2d point = clockwiseOrderedListOfPoints.get(i); if (Double.isNaN(point.getX())) return true; if (Double.isNaN(point.getY())) return true; } return false; }
public boolean epsilonEquals(SimpleFootstep otherFootstep, double epsilon) { this.foothold.update(); otherFootstep.foothold.update(); if (!this.robotSide.equals(otherFootstep.robotSide)) return false; if (!this.soleFramePose.epsilonEquals(otherFootstep.soleFramePose, epsilon)) return false; if (!this.foothold.epsilonEquals(otherFootstep.foothold, epsilon)) return false; return true; } }
/** * Creates an empty convex polygon, adds N new vertices using an array of {@code Point2d}, updates the vertices so they are clockwise ordered, and initializes some essential numbers such as the centroid. * @param vertices {@code double[>=numberOfVertices][>=2]} the array of points that is used to creates the vertices. The each row contains one point whereas the (at least) two columns contains the coordinates x and y. * @param numberOfVertices {@code int} that is used to determine the number of vertices of the polygon. * Note the: {@code pointListArray.length} can be greater or equal to numberOfVertices. */ public ConvexPolygon2d(double[][] vertices, int numberOfVertices) { clear(); addVertices(vertices, numberOfVertices); update(); }
/** * This method does: * 1- {@code clear()}; * 2- {@code addVertices(vertices, numberOfVertices)}; * 3- {@code update()}. * @param vertices {@code Point2d[]} 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.length} can be greater or equal to numberOfVertices. */ public void setAndUpdate(Point2d[] vertices, int numberOfVertices) { clear(); addVertices(vertices, numberOfVertices); update(); }
/** * This method does: * 1- {@code clear()}; * 2- {@code addVertices(vertices, numberOfVertices)}; * 3- {@code update()}. * @param vertices {@code double[>=numberOfVertices][>=2]} * @param numberOfVertices {@code int} */ public void setAndUpdate(double[][] vertices, int numberOfVertices) { clear(); addVertices(vertices, numberOfVertices); update(); }
/** * This method does: * 1- clear(); * 2- addVertices(otherPolygon); * 3- 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 ConvexPolygon2d} */ public void setAndUpdate(ConvexPolygon2d otherPolygon) { clear(); addVertices(otherPolygon); update(); }
private boolean checkArea(FootstepDataMessage footstep, double percentageToCheckAgainst, SteppingParameters parameters){ if (footstep.predictedContactPoints == null) return true; ConvexPolygon2d convexPolygon2d = new ConvexPolygon2d(footstep.getPredictedContactPoints()); convexPolygon2d.update(); double area = convexPolygon2d.getArea(); double percentage = area / parameters.getFootstepArea(); if (percentage + EPSILON < percentageToCheckAgainst) return false; return true; } }
private static ConvexPolygon2d createRectanglePolygon(double lengthX, double widthY) { ConvexPolygon2d convexPolygon = new ConvexPolygon2d(); convexPolygon.addVertex(lengthX / 2.0, widthY / 2.0); convexPolygon.addVertex(-lengthX / 2.0, widthY / 2.0); convexPolygon.addVertex(-lengthX / 2.0, -widthY / 2.0); convexPolygon.addVertex(lengthX / 2.0, -widthY / 2.0); convexPolygon.update(); return convexPolygon; }
public GenericFootstepSnappingParameters(){ super(null, null, 0.3, Math.PI/4, 0.0, 0.01, 0.025, Math.PI/24, 1); collisionPolygon = new ConvexPolygon2d(); collisionPolygon.addVertex(0.1, 0.05); collisionPolygon.addVertex(0.1, -0.05); collisionPolygon.addVertex(-0.05,-0.05); collisionPolygon.addVertex(-0.05, 0.05); collisionPolygon.update(); supportPolygon = collisionPolygon; }
public static ConvexPolygon2d createDefaultFootPolygon() { double footLength = 0.2; double footWidth = 0.1; ConvexPolygon2d footPolygon = new ConvexPolygon2d(); footPolygon.addVertex(footLength / 2.0, footWidth / 2.0); footPolygon.addVertex(footLength / 2.0, -footWidth / 2.0); footPolygon.addVertex(-footLength / 2.0, footWidth / 2.0); footPolygon.addVertex(-footLength / 2.0, -footWidth / 2.0); footPolygon.update(); return footPolygon; }
private void updateConvexHull() { convexHull.clear(); for (int i = 0; i < this.getNumberOfConvexPolygons(); i++) { ConvexPolygon2d convexPolygon = this.getConvexPolygon(i); for (int j = 0; j < convexPolygon.getNumberOfVertices(); j++) convexHull.addVertex(convexPolygon.getVertex(j)); } convexHull.update(); }
public BasicFootstepMask(List<Point2d> footstepShape, double maskBufferSize) { this.safetyBuffer = maskBufferSize; footShapeWithBufferPolygon.clear(); for (Point2d vertex : footstepShape) { footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
private void drawSupportPolygon(QuadrupedSupportPolygon supportPolygon, YoFrameConvexPolygon2d yoFramePolygon) { ConvexPolygon2d polygon = new ConvexPolygon2d(); for(RobotQuadrant quadrant : supportPolygon.getSupportingQuadrantsInOrder()) { FramePoint footstep = supportPolygon.getFootstep(quadrant); polygon.addVertex(footstep.getX(), footstep.getY()); } polygon.update(); yoFramePolygon.setConvexPolygon2d(polygon); }
public BasicFootstepMask(ContactablePlaneBody foot, double maskBufferSize) { this.safetyBuffer = maskBufferSize; footShapeWithBufferPolygon.clear(); for (FramePoint2d vertex : foot.getContactPoints2d()) { footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
public BasicFootstepMask(ConvexPolygon2d footstepShape, double maskBufferSize) { int numVertices = footstepShape.getNumberOfVertices(); Point2d vertex; for (int i = 0; i < numVertices; i++) { vertex = footstepShape.getVertex(i); footShapeWithBufferPolygon.addVertex(inflate(vertex.getX()), inflate(vertex.getY())); } footShapeWithBufferPolygon.update(); }
private void drawSupportPolygon(QuadrupedSupportPolygon supportPolygon, YoFrameConvexPolygon2d yoFramePolygon) { ConvexPolygon2d polygon = new ConvexPolygon2d(); for(RobotQuadrant quadrant : supportPolygon.getSupportingQuadrantsInOrder()) { FramePoint footstep = supportPolygon.getFootstep(quadrant); polygon.addVertex(footstep.getX(), footstep.getY()); } polygon.update(); yoFramePolygon.setConvexPolygon2d(polygon); } }