package lujing;
|
|
import java.util.ArrayList;
|
import java.util.Collections;
|
import java.util.List;
|
import org.locationtech.jts.geom.Coordinate;
|
import org.locationtech.jts.geom.Envelope;
|
import org.locationtech.jts.geom.Geometry;
|
import org.locationtech.jts.geom.GeometryFactory;
|
import org.locationtech.jts.geom.LineString;
|
import org.locationtech.jts.geom.MultiLineString;
|
import org.locationtech.jts.geom.Polygon;
|
import org.locationtech.jts.geom.MultiPolygon;
|
|
/**
|
* 优化后的割草路径规划类
|
* 修复：解决路径超出地块边界的问题，增加安全边距计算的健壮性。
|
*/
|
public final class Lunjingguihua {
|
|
private Lunjingguihua() {
|
throw new IllegalStateException("Utility class");
|
}
|
|
// 5参数核心生成方法
|
public static List<PathSegment> generatePathSegments(String polygonCoords,
|
String obstaclesCoords,
|
String mowingWidth,
|
String safetyDistStr,
|
String modeStr) {
|
List<Coordinate> polygon = parseCoordinates(polygonCoords);
|
if (polygon.size() < 4) {
|
throw new IllegalArgumentException("多边形坐标数量不足");
|
}
|
|
double width = parseDoubleOrDefault(mowingWidth, 0.34);
|
// 如果传入空，设为 NaN，在 PlannerCore 中进行智能计算
|
double safetyDistance = parseDoubleOrDefault(safetyDistStr, Double.NaN);
|
|
List<List<Coordinate>> obstacles = parseObstacles(obstaclesCoords);
|
String mode = normalizeMode(modeStr);
|
|
PlannerCore planner = new PlannerCore(polygon, width, safetyDistance, mode, obstacles);
|
return planner.generate();
|
}
|
|
// 4参数重载，适配 AddDikuai.java
|
public static List<PathSegment> generatePathSegments(String polygonCoords,
|
String obstaclesCoords,
|
String mowingWidth,
|
String modeStr) {
|
return generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
|
}
|
|
// 5参数路径字符串生成
|
public static String generatePathFromStrings(String polygonCoords,
|
String obstaclesCoords,
|
String mowingWidth,
|
String safetyDistStr,
|
String modeStr) {
|
List<PathSegment> segments = generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, safetyDistStr, modeStr);
|
return formatPathSegments(segments);
|
}
|
|
// 4参数路径字符串生成重载
|
public static String generatePathFromStrings(String polygonCoords,
|
String obstaclesCoords,
|
String mowingWidth,
|
String modeStr) {
|
return generatePathFromStrings(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
|
}
|
|
public static String formatPathSegments(List<PathSegment> path) {
|
if (path == null || path.isEmpty()) return "";
|
StringBuilder sb = new StringBuilder();
|
Coordinate last = null;
|
for (PathSegment segment : path) {
|
if (last == null || !equals2D(last, segment.start)) {
|
appendPoint(sb, segment.start);
|
}
|
appendPoint(sb, segment.end);
|
last = segment.end;
|
}
|
return sb.toString();
|
}
|
|
public static List<Coordinate> parseCoordinates(String s) {
|
List<Coordinate> list = new ArrayList<>();
|
if (s == null || s.trim().isEmpty()) return list;
|
// 增强正则：处理可能存在的多种分隔符
|
String[] pts = s.split("[;\\s]+");
|
for (String p : pts) {
|
String trimmed = p.trim().replace("(", "").replace(")", "");
|
if (trimmed.isEmpty()) continue;
|
String[] xy = trimmed.split("[,，\\s]+");
|
if (xy.length >= 2) {
|
try {
|
double x = Double.parseDouble(xy[0].trim());
|
double y = Double.parseDouble(xy[1].trim());
|
// 过滤无效坐标
|
if (!Double.isNaN(x) && !Double.isNaN(y) && !Double.isInfinite(x) && !Double.isInfinite(y)) {
|
list.add(new Coordinate(x, y));
|
}
|
} catch (NumberFormatException ex) {
|
// 忽略解析错误的点
|
}
|
}
|
}
|
// 确保多边形闭合
|
if (list.size() > 2 && !list.get(0).equals2D(list.get(list.size() - 1))) {
|
list.add(new Coordinate(list.get(0)));
|
}
|
return list;
|
}
|
|
public static List<List<Coordinate>> parseObstacles(String str) {
|
List<List<Coordinate>> obs = new ArrayList<>();
|
if (str == null || str.trim().isEmpty()) return obs;
|
java.util.regex.Pattern pattern = java.util.regex.Pattern.compile("\\(([^)]+)\\)");
|
java.util.regex.Matcher matcher = pattern.matcher(str);
|
while (matcher.find()) {
|
List<Coordinate> coords = parseCoordinates(matcher.group(1));
|
if (coords.size() >= 3) obs.add(coords);
|
}
|
if (obs.isEmpty()) {
|
List<Coordinate> coords = parseCoordinates(str);
|
if (coords.size() >= 3) obs.add(coords);
|
}
|
return obs;
|
}
|
|
private static double parseDoubleOrDefault(String value, double defaultValue) {
|
if (value == null || value.trim().isEmpty()) return defaultValue;
|
try {
|
return Double.parseDouble(value.trim());
|
} catch (NumberFormatException ex) {
|
return defaultValue;
|
}
|
}
|
|
private static String normalizeMode(String modeStr) {
|
return (modeStr != null && (modeStr.equals("1") || modeStr.equalsIgnoreCase("spiral"))) ? "spiral" : "parallel";
|
}
|
|
private static boolean equals2D(Coordinate a, Coordinate b) {
|
if (a == b) return true;
|
if (a == null || b == null) return false;
|
return a.distance(b) < 1e-4;
|
}
|
|
private static void appendPoint(StringBuilder sb, Coordinate point) {
|
if (sb.length() > 0) sb.append(";");
|
sb.append(String.format("%.3f,%.3f", point.x, point.y));
|
}
|
|
public static final class PathSegment {
|
public Coordinate start, end;
|
public boolean isMowing;
|
public boolean isStartPoint, isEndPoint;
|
|
public PathSegment(Coordinate start, Coordinate end, boolean isMowing) {
|
this.start = start;
|
this.end = end;
|
this.isMowing = isMowing;
|
}
|
|
public void setAsStartPoint() { this.isStartPoint = true; }
|
public void setAsEndPoint() { this.isEndPoint = true; }
|
}
|
|
public 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();
|
|
// 1. 全参数构造函数
|
public 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<>();
|
|
// FIX: 增加默认安全边距。原逻辑为 width/2 + 0.05，容易造成误差出界。
|
// 现改为 width/2 + 0.2 (20cm余量)，确保割草机实体完全在界内。
|
if (Double.isNaN(safetyDistance) || safetyDistance <= 0) {
|
this.safetyDistance = (width / 2.0) + 0.20;
|
} else {
|
this.safetyDistance = safetyDistance;
|
}
|
}
|
|
// 2. 4参数构造函数
|
public PlannerCore(List<Coordinate> polygon, double width, String mode, List<List<Coordinate>> obstacles) {
|
this(polygon, width, Double.NaN, mode, obstacles);
|
}
|
|
public List<PathSegment> generate() {
|
if ("spiral".equals(mode)) return generateSpiralPath();
|
return generateDividedParallelPath();
|
}
|
|
public List<PathSegment> generateParallelPath() {
|
return generateDividedParallelPath();
|
}
|
|
private List<PathSegment> generateDividedParallelPath() {
|
List<PathSegment> totalPath = new ArrayList<>();
|
Geometry safeArea = buildSafeArea();
|
|
if (safeArea == null || safeArea.isEmpty()) return totalPath;
|
|
List<Polygon> subRegions = new ArrayList<>();
|
if (safeArea instanceof Polygon) subRegions.add((Polygon) safeArea);
|
else if (safeArea instanceof MultiPolygon) {
|
for (int i = 0; i < safeArea.getNumGeometries(); i++) {
|
subRegions.add((Polygon) safeArea.getGeometryN(i));
|
}
|
}
|
|
for (Polygon region : subRegions) {
|
if (region.isEmpty()) continue;
|
|
Vector2D baseDir = calculateMainDirection(region);
|
Vector2D perp = baseDir.rotate90CCW();
|
Envelope env = region.getEnvelopeInternal();
|
|
double minProj = Double.MAX_VALUE, maxProj = -Double.MAX_VALUE;
|
Coordinate[] coords = region.getCoordinates();
|
for (Coordinate c : coords) {
|
double p = perp.dot(new Vector2D(c));
|
minProj = Math.min(minProj, p);
|
maxProj = Math.max(maxProj, p);
|
}
|
|
int lineIdx = 0;
|
// 从 minProj + width/2 开始，确保第一条线在安全区域内侧
|
for (double d = minProj + width / 2.0; d <= maxProj; d += width) {
|
LineString scanLine = createScanLine(d, perp, baseDir, env);
|
|
try {
|
Geometry intersections = region.intersection(scanLine);
|
if (intersections.isEmpty()) continue;
|
|
List<LineString> parts = extractLineStrings(intersections);
|
|
// 按照扫描方向排序，处理凹多边形或障碍物
|
parts.sort((a, b) -> Double.compare(
|
baseDir.dot(new Vector2D(a.getCoordinateN(0))),
|
baseDir.dot(new Vector2D(b.getCoordinateN(0)))
|
));
|
|
// 蛇形路径：奇数行反转
|
if (lineIdx % 2 != 0) Collections.reverse(parts);
|
|
for (LineString part : parts) {
|
Coordinate[] cs = part.getCoordinates();
|
if (cs.length < 2) continue;
|
|
if (lineIdx % 2 != 0) reverseArray(cs);
|
|
// 确保点坐标有效
|
totalPath.add(new PathSegment(cs[0], cs[cs.length - 1], true));
|
}
|
lineIdx++;
|
} catch (Exception e) {
|
// 忽略极其罕见的拓扑异常，防止崩溃
|
}
|
}
|
}
|
return markStartEnd(totalPath);
|
}
|
|
private Geometry buildSafeArea() {
|
try {
|
Polygon poly = gf.createPolygon(gf.createLinearRing(polygon.toArray(new Coordinate[0])));
|
|
// 1. 初始修复：处理自相交
|
if (!poly.isValid()) poly = (Polygon) poly.buffer(0);
|
|
// 2. 内缩生成安全区域
|
Geometry safe = poly.buffer(-safetyDistance);
|
|
// 3. 二次修复：负缓冲后可能产生不规范几何体
|
if (!safe.isValid()) safe = safe.buffer(0);
|
|
// 4. 处理障碍物
|
for (List<Coordinate> obsCoords : obstacles) {
|
if (obsCoords.size() < 3) continue;
|
try {
|
Polygon obs = gf.createPolygon(gf.createLinearRing(obsCoords.toArray(new Coordinate[0])));
|
if (!obs.isValid()) obs = (Polygon) obs.buffer(0);
|
// 障碍物外扩安全距离
|
safe = safe.difference(obs.buffer(safetyDistance));
|
} catch (Exception e) {
|
// 忽略错误的障碍物数据
|
}
|
}
|
|
// 5. 最终清理
|
if (!safe.isValid()) safe = safe.buffer(0);
|
return safe;
|
} catch (Exception e) {
|
// 如果几何构建完全失败，返回空
|
return gf.createPolygon();
|
}
|
}
|
|
private Vector2D calculateMainDirection(Polygon region) {
|
Coordinate[] coords = region.getExteriorRing().getCoordinates();
|
double maxLen = -1;
|
Vector2D bestDir = new Vector2D(1, 0);
|
|
// 寻找最长边作为主方向，减少转弯次数
|
for (int i = 0; i < coords.length - 1; i++) {
|
double d = coords[i].distance(coords[i+1]);
|
if (d > maxLen && d > 1e-4) {
|
maxLen = d;
|
bestDir = new Vector2D(coords[i+1].x - coords[i].x, coords[i+1].y - coords[i].y).normalize();
|
}
|
}
|
return bestDir;
|
}
|
|
private List<LineString> extractLineStrings(Geometry geom) {
|
List<LineString> list = new ArrayList<>();
|
if (geom instanceof LineString) list.add((LineString) geom);
|
else if (geom instanceof MultiLineString) {
|
for (int i = 0; i < geom.getNumGeometries(); i++) list.add((LineString) geom.getGeometryN(i));
|
} else if (geom instanceof org.locationtech.jts.geom.GeometryCollection) {
|
for (int i = 0; i < geom.getNumGeometries(); i++) {
|
if (geom.getGeometryN(i) instanceof LineString) {
|
list.add((LineString) geom.getGeometryN(i));
|
}
|
}
|
}
|
return list;
|
}
|
|
private LineString createScanLine(double dist, Vector2D perp, Vector2D baseDir, Envelope env) {
|
// 扩大扫描线长度，确保覆盖旋转后的多边形
|
double size = Math.max(env.getWidth(), env.getHeight());
|
// 处理退化包围盒
|
if (size < 1.0) size = 1000.0;
|
|
double len = size * 3.0; // 3倍尺寸确保足够长
|
|
// 中心点计算：在垂直方向上距离原点 dist 的位置
|
Vector2D center = perp.mul(dist);
|
|
return gf.createLineString(new Coordinate[]{
|
new Coordinate(center.x + baseDir.x * len, center.y + baseDir.y * len),
|
new Coordinate(center.x - baseDir.x * len, center.y - baseDir.y * len)
|
});
|
}
|
|
private List<PathSegment> markStartEnd(List<PathSegment> path) {
|
if (!path.isEmpty()) {
|
path.get(0).setAsStartPoint();
|
path.get(path.size() - 1).setAsEndPoint();
|
}
|
return path;
|
}
|
|
private void reverseArray(Coordinate[] arr) {
|
for (int i = 0; i < arr.length / 2; i++) {
|
Coordinate t = arr[i];
|
arr[i] = arr[arr.length - 1 - i];
|
arr[arr.length - 1 - i] = t;
|
}
|
}
|
|
List<PathSegment> generateSpiralPath() { return new ArrayList<>(); }
|
}
|
|
private static final class Vector2D {
|
final double x, y;
|
Vector2D(double x, double y) { this.x = x; this.y = y; }
|
Vector2D(Coordinate c) { this.x = c.x; this.y = c.y; }
|
|
Vector2D normalize() {
|
double len = Math.hypot(x, y);
|
return len < 1e-9 ? new Vector2D(1, 0) : new Vector2D(x / len, y / len);
|
}
|
Vector2D rotate90CCW() { return new Vector2D(-y, x); }
|
double dot(Vector2D v) { return x * v.x + y * v.y; }
|
Vector2D mul(double k) { return new Vector2D(x * k, y * k); }
|
}
|
}
|
