/** {@inheritDoc} */ @Override default FramePoint2DReadOnly getNextVertex(int index) { return (FramePoint2DReadOnly) ConvexPolygon2DReadOnly.super.getNextVertex(index); }
Point2DReadOnly endVertex = polygon.getNextVertex(i); edgeToPack2.set(endVertex, polygon.getNextVertex(polygon.getNextVertexIndex(i))); return 2;
/** * 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)); }
vectorToNextPointOnPolygonP.set(polygonP.getNextVertex(currentPPolygonPointIndex)); vectorToNextPointOnPolygonP.sub(polygonP.getVertex(currentPPolygonPointIndex)); vectorToNextPointOnPolygonP.normalize(); vectorToNextPointOnPolygonQ.set(polygonQ.getNextVertex(currentQPolygonPointIndex)); vectorToNextPointOnPolygonQ.sub(polygonQ.getVertex(currentQPolygonPointIndex)); vectorToNextPointOnPolygonQ.normalize();
/** * 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 constrainPolygonInsideOtherPolygon(ConvexPolygon2DReadOnly polygonExteriorInWorld, ConvexPolygon2DReadOnly polygonInteriorRelative, DenseMatrix64F A, DenseMatrix64F b, double deltaInside) { int constraints = polygonExteriorInWorld.getNumberOfVertices(); A.reshape(constraints, 2); b.reshape(constraints, 1); Vector2D tempVector = new Vector2D(); for (int exteriorVertexIndex = 0; exteriorVertexIndex < constraints; exteriorVertexIndex++) { Point2DReadOnly firstPoint = polygonExteriorInWorld.getVertex(exteriorVertexIndex); Point2DReadOnly secondPoint = polygonExteriorInWorld.getNextVertex(exteriorVertexIndex); tempVector.set(secondPoint); tempVector.sub(firstPoint); tempVector.normalize(); A.set(exteriorVertexIndex, 0, -tempVector.getY()); A.set(exteriorVertexIndex, 1, tempVector.getX()); double additionalDistanceInside = 0.0; for (int interiorVertexIndex = 0; interiorVertexIndex < polygonInteriorRelative.getNumberOfVertices(); interiorVertexIndex++) { Point2DReadOnly interiorVertex = polygonInteriorRelative.getVertex(interiorVertexIndex); additionalDistanceInside = Math.max(additionalDistanceInside, tempVector.getY() * interiorVertex.getX() - tempVector.getX() * interiorVertex.getY()); } A.set(exteriorVertexIndex, 0, -tempVector.getY()); A.set(exteriorVertexIndex, 1, tempVector.getX()); b.set(exteriorVertexIndex, -(deltaInside + additionalDistanceInside) + firstPoint.getY() * (tempVector.getX()) - firstPoint.getX() * (tempVector.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; }
public static void convertToInequalityConstraintsLine(ConvexPolygon2DReadOnly polygon, DenseMatrix64F A, DenseMatrix64F b, double deltaInside) { A.reshape(4, 2); b.reshape(4, 1); Vector2D tempVector = new Vector2D(); // constrain to lying on 2d line Point2DReadOnly firstPoint = polygon.getVertex(0); Point2DReadOnly secondPoint = polygon.getNextVertex(0); tempVector.set(secondPoint); tempVector.sub(firstPoint); tempVector.normalize(); // set regular line A.set(0, 0, -tempVector.getY()); A.set(0, 1, tempVector.getX()); b.set(0, firstPoint.getY() * tempVector.getX() - firstPoint.getX() * tempVector.getY()); A.set(1, 0, tempVector.getY()); A.set(1, 1, -tempVector.getX()); b.set(1, -b.get(0)); // set first point boundary line // // TODO: 5/2/17 add in deltaInside A.set(2, 0, -tempVector.getX()); A.set(2, 1, -tempVector.getY()); b.set(2, -deltaInside - firstPoint.getX() * tempVector.getX() - firstPoint.getY() * tempVector.getY()); // set second point boundary line A.set(3, 0, tempVector.getX()); A.set(3, 1, tempVector.getY()); b.set(3, -deltaInside + secondPoint.getX() * tempVector.getX() + secondPoint.getY() * tempVector.getY()); }