| | |
|---|
| | | package lujing; |
|---|
| | | |
|---|
| | | import java.util.ArrayList; |
|---|
| | | import java.util.Collections; |
|---|
| | | import java.util.List; |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 无障碍物凸形草地路径规划类 (优化版) |
|---|
| | | * 凸形草地路径规划 (围边优化版) |
|---|
| | | * 优化重点：围边坐标对齐扫描起点、全路径连贯性 |
|---|
| | | */ |
|---|
| | | 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 { |
|---|
| | | double x, y; |
|---|
| | | |
|---|
| | | public Point(double x, double y) { |
|---|
| | | this.x = x; |
|---|
| | | this.y = y; |
|---|
| | | } |
|---|
| | | |
|---|
| | | public double x, 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.34" |
|---|
| | | * @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("割草宽度或安全边距格式错误"); |
|---|
| | | return planPathCore(originalPolygon, width, margin); |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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<>(); |
|---|
| | | |
|---|
| | | // 2. 预计算最优角度和填充路径的第一个点 |
|---|
| | | double bestAngle = findOptimalScanAngle(workArea); |
|---|
| | | Point firstScanStart = getFirstScanStartPoint(workArea, bestAngle, width); |
|---|
| | | |
|---|
| | | // 3. 对齐围边起点：让围边的最后一个点刚好连接扫描填充的起点 |
|---|
| | | List<Point> alignedWorkArea = alignBoundaryToStart(workArea, firstScanStart); |
|---|
| | | |
|---|
| | | List<PathSegment> finalPath = new ArrayList<>(); |
|---|
| | | |
|---|
| | | // 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)); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 2. 调用核心算法 |
|---|
| | | return planPathCore(originalPolygon, mowingWidth, safetyMargin); |
|---|
| | | // 5. 【第二阶段】生成内部弓字形路径 |
|---|
| | | // 从围边闭合点（alignedWorkArea.get(0)）开始连接 |
|---|
| | | Point currentPos = alignedWorkArea.get(0); |
|---|
| | | List<PathSegment> zigZagLines = generateZigZagPath(workArea, bestAngle, width, currentPos); |
|---|
| | | |
|---|
| | | finalPath.addAll(zigZagLines); |
|---|
| | | |
|---|
| | | return finalPath; |
|---|
| | | } |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 核心算法逻辑 (强类型入参，便于测试和内部调用) |
|---|
| | | * 寻找弓字形的第一条线的起点 |
|---|
| | | */ |
|---|
| | | private static List<PathSegment> planPathCore(List<Point> originalPolygon, double mowingWidth, double safetyMargin) { |
|---|
| | | // 优化：三角形也是合法的凸多边形，限制改为小于3 |
|---|
| | | if (originalPolygon == null || originalPolygon.size() < 3) { |
|---|
| | | throw new IllegalArgumentException("多边形坐标点不足3个，无法构成有效区域"); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 确保多边形是逆时针方向 |
|---|
| | | ensureCCW(originalPolygon); |
|---|
| | | |
|---|
| | | // 1. 根据安全边距内缩 |
|---|
| | | List<Point> shrunkPolygon = shrinkPolygon(originalPolygon, safetyMargin); |
|---|
| | | |
|---|
| | | // 优化：内缩后如果多边形失效（例如地块太窄，内缩后消失），直接返回空路径，避免后续报错 |
|---|
| | | if (shrunkPolygon.size() < 3) { |
|---|
| | | // 这里可以记录日志：地块过小，无法满足安全距离作业 |
|---|
| | | return new ArrayList<>(); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 2. 计算最长边角度 (作为扫描方向) |
|---|
| | | double angle = calculateLongestEdgeAngle(originalPolygon); |
|---|
| | | |
|---|
| | | // 3. & 4. 旋转扫描并剪裁 |
|---|
| | | List<PathSegment> mowingLines = generateClippedMowingLines(shrunkPolygon, angle, mowingWidth); |
|---|
| | | |
|---|
| | | // 5. 弓字形连接 |
|---|
| | | return connectPathSegments(mowingLines); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // ================= 核心算法辅助方法 ================= |
|---|
| | | |
|---|
| | | private static List<Point> shrinkPolygon(List<Point> polygon, double margin) { |
|---|
| | | List<Point> newPoints = new ArrayList<>(); |
|---|
| | | int n = polygon.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); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | return newPoints; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static double calculateLongestEdgeAngle(List<Point> polygon) { |
|---|
| | | double maxDistSq = -1; |
|---|
| | | double angle = 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 dx = p2.x - p1.x; |
|---|
| | | double dy = p2.y - p1.y; |
|---|
| | | double distSq = dx * dx + dy * dy; |
|---|
| | | |
|---|
| | | if (distSq > maxDistSq) { |
|---|
| | | maxDistSq = distSq; |
|---|
| | | angle = Math.atan2(dy, dx); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | return angle; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static List<PathSegment> generateClippedMowingLines(List<Point> polygon, double angle, double width) { |
|---|
| | | List<PathSegment> segments = new ArrayList<>(); |
|---|
| | | |
|---|
| | | // 旋转至水平 |
|---|
| | | List<Point> rotatedPoly = rotatePolygon(polygon, -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 : rotatedPoly) { |
|---|
| | | if (p.y < minY) minY = p.y; |
|---|
| | | if (p.y > maxY) maxY = p.y; |
|---|
| | | } |
|---|
| | | for (Point p : rotated) minY = Math.min(minY, p.y); |
|---|
| | | |
|---|
| | | // 优化：起始扫描线增加一个微小的偏移量 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); |
|---|
| | | |
|---|
| | | // 优化：忽略水平线段 (p1.y == p2.y)，避免除零错误，水平线段不参与垂直扫描线求交 |
|---|
| | | if (Math.abs(p1.y - p2.y) < EPSILON) continue; |
|---|
| | | |
|---|
| | | // 判断线段是否跨越 currentY |
|---|
| | | // 使用严格的不等式逻辑配合范围判断 |
|---|
| | | double minP = Math.min(p1.y, p2.y); |
|---|
| | | double maxP = Math.max(p1.y, p2.y); |
|---|
| | | |
|---|
| | | if (currentY >= minP && currentY < maxP) { |
|---|
| | | // 线性插值求X |
|---|
| | | double x = p1.x + (currentY - p1.y) * (p2.x - p1.x) / (p2.y - p1.y); |
|---|
| | | xIntersections.add(x); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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); // 这里的currentY实际上带了epsilon，还原时没问题 |
|---|
| | | Point rEnd = rotatePoint(new Point(xEnd, currentY), angle); |
|---|
| | | segments.add(new PathSegment(rStart, rEnd, true)); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | currentY += width; |
|---|
| | | } |
|---|
| | | |
|---|
| | | return segments; |
|---|
| | | 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> connectPathSegments(List<PathSegment> lines) { |
|---|
| | | /** |
|---|
| | | * 重组多边形顶点，使得索引0的点最靠近填充起点 |
|---|
| | | */ |
|---|
| | | 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; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | 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> 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) { |
|---|
| | | // 忽略格式错误的单个点 |
|---|
| | | } |
|---|
| | | 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 = 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 list; |
|---|
| | | return xIntersections; |
|---|
| | | } |
|---|
| | | |
|---|
| | | // Shoelace公式判断方向并调整 |
|---|
| | | private static void ensureCCW(List<Point> polygon) { |
|---|
| | | double sum = 0; |
|---|
| | | // --- 几何基础工具 --- |
|---|
| | | |
|---|
| | | 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 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); |
|---|
| | | Point p2 = polygon.get((i + 1) % polygon.size()); |
|---|
| | | sum += (p2.x - p1.x) * (p2.y + p1.y); |
|---|
| | | 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; } |
|---|
| | | } |
|---|
| | | // 假设标准笛卡尔坐标系，sum > 0 为顺时针，需要反转为逆时针 |
|---|
| | | if (sum > 0) { |
|---|
| | | Collections.reverse(polygon); |
|---|
| | | return bestA; |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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; } |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | |
|---|
| | | // 防止除以0 |
|---|
| | | 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); |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | return maxY - minY; |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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; |
|---|
| | | } |
|---|
| | | } |
|---|
