/** {@inheritDoc} */ @Override default FramePoint2DReadOnly getVertex(int index) { return (FramePoint2DReadOnly) ConvexPolygon2DReadOnly.super.getVertex(index); }
protected static void getConnectingEdges(ConvexPolygon2DReadOnly polygon1, ConvexPolygon2DReadOnly polygon2, LineSegment2DBasics connectingEdge1ToPack, LineSegment2DBasics connectingEdge2ToPack, int[][] verticesIndices) { connectingEdge1ToPack.set(polygon1.getVertex(verticesIndices[0][0]), polygon2.getVertex(verticesIndices[1][0])); connectingEdge2ToPack.set(polygon2.getVertex(verticesIndices[1][1]), polygon1.getVertex(verticesIndices[0][1])); }
/** * Takes in two ranges each of which are of size at most two and returns the two points on each * respective polygon which are closest to one another */ private void getClosestPointsFromRemainingEdgesAndVertices(ConvexPolygon2DReadOnly polygon1, ConvexPolygon2DReadOnly polygon2, int v1Start, int v1End, int v2Start, int v2End, Point2DBasics point1ToPack, Point2DBasics point2ToPack) { if ((v1Start == v1End) && (v2Start == v2End)) { point1ToPack.set(polygon1.getVertex(v1Start)); point2ToPack.set(polygon2.getVertex(v2Start)); } else if (v1Start == v1End) { finalPhasePointAndEdge(polygon2.getVertex(v2Start), polygon2.getVertex(v2End), polygon1.getVertex(v1Start), point1ToPack, point2ToPack); } else if (v2Start == v2End) { finalPhasePointAndEdge(polygon1.getVertex(v1Start), polygon1.getVertex(v1End), polygon2.getVertex(v2Start), point2ToPack, point1ToPack); } else { finalPhaseTwoEdges(polygon1.getVertex(v1Start), polygon1.getVertex(v1End), polygon2.getVertex(v2Start), polygon2.getVertex(v2End), point1ToPack, point2ToPack); } }
/** * Gets the read-only reference to the vertex located after the {@code index}<sup>th</sup> vertex * of this polygon. * <p> * Note that the first vertex has the lowest x-coordinate. * </p> * * @param index the index of the vertex. * @return the read-only reference to the next vertex. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. * @throws IndexOutOfBoundsException if the given {@code index} is negative or greater or equal * than this polygon's number of vertices. * @throws EmptyPolygonException if this polygon is empty when calling this method. */ default Point2DReadOnly getNextVertex(int index) { return getVertex(getNextVertexIndex(index)); }
/** * Gets the read-only reference to the vertex located before the {@code index}<sup>th</sup> * vertex of this polygon. * <p> * Note that this polygon's vertices are clockwise ordered and that the first vertex has the * lowest x-coordinate. * </p> * * @param index the index of the vertex in the clockwise ordered list. * @return the read-only reference to the previous vertex. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. * @throws IndexOutOfBoundsException if the given {@code index} is negative or greater or equal * than this polygon's number of vertices. * @throws EmptyPolygonException if this polygon is empty when calling this method. */ default Point2DReadOnly getPreviousVertex(int index) { return getVertex(getPreviousVertexIndex(index)); }
/** * Gets the read-only reference to the {@code index}<sup>th</sup> vertex of this polygon. * <p> * This method calculates a new index to emulate a counter-clockwise ordering of this polygon's * vertices. The first vertex has the lowest x-coordinate. * </p> * * @param index the index of the vertex in the counter-clockwise ordered list. * @return the read-only reference to the vertex. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. * @throws IndexOutOfBoundsException if the given {@code index} is negative or greater or equal * than this polygon's number of vertices. * @throws EmptyPolygonException if this polygon is empty when calling this method. */ default Point2DReadOnly getVertexCCW(int index) { return getVertex(getNumberOfVertices() - 1 - index); }
/** * Determines if the polygonToTest is inside the convex polygon. */ public static boolean isPolygonInside(ConvexPolygon2DReadOnly polygonToTest, double epsilon, ConvexPolygon2DReadOnly polygon) { for (int i = 0; i < polygonToTest.getNumberOfVertices(); i++) { if (!polygon.isPointInside(polygonToTest.getVertex(i), epsilon)) return false; } return true; }
private double computeDistanceToClosestEdge(Point2D pointInsideConvexPolygon, ConvexPolygon2DReadOnly convexPolygon) { double minDistanceToEdge = Double.POSITIVE_INFINITY; double distanceToEdge = 0.0; int numberOfVertices = convexPolygon.getNumberOfVertices(); for (int i = 0; i < numberOfVertices - 1; i++) { Point2DReadOnly vertex = convexPolygon.getVertex(i); Point2DReadOnly vertex2 = convexPolygon.getVertex(i + 1); Point2D projectedPoint = projectPointOntoEdge(vertex, vertex2, pointInsideConvexPolygon); distanceToEdge = pointInsideConvexPolygon.distance(projectedPoint); if (distanceToEdge < minDistanceToEdge) { minDistanceToEdge = distanceToEdge; } } return minDistanceToEdge; }
/** * Packs the endpoints of an edge of this polygon into {@code edgeToPack}. * * @param edgeIndex index of the vertex that starts the edge. * @param edgeToPack line segment used to store the edge endpoints. Modified. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. * @throws IndexOutOfBoundsException if the given {@code index} is negative or greater or equal * than this polygon's number of vertices. * @throws EmptyPolygonException if this polygon is empty when calling this method. */ default void getEdge(int edgeIndex, LineSegment2DBasics edgeToPack) { edgeToPack.set(getVertex(edgeIndex), getNextVertex(edgeIndex)); }
/** * Finds the index of the closest vertex to the query. * <p> * Edge cases: * <ul> * <li>If the polygon has no vertices, this method fails and returns {@code false}. * </ul> * </p> * * @param point the coordinates of the query. Not modified. * @param vertexToPack point used to store the result. Modified. * @return whether this method succeeded or not. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. */ default boolean getClosestVertex(Point2DReadOnly point, Point2DBasics vertexToPack) { int vertexIndex = getClosestVertexIndex(point); if (vertexIndex == -1) return false; vertexToPack.set(getVertex(vertexIndex)); return true; }
/** * Finds the index of the closest vertex to the given line. * <p> * WARNING: This method generates garbage. * </p> * <p> * Edge cases: * <ul> * <li>If the polygon has no vertices, this method fails and returns {@code false}. * </ul> * </p> * * @param line the query. Not modified. * @return the coordinates of the closest vertex or {@code null} if this method failed. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. */ default Point2DBasics getClosestVertexCopy(Line2DReadOnly line) { int vertexIndex = getClosestVertexIndex(line); if (vertexIndex == -1) return null; return new Point2D(getVertex(vertexIndex)); }
/** * Finds the index of the closest vertex to the query. * <p> * WARNING: This method generates garbage. * </p> * <p> * Edge cases: * <ul> * <li>If the polygon has no vertices, this method fails and returns {@code null}. * </ul> * </p> * * @param point the coordinates of the query. Not modified. * @return the coordinates of the closest vertex, or {@code null} if this method failed. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. */ default Point2DBasics getClosestVertexCopy(Point2DReadOnly point) { int vertexIndex = getClosestVertexIndex(point); if (vertexIndex == -1) return null; return new Point2D(getVertex(vertexIndex)); }
/** * Finds the index of the closest vertex to the given line. * <p> * Edge cases: * <ul> * <li>If the polygon has no vertices, this method fails and returns {@code false}. * </ul> * </p> * * @param line the query. Not modified. * @param vertexToPack point used to store the result. Modified. * @return whether this method succeeded or not. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. */ default boolean getClosestVertex(Line2DReadOnly line, Point2DBasics vertexToPack) { int vertexIndex = getClosestVertexIndex(line); if (vertexIndex == -1) return false; vertexToPack.set(getVertex(vertexIndex)); return true; }
private static boolean computeIntersectionOfPolygonsIfOnePolygonHasExactlyOneVertex(ConvexPolygon2DReadOnly polygonWithExactlyOneVertex, ConvexPolygon2DReadOnly otherPolygon, ConvexPolygon2DBasics intersectingPolygon) { if (otherPolygon.pointIsOnPerimeter(polygonWithExactlyOneVertex.getVertex(0))) { intersectingPolygon.set(polygonWithExactlyOneVertex); return false; } else { intersectingPolygon.clearAndUpdate(); return false; } }
/** * Adds a subset of this polygon's vertices into the given list. * <p> * The subset consists of the vertices from the vertex at {@code startIndexInclusive} to the * vertex {@code endIndexInclusive} while going from start to end in a clockwise order. * </p> * * @param startIndexInclusive the index of the first vertex to add. * @param endIndexInclusive the index of the last vertex to add. * @param pointListToPack the list into which the vertices are to be added. * @throws OutdatedPolygonException if {@link ConvexPolygon2DBasics#update()} has not been called * since last time this polygon's vertices were edited. * @throws EmptyPolygonException if this polygon is empty when calling this method. */ default void getPointsInClockwiseOrder(int startIndexInclusive, int endIndexInclusive, List<Point2DReadOnly> pointListToPack) { checkIfUpToDate(); checkIndexInBoundaries(startIndexInclusive); checkIndexInBoundaries(endIndexInclusive); int index = startIndexInclusive; while (true) { pointListToPack.add(getVertex(index)); if (index == endIndexInclusive) break; index = getNextVertexIndex(index); } }
/** * Packs a vector that is orthogonal to the given edge, facing towards the outside of the polygon */ public static void getEdgeNormal(int edgeIndex, Vector2DBasics normalToPack, ConvexPolygon2DReadOnly polygon) { Point2DReadOnly edgeStart = polygon.getVertex(edgeIndex); Point2DReadOnly edgeEnd = polygon.getNextVertex(edgeIndex); double edgeVectorX = edgeEnd.getX() - edgeStart.getX(); double edgeVectorY = edgeEnd.getY() - edgeStart.getY(); normalToPack.set(-edgeVectorY, edgeVectorX); normalToPack.normalize(); }
public static void convertToInequalityConstraintsPolygon(ConvexPolygon2DReadOnly polygon, DenseMatrix64F A, DenseMatrix64F b, double deltaInside) { int constraints = polygon.getNumberOfVertices(); A.reshape(constraints, 2); b.reshape(constraints, 1); for (int i = 0; i < constraints; i++) { Point2DReadOnly firstPoint = polygon.getVertex(i); Point2DReadOnly secondPoint = polygon.getNextVertex(i); double x = secondPoint.getX() - firstPoint.getX(); double y = secondPoint.getY() - firstPoint.getY(); double norm = Math.sqrt(x * x + y * y); x = x / norm; y = y / norm; A.set(i, 0, -y); A.set(i, 1, x); b.set(i, -deltaInside + firstPoint.getY() * x - firstPoint.getX() * y); // A.set(i, 0, firstPoint.y - secondPoint.y); // A.set(i, 1, -firstPoint.x + secondPoint.x); // b.set(i, firstPoint.y * (secondPoint.x - firstPoint.x) - firstPoint.x * (secondPoint.y - firstPoint.y)); } }
/** * Checks if a line intersects the edge with the given index. */ public static boolean doesLineIntersectEdge(Line2DReadOnly line, int edgeIndex, ConvexPolygon2DReadOnly polygon) { if (polygon.getNumberOfVertices() < 2) return false; Point2DReadOnly edgeStart = polygon.getVertex(edgeIndex); Point2DReadOnly edgeEnd = polygon.getNextVertex(edgeIndex); double lineDirectionX = line.getDirectionX(); double lineDirectionY = line.getDirectionY(); double edgeDirectionX = edgeEnd.getX() - edgeStart.getX(); double edgeDirectionY = edgeEnd.getY() - edgeStart.getY(); if (EuclidGeometryTools.areVector2DsParallel(lineDirectionX, lineDirectionY, edgeDirectionX, edgeDirectionY, EuclidGeometryTools.ONE_TEN_MILLIONTH)) return false; else return EuclidGeometryTools.doLine2DAndLineSegment2DIntersect(line.getPoint(), line.getDirection(), edgeStart, edgeEnd); }
public static void convertToInequalityConstraintsPoint(ConvexPolygon2DReadOnly polygon, DenseMatrix64F A, DenseMatrix64F b) { A.reshape(4, 2); b.reshape(4, 1); A.set(0, 0, 1.0); A.set(0, 1, 0.0); A.set(1, 0, 0.0); A.set(1, 1, 1.0); A.set(2, 0, -1.0); A.set(2, 1, 0.0); A.set(3, 0, 0.0); A.set(3, 1, -1.0); Point2DReadOnly point = polygon.getVertex(0); b.set(0, 0, point.getX()); b.set(1, 0, point.getY()); b.set(2, 0, -point.getX()); b.set(3, 0, -point.getY()); }
/** * Uses the fact that if a line passes through a vertex of a convex polygon, the angles to the * adjacent edges are going to be in opposite directions * * @return Whether or not the line including the point and vertex is tangent to the polygon */ private boolean pointMakesTangentToPolygon(ConvexPolygon2DReadOnly polygon, Point2DReadOnly point, int vertexIndex, double epsilon) { Point2DReadOnly vertex = polygon.getVertex(vertexIndex); Point2DReadOnly previous = polygon.getPreviousVertex(vertexIndex); Point2DReadOnly next = polygon.getNextVertex(vertexIndex); base.set(point.getX() - vertex.getX(), point.getY() - vertex.getY()); first.set(previous.getX() - vertex.getX(), previous.getY() - vertex.getY()); second.set(next.getX() - vertex.getX(), next.getY() - vertex.getY()); double firstAngle = base.angle(first); double secondAngle = base.angle(second); if (firstAngle * secondAngle >= 0) { // if both angles have the same sign, the line does not pass through the polygon return true; } if (MathTools.epsilonEquals(firstAngle, 0, epsilon) || MathTools.epsilonEquals(secondAngle, 0, epsilon)) { // if either angle is close to 0, assume floating point arithmetic error return true; } return false; }