package lujing;
|
|
import java.util.ArrayList;
|
import java.util.Collections;
|
import java.util.List;
|
|
/**
|
* 无障碍物凸形草地路径规划类 (终极优化版)
|
* 特性：
|
* 1. 最小投影宽度方向选择 (效率最高，转弯最少)
|
* 2. 边缘轮廓优先切割 (无死角覆盖)
|
* 3. 支持外部传入已包含重叠率的宽度参数
|
*/
|
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;
|
}
|
|
@Override
|
public String toString() {
|
return String.format("(%.4f, %.4f)", x, y);
|
}
|
}
|
|
/**
|
* 对外公开的静态调用方法
|
*
|
* @param boundaryCoordsStr 地块边界坐标字符串 "x1,y1;x2,y2;..."
|
* @param mowingWidthStr 有效割草宽度字符串 (已包含重叠率)，如 "0.30"
|
* @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;
|
|
try {
|
mowingWidth = Double.parseDouble(mowingWidthStr);
|
safetyMargin = Double.parseDouble(safetyMarginStr);
|
} catch (NumberFormatException e) {
|
throw new IllegalArgumentException("割草宽度或安全边距格式错误");
|
}
|
|
// 2. 调用核心算法
|
return planPathCore(originalPolygon, mowingWidth, safetyMargin);
|
}
|
|
/**
|
* 核心算法逻辑
|
*/
|
private static List<PathSegment> planPathCore(List<Point> originalPolygon, double width, double safetyMargin) {
|
if (originalPolygon == null || originalPolygon.size() < 3) {
|
return new ArrayList<>(); // 或抛出异常，视业务需求而定
|
}
|
|
// 确保多边形是逆时针方向
|
ensureCCW(originalPolygon);
|
|
// 1. 根据安全边距内缩，得到实际作业区域
|
List<Point> workAreaPolygon = shrinkPolygon(originalPolygon, safetyMargin);
|
|
// 如果内缩后区域失效（如地块太小），返回空路径
|
if (workAreaPolygon.size() < 3) {
|
return new ArrayList<>();
|
}
|
|
List<PathSegment> finalPath = new ArrayList<>();
|
|
// 2. [优化] 优先生成轮廓路径 (Contour Pass)
|
// 沿作业边界走一圈，确保边缘整齐且无遗漏
|
addContourPath(workAreaPolygon, finalPath);
|
|
// 3. [优化] 计算最佳扫描角度
|
// 寻找让多边形投影高度最小的角度，从而最小化转弯次数
|
double bestAngle = findOptimalScanAngle(workAreaPolygon);
|
|
// 4. 生成内部弓字形路径
|
// 直接使用传入的 width (已包含重叠率)
|
List<PathSegment> zigZagPaths = generateClippedMowingLines(workAreaPolygon, bestAngle, width);
|
|
// 5. 连接轮廓路径和弓字形路径
|
if (!finalPath.isEmpty() && !zigZagPaths.isEmpty()) {
|
Point contourEnd = finalPath.get(finalPath.size() - 1).end;
|
Point zigzagStart = zigZagPaths.get(0).start;
|
|
// 如果轮廓终点与弓字形起点不重合，添加过渡段
|
if (distanceSq(contourEnd, zigzagStart) > EPSILON) {
|
finalPath.add(new PathSegment(contourEnd, zigzagStart, false));
|
}
|
}
|
|
// 6. 合并弓字形路径
|
finalPath.addAll(connectPathSegments(zigZagPaths));
|
|
return finalPath;
|
}
|
|
// ================= 核心逻辑辅助方法 =================
|
|
/**
|
* 添加轮廓路径 (围着多边形走一圈)
|
*/
|
private static void addContourPath(List<Point> polygon, List<PathSegment> path) {
|
int n = polygon.size();
|
for (int i = 0; i < n; i++) {
|
Point p1 = polygon.get(i);
|
Point p2 = polygon.get((i + 1) % n);
|
path.add(new PathSegment(p1, p2, true));
|
}
|
}
|
|
/**
|
* 寻找最优扫描角度 (最小投影高度法)
|
*/
|
private static double findOptimalScanAngle(List<Point> polygon) {
|
double minHeight = Double.MAX_VALUE;
|
double bestAngle = 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 currentAngle = Math.atan2(p2.y - p1.y, p2.x - p1.x);
|
|
// 计算在这个角度下的投影高度
|
double height = calculatePolygonHeightAtAngle(polygon, currentAngle);
|
|
if (height < minHeight) {
|
minHeight = height;
|
bestAngle = currentAngle;
|
}
|
}
|
return bestAngle;
|
}
|
|
/**
|
* 计算多边形在特定旋转角度下的Y轴投影高度
|
*/
|
private static double calculatePolygonHeightAtAngle(List<Point> poly, double angle) {
|
double minY = Double.MAX_VALUE;
|
double maxY = -Double.MAX_VALUE;
|
|
double cos = Math.cos(-angle);
|
double sin = Math.sin(-angle);
|
|
for (Point p : poly) {
|
// 只需计算旋转后的Y坐标
|
double rotatedY = p.x * sin + p.y * cos;
|
if (rotatedY < minY) minY = rotatedY;
|
if (rotatedY > maxY) maxY = rotatedY;
|
}
|
return maxY - minY;
|
}
|
|
private static List<PathSegment> generateClippedMowingLines(List<Point> polygon, double angle, double width) {
|
List<PathSegment> segments = new ArrayList<>();
|
|
// 旋转多边形至水平
|
List<Point> rotatedPoly = 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;
|
}
|
|
// 起始扫描线位置：
|
// 从 minY + width/2 开始，因为之前已经走了轮廓线(Contour Pass)。
|
// 轮廓线负责清理边缘区域，内部填充线保持 width 的间距即可。
|
// 加上 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);
|
|
// 忽略水平线段
|
if (Math.abs(p1.y - p2.y) < EPSILON) continue;
|
|
double minP = Math.min(p1.y, p2.y);
|
double maxP = Math.max(p1.y, p2.y);
|
|
if (currentY >= minP && currentY < maxP) {
|
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);
|
Point rEnd = rotatePoint(new Point(xEnd, currentY), angle);
|
segments.add(new PathSegment(rStart, rEnd, true));
|
}
|
}
|
// 步进
|
currentY += width;
|
}
|
|
return segments;
|
}
|
|
private static List<PathSegment> connectPathSegments(List<PathSegment> lines) {
|
List<PathSegment> result = new ArrayList<>();
|
if (lines.isEmpty()) return result;
|
|
for (int i = 0; i < lines.size(); i++) {
|
PathSegment currentLine = lines.get(i);
|
Point actualStart, actualEnd;
|
|
// 弓字形规划：偶数行正向，奇数行反向
|
if (i % 2 == 0) {
|
actualStart = currentLine.start;
|
actualEnd = currentLine.end;
|
} else {
|
actualStart = currentLine.end;
|
actualEnd = currentLine.start;
|
}
|
|
// 添加过渡段
|
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));
|
}
|
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) {
|
// 忽略格式错误
|
}
|
}
|
}
|
return list;
|
}
|
|
private static void ensureCCW(List<Point> polygon) {
|
double sum = 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);
|
}
|
if (sum > 0) {
|
Collections.reverse(polygon);
|
}
|
}
|
|
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 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);
|
|
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);
|
}
|
|
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 List<Point> rotatePolygon(List<Point> poly, double angle) {
|
List<Point> res = new ArrayList<>();
|
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);
|
}
|
}
|
