/** * Determines if edge i of the polygon is parallel to the given direction. If the edge is too * short to determine its direction this method will return false. */ public static boolean isEdgeParallel(int edgeIndex, Vector2d direction, ConvexPolygon2d polygon) { Point2d edgeStart = polygon.getVertex(edgeIndex); Point2d edgeEnd = polygon.getNextVertex(edgeIndex); double edgeDirectionX = edgeEnd.x - edgeStart.x; double edgeDirectionY = edgeEnd.y - edgeStart.y; double crossProduct = -edgeDirectionY * direction.x + edgeDirectionX * direction.y; return Math.abs(crossProduct) < epsilon; }
Point2d endVertex = polygon.getNextVertex(i); edgeToPack2.set(endVertex, polygon.getNextVertex(polygon.getNextVertexIndex(i))); return 2;
/** * Determines whether an observer can see the outside of the given edge. The edge index corresponds to * the vertex at the start of the edge when moving clockwise around the polygon. Will return false if the * observer is on the edge. */ public static boolean canObserverSeeEdge(int edgeIndex, double observerX, double observerY, ConvexPolygon2d polygon) { Point2d vertexOne = polygon.getVertex(edgeIndex); Point2d vertexTwo = polygon.getNextVertex(edgeIndex); double edgeVectorX = vertexTwo.x - vertexOne.x; double edgeVectorY = vertexTwo.y - vertexOne.y; return GeometryTools.isPointOnSideOfLine(observerX, observerY, vertexOne.x, vertexOne.y, edgeVectorX, edgeVectorY, RobotSide.LEFT); }
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();
/** * Packs a vector that is orthogonal to the given edge, facing towards the outside of the polygon */ public static void getEdgeNormal(int edgeIndex, Vector2d normalToPack, ConvexPolygon2d polygon) { Point2d edgeStart = polygon.getVertex(edgeIndex); Point2d edgeEnd = polygon.getNextVertex(edgeIndex); double edgeVectorX = edgeEnd.x - edgeStart.x; double edgeVectorY = edgeEnd.y - edgeStart.y; normalToPack.set(-edgeVectorY, edgeVectorX); normalToPack.normalize(); }
/** * Packs the closest edge to the given point. */ public static boolean getClosestEdge(Point2d point, ConvexPolygon2d polygon, LineSegment2d edgeToPack) { int edgeIndex = getClosestEdgeIndex(point, polygon); if (edgeIndex == -1) return false; edgeToPack.set(polygon.getVertex(edgeIndex), polygon.getNextVertex(edgeIndex)); return true; }
/** * Returns distance from the point to the boundary of this polygon. The return value * is negative if the point is inside the polygon. */ public static double getSignedDistance(Point2d point, ConvexPolygon2d polygon) { double closestDistance = Double.POSITIVE_INFINITY; for (int index = 0; index < polygon.getNumberOfVertices(); index++) { Point2d pointOne = polygon.getVertex(index); Point2d pointTwo = polygon.getNextVertex(index); double distance = GeometryTools.distanceFromPointToLineSegment(point, pointOne, pointTwo); if (distance < closestDistance) closestDistance = distance; } if (isPointInside(point, polygon)) return -closestDistance; return closestDistance; }
/** * Checks if a line intersects the edge with the given index. */ public static boolean doesLineIntersectEdge(Line2d line, int edgeIndex, ConvexPolygon2d polygon) { if (!polygon.hasAtLeastTwoVertices()) return false; Point2d edgePointOne = polygon.getVertex(edgeIndex); Point2d edgePointTwo = polygon.getNextVertex(edgeIndex); double edgeVectorX = edgePointTwo.x - edgePointOne.x; double edgeVectorY = edgePointTwo.y - edgePointOne.y; double lambdaOne = getIntersectionLambda(edgePointOne.x, edgePointOne.y, edgeVectorX, edgeVectorY, line.getPoint().x, line.getPoint().y, line.getNormalizedVector().x, line.getNormalizedVector().y); if (Double.isNaN(lambdaOne) || lambdaOne < 0.0 || lambdaOne > 1.0) return false; return true; }
/** * Packs the index of the closest edge to the given point. The index corresponds to the index * of the vertex at the start of the edge. */ public static int getClosestEdgeIndex(Point2d point, ConvexPolygon2d polygon) { int index = -1; if (!polygon.hasAtLeastTwoVertices()) return index; double minDistance = Double.POSITIVE_INFINITY; for (int i = 0; i < polygon.getNumberOfVertices(); i++) { Point2d start = polygon.getVertex(i); Point2d end = polygon.getNextVertex(i); double distance = GeometryTools.distanceFromPointToLineSegment(point, start, end); if (distance < minDistance) { index = i; minDistance = distance; } } return index; }
Point2d point3 = polygon.getNextVertex(closestVertexIndex);
Point2d edgeEnd = polygon.getNextVertex(i); double distanceToEdgeLine = GeometryTools.distanceFromPointToLine(pointX, pointY, edgeStart.x, edgeStart.y, edgeEnd.x - edgeStart.x, edgeEnd.y - edgeStart.y);
Point2d secondPoint = polygon.getNextVertex(i);
/** * 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 static boolean pointMakesTangentToPolygon(ConvexPolygon2d polygon, Point2d point, int vertexIndex, double epsilon) { Point2d vertex = polygon.getVertex(vertexIndex); Point2d previous = polygon.getPreviousVertex(vertexIndex); Point2d next = polygon.getNextVertex(vertexIndex); Vector2d base = new Vector2d(point.getX() - vertex.getX(), point.getY() - vertex.getY()); Vector2d first = new Vector2d(previous.getX() - vertex.getX(), previous.getY() - vertex.getY()); Vector2d second = new Vector2d(next.getX() - vertex.getX(), next.getY() - vertex.getY()); double firstAngle = GeometryTools.getAngleFromFirstToSecondVector(base, first); double secondAngle = GeometryTools.getAngleFromFirstToSecondVector(base, 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; }
Point2d edgeEnd = polygon.getNextVertex(i);
Point2d pointTwo = polygon.getNextVertex(index);
Point2d edgeEnd = polygon.getNextVertex(i); double edgeVectorX = edgeEnd.x - edgeStart.x; double edgeVectorY = edgeEnd.y - edgeStart.y;