| | |
| | | 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); |
| | | } |
| | | } |
| | | } |