| | |
|---|
| | | import java.util.List; |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 无障碍物凸形草地路径规划类 (终极优化版) |
|---|
| | | * 特性： |
|---|
| | | * 1. 最小投影宽度方向选择 (效率最高，转弯最少) |
|---|
| | | * 2. 边缘轮廓优先切割 (无死角覆盖) |
|---|
| | | * 3. 支持外部传入已包含重叠率的宽度参数 |
|---|
| | | * 凸形草地路径规划 (围边优化版) |
|---|
| | | * 优化重点：围边坐标对齐扫描起点、全路径连贯性 |
|---|
| | | */ |
|---|
| | | public class AoxinglujingNoObstacle { |
|---|
| | | |
|---|
| | | // 引入极小值用于浮点数比较，处理几何精度误差 |
|---|
| | | private static final double EPSILON = 1e-6; |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 路径段类 |
|---|
| | | */ |
|---|
| | | public static class PathSegment { |
|---|
| | | public Point start; |
|---|
| | | public Point end; |
|---|
| | | public boolean isMowing; // true为割草工作段，false为过渡段 |
|---|
| | | |
|---|
| | | public PathSegment(Point start, Point end, boolean isMowing) { |
|---|
| | | this.start = start; |
|---|
| | | this.end = end; |
|---|
| | | this.isMowing = isMowing; |
|---|
| | | } |
|---|
| | | |
|---|
| | | @Override |
|---|
| | | public String toString() { |
|---|
| | | return String.format("[%s -> %s, isMowing=%b]", start, end, isMowing); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 坐标点类 |
|---|
| | | */ |
|---|
| | | public static class Point { |
|---|
| | | public double x, y; |
|---|
| | | |
|---|
| | | public Point(double x, double y) { |
|---|
| | | this.x = x; |
|---|
| | | this.y = y; |
|---|
| | | } |
|---|
| | | |
|---|
| | | public Point(double x, double y) { this.x = x; this.y = y; } |
|---|
| | | @Override |
|---|
| | | public String toString() { |
|---|
| | | return String.format("(%.4f, %.4f)", x, y); |
|---|
| | | public String toString() { return String.format("%.6f,%.6f", x, y); } |
|---|
| | | } |
|---|
| | | |
|---|
| | | public static class PathSegment { |
|---|
| | | public Point start, end; |
|---|
| | | public boolean isMowing; |
|---|
| | | public PathSegment(Point start, Point end, boolean isMowing) { |
|---|
| | | this.start = start; this.end = end; this.isMowing = isMowing; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 对外公开的静态调用方法 |
|---|
| | | * |
|---|
| | | * @param boundaryCoordsStr 地块边界坐标字符串 "x1,y1;x2,y2;..." |
|---|
| | | * @param mowingWidthStr 有效割草宽度字符串 (已包含重叠率)，如 "0.30" |
|---|
| | | * @param safetyMarginStr 安全边距字符串，如 "0.2" |
|---|
| | | * @return 路径段列表 |
|---|
| | | * 对外主接口 |
|---|
| | | */ |
|---|
| | | public static List<PathSegment> planPath(String boundaryCoordsStr, String mowingWidthStr, String safetyMarginStr) { |
|---|
| | | // 1. 解析参数 |
|---|
| | | List<Point> originalPolygon = parseCoords(boundaryCoordsStr); |
|---|
| | | double mowingWidth; |
|---|
| | | double safetyMargin; |
|---|
| | | double width = Double.parseDouble(mowingWidthStr); |
|---|
| | | double margin = Double.parseDouble(safetyMarginStr); |
|---|
| | | |
|---|
| | | try { |
|---|
| | | mowingWidth = Double.parseDouble(mowingWidthStr); |
|---|
| | | safetyMargin = Double.parseDouble(safetyMarginStr); |
|---|
| | | } catch (NumberFormatException e) { |
|---|
| | | throw new IllegalArgumentException("割草宽度或安全边距格式错误"); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 2. 调用核心算法 |
|---|
| | | return planPathCore(originalPolygon, mowingWidth, safetyMargin); |
|---|
| | | return planPathCore(originalPolygon, width, margin); |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 核心算法逻辑 |
|---|
| | | * 核心路径规划逻辑 |
|---|
| | | * |
|---|
| | | * @param originalPolygon 原始多边形顶点列表 |
|---|
| | | * @param width 割草宽度 |
|---|
| | | * @param margin 安全边距 |
|---|
| | | * @return 规划好的路径段列表 |
|---|
| | | */ |
|---|
| | | private static List<PathSegment> planPathCore(List<Point> originalPolygon, double width, double safetyMargin) { |
|---|
| | | if (originalPolygon == null || originalPolygon.size() < 3) { |
|---|
| | | return new ArrayList<>(); // 或抛出异常，视业务需求而定 |
|---|
| | | } |
|---|
| | | private static List<PathSegment> planPathCore(List<Point> originalPolygon, double width, double margin) { |
|---|
| | | if (originalPolygon.size() < 3) return new ArrayList<>(); |
|---|
| | | |
|---|
| | | // 确保多边形是逆时针方向 |
|---|
| | | // 1. 确保逆时针并进行安全内缩 |
|---|
| | | ensureCCW(originalPolygon); |
|---|
| | | List<Point> workArea = shrinkPolygon(originalPolygon, margin); |
|---|
| | | if (workArea.size() < 3) return new ArrayList<>(); |
|---|
| | | |
|---|
| | | // 1. 根据安全边距内缩，得到实际作业区域 |
|---|
| | | List<Point> workAreaPolygon = shrinkPolygon(originalPolygon, safetyMargin); |
|---|
| | | // 2. 预计算最优角度和填充路径的第一个点 |
|---|
| | | double bestAngle = findOptimalScanAngle(workArea); |
|---|
| | | Point firstScanStart = getFirstScanStartPoint(workArea, bestAngle, width); |
|---|
| | | |
|---|
| | | // 如果内缩后区域失效（如地块太小），返回空路径 |
|---|
| | | if (workAreaPolygon.size() < 3) { |
|---|
| | | return new ArrayList<>(); |
|---|
| | | } |
|---|
| | | // 3. 对齐围边起点：让围边的最后一个点刚好连接扫描填充的起点 |
|---|
| | | List<Point> alignedWorkArea = alignBoundaryToStart(workArea, firstScanStart); |
|---|
| | | |
|---|
| | | List<PathSegment> finalPath = new ArrayList<>(); |
|---|
| | | |
|---|
| | | // 2. [优化] 优先生成轮廓路径 (Contour Pass) |
|---|
| | | // 沿作业边界走一圈，确保边缘整齐且无遗漏 |
|---|
| | | addContourPath(workAreaPolygon, finalPath); |
|---|
| | | |
|---|
| | | // 3. [优化] 计算最佳扫描角度 |
|---|
| | | // 寻找让多边形投影高度最小的角度，从而最小化转弯次数 |
|---|
| | | double bestAngle = findOptimalScanAngle(workAreaPolygon); |
|---|
| | | |
|---|
| | | // 4. 生成内部弓字形路径 |
|---|
| | | // 直接使用传入的 width (已包含重叠率) |
|---|
| | | List<PathSegment> zigZagPaths = generateClippedMowingLines(workAreaPolygon, bestAngle, width); |
|---|
| | | |
|---|
| | | // 5. 连接轮廓路径和弓字形路径 |
|---|
| | | if (!finalPath.isEmpty() && !zigZagPaths.isEmpty()) { |
|---|
| | | Point contourEnd = finalPath.get(finalPath.size() - 1).end; |
|---|
| | | Point zigzagStart = zigZagPaths.get(0).start; |
|---|
| | | |
|---|
| | | // 如果轮廓终点与弓字形起点不重合，添加过渡段 |
|---|
| | | if (distanceSq(contourEnd, zigzagStart) > EPSILON) { |
|---|
| | | finalPath.add(new PathSegment(contourEnd, zigzagStart, false)); |
|---|
| | | } |
|---|
| | | // 4. 【第一阶段】添加围边坐标路径 |
|---|
| | | for (int i = 0; i < alignedWorkArea.size(); i++) { |
|---|
| | | Point p1 = alignedWorkArea.get(i); |
|---|
| | | Point p2 = alignedWorkArea.get((i + 1) % alignedWorkArea.size()); |
|---|
| | | finalPath.add(new PathSegment(p1, p2, true)); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 6. 合并弓字形路径 |
|---|
| | | finalPath.addAll(connectPathSegments(zigZagPaths)); |
|---|
| | | // 5. 【第二阶段】生成内部弓字形路径 |
|---|
| | | // 从围边闭合点（alignedWorkArea.get(0)）开始连接 |
|---|
| | | Point currentPos = alignedWorkArea.get(0); |
|---|
| | | List<PathSegment> zigZagLines = generateZigZagPath(workArea, bestAngle, width, currentPos); |
|---|
| | | |
|---|
| | | finalPath.addAll(zigZagLines); |
|---|
| | | |
|---|
| | | return finalPath; |
|---|
| | | } |
|---|
| | | |
|---|
| | | // ================= 核心逻辑辅助方法 ================= |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 添加轮廓路径 (围着多边形走一圈) |
|---|
| | | * 寻找弓字形的第一条线的起点 |
|---|
| | | * |
|---|
| | | * @param polygon 多边形顶点列表 |
|---|
| | | * @param angle 扫描角度 |
|---|
| | | * @param width 割草宽度 |
|---|
| | | * @return 扫描起点的坐标 |
|---|
| | | */ |
|---|
| | | private static void addContourPath(List<Point> polygon, List<PathSegment> path) { |
|---|
| | | int n = polygon.size(); |
|---|
| | | for (int i = 0; i < n; i++) { |
|---|
| | | Point p1 = polygon.get(i); |
|---|
| | | Point p2 = polygon.get((i + 1) % n); |
|---|
| | | path.add(new PathSegment(p1, p2, true)); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 寻找最优扫描角度 (最小投影高度法) |
|---|
| | | */ |
|---|
| | | private static double findOptimalScanAngle(List<Point> polygon) { |
|---|
| | | double minHeight = Double.MAX_VALUE; |
|---|
| | | double bestAngle = 0; |
|---|
| | | int n = polygon.size(); |
|---|
| | | |
|---|
| | | // 遍历每一条边，计算以该边为“底”时，多边形的高度 |
|---|
| | | for (int i = 0; i < n; i++) { |
|---|
| | | Point p1 = polygon.get(i); |
|---|
| | | Point p2 = polygon.get((i + 1) % n); |
|---|
| | | |
|---|
| | | // 当前边的角度 |
|---|
| | | double currentAngle = Math.atan2(p2.y - p1.y, p2.x - p1.x); |
|---|
| | | |
|---|
| | | // 计算在这个角度下的投影高度 |
|---|
| | | double height = calculatePolygonHeightAtAngle(polygon, currentAngle); |
|---|
| | | |
|---|
| | | if (height < minHeight) { |
|---|
| | | minHeight = height; |
|---|
| | | bestAngle = currentAngle; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | return bestAngle; |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 计算多边形在特定旋转角度下的Y轴投影高度 |
|---|
| | | */ |
|---|
| | | private static double calculatePolygonHeightAtAngle(List<Point> poly, double angle) { |
|---|
| | | private static Point getFirstScanStartPoint(List<Point> polygon, double angle, double width) { |
|---|
| | | List<Point> rotated = rotatePolygon(polygon, -angle); |
|---|
| | | double minY = Double.MAX_VALUE; |
|---|
| | | double maxY = -Double.MAX_VALUE; |
|---|
| | | for (Point p : rotated) minY = Math.min(minY, p.y); |
|---|
| | | |
|---|
| | | double cos = Math.cos(-angle); |
|---|
| | | double sin = Math.sin(-angle); |
|---|
| | | |
|---|
| | | for (Point p : poly) { |
|---|
| | | // 只需计算旋转后的Y坐标 |
|---|
| | | double rotatedY = p.x * sin + p.y * cos; |
|---|
| | | if (rotatedY < minY) minY = rotatedY; |
|---|
| | | if (rotatedY > maxY) maxY = rotatedY; |
|---|
| | | } |
|---|
| | | return maxY - minY; |
|---|
| | | double startY = minY + width + EPSILON; |
|---|
| | | List<Double> xIntersections = getXIntersections(rotated, startY); |
|---|
| | | if (xIntersections.isEmpty()) return polygon.get(0); |
|---|
| | | |
|---|
| | | Collections.sort(xIntersections); |
|---|
| | | return rotatePoint(new Point(xIntersections.get(0), startY), angle); |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static List<PathSegment> generateClippedMowingLines(List<Point> polygon, double angle, double width) { |
|---|
| | | List<PathSegment> segments = new ArrayList<>(); |
|---|
| | | |
|---|
| | | // 旋转多边形至水平 |
|---|
| | | List<Point> rotatedPoly = rotatePolygon(polygon, -angle); |
|---|
| | | |
|---|
| | | double minY = Double.MAX_VALUE; |
|---|
| | | double maxY = -Double.MAX_VALUE; |
|---|
| | | for (Point p : rotatedPoly) { |
|---|
| | | if (p.y < minY) minY = p.y; |
|---|
| | | if (p.y > maxY) maxY = p.y; |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 起始扫描线位置： |
|---|
| | | // 从 minY + width/2 开始，因为之前已经走了轮廓线(Contour Pass)。 |
|---|
| | | // 轮廓线负责清理边缘区域，内部填充线保持 width 的间距即可。 |
|---|
| | | // 加上 EPSILON 防止浮点数刚好落在边界上导致的判断误差 |
|---|
| | | double currentY = minY + width / 2.0 + EPSILON; |
|---|
| | | |
|---|
| | | while (currentY < maxY) { |
|---|
| | | List<Double> xIntersections = new ArrayList<>(); |
|---|
| | | int n = rotatedPoly.size(); |
|---|
| | | |
|---|
| | | for (int i = 0; i < n; i++) { |
|---|
| | | Point p1 = rotatedPoly.get(i); |
|---|
| | | Point p2 = rotatedPoly.get((i + 1) % n); |
|---|
| | | |
|---|
| | | // 忽略水平线段 |
|---|
| | | if (Math.abs(p1.y - p2.y) < EPSILON) continue; |
|---|
| | | |
|---|
| | | double minP = Math.min(p1.y, p2.y); |
|---|
| | | double maxP = Math.max(p1.y, p2.y); |
|---|
| | | |
|---|
| | | if (currentY >= minP && currentY < maxP) { |
|---|
| | | double x = p1.x + (currentY - p1.y) * (p2.x - p1.x) / (p2.y - p1.y); |
|---|
| | | xIntersections.add(x); |
|---|
| | | } |
|---|
| | | /** |
|---|
| | | * 重组多边形顶点，使得索引0的点最靠近填充起点 |
|---|
| | | * |
|---|
| | | * @param polygon 多边形顶点列表 |
|---|
| | | * @param target 目标点（填充起点） |
|---|
| | | * @return 重组后的多边形顶点列表 |
|---|
| | | */ |
|---|
| | | private static List<Point> alignBoundaryToStart(List<Point> polygon, Point target) { |
|---|
| | | int bestIdx = 0; |
|---|
| | | double minDist = Double.MAX_VALUE; |
|---|
| | | for (int i = 0; i < polygon.size(); i++) { |
|---|
| | | double d = Math.hypot(polygon.get(i).x - target.x, polygon.get(i).y - target.y); |
|---|
| | | if (d < minDist) { |
|---|
| | | minDist = d; |
|---|
| | | bestIdx = i; |
|---|
| | | } |
|---|
| | | |
|---|
| | | Collections.sort(xIntersections); |
|---|
| | | |
|---|
| | | if (xIntersections.size() >= 2) { |
|---|
| | | // 取最左和最右交点 |
|---|
| | | double xStart = xIntersections.get(0); |
|---|
| | | double xEnd = xIntersections.get(xIntersections.size() - 1); |
|---|
| | | |
|---|
| | | if (xEnd - xStart > EPSILON) { |
|---|
| | | // 反向旋转回原坐标系 |
|---|
| | | Point rStart = rotatePoint(new Point(xStart, currentY), angle); |
|---|
| | | Point rEnd = rotatePoint(new Point(xEnd, currentY), angle); |
|---|
| | | segments.add(new PathSegment(rStart, rEnd, true)); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | // 步进 |
|---|
| | | currentY += width; |
|---|
| | | } |
|---|
| | | |
|---|
| | | return segments; |
|---|
| | | List<Point> aligned = new ArrayList<>(); |
|---|
| | | for (int i = 0; i < polygon.size(); i++) { |
|---|
| | | aligned.add(polygon.get((bestIdx + i) % polygon.size())); |
|---|
| | | } |
|---|
| | | return aligned; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static List<PathSegment> connectPathSegments(List<PathSegment> lines) { |
|---|
| | | /** |
|---|
| | | * 生成弓字形扫描路径 |
|---|
| | | * |
|---|
| | | * @param polygon 多边形顶点列表 |
|---|
| | | * @param angle 扫描角度 |
|---|
| | | * @param width 割草宽度 |
|---|
| | | * @param startPoint 起始点 |
|---|
| | | * @return 弓字形路径段列表 |
|---|
| | | */ |
|---|
| | | private static List<PathSegment> generateZigZagPath(List<Point> polygon, double angle, double width, Point startPoint) { |
|---|
| | | List<PathSegment> result = new ArrayList<>(); |
|---|
| | | if (lines.isEmpty()) return result; |
|---|
| | | List<Point> rotated = rotatePolygon(polygon, -angle); |
|---|
| | | |
|---|
| | | for (int i = 0; i < lines.size(); i++) { |
|---|
| | | PathSegment currentLine = lines.get(i); |
|---|
| | | Point actualStart, actualEnd; |
|---|
| | | double minY = Double.MAX_VALUE, maxY = -Double.MAX_VALUE; |
|---|
| | | for (Point p : rotated) { |
|---|
| | | minY = Math.min(minY, p.y); |
|---|
| | | maxY = Math.max(maxY, p.y); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 弓字形规划：偶数行正向，奇数行反向 |
|---|
| | | if (i % 2 == 0) { |
|---|
| | | actualStart = currentLine.start; |
|---|
| | | actualEnd = currentLine.end; |
|---|
| | | } else { |
|---|
| | | actualStart = currentLine.end; |
|---|
| | | actualEnd = currentLine.start; |
|---|
| | | Point currentPos = startPoint; |
|---|
| | | boolean leftToRight = true; |
|---|
| | | // 起点从 minY + width 开始，因为边缘已经围边割过 |
|---|
| | | for (double y = minY + width; y < maxY - width / 2; y += width) { |
|---|
| | | List<Double> xInt = getXIntersections(rotated, y); |
|---|
| | | if (xInt.size() < 2) continue; |
|---|
| | | Collections.sort(xInt); |
|---|
| | | |
|---|
| | | double xStart = leftToRight ? xInt.get(0) : xInt.get(xInt.size() - 1); |
|---|
| | | double xEnd = leftToRight ? xInt.get(xInt.size() - 1) : xInt.get(0); |
|---|
| | | |
|---|
| | | Point pS = rotatePoint(new Point(xStart, y), angle); |
|---|
| | | Point pE = rotatePoint(new Point(xEnd, y), angle); |
|---|
| | | |
|---|
| | | // 添加过渡段 (如果是从围边切换过来或者换行) |
|---|
| | | if (Math.hypot(currentPos.x - pS.x, currentPos.y - pS.y) > 0.05) { |
|---|
| | | result.add(new PathSegment(currentPos, pS, false)); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 添加过渡段 |
|---|
| | | if (i > 0) { |
|---|
| | | Point prevEnd = result.get(result.size() - 1).end; |
|---|
| | | if (distanceSq(prevEnd, actualStart) > EPSILON) { |
|---|
| | | result.add(new PathSegment(prevEnd, actualStart, false)); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | result.add(new PathSegment(actualStart, actualEnd, true)); |
|---|
| | | result.add(new PathSegment(pS, pE, true)); |
|---|
| | | currentPos = pE; |
|---|
| | | leftToRight = !leftToRight; |
|---|
| | | } |
|---|
| | | return result; |
|---|
| | | } |
|---|
| | | |
|---|
| | | // ================= 基础几何工具 ================= |
|---|
| | | |
|---|
| | | private static List<Point> parseCoords(String s) { |
|---|
| | | List<Point> list = new ArrayList<>(); |
|---|
| | | if (s == null || s.trim().isEmpty()) return list; |
|---|
| | | |
|---|
| | | String[] parts = s.split(";"); |
|---|
| | | for (String part : parts) { |
|---|
| | | String[] xy = part.split(","); |
|---|
| | | if (xy.length >= 2) { |
|---|
| | | try { |
|---|
| | | double x = Double.parseDouble(xy[0].trim()); |
|---|
| | | double y = Double.parseDouble(xy[1].trim()); |
|---|
| | | list.add(new Point(x, y)); |
|---|
| | | } catch (NumberFormatException e) { |
|---|
| | | // 忽略格式错误 |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | | return list; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static void ensureCCW(List<Point> polygon) { |
|---|
| | | double sum = 0; |
|---|
| | | for (int i = 0; i < polygon.size(); i++) { |
|---|
| | | Point p1 = polygon.get(i); |
|---|
| | | Point p2 = polygon.get((i + 1) % polygon.size()); |
|---|
| | | sum += (p2.x - p1.x) * (p2.y + p1.y); |
|---|
| | | } |
|---|
| | | if (sum > 0) { |
|---|
| | | Collections.reverse(polygon); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static List<Point> shrinkPolygon(List<Point> polygon, double margin) { |
|---|
| | | List<Point> newPoints = new ArrayList<>(); |
|---|
| | | int n = polygon.size(); |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 获取扫描线与多边形的交点X坐标列表 |
|---|
| | | * |
|---|
| | | * @param rotatedPoly 旋转后的多边形 |
|---|
| | | * @param y 扫描线的Y坐标 |
|---|
| | | * @return 交点X坐标列表 |
|---|
| | | */ |
|---|
| | | private static List<Double> getXIntersections(List<Point> rotatedPoly, double y) { |
|---|
| | | List<Double> xIntersections = new ArrayList<>(); |
|---|
| | | int n = rotatedPoly.size(); |
|---|
| | | for (int i = 0; i < n; i++) { |
|---|
| | | Point p1 = polygon.get(i); |
|---|
| | | Point p2 = polygon.get((i + 1) % n); |
|---|
| | | Point p0 = polygon.get((i - 1 + n) % n); |
|---|
| | | |
|---|
| | | Line line1 = offsetLine(p1, p2, margin); |
|---|
| | | Line line0 = offsetLine(p0, p1, margin); |
|---|
| | | |
|---|
| | | Point intersection = getIntersection(line0, line1); |
|---|
| | | if (intersection != null) { |
|---|
| | | newPoints.add(intersection); |
|---|
| | | Point p1 = rotatedPoly.get(i); |
|---|
| | | Point p2 = rotatedPoly.get((i + 1) % n); |
|---|
| | | if ((p1.y <= y && p2.y > y) || (p2.y <= y && p1.y > y)) { |
|---|
| | | double x = p1.x + (y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y); |
|---|
| | | xIntersections.add(x); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | return newPoints; |
|---|
| | | return xIntersections; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static class Line { |
|---|
| | | double a, b, c; |
|---|
| | | public Line(double a, double b, double c) { this.a = a; this.b = b; this.c = c; } |
|---|
| | | // --- 几何基础工具 --- |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 多边形内缩（计算安全工作区域） |
|---|
| | | * |
|---|
| | | * @param polygon 原始多边形 |
|---|
| | | * @param margin 内缩距离 |
|---|
| | | * @return 内缩后的多边形 |
|---|
| | | */ |
|---|
| | | private static List<Point> shrinkPolygon(List<Point> polygon, double margin) { |
|---|
| | | List<Point> result = new ArrayList<>(); |
|---|
| | | int n = polygon.size(); |
|---|
| | | for (int i = 0; i < n; i++) { |
|---|
| | | Point pPrev = polygon.get((i - 1 + n) % n); |
|---|
| | | Point pCurr = polygon.get(i); |
|---|
| | | Point pNext = polygon.get((i + 1) % n); |
|---|
| | | |
|---|
| | | double d1x = pCurr.x - pPrev.x, d1y = pCurr.y - pPrev.y; |
|---|
| | | double l1 = Math.hypot(d1x, d1y); |
|---|
| | | double d2x = pNext.x - pCurr.x, d2y = pNext.y - pCurr.y; |
|---|
| | | double l2 = Math.hypot(d2x, d2y); |
|---|
| | | |
|---|
| | | double n1x = -d1y / l1, n1y = d1x / l1; |
|---|
| | | double n2x = -d2y / l2, n2y = d2x / l2; |
|---|
| | | |
|---|
| | | double bx = n1x + n2x, by = n1y + n2y; |
|---|
| | | double bLen = Math.hypot(bx, by); |
|---|
| | | if (bLen < EPSILON) { bx = n1x; by = n1y; } |
|---|
| | | else { bx /= bLen; by /= bLen; } |
|---|
| | | |
|---|
| | | double cosHalf = n1x * bx + n1y * by; |
|---|
| | | double d = margin / Math.max(cosHalf, 0.1); |
|---|
| | | result.add(new Point(pCurr.x + bx * d, pCurr.y + by * d)); |
|---|
| | | } |
|---|
| | | return result; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static Line offsetLine(Point p1, Point p2, double dist) { |
|---|
| | | double dx = p2.x - p1.x; |
|---|
| | | double dy = p2.y - p1.y; |
|---|
| | | double len = Math.sqrt(dx * dx + dy * dy); |
|---|
| | | |
|---|
| | | if (len < EPSILON) return new Line(0, 0, 0); |
|---|
| | | |
|---|
| | | double nx = -dy / len; |
|---|
| | | double ny = dx / len; |
|---|
| | | |
|---|
| | | // 向左侧平移（假设逆时针） |
|---|
| | | double newX = p1.x + nx * dist; |
|---|
| | | double newY = p1.y + ny * dist; |
|---|
| | | |
|---|
| | | double a = -dy; |
|---|
| | | double b = dx; |
|---|
| | | double c = -a * newX - b * newY; |
|---|
| | | return new Line(a, b, c); |
|---|
| | | /** |
|---|
| | | * 寻找最优扫描角度（使扫描线数量最少） |
|---|
| | | * |
|---|
| | | * @param polygon 多边形 |
|---|
| | | * @return 最优角度（弧度） |
|---|
| | | */ |
|---|
| | | private static double findOptimalScanAngle(List<Point> polygon) { |
|---|
| | | double minH = Double.MAX_VALUE; |
|---|
| | | double bestA = 0; |
|---|
| | | for (int i = 0; i < polygon.size(); i++) { |
|---|
| | | Point p1 = polygon.get(i), p2 = polygon.get((i + 1) % polygon.size()); |
|---|
| | | double angle = Math.atan2(p2.y - p1.y, p2.x - p1.x); |
|---|
| | | double h = calculatePolygonHeightAtAngle(polygon, angle); |
|---|
| | | if (h < minH) { minH = h; bestA = angle; } |
|---|
| | | } |
|---|
| | | return bestA; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static Point getIntersection(Line l1, Line l2) { |
|---|
| | | double det = l1.a * l2.b - l2.a * l1.b; |
|---|
| | | if (Math.abs(det) < EPSILON) return null; |
|---|
| | | double x = (l1.b * l2.c - l2.b * l1.c) / det; |
|---|
| | | double y = (l2.a * l1.c - l1.a * l2.c) / det; |
|---|
| | | return new Point(x, y); |
|---|
| | | /** |
|---|
| | | * 计算多边形在特定角度下的高度（投影长度） |
|---|
| | | * |
|---|
| | | * @param poly 多边形 |
|---|
| | | * @param angle 角度 |
|---|
| | | * @return 高度 |
|---|
| | | */ |
|---|
| | | private static double calculatePolygonHeightAtAngle(List<Point> poly, double angle) { |
|---|
| | | double minY = Double.MAX_VALUE, maxY = -Double.MAX_VALUE; |
|---|
| | | double sin = Math.sin(-angle), cos = Math.cos(-angle); |
|---|
| | | for (Point p : poly) { |
|---|
| | | double ry = p.x * sin + p.y * cos; |
|---|
| | | minY = Math.min(minY, ry); maxY = Math.max(maxY, ry); |
|---|
| | | } |
|---|
| | | return maxY - minY; |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 旋转点 |
|---|
| | | * |
|---|
| | | * @param p 点 |
|---|
| | | * @param angle 旋转角度 |
|---|
| | | * @return 旋转后的点 |
|---|
| | | */ |
|---|
| | | private static Point rotatePoint(Point p, double angle) { |
|---|
| | | double cos = Math.cos(angle); |
|---|
| | | double sin = Math.sin(angle); |
|---|
| | | return new Point(p.x * cos - p.y * sin, p.x * sin + p.y * cos); |
|---|
| | | double c = Math.cos(angle), s = Math.sin(angle); |
|---|
| | | return new Point(p.x * c - p.y * s, p.x * s + p.y * c); |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 旋转多边形 |
|---|
| | | * |
|---|
| | | * @param poly 多边形 |
|---|
| | | * @param angle 旋转角度 |
|---|
| | | * @return 旋转后的多边形 |
|---|
| | | */ |
|---|
| | | private static List<Point> rotatePolygon(List<Point> poly, double angle) { |
|---|
| | | List<Point> res = new ArrayList<>(); |
|---|
| | | for (Point p : poly) { |
|---|
| | | res.add(rotatePoint(p, angle)); |
|---|
| | | } |
|---|
| | | for (Point p : poly) res.add(rotatePoint(p, angle)); |
|---|
| | | return res; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static double distanceSq(Point p1, Point p2) { |
|---|
| | | return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y); |
|---|
| | | /** |
|---|
| | | * 确保多边形顶点为逆时针顺序 |
|---|
| | | * |
|---|
| | | * @param poly 多边形 |
|---|
| | | */ |
|---|
| | | private static void ensureCCW(List<Point> poly) { |
|---|
| | | double s = 0; |
|---|
| | | for (int i = 0; i < poly.size(); i++) { |
|---|
| | | Point p1 = poly.get(i), p2 = poly.get((i + 1) % poly.size()); |
|---|
| | | s += (p2.x - p1.x) * (p2.y + p1.y); |
|---|
| | | } |
|---|
| | | if (s > 0) Collections.reverse(poly); |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 解析坐标字符串 |
|---|
| | | * |
|---|
| | | * @param s 坐标字符串 (格式: "x1,y1;x2,y2;...") |
|---|
| | | * @return 点列表 |
|---|
| | | */ |
|---|
| | | private static List<Point> parseCoords(String s) { |
|---|
| | | List<Point> list = new ArrayList<>(); |
|---|
| | | for (String p : s.split(";")) { |
|---|
| | | String[] xy = p.split(","); |
|---|
| | | if (xy.length >= 2) list.add(new Point(Double.parseDouble(xy[0]), Double.parseDouble(xy[1]))); |
|---|
| | | } |
|---|
| | | return list; |
|---|
| | | } |
|---|
| | | } |
|---|
