新增有障碍物的路径规划算法和优化没有障碍物的路径算法

张世豪

6 天以前 1cf1ecbc75c6d14b40efb3161e7db0b8b64f7de2

 src/lujing/Lunjingguihua.java

@@ -30,16 +30,18 @@
    /**
     * 生成割草路径段列表。
     *
     * @param polygonCoords 多边形边界坐标，格式如 "x1,y1;x2,y2;..."（米）
     * @param polygonCoords   多边形边界坐标，格式如 "x1,y1;x2,y2;..."（米）
     * @param obstaclesCoords 障碍物坐标，支持多个括号段或圆形定义，例 "(x1,y1;x2,y2)(cx,cy;px,py)"
     * @param mowingWidth 割草宽度字符串，米单位，允许保留两位小数
     * @param modeStr 割草模式，"0"/空为平行线，"1" 或 "spiral" 表示螺旋模式（当前仅平行线实现）
     * @param mowingWidth     割草宽度字符串，米单位，允许保留两位小数
     * @param safetyDistStr   安全距离字符串，米单位。路径将与边界和障碍物保持此距离。
     * @param modeStr         割草模式，"0"/空为平行线，"1" 或 "spiral" 表示螺旋模式（当前仅平行线实现）
     * @return 路径段列表，按行驶顺序排列
     */
    public static List<PathSegment> generatePathSegments(String polygonCoords,
                                                          String obstaclesCoords,
                                                          String mowingWidth,
                                                          String modeStr) {
                                                         String obstaclesCoords,
                                                         String mowingWidth,
                                                         String safetyDistStr,
                                                         String modeStr) {
        List<Coordinate> polygon = parseCoordinates(polygonCoords);
        if (polygon.size() < 4) {
            throw new IllegalArgumentException("多边形坐标数量不足，至少需要三个点");
@@ -49,30 +51,55 @@
        if (width <= 0) {
            throw new IllegalArgumentException("割草宽度必须大于 0");
        }
        
        // 解析安全距离，如果未设置或无效，默认为 NaN (在 PlannerCore 中处理默认值)
        double safetyDistance = parseDoubleOrDefault(safetyDistStr, Double.NaN);

        List<List<Coordinate>> obstacles = parseObstacles(obstaclesCoords);
        String mode = normalizeMode(modeStr);

        PlannerCore planner = new PlannerCore(polygon, width, mode, obstacles);
        PlannerCore planner = new PlannerCore(polygon, width, safetyDistance, mode, obstacles);
        return planner.generate();
    }

    /**
     * 保持向后兼容的重载方法（不带 safeDistance，使用默认计算）
     */
    public static List<PathSegment> generatePathSegments(String polygonCoords,
                                                         String obstaclesCoords,
                                                         String mowingWidth,
                                                         String modeStr) {
        return generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
    }

    /**
     * 通过字符串参数生成割草路径坐标。
     *
     * @param polygonCoords 多边形边界坐标，格式如 "x1,y1;x2,y2;..."（米）
     * @param polygonCoords   多边形边界坐标，格式如 "x1,y1;x2,y2;..."（米）
     * @param obstaclesCoords 障碍物坐标，支持多个括号段或圆形定义，例 "(x1,y1;x2,y2)(cx,cy;px,py)"
     * @param mowingWidth 割草宽度字符串，米单位，允许保留两位小数
     * @param modeStr 割草模式，"0"/空为平行线，"1" 或 "spiral" 表示螺旋模式（当前仅平行线实现）
     * @param mowingWidth     割草宽度字符串，米单位，允许保留两位小数
     * @param safetyDistStr   安全距离字符串，米单位。
     * @param modeStr         割草模式，"0"/空为平行线，"1" 或 "spiral" 表示螺旋模式（当前仅平行线实现）
     * @return 连续路径坐标字符串，顺序紧跟割草机行进路线
     */
    public static String generatePathFromStrings(String polygonCoords,
                                                 String obstaclesCoords,
                                                 String mowingWidth,
                                                 String safetyDistStr,
                                                 String modeStr) {
        List<PathSegment> segments = generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, modeStr);
        List<PathSegment> segments = generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, safetyDistStr, modeStr);
        return formatPathSegments(segments);
    }
    
    /**
     * 保持向后兼容的重载方法
     */
    public static String generatePathFromStrings(String polygonCoords,
                                                 String obstaclesCoords,
                                                 String mowingWidth,
                                                 String modeStr) {
        return generatePathFromStrings(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
    }

    /**
     * 将路径段列表转换为坐标字符串。
@@ -168,7 +195,7 @@
        try {
            return Double.parseDouble(value.trim());
        } catch (NumberFormatException ex) {
            throw new IllegalArgumentException("割草宽度格式不正确: " + value, ex);
            throw new IllegalArgumentException("格式不正确: " + value, ex);
        }
    }

@@ -227,7 +254,7 @@
        public boolean isStartPoint;
        public boolean isEndPoint;

        PathSegment(Coordinate start, Coordinate end, boolean isMowing) {
        public PathSegment(Coordinate start, Coordinate end, boolean isMowing) {
            this.start = start;
            this.end = end;
            this.isMowing = isMowing;
@@ -251,28 +278,53 @@
    /**
     * 内部核心规划器，实现与 MowingPathPlanner 等效的逻辑。
     */
    private static final class PlannerCore {
    static final class PlannerCore {
        private final List<Coordinate> polygon;
        private final double width;
        private final double safetyDistance; // 新增安全距离字段
        private final String mode;
        private final List<List<Coordinate>> obstacles;
        private final GeometryFactory gf = new GeometryFactory();

        PlannerCore(List<Coordinate> polygon, double width, String mode, List<List<Coordinate>> obstacles) {
        PlannerCore(List<Coordinate> polygon, double width, double safetyDistance, String mode, List<List<Coordinate>> obstacles) {
            this.polygon = polygon;
            this.width = width;
            this.mode = mode;
            this.obstacles = obstacles != null ? obstacles : new ArrayList<>();
            
            // 初始化安全距离逻辑
            if (Double.isNaN(safetyDistance)) {
                // 如果未提供，使用默认值：宽度的一半 + 0.05米
                this.safetyDistance = width / 2.0 + 0.05;
            } else {
                // 如果提供了，使用提供的值，但至少要保证机器中心不碰壁（宽度一半）
                // 允许用户设置比 width/2 更大的值来远离边界
                this.safetyDistance = Math.max(safetyDistance, width / 2.0);
            }
        }
        
        // 兼容旧构造函数
        PlannerCore(List<Coordinate> polygon, double width, String mode, List<List<Coordinate>> obstacles) {
            this(polygon, width, Double.NaN, mode, obstacles);
        }

        List<PathSegment> generate() {
            // 如果有障碍物，使用带障碍物避让的路径规划器
            if (!obstacles.isEmpty()) {
                // 使用计算好的安全距离
                ObstaclePathPlanner obstaclePlanner = new ObstaclePathPlanner(
                    polygon, width, mode, obstacles, this.safetyDistance);
                return obstaclePlanner.generate();
            }
            
            // 没有障碍物时使用原有逻辑
            if ("spiral".equals(mode)) {
                return generateSpiralPath();
            }
            return generateParallelPath();
        }

        private List<PathSegment> generateParallelPath() {
        List<PathSegment> generateParallelPath() {
            List<PathSegment> path = new ArrayList<>();
            Geometry safeArea = buildSafeArea();
            if (safeArea == null || safeArea.isEmpty()) {
@@ -285,7 +337,7 @@
                                            longest.end.y - longest.start.y).normalize();
            Vector2D perp = baseDir.rotate90CCW();
            Vector2D baseStartVec = new Vector2D(longest.start.x, longest.start.y);
            double baseProjection = perp.dot(baseStartVec); // keep offsets relative to the longest edge start
            double baseProjection = perp.dot(baseStartVec); 

            double minProj = Double.POSITIVE_INFINITY;
            double maxProj = Double.NEGATIVE_INFINITY;
@@ -361,7 +413,7 @@
            return path;
        }

        private List<PathSegment> generateSpiralPath() {
        List<PathSegment> generateSpiralPath() {
            Geometry safeArea = buildSafeArea();
            if (safeArea == null || safeArea.isEmpty()) {
                System.err.println("安全区域为空，无法生成螺旋路径");
@@ -418,7 +470,10 @@
                    }
                }

                Geometry shrunk = shrinkStraight(result, width / 2.0);
                // 修改：使用传入的 safetyDistance 来进行边界内缩
                // 之前是 width / 2.0，现在使用 this.safetyDistance
                // 这确保了路径规划区域与边界保持用户指定的距离
                Geometry shrunk = shrinkStraight(result, this.safetyDistance);
                return shrunk.isEmpty() ? result : shrunk;
            } catch (Exception ex) {
                System.err.println("构建安全区域失败: " + ex.getMessage());
@@ -620,4 +675,4 @@
            this.index = index;
        }
    }
}
}