package lujing;
|
import java.util.ArrayList;
|
import java.util.Collections;
|
import java.util.List;
|
|
/**
|
* 无障碍物凸形草地路径规划类 (优化版)
|
*/
|
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.34"
|
* @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 mowingWidth, double safetyMargin) {
|
// 优化：三角形也是合法的凸多边形，限制改为小于3
|
if (originalPolygon == null || originalPolygon.size() < 3) {
|
throw new IllegalArgumentException("多边形坐标点不足3个，无法构成有效区域");
|
}
|
|
// 确保多边形是逆时针方向
|
ensureCCW(originalPolygon);
|
|
// 1. 根据安全边距内缩
|
List<Point> shrunkPolygon = shrinkPolygon(originalPolygon, safetyMargin);
|
|
// 优化：内缩后如果多边形失效（例如地块太窄，内缩后消失），直接返回空路径，避免后续报错
|
if (shrunkPolygon.size() < 3) {
|
// 这里可以记录日志：地块过小，无法满足安全距离作业
|
return new ArrayList<>();
|
}
|
|
// 2. 计算最长边角度 (作为扫描方向)
|
double angle = calculateLongestEdgeAngle(originalPolygon);
|
|
// 3. & 4. 旋转扫描并剪裁
|
List<PathSegment> mowingLines = generateClippedMowingLines(shrunkPolygon, angle, mowingWidth);
|
|
// 5. 弓字形连接
|
return connectPathSegments(mowingLines);
|
}
|
|
// ================= 核心算法辅助方法 =================
|
|
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 double calculateLongestEdgeAngle(List<Point> polygon) {
|
double maxDistSq = -1;
|
double angle = 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 dx = p2.x - p1.x;
|
double dy = p2.y - p1.y;
|
double distSq = dx * dx + dy * dy;
|
|
if (distSq > maxDistSq) {
|
maxDistSq = distSq;
|
angle = Math.atan2(dy, dx);
|
}
|
}
|
return angle;
|
}
|
|
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;
|
}
|
|
// 优化：起始扫描线增加一个微小的偏移量 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);
|
|
// 优化：忽略水平线段 (p1.y == p2.y)，避免除零错误，水平线段不参与垂直扫描线求交
|
if (Math.abs(p1.y - p2.y) < EPSILON) continue;
|
|
// 判断线段是否跨越 currentY
|
// 使用严格的不等式逻辑配合范围判断
|
double minP = Math.min(p1.y, p2.y);
|
double maxP = Math.max(p1.y, p2.y);
|
|
if (currentY >= minP && currentY < maxP) {
|
// 线性插值求X
|
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); // 这里的currentY实际上带了epsilon，还原时没问题
|
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;
|
}
|
|
// Shoelace公式判断方向并调整
|
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);
|
}
|
// 假设标准笛卡尔坐标系，sum > 0 为顺时针，需要反转为逆时针
|
if (sum > 0) {
|
Collections.reverse(polygon);
|
}
|
}
|
|
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);
|
|
// 防止除以0
|
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);
|
}
|
}
|
