public void setPoints(ConvexPolygon2d polygon) { this.points.clear(); for (int i = 0; i < polygon.getNumberOfVertices(); i++) this.points.add(polygon.getVertex(i)); }
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(); }
/** * Adds new vertices to this polygon from another convex polygon. * Note that this method recycles memory. * @param otherPolygon {@code ConvexPolygon2d} the other convex polygon that is used to add new vertices to this polygon. */ public void addVertices(ConvexPolygon2d otherPolygon) { isUpToDate = false; for (int i = 0; i < otherPolygon.getNumberOfVertices(); i++) addVertex(otherPolygon.getVertex(i)); }
@Override public ConvexPolygon2d applyTransformCopy(RigidBodyTransform transform) { ConvexPolygon2d copy = new ConvexPolygon2d(this); copy.applyTransform(transform); return copy; }
polygonToPack.setAndUpdate(polygonQ); return true; if (polygonQ.hasExactlyTwoVertices()) Point2d vertex0 = polygonQ.getVertex(0); Point2d vertex1 = polygonQ.getVertex(1); if (vertex0.distance(vertex1) < 2.0 * distance) polygonToPack.clear(); polygonToPack.addVertex(midPoint); polygonToPack.update(); return false; polygonToPack.setAndUpdate(newVertices, 2); if (polygonQ.hasExactlyOneVertex()) polygonToPack.setAndUpdate(polygonQ); int leftMostIndexOnPolygonQ = polygonQ.getMinXIndex(); Point2d vertexQ = polygonQ.getVertex(leftMostIndexOnPolygonQ); int nextVertexQIndex = polygonQ.getNextVertexIndex(leftMostIndexOnPolygonQ); Point2d nextVertexQ = polygonQ.getVertex(nextVertexQIndex); for (int i = 0; i < polygonQ.getNumberOfVertices(); i++) nextVertexQIndex = polygonQ.getNextVertexIndex(nextVertexQIndex); nextVertexQ = polygonQ.getVertex(nextVertexQIndex);
if (polygonP == null || polygonP.isEmpty()) return false; if (polygonQ == null || polygonQ.isEmpty()) return false; if (polygonP.hasExactlyTwoVertices() && polygonQ.hasAtLeastTwoVertices()) else if (polygonQ.hasExactlyTwoVertices()) if (polygonP.hasExactlyOneVertex() && polygonQ.hasAtLeastOneVertex()) else if (polygonQ.hasExactlyOneVertex()) int currentPPolygonPointIndex = polygonP.getMinXIndex(); int initialPolygonPIndex = polygonP.getMinXIndex(); Point2d currentPolygonPPoint = polygonP.getVertex(currentPPolygonPointIndex); int currentQPolygonPointIndex = polygonQ.getMinXIndex(); int initialPolygonQIndex = polygonQ.getMinXIndex(); Point2d currentPolygonQPoint = polygonQ.getVertex(currentQPolygonPointIndex); Vector2d vectorToNextPointOnPolygonP = new Vector2d(polygonP.getNextVertex(currentPPolygonPointIndex)); vectorToNextPointOnPolygonP.sub(polygonP.getVertex(currentPPolygonPointIndex)); vectorToNextPointOnPolygonP.normalize(); Vector2d vectorToNextPointOnPolygonQ = new Vector2d(polygonQ.getNextVertex(currentQPolygonPointIndex)); vectorToNextPointOnPolygonQ.sub(polygonQ.getVertex(currentQPolygonPointIndex)); vectorToNextPointOnPolygonQ.normalize();
/** * 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(); }
partialFootholdPolygon.clear(); partialFootholdPolygon.addVertices(footstep.getPredictedContactPoints(), footstep.getPredictedContactPoints().size()); partialFootholdPolygon.update(); if (partialFootholdPolygon.getNumberOfVertices() != 4) for (int j = 0; j < 4; j++) fourPartialFootholdCorners.add(new Point2d(partialFootholdPolygon.getVertex(j)));
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; } }
@Override public boolean epsilonEquals(ConvexPolygon2d convexPolygon, double threshold) { checkIfUpToDate(); if (getNumberOfVertices() != convexPolygon.getNumberOfVertices()) return false; for (int i = 0; i < convexPolygon.getNumberOfVertices(); i++) { if (!convexPolygon.getVertex(i).epsilonEquals(getVertex(i), threshold)) return false; } return true; }
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(); }
public int getNumberOfVertices() { return convexPolygon.getNumberOfVertices(); }
public static ConvexPolygon2d translatePolygonCopy(Tuple2d translation, ConvexPolygon2d polygon) { ConvexPolygon2d ret = new ConvexPolygon2d(polygon); translatePolygon(translation, ret); return ret; }
intersectionPolygonToPack.clear(); ConvexPolygon2d intersectingPolygon = convexPolygons.get(i).intersectionWith(projectedPolygon); intersectionArea += intersectingPolygon.getArea(); intersectingPolygon.applyTransformAndProjectToXYPlane(regionToPolygonTransform); intersectionPolygonToPack.addVertices(intersectingPolygon); intersectionPolygonToPack.update();
protected void addVerticesInClockwiseOrderInPolygon(int startIndexInclusive, int endIndexInclusive, ConvexPolygon2d polygonToPack) { checkIfUpToDate(); int index = startIndexInclusive; while (true) { polygonToPack.addVertex(getVertex(index)); if (index == endIndexInclusive) break; index = getNextVertexIndex(index); } }
private static boolean computeIntersectionOfPolygonsIfOnePolygonHasExactlyOneVertex(ConvexPolygon2d polygonWithExactlyOneVertex, ConvexPolygon2d otherPolygon, ConvexPolygon2d intersectingPolygon) { if (otherPolygon.pointIsOnPerimeter(polygonWithExactlyOneVertex.getVertex(0))) { intersectingPolygon.setAndUpdate(polygonWithExactlyOneVertex); return false; } else { intersectingPolygon.clearAndUpdate(); return false; } }
private static boolean computeIntersectionOfPolygonsIfOnePolygonHasExactlyTwoVerticesAndTheOtherHasAtLeastTwoVertices( ConvexPolygon2d polygonWithExactlyTwoVertices, ConvexPolygon2d polygonWithAtLeastTwoVertices, ConvexPolygon2d intersectingPolygon) { LineSegment2d polygonWithTwoVerticesAsLineSegment = new LineSegment2d(polygonWithExactlyTwoVertices.getVertex(0), polygonWithExactlyTwoVertices.getVertex(1)); Point2d[] intersection = polygonWithAtLeastTwoVertices.intersectionWith(polygonWithTwoVerticesAsLineSegment); if (intersection == null) { intersectingPolygon.clearAndUpdate(); return false; } else { intersectingPolygon.setAndUpdate(intersection, intersection.length); return true; } }
public void getFrameVertex(int vertexIndex, FrameTuple2d<?, ?> vertexToPack) { convexPolygon.checkIfUpToDate(); convexPolygon.checkNonEmpty(); convexPolygon.checkIndexInBoundaries(vertexIndex); vertexToPack.setIncludingFrame(referenceFrame, convexPolygon.getVertex(vertexIndex)); }
ConvexPolygon2d footPolygonInRegion = new ConvexPolygon2d(footStepPolygon); footPolygonInRegion.applyTransformAndProjectToXYPlane(soleToRegion); footPolygonInRegion.setAndUpdate(footStepPolygon); footPolygonInRegion.applyTransformAndProjectToXYPlane(soleToRegion); ConvexPolygon2d foothold = regionToMoveTo.getConvexHull().intersectionWith(footPolygonInRegion); soleToRegion.invert(); foothold.applyTransformAndProjectToXYPlane(soleToRegion); return foothold;