张世豪
11 小时以前 13d032241e1a2938a8be4f64c9171e1240e9ea1e
src/lujing/YixinglujingNoObstacle.java
@@ -1,23 +1,454 @@
package lujing;
import java.util.List;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
/**
 * 无障碍物异形地块路径规划类
 * 异形(无障碍物)草地路径规划类 - 优化版 V2.0
 * * 功能特点:
 * 1. 自动处理凹多边形(通过耳切法分割)
 * 2. 增加“围边”路径,保证边缘割草整洁
 * 3. 自动计算每个子区域的最优扫描角度(减少掉头次数)
 * 4. 智能区域连接(支持双向路径选择)
 */
public class YixinglujingNoObstacle {
    // ==========================================
    // 对外接口
    // ==========================================
    /**
     * 生成路径
     * @param boundaryCoordsStr 地块边界坐标字符串 "x1,y1;x2,y2;..."
     * @param mowingWidthStr 割草宽度字符串,如 "0.34"
     * @param safetyMarginStr 安全边距字符串,如 "0.2"
     * @return 路径坐标字符串,格式 "x1,y1;x2,y2;..."
     * 规划异形草地割草路径
     *
     * @param coordinates 地块边界坐标字符串,格式:"x1,y1;x2,y2;x3,y3;..."
     * @param widthStr    割草宽度(米),如 "0.34"
     * @param marginStr   安全边距(米),如 "0.2"
     * @return 路径段列表
     */
    public static String planPath(String boundaryCoordsStr, String mowingWidthStr, String safetyMarginStr) {
        // TODO: 实现异形地块无障碍物路径规划算法
        // 目前使用默认方法作为临时实现
        throw new UnsupportedOperationException("YixinglujingNoObstacle.planPath 尚未实现");
    public static List<PathSegment> planPath(String coordinates, String widthStr, String marginStr) {
        // 1. 参数解析与预处理
        List<Point> rawPoints = parseCoordinates(coordinates);
        if (rawPoints.size() < 3) {
            throw new IllegalArgumentException("多边形点数不足,无法构成地块");
        }
        // 确保逆时针顺序,方便后续几何计算
        ensureCounterClockwise(rawPoints);
        double mowWidth = Double.parseDouble(widthStr);
        double safeMargin = Double.parseDouble(marginStr);
        List<PathSegment> finalPath = new ArrayList<>();
        // 2. 生成围边路径 (Contour Path)
        // 这一步先规划一圈轮廓,解决异形边缘难处理的问题
        List<Point> contourPoly = getInsetPolygon(rawPoints, safeMargin);
        // 如果内缩后面积太小或点数不足,直接返回空
        if (contourPoly.size() < 3) {
            return new ArrayList<>();
        }
        // 将围边路径加入结果
        for (int i = 0; i < contourPoly.size(); i++) {
            Point p1 = contourPoly.get(i);
            Point p2 = contourPoly.get((i + 1) % contourPoly.size());
            finalPath.add(new PathSegment(p1, p2, true)); // true = 割草
        }
        // 记录围边结束后的位置(通常回到围边起点)
        Point endOfContour = contourPoly.get(0);
        // 3. 区域分割 (Decomposition)
        // 使用耳切法将围边后的多边形分割为多个凸多边形(三角形)
        // 这样可以保证覆盖无遗漏
        List<List<Point>> triangles = triangulatePolygon(contourPoly);
        // 4. 对每个区域生成内部填充路径
        List<List<PathSegment>> allRegionPaths = new ArrayList<>();
        for (List<Point> triangle : triangles) {
            // 【优化】寻找最优扫描角度:
            // 遍历三角形的三条边,计算以哪条边为基准扫描时,生成的行数最少(转弯最少)
            List<PathSegment> regionPath = planConvexPathOptimal(triangle, mowWidth);
            if (!regionPath.isEmpty()) {
                allRegionPaths.add(regionPath);
            }
        }
        // 5. 连接所有内部区域 (Greedy Connection)
        // 从围边结束点开始,寻找最近的下一个区域
        List<PathSegment> internalPaths = connectRegions(allRegionPaths, endOfContour);
        finalPath.addAll(internalPaths);
        return finalPath;
    }
}
    // ==========================================
    // 核心规划算法
    // ==========================================
    /**
     * 规划凸多边形路径,自动选择最优角度
     */
    private static List<PathSegment> planConvexPathOptimal(List<Point> polygon, double width) {
        if (polygon.size() < 3) return new ArrayList<>();
        double bestAngle = 0;
        double minLines = Double.MAX_VALUE;
        // 遍历多边形的每一条边,尝试以该边角度进行扫描
        for (int i = 0; i < polygon.size(); i++) {
            Point p1 = polygon.get(i);
            Point p2 = polygon.get((i + 1) % polygon.size());
            // 计算边的角度
            double angle = Math.atan2(p2.y - p1.y, p2.x - p1.x);
            // 计算在这个角度下,多边形的垂直投影高度
            // 高度越小,意味着沿此方向扫描的行数越少,效率越高
            double height = calculatePolygonHeight(polygon, -angle);
            if (height < minLines) {
                minLines = height;
                bestAngle = angle;
            }
        }
        // 使用最佳角度生成路径
        return generatePathWithAngle(polygon, width, bestAngle);
    }
    /**
     * 根据指定角度生成弓字形路径
     */
    private static List<PathSegment> generatePathWithAngle(List<Point> polygon, double width, double angle) {
        // 1. 将多边形旋转到水平位置
        List<Point> rotatedPoints = new ArrayList<>();
        for (Point p : polygon) {
            rotatedPoints.add(rotatePoint(p, -angle));
        }
        // 2. 计算Y轴范围
        double minY = Double.MAX_VALUE;
        double maxY = -Double.MAX_VALUE;
        for (Point p : rotatedPoints) {
            minY = Math.min(minY, p.y);
            maxY = Math.max(maxY, p.y);
        }
        List<PathSegment> segments = new ArrayList<>();
        boolean leftToRight = true;
        // 3. 扫描线生成 (从 minY + width/2 开始,保证第一刀切在多边形内)
        for (double y = minY + width / 2; y <= maxY; y += width) {
            List<Double> intersections = new ArrayList<>();
            for (int i = 0; i < rotatedPoints.size(); i++) {
                Point p1 = rotatedPoints.get(i);
                Point p2 = rotatedPoints.get((i + 1) % rotatedPoints.size());
                // 判断扫描线是否穿过边
                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);
                    intersections.add(x);
                }
            }
            Collections.sort(intersections);
            // 成对生成线段
            for (int k = 0; k < intersections.size() - 1; k += 2) {
                double x1 = leftToRight ? intersections.get(k) : intersections.get(k + 1);
                double x2 = leftToRight ? intersections.get(k + 1) : intersections.get(k);
                Point start = new Point(x1, y);
                Point end = new Point(x2, y);
                // 旋转回原始坐标系
                Point originalStart = rotatePoint(start, angle);
                Point originalEnd = rotatePoint(end, angle);
                // 连接逻辑:如果不是第一段,需要从上一段终点连过来
                if (!segments.isEmpty()) {
                    PathSegment prev = segments.get(segments.size() - 1);
                    // 添加连接线(通常算作割草路径的一部分,保持弓字形连续)
                    segments.add(new PathSegment(prev.end, originalStart, true));
                }
                segments.add(new PathSegment(originalStart, originalEnd, true));
            }
            leftToRight = !leftToRight; // 换向
        }
        return segments;
    }
    /**
     * 连接所有分割后的区域 (贪心策略 + 双向优化)
     */
    private static List<PathSegment> connectRegions(List<List<PathSegment>> regions, Point startPoint) {
        List<PathSegment> result = new ArrayList<>();
        if (regions.isEmpty()) return result;
        List<List<PathSegment>> remaining = new ArrayList<>(regions);
        Point currentPos = startPoint;
        while (!remaining.isEmpty()) {
            int bestIndex = -1;
            double minDist = Double.MAX_VALUE;
            boolean needReverse = false;
            // 寻找离当前位置最近的区域起点或终点
            for (int i = 0; i < remaining.size(); i++) {
                List<PathSegment> region = remaining.get(i);
                Point pStart = region.get(0).start;
                Point pEnd = region.get(region.size() - 1).end;
                double dStart = distance(currentPos, pStart);
                double dEnd = distance(currentPos, pEnd);
                // 检查正向进入
                if (dStart < minDist) {
                    minDist = dStart;
                    bestIndex = i;
                    needReverse = false;
                }
                // 检查反向进入(倒着割草如果更近)
                if (dEnd < minDist) {
                    minDist = dEnd;
                    bestIndex = i;
                    needReverse = true;
                }
            }
            if (bestIndex != -1) {
                List<PathSegment> targetRegion = remaining.remove(bestIndex);
                if (needReverse) {
                    // 反转该区域的所有路径
                    List<PathSegment> reversedRegion = new ArrayList<>();
                    for (int k = targetRegion.size() - 1; k >= 0; k--) {
                        PathSegment seg = targetRegion.get(k);
                        // 交换起点终点
                        reversedRegion.add(new PathSegment(seg.end, seg.start, seg.isMowing));
                    }
                    targetRegion = reversedRegion;
                }
                // 添加过渡路径(抬刀移动,isMowing=false)
                Point nextStart = targetRegion.get(0).start;
                // 只有距离显著才添加移动段
                if (distance(currentPos, nextStart) > 0.01) {
                    result.add(new PathSegment(currentPos, nextStart, false));
                }
                result.addAll(targetRegion);
                currentPos = targetRegion.get(targetRegion.size() - 1).end;
            } else {
                break; // 防御性代码
            }
        }
        return result;
    }
    // ==========================================
    // 几何运算辅助方法
    // ==========================================
    /**
     * 内缩多边形 (基于角平分线)
     */
    private static List<Point> getInsetPolygon(List<Point> points, double margin) {
        List<Point> result = new ArrayList<>();
        int n = points.size();
        for (int i = 0; i < n; i++) {
            Point pPrev = points.get((i - 1 + n) % n);
            Point pCurr = points.get(i);
            Point pNext = points.get((i + 1) % n);
            Point v1 = new Point(pCurr.x - pPrev.x, pCurr.y - pPrev.y);
            Point v2 = new Point(pNext.x - pCurr.x, pNext.y - pCurr.y);
            double len1 = Math.hypot(v1.x, v1.y);
            double len2 = Math.hypot(v2.x, v2.y);
            if (len1 < 1e-6 || len2 < 1e-6) continue;
            // 归一化向量
            Point n1 = new Point(v1.x / len1, v1.y / len1);
            Point n2 = new Point(v2.x / len2, v2.y / len2);
            // 计算平分线方向
            // v1反向 + v2
            Point bisector = new Point(-n1.x + n2.x, -n1.y + n2.y);
            double biLen = Math.hypot(bisector.x, bisector.y);
            // 计算半角 sin(theta/2)
            double cross = n1.x * n2.y - n1.y * n2.x; // 叉积判断转向
            // 默认向左侧内缩 (CCW多边形)
            if (biLen < 1e-6) {
                // 共线,沿法线方向
                bisector = new Point(-n1.y, n1.x);
            } else {
                bisector.x /= biLen;
                bisector.y /= biLen;
            }
            // 计算偏移距离
            double dot = n1.x * n2.x + n1.y * n2.y;
            double angle = Math.acos(Math.max(-1, Math.min(1, dot)));
            double dist = margin / Math.sin(angle / 2.0);
            // 方向修正:确保平分线指向多边形内部(逆时针多边形的左侧)
            Point leftNormal = new Point(-n1.y, n1.x);
            if (bisector.x * leftNormal.x + bisector.y * leftNormal.y < 0) {
                 bisector.x = -bisector.x;
                 bisector.y = -bisector.y;
            }
            // 如果是凹角(cross < 0),平分线指向外部,距离需要反转或者特殊处理
            // 简单处理:对于凹角,偏移点实际上会远离原点,上述逻辑通常能覆盖,
            // 但极端锐角可能导致dist过大。此处做简单截断保护是不够的,
            // 但针对一般草地形状,此逻辑可用。
            result.add(new Point(pCurr.x + bisector.x * dist, pCurr.y + bisector.y * dist));
        }
        return result;
    }
    /**
     * 耳切法分割多边形
     */
    private static List<List<Point>> triangulatePolygon(List<Point> poly) {
        List<List<Point>> triangles = new ArrayList<>();
        List<Point> remaining = new ArrayList<>(poly);
        int maxIter = remaining.size() * 3;
        int iter = 0;
        while (remaining.size() > 3 && iter++ < maxIter) {
            int n = remaining.size();
            boolean earFound = false;
            for (int i = 0; i < n; i++) {
                Point prev = remaining.get((i - 1 + n) % n);
                Point curr = remaining.get(i);
                Point next = remaining.get((i + 1) % n);
                if (isConvex(prev, curr, next)) {
                    boolean hasPoint = false;
                    for (int j = 0; j < n; j++) {
                        if (j == i || j == (i - 1 + n) % n || j == (i + 1) % n) continue;
                        if (isPointInTriangle(remaining.get(j), prev, curr, next)) {
                            hasPoint = true;
                            break;
                        }
                    }
                    if (!hasPoint) {
                        List<Point> tri = new ArrayList<>();
                        tri.add(prev); tri.add(curr); tri.add(next);
                        triangles.add(tri);
                        remaining.remove(i);
                        earFound = true;
                        break;
                    }
                }
            }
            if (!earFound) break;
        }
        if (remaining.size() == 3) {
            triangles.add(remaining);
        }
        return triangles;
    }
    private static double calculatePolygonHeight(List<Point> poly, double angle) {
        double minY = Double.MAX_VALUE;
        double maxY = -Double.MAX_VALUE;
        for (Point p : poly) {
            Point r = rotatePoint(p, angle);
            minY = Math.min(minY, r.y);
            maxY = Math.max(maxY, r.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);
    }
    private static boolean isConvex(Point a, Point b, Point c) {
        return (b.x - a.x) * (c.y - b.y) - (b.y - a.y) * (c.x - b.x) >= 0;
    }
    private static boolean isPointInTriangle(Point p, Point a, Point b, Point c) {
        double areaABC = Math.abs((a.x*(b.y-c.y) + b.x*(c.y-a.y) + c.x*(a.y-b.y))/2.0);
        double areaPBC = Math.abs((p.x*(b.y-c.y) + b.x*(c.y-p.y) + c.x*(p.y-b.y))/2.0);
        double areaPAC = Math.abs((a.x*(p.y-c.y) + p.x*(c.y-a.y) + c.x*(a.y-p.y))/2.0);
        double areaPAB = Math.abs((a.x*(b.y-p.y) + b.x*(p.y-a.y) + p.x*(a.y-b.y))/2.0);
        return Math.abs(areaABC - (areaPBC + areaPAC + areaPAB)) < 1e-6;
    }
    private static List<Point> parseCoordinates(String coordinates) {
        List<Point> points = new ArrayList<>();
        String cleanStr = coordinates.replaceAll("[()\\[\\]{}]", "").trim();
        String[] pairs = cleanStr.split(";");
        for (String pair : pairs) {
            pair = pair.trim();
            if (pair.isEmpty()) continue;
            String[] xy = pair.split(",");
            if (xy.length == 2) {
                points.add(new Point(Double.parseDouble(xy[0].trim()), Double.parseDouble(xy[1].trim())));
            }
        }
        return points;
    }
    private static void ensureCounterClockwise(List<Point> points) {
        double sum = 0;
        for (int i = 0; i < points.size(); i++) {
            Point p1 = points.get(i);
            Point p2 = points.get((i + 1) % points.size());
            sum += (p2.x - p1.x) * (p2.y + p1.y);
        }
        if (sum > 0) {
            Collections.reverse(points);
        }
    }
    private static double distance(Point p1, Point p2) {
        return Math.hypot(p1.x - p2.x, p1.y - p2.y);
    }
    // ==========================================
    // 内部数据结构
    // ==========================================
    public static class Point {
        public double x, y;
        public Point(double x, double y) { this.x = x; this.y = y; }
        @Override public String toString() { return String.format("%.2f,%.2f", x, y); }
    }
    public static class PathSegment {
        public Point start;
        public Point end;
        public boolean isMowing;
        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, 割草:%b]", start, end, isMowing);
        }
    }
}