package lujing;
|
|
import java.awt.geom.Line2D;
|
import java.util.ArrayList;
|
import java.util.Arrays;
|
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.LinearRing;
|
import org.locationtech.jts.geom.MultiLineString;
|
import org.locationtech.jts.geom.MultiPolygon;
|
import org.locationtech.jts.geom.Polygon;
|
import org.locationtech.jts.operation.union.CascadedPolygonUnion;
|
|
/**
|
* 割草路径规划实用类，供其他项目直接调用。
|
* 提供字符串入参的割草路径生成能力，并封装必要的解析与几何处理逻辑。
|
*/
|
public final class Lunjingguihua {
|
|
private Lunjingguihua() {
|
throw new IllegalStateException("Utility class");
|
}
|
|
/**
|
* 生成割草路径段列表。
|
*
|
* @param polygonCoords 多边形边界坐标，格式如 "x1,y1;x2,y2;..."（米）
|
* @param obstaclesCoords 障碍物坐标，支持多个括号段或圆形定义，例 "(x1,y1;x2,y2)(cx,cy;px,py)"
|
* @param mowingWidth 割草宽度字符串，米单位，允许保留两位小数
|
* @param modeStr 割草模式，"0"/空为平行线，"1" 或 "spiral" 表示螺旋模式（当前仅平行线实现）
|
* @return 路径段列表，按行驶顺序排列
|
*/
|
public static List<PathSegment> generatePathSegments(String polygonCoords,
|
String obstaclesCoords,
|
String mowingWidth,
|
String modeStr) {
|
List<Coordinate> polygon = parseCoordinates(polygonCoords);
|
if (polygon.size() < 4) {
|
throw new IllegalArgumentException("多边形坐标数量不足，至少需要三个点");
|
}
|
|
double width = parseDoubleOrDefault(mowingWidth, 2.0);
|
if (width <= 0) {
|
throw new IllegalArgumentException("割草宽度必须大于 0");
|
}
|
|
List<List<Coordinate>> obstacles = parseObstacles(obstaclesCoords);
|
String mode = normalizeMode(modeStr);
|
|
PlannerCore planner = new PlannerCore(polygon, width, mode, obstacles);
|
return planner.generate();
|
}
|
|
/**
|
* 通过字符串参数生成割草路径坐标。
|
*
|
* @param polygonCoords 多边形边界坐标，格式如 "x1,y1;x2,y2;..."（米）
|
* @param obstaclesCoords 障碍物坐标，支持多个括号段或圆形定义，例 "(x1,y1;x2,y2)(cx,cy;px,py)"
|
* @param mowingWidth 割草宽度字符串，米单位，允许保留两位小数
|
* @param modeStr 割草模式，"0"/空为平行线，"1" 或 "spiral" 表示螺旋模式（当前仅平行线实现）
|
* @return 连续路径坐标字符串，顺序紧跟割草机行进路线
|
*/
|
public static String generatePathFromStrings(String polygonCoords,
|
String obstaclesCoords,
|
String mowingWidth,
|
String modeStr) {
|
List<PathSegment> segments = generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, modeStr);
|
return formatPathSegments(segments);
|
}
|
|
/**
|
* 将路径段列表转换为坐标字符串。
|
*/
|
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 (!equals2D(last, segment.start)) {
|
appendPoint(sb, segment.start);
|
last = new Coordinate(segment.start);
|
}
|
if (!equals2D(last, segment.end)) {
|
appendPoint(sb, segment.end);
|
last = new Coordinate(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 {
|
list.add(new Coordinate(Double.parseDouble(xy[0].trim()),
|
Double.parseDouble(xy[1].trim())));
|
} catch (NumberFormatException ex) {
|
System.err.println("坐标解析失败: " + trimmed);
|
}
|
}
|
}
|
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);
|
} else if (coords.size() == 2) {
|
List<Coordinate> circle = approximateCircle(coords.get(0), coords.get(1));
|
if (!circle.isEmpty()) {
|
obs.add(circle);
|
}
|
}
|
}
|
if (obs.isEmpty()) {
|
List<Coordinate> coords = parseCoordinates(str);
|
if (coords.size() >= 3) {
|
obs.add(coords);
|
} else if (coords.size() == 2) {
|
List<Coordinate> circle = approximateCircle(coords.get(0), coords.get(1));
|
if (!circle.isEmpty()) {
|
obs.add(circle);
|
}
|
}
|
}
|
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) {
|
throw new IllegalArgumentException("割草宽度格式不正确: " + value, ex);
|
}
|
}
|
|
private static String normalizeMode(String modeStr) {
|
if (modeStr == null) {
|
return "parallel";
|
}
|
String trimmed = modeStr.trim().toLowerCase();
|
if ("1".equals(trimmed) || "spiral".equals(trimmed)) {
|
return "spiral";
|
}
|
return "parallel";
|
}
|
|
private static boolean equals2D(Coordinate a, Coordinate b) {
|
if (a == b) {
|
return true;
|
}
|
if (a == null || b == null) {
|
return false;
|
}
|
return a.equals2D(b);
|
}
|
|
private static void appendPoint(StringBuilder sb, Coordinate point) {
|
if (sb.length() > 0) {
|
sb.append(";");
|
}
|
sb.append(String.format("%.2f,%.2f", point.x, point.y));
|
}
|
|
private static List<Coordinate> approximateCircle(Coordinate center, Coordinate edge) {
|
double radius = center.distance(edge);
|
if (radius <= 0) {
|
return Collections.emptyList();
|
}
|
int segments = 36;
|
List<Coordinate> circle = new ArrayList<>(segments + 1);
|
for (int i = 0; i < segments; i++) {
|
double angle = 2 * Math.PI * i / segments;
|
double x = center.x + radius * Math.cos(angle);
|
double y = center.y + radius * Math.sin(angle);
|
circle.add(new Coordinate(x, y));
|
}
|
circle.add(new Coordinate(circle.get(0)));
|
return circle;
|
}
|
|
/**
|
* 路径段数据结构。
|
*/
|
public static final class PathSegment {
|
public Coordinate start;
|
public Coordinate end;
|
public boolean isMowing;
|
public boolean isStartPoint;
|
public boolean isEndPoint;
|
|
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;
|
}
|
|
@Override
|
public String toString() {
|
return String.format("PathSegment[(%.2f,%.2f)->(%.2f,%.2f) mowing=%b start=%b end=%b]",
|
start.x, start.y, end.x, end.y, isMowing, isStartPoint, isEndPoint);
|
}
|
}
|
|
/**
|
* 内部核心规划器，实现与 MowingPathPlanner 等效的逻辑。
|
*/
|
private static final class PlannerCore {
|
private final List<Coordinate> polygon;
|
private final double width;
|
private final String mode;
|
private final List<List<Coordinate>> obstacles;
|
private final GeometryFactory gf = new GeometryFactory();
|
|
PlannerCore(List<Coordinate> polygon, double width, String mode, List<List<Coordinate>> obstacles) {
|
this.polygon = polygon;
|
this.width = width;
|
this.mode = mode;
|
this.obstacles = obstacles != null ? obstacles : new ArrayList<>();
|
}
|
|
List<PathSegment> generate() {
|
if ("spiral".equals(mode)) {
|
return generateSpiralPath();
|
}
|
return generateParallelPath();
|
}
|
|
private List<PathSegment> generateParallelPath() {
|
List<PathSegment> path = new ArrayList<>();
|
Geometry safeArea = buildSafeArea();
|
if (safeArea == null || safeArea.isEmpty()) {
|
System.err.println("安全区域为空，无法生成路径");
|
return path;
|
}
|
|
LineSegment longest = findLongestEdge(polygon);
|
Vector2D baseDir = new Vector2D(longest.end.x - longest.start.x,
|
longest.end.y - longest.start.y).normalize();
|
Vector2D perp = baseDir.rotate90CCW();
|
Vector2D baseStartVec = new Vector2D(longest.start.x, longest.start.y);
|
double baseProjection = perp.dot(baseStartVec); // keep offsets relative to the longest edge start
|
|
double minProj = Double.POSITIVE_INFINITY;
|
double maxProj = Double.NEGATIVE_INFINITY;
|
Coordinate[] supportCoords = safeArea.getCoordinates();
|
if (supportCoords != null && supportCoords.length > 0) {
|
for (Coordinate coord : supportCoords) {
|
double projection = perp.dot(new Vector2D(coord.x, coord.y)) - baseProjection;
|
if (projection < minProj) {
|
minProj = projection;
|
}
|
if (projection > maxProj) {
|
maxProj = projection;
|
}
|
}
|
} else {
|
Envelope env = safeArea.getEnvelopeInternal();
|
minProj = perp.dot(new Vector2D(env.getMinX(), env.getMinY())) - baseProjection;
|
maxProj = perp.dot(new Vector2D(env.getMaxX(), env.getMaxY())) - baseProjection;
|
}
|
if (minProj > maxProj) {
|
double tmp = minProj;
|
minProj = maxProj;
|
maxProj = tmp;
|
}
|
double first = minProj - width / 2.0;
|
|
Geometry originalPoly = createPolygonFromCoordinates(polygon);
|
Coordinate lastEnd = null;
|
int idx = 0;
|
|
for (double offset = first; offset <= maxProj + width / 2.0; offset += width) {
|
Line2D.Double raw = createInfiniteLine(longest, perp, offset);
|
List<Line2D.Double> segs = clipLineToPolygon(raw, safeArea);
|
if (segs.isEmpty()) {
|
continue;
|
}
|
|
List<Line2D.Double> finalSegs = new ArrayList<>();
|
for (Line2D.Double seg : segs) {
|
finalSegs.addAll(clipLineToPolygon(seg, originalPoly));
|
}
|
if (finalSegs.isEmpty()) {
|
continue;
|
}
|
|
finalSegs.sort((a, b) -> Double.compare(baseDir.dot(midV(a)), baseDir.dot(midV(b))));
|
boolean even = (idx % 2 == 0);
|
for (Line2D.Double seg : finalSegs) {
|
Coordinate entry = even ? new Coordinate(seg.x1, seg.y1)
|
: new Coordinate(seg.x2, seg.y2);
|
Coordinate exit = even ? new Coordinate(seg.x2, seg.y2)
|
: new Coordinate(seg.x1, seg.y1);
|
|
if (lastEnd != null && lastEnd.distance(entry) > 1e-3) {
|
path.add(new PathSegment(lastEnd, entry, false));
|
}
|
|
PathSegment mowingSeg = new PathSegment(entry, exit, true);
|
if (path.isEmpty()) {
|
mowingSeg.setAsStartPoint();
|
}
|
path.add(mowingSeg);
|
lastEnd = exit;
|
}
|
idx++;
|
}
|
|
if (!path.isEmpty()) {
|
path.get(path.size() - 1).setAsEndPoint();
|
}
|
|
postProcess(path);
|
return path;
|
}
|
|
private List<PathSegment> generateSpiralPath() {
|
Geometry safeArea = buildSafeArea();
|
if (safeArea == null || safeArea.isEmpty()) {
|
System.err.println("安全区域为空，无法生成螺旋路径");
|
return new ArrayList<>();
|
}
|
|
List<PathSegment> spiral = luoxuan.generateSpiralPath(safeArea, width);
|
if (spiral.isEmpty()) {
|
return spiral;
|
}
|
|
postProcess(spiral);
|
PathSegment firstMowing = null;
|
PathSegment endCandidate = null;
|
for (int i = 0; i < spiral.size(); i++) {
|
PathSegment seg = spiral.get(i);
|
if (seg != null && seg.isMowing) {
|
if (firstMowing == null) {
|
firstMowing = seg;
|
}
|
endCandidate = seg;
|
}
|
}
|
if (firstMowing != null) {
|
firstMowing.setAsStartPoint();
|
}
|
if (endCandidate != null && endCandidate != firstMowing) {
|
endCandidate.setAsEndPoint();
|
}
|
return spiral;
|
}
|
|
private Geometry buildSafeArea() {
|
try {
|
Coordinate[] coords = polygon.toArray(new Coordinate[0]);
|
if (!coords[0].equals2D(coords[coords.length - 1])) {
|
coords = Arrays.copyOf(coords, coords.length + 1);
|
coords[coords.length - 1] = coords[0];
|
}
|
Polygon origin = gf.createPolygon(gf.createLinearRing(coords));
|
Geometry result = origin;
|
|
if (!obstacles.isEmpty()) {
|
List<Geometry> obsGeom = new ArrayList<>();
|
for (List<Coordinate> obs : obstacles) {
|
Geometry g = createPolygonFromCoordinates(obs);
|
if (g != null && !g.isEmpty()) {
|
obsGeom.add(g);
|
}
|
}
|
if (!obsGeom.isEmpty()) {
|
Geometry union = CascadedPolygonUnion.union(obsGeom);
|
result = origin.difference(union);
|
}
|
}
|
|
Geometry shrunk = shrinkStraight(result, width / 2.0);
|
return shrunk.isEmpty() ? result : shrunk;
|
} catch (Exception ex) {
|
System.err.println("构建安全区域失败: " + ex.getMessage());
|
return gf.createPolygon();
|
}
|
}
|
|
private LineSegment findLongestEdge(List<Coordinate> ring) {
|
double max = -1.0;
|
LineSegment best = null;
|
for (int i = 0; i < ring.size() - 1; i++) {
|
double d = ring.get(i).distance(ring.get(i + 1));
|
if (d > max) {
|
max = d;
|
best = new LineSegment(ring.get(i), ring.get(i + 1), i);
|
}
|
}
|
return best;
|
}
|
|
private Line2D.Double createInfiniteLine(LineSegment base, Vector2D perp, double offset) {
|
Vector2D baseStart = new Vector2D(base.start.x, base.start.y);
|
Vector2D baseDir = new Vector2D(base.end.x - base.start.x,
|
base.end.y - base.start.y).normalize();
|
Vector2D center = baseStart.add(perp.mul(offset));
|
double ext = 1.5 * diagonalLength();
|
Vector2D p1 = center.sub(baseDir.mul(ext));
|
Vector2D p2 = center.add(baseDir.mul(ext));
|
return new Line2D.Double(p1.x, p1.y, p2.x, p2.y);
|
}
|
|
private List<Line2D.Double> clipLineToPolygon(Line2D.Double line, Geometry poly) {
|
List<Line2D.Double> list = new ArrayList<>();
|
LineString ls = gf.createLineString(new Coordinate[]{
|
new Coordinate(line.x1, line.y1),
|
new Coordinate(line.x2, line.y2)
|
});
|
Geometry inter = poly.intersection(ls);
|
if (inter.isEmpty()) {
|
return list;
|
}
|
|
if (inter instanceof LineString) {
|
addCoords((LineString) inter, list);
|
} else if (inter instanceof MultiLineString) {
|
MultiLineString mls = (MultiLineString) inter;
|
for (int i = 0; i < mls.getNumGeometries(); i++) {
|
addCoords((LineString) mls.getGeometryN(i), list);
|
}
|
}
|
return list;
|
}
|
|
private void addCoords(LineString ls, List<Line2D.Double> bucket) {
|
Coordinate[] cs = ls.getCoordinateSequence().toCoordinateArray();
|
for (int i = 0; i < cs.length - 1; i++) {
|
bucket.add(new Line2D.Double(cs[i].x, cs[i].y, cs[i + 1].x, cs[i + 1].y));
|
}
|
}
|
|
private Geometry createPolygonFromCoordinates(List<Coordinate> coords) {
|
if (coords.size() < 3) {
|
return gf.createPolygon();
|
}
|
List<Coordinate> closed = new ArrayList<>(coords);
|
if (!closed.get(0).equals2D(closed.get(closed.size() - 1))) {
|
closed.add(new Coordinate(closed.get(0)));
|
}
|
LinearRing shell = gf.createLinearRing(closed.toArray(new Coordinate[0]));
|
Polygon polygonGeom = gf.createPolygon(shell);
|
return polygonGeom.isValid() ? polygonGeom : (Polygon) polygonGeom.buffer(0);
|
}
|
|
private double diagonalLength() {
|
double minX = polygon.stream().mapToDouble(c -> c.x).min().orElse(0);
|
double maxX = polygon.stream().mapToDouble(c -> c.x).max().orElse(0);
|
double minY = polygon.stream().mapToDouble(c -> c.y).min().orElse(0);
|
double maxY = polygon.stream().mapToDouble(c -> c.y).max().orElse(0);
|
return Math.hypot(maxX - minX, maxY - minY);
|
}
|
|
private Vector2D midV(Line2D.Double l) {
|
return new Vector2D((l.x1 + l.x2) / 2.0, (l.y1 + l.y2) / 2.0);
|
}
|
|
private void postProcess(List<PathSegment> path) {
|
if (path == null || path.isEmpty()) {
|
return;
|
}
|
List<PathSegment> tmp = new ArrayList<>(path);
|
path.clear();
|
Coordinate prevEnd = null;
|
for (PathSegment seg : tmp) {
|
if (prevEnd != null && seg.start.distance(prevEnd) < 1e-3) {
|
seg.start = prevEnd;
|
}
|
if (!seg.isMowing && !path.isEmpty()) {
|
PathSegment last = path.get(path.size() - 1);
|
if (!last.isMowing && isCollinear(last.start, last.end, seg.end)) {
|
last.end = seg.end;
|
prevEnd = seg.end;
|
continue;
|
}
|
}
|
path.add(seg);
|
prevEnd = seg.end;
|
}
|
}
|
|
private boolean isCollinear(Coordinate a, Coordinate b, Coordinate c) {
|
double dx1 = b.x - a.x;
|
double dy1 = b.y - a.y;
|
double dx2 = c.x - b.x;
|
double dy2 = c.y - b.y;
|
double cross = dx1 * dy2 - dy1 * dx2;
|
return Math.abs(cross) < 1e-6;
|
}
|
|
private Geometry shrinkStraight(Geometry outer, double dist) {
|
Geometry buf = outer.buffer(-dist);
|
if (buf.isEmpty()) {
|
return buf;
|
}
|
|
Geometry poly = (buf instanceof Polygon) ? buf
|
: (buf instanceof MultiPolygon) ? ((MultiPolygon) buf).getGeometryN(0) : null;
|
if (!(poly instanceof Polygon)) {
|
return buf;
|
}
|
|
Coordinate[] ring = ((Polygon) poly).getExteriorRing().getCoordinateSequence().toCoordinateArray();
|
List<Coordinate> straight = new ArrayList<>();
|
final double EPS = 1e-3;
|
for (int i = 0; i < ring.length - 1; i++) {
|
Coordinate prev = (i == 0) ? ring[ring.length - 2] : ring[i - 1];
|
Coordinate curr = ring[i];
|
Coordinate next = ring[i + 1];
|
double cross = Math.abs((next.x - curr.x) * (curr.y - prev.y)
|
- (curr.x - prev.x) * (next.y - curr.y));
|
if (cross > EPS) {
|
straight.add(curr);
|
}
|
}
|
if (straight.isEmpty()) {
|
return buf;
|
}
|
straight.add(new Coordinate(straight.get(0)));
|
return straight.size() < 4 ? gf.createPolygon()
|
: gf.createPolygon(gf.createLinearRing(straight.toArray(new Coordinate[0])));
|
}
|
}
|
|
private static final class Vector2D {
|
final double x;
|
final double y;
|
|
Vector2D(double x, double y) {
|
this.x = x;
|
this.y = y;
|
}
|
|
Vector2D normalize() {
|
double len = Math.hypot(x, y);
|
if (len < 1e-12) {
|
return new Vector2D(1, 0);
|
}
|
return 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 sub(Vector2D v) {
|
return new Vector2D(x - v.x, y - v.y);
|
}
|
|
Vector2D add(Vector2D v) {
|
return new Vector2D(x + v.x, y + v.y);
|
}
|
|
Vector2D mul(double k) {
|
return new Vector2D(x * k, y * k);
|
}
|
}
|
|
private static final class LineSegment {
|
final Coordinate start;
|
final Coordinate end;
|
final int index;
|
|
LineSegment(Coordinate start, Coordinate end, int index) {
|
this.start = start;
|
this.end = end;
|
this.index = index;
|
}
|
}
|
}
|
