package lujing;
|
|
import java.util.ArrayList;
|
import java.util.Collections;
|
import java.util.List;
|
|
/**
|
* 异形（无障碍物）草地路径规划类 - 优化版 V2.0
|
* * 功能特点：
|
* 1. 自动处理凹多边形（通过耳切法分割）
|
* 2. 增加“围边”路径，保证边缘割草整洁
|
* 3. 自动计算每个子区域的最优扫描角度（减少掉头次数）
|
* 4. 智能区域连接（支持双向路径选择）
|
*/
|
public class YixinglujingNoObstacle {
|
|
// ==========================================
|
// 对外接口
|
// ==========================================
|
|
/**
|
* 规划异形草地割草路径
|
*
|
* @param coordinates 地块边界坐标字符串，格式："x1,y1;x2,y2;x3,y3;..."
|
* @param widthStr 割草宽度（米），如 "0.34"
|
* @param marginStr 安全边距（米），如 "0.2"
|
* @return 路径段列表
|
*/
|
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);
|
}
|
}
|
}
|
