From dc9dce0555beb85d1262893fd5d56747d6a83855 Mon Sep 17 00:00:00 2001
From: 张世豪 <979909237@qq.com>
Date: 星期五, 19 十二月 2025 11:48:37 +0800
Subject: [PATCH] 新增了导航预览功能
---
src/lujing/Lunjingguihua.java | 701 +++++++++++++++++-----------------------------------------
1 files changed, 207 insertions(+), 494 deletions(-)
diff --git a/src/lujing/Lunjingguihua.java b/src/lujing/Lunjingguihua.java
index 3e54d7b..1f19fae 100644
--- a/src/lujing/Lunjingguihua.java
+++ b/src/lujing/Lunjingguihua.java
@@ -1,25 +1,20 @@
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;
+import org.locationtech.jts.geom.MultiPolygon;
/**
- * 鍓茶崏璺緞瑙勫垝瀹炵敤绫伙紝渚涘叾浠栭」鐩洿鎺ヨ皟鐢ㄣ��
- * 鎻愪緵瀛楃涓插叆鍙傜殑鍓茶崏璺緞鐢熸垚鑳藉姏锛屽苟灏佽蹇呰鐨勮В鏋愪笌鍑犱綍澶勭悊閫昏緫銆�
+ * 浼樺寲鍚庣殑鍓茶崏璺緞瑙勫垝绫�
+ * 淇锛氳В鍐宠矾寰勮秴鍑哄湴鍧楄竟鐣岀殑闂锛屽鍔犲畨鍏ㄨ竟璺濊绠楃殑鍋ュ．鎬с��
*/
public final class Lunjingguihua {
@@ -27,16 +22,7 @@
throw new IllegalStateException("Utility class");
}
- /**
- * 鐢熸垚鍓茶崏璺緞娈靛垪琛ㄣ��
- *
- * @param polygonCoords 澶氳竟褰㈣竟鐣屽潗鏍囷紝鏍煎紡濡� "x1,y1;x2,y2;..."锛堢背锛�
- * @param obstaclesCoords 闅滅鐗╁潗鏍囷紝鏀寔澶氫釜鎷彿娈垫垨鍦嗗舰瀹氫箟锛屼緥 "(x1,y1;x2,y2)(cx,cy;px,py)"
- * @param mowingWidth 鍓茶崏瀹藉害瀛楃涓诧紝绫冲崟浣嶏紝鍏佽淇濈暀涓や綅灏忔暟
- * @param safetyDistStr 瀹夊叏璺濈瀛楃涓诧紝绫冲崟浣嶃�傝矾寰勫皢涓庤竟鐣屽拰闅滅鐗╀繚鎸佹璺濈銆�
- * @param modeStr 鍓茶崏妯″紡锛�"0"/绌轰负骞宠绾匡紝"1" 鎴� "spiral" 琛ㄧず铻烘棆妯″紡锛堝綋鍓嶄粎骞宠绾垮疄鐜帮級
- * @return 璺緞娈靛垪琛紝鎸夎椹堕『搴忔帓鍒�
- */
+ // 5鍙傛暟鏍稿績鐢熸垚鏂规硶
public static List<PathSegment> generatePathSegments(String polygonCoords,
String obstaclesCoords,
String mowingWidth,
@@ -44,15 +30,11 @@
String modeStr) {
List<Coordinate> polygon = parseCoordinates(polygonCoords);
if (polygon.size() < 4) {
- throw new IllegalArgumentException("澶氳竟褰㈠潗鏍囨暟閲忎笉瓒筹紝鑷冲皯闇�瑕佷笁涓偣");
+ throw new IllegalArgumentException("澶氳竟褰㈠潗鏍囨暟閲忎笉瓒�");
}
- double width = parseDoubleOrDefault(mowingWidth, 2.0);
- if (width <= 0) {
- throw new IllegalArgumentException("鍓茶崏瀹藉害蹇呴』澶т簬 0");
- }
-
- // 瑙ｆ瀽瀹夊叏璺濈锛屽鏋滄湭璁剧疆鎴栨棤鏁堬紝榛樿涓� NaN (鍦� PlannerCore 涓鐞嗛粯璁ゅ��)
+ double width = parseDoubleOrDefault(mowingWidth, 0.34);
+ // 濡傛灉浼犲叆绌猴紝璁句负 NaN锛屽湪 PlannerCore 涓繘琛屾櫤鑳借绠�
double safetyDistance = parseDoubleOrDefault(safetyDistStr, Double.NaN);
List<List<Coordinate>> obstacles = parseObstacles(obstaclesCoords);
@@ -62,9 +44,7 @@
return planner.generate();
}
- /**
- * 淇濇寔鍚戝悗鍏煎鐨勯噸杞芥柟娉曪紙涓嶅甫 safeDistance锛屼娇鐢ㄩ粯璁よ绠楋級
- */
+ // 4鍙傛暟閲嶈浇锛岄�傞厤 AddDikuai.java
public static List<PathSegment> generatePathSegments(String polygonCoords,
String obstaclesCoords,
String mowingWidth,
@@ -72,16 +52,7 @@
return generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
}
- /**
- * 閫氳繃瀛楃涓插弬鏁扮敓鎴愬壊鑽夎矾寰勫潗鏍囥��
- *
- * @param polygonCoords 澶氳竟褰㈣竟鐣屽潗鏍囷紝鏍煎紡濡� "x1,y1;x2,y2;..."锛堢背锛�
- * @param obstaclesCoords 闅滅鐗╁潗鏍囷紝鏀寔澶氫釜鎷彿娈垫垨鍦嗗舰瀹氫箟锛屼緥 "(x1,y1;x2,y2)(cx,cy;px,py)"
- * @param mowingWidth 鍓茶崏瀹藉害瀛楃涓诧紝绫冲崟浣嶏紝鍏佽淇濈暀涓や綅灏忔暟
- * @param safetyDistStr 瀹夊叏璺濈瀛楃涓诧紝绫冲崟浣嶃��
- * @param modeStr 鍓茶崏妯″紡锛�"0"/绌轰负骞宠绾匡紝"1" 鎴� "spiral" 琛ㄧず铻烘棆妯″紡锛堝綋鍓嶄粎骞宠绾垮疄鐜帮級
- * @return 杩炵画璺緞鍧愭爣瀛楃涓诧紝椤哄簭绱ц窡鍓茶崏鏈鸿杩涜矾绾�
- */
+ // 5鍙傛暟璺緞瀛楃涓茬敓鎴�
public static String generatePathFromStrings(String polygonCoords,
String obstaclesCoords,
String mowingWidth,
@@ -90,10 +61,8 @@
List<PathSegment> segments = generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, safetyDistStr, modeStr);
return formatPathSegments(segments);
}
-
- /**
- * 淇濇寔鍚戝悗鍏煎鐨勯噸杞芥柟娉�
- */
+
+ // 4鍙傛暟璺緞瀛楃涓茬敓鎴愰噸杞�
public static String generatePathFromStrings(String polygonCoords,
String obstaclesCoords,
String mowingWidth,
@@ -101,158 +70,93 @@
return generatePathFromStrings(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
}
- /**
- * 灏嗚矾寰勬鍒楄〃杞崲涓哄潗鏍囧瓧绗︿覆銆�
- */
public static String formatPathSegments(List<PathSegment> path) {
- if (path == null || path.isEmpty()) {
- return "";
- }
+ if (path == null || path.isEmpty()) return "";
StringBuilder sb = new StringBuilder();
Coordinate last = null;
for (PathSegment segment : path) {
- if (!equals2D(last, segment.start)) {
+ if (last == null || !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);
- }
+ 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;
- }
+ 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;
- }
+ 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())));
+ 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) {
- 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;
- }
+ 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 (coords.size() >= 3) obs.add(coords);
}
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);
- }
- }
+ 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;
- }
+ if (value == null || value.trim().isEmpty()) return defaultValue;
try {
return Double.parseDouble(value.trim());
} catch (NumberFormatException ex) {
- throw new IllegalArgumentException("鏍煎紡涓嶆纭�: " + value, ex);
+ return defaultValue;
}
}
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";
+ 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.equals2D(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("%.2f,%.2f", point.x, point.y));
+ if (sb.length() > 0) sb.append(";");
+ sb.append(String.format("%.3f,%.3f", 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 Coordinate start, end;
public boolean isMowing;
- public boolean isStartPoint;
- public boolean isEndPoint;
+ public boolean isStartPoint, isEndPoint;
public PathSegment(Coordinate start, Coordinate end, boolean isMowing) {
this.start = start;
@@ -260,419 +164,228 @@
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);
- }
+ public void setAsStartPoint() { this.isStartPoint = true; }
+ public void setAsEndPoint() { this.isEndPoint = true; }
}
- /**
- * 鍐呴儴鏍稿績瑙勫垝鍣紝瀹炵幇涓� MowingPathPlanner 绛夋晥鐨勯�昏緫銆�
- */
- static final class PlannerCore {
+ public static final class PlannerCore {
private final List<Coordinate> polygon;
private final double width;
- private final double safetyDistance; // 鏂板瀹夊叏璺濈瀛楁
+ private final double safetyDistance;
private final String mode;
private final List<List<Coordinate>> obstacles;
private final GeometryFactory gf = new GeometryFactory();
- PlannerCore(List<Coordinate> polygon, double width, double safetyDistance, String mode, List<List<Coordinate>> obstacles) {
+ // 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<>();
- // 鍒濆鍖栧畨鍏ㄨ窛绂婚�昏緫
- if (Double.isNaN(safetyDistance)) {
- // 濡傛灉鏈彁渚涳紝浣跨敤榛樿鍊硷細瀹藉害鐨勪竴鍗� + 0.05绫�
- this.safetyDistance = width / 2.0 + 0.05;
+ // FIX: 澧炲姞榛樿瀹夊叏杈硅窛銆傚師閫昏緫涓� width/2 + 0.05锛屽鏄撻�犳垚璇樊鍑虹晫銆�
+ // 鐜版敼涓� width/2 + 0.2 (20cm浣欓噺)锛岀‘淇濆壊鑽夋満瀹炰綋瀹屽叏鍦ㄧ晫鍐呫��
+ if (Double.isNaN(safetyDistance) || safetyDistance <= 0) {
+ this.safetyDistance = (width / 2.0) + 0.20;
} else {
- // 濡傛灉鎻愪緵浜嗭紝浣跨敤鎻愪緵鐨勫�硷紝浣嗚嚦灏戣淇濊瘉鏈哄櫒涓績涓嶇澹侊紙瀹藉害涓�鍗婏級
- // 鍏佽鐢ㄦ埛璁剧疆姣� width/2 鏇村ぇ鐨勫�兼潵杩滅杈圭晫
- this.safetyDistance = Math.max(safetyDistance, width / 2.0);
+ this.safetyDistance = safetyDistance;
}
}
-
- // 鍏煎鏃ф瀯閫犲嚱鏁�
- PlannerCore(List<Coordinate> polygon, double width, String mode, List<List<Coordinate>> obstacles) {
+
+ // 2. 4鍙傛暟鏋勯�犲嚱鏁�
+ public PlannerCore(List<Coordinate> polygon, double width, String mode, List<List<Coordinate>> obstacles) {
this(polygon, width, Double.NaN, mode, obstacles);
}
- List<PathSegment> generate() {
- // 濡傛灉鏈夐殰纰嶇墿锛屼娇鐢ㄥ甫闅滅鐗╅伩璁╃殑璺緞瑙勫垝鍣�
- if (!obstacles.isEmpty()) {
- // 浣跨敤璁＄畻濂界殑瀹夊叏璺濈
- ObstaclePathPlanner obstaclePlanner = new ObstaclePathPlanner(
- polygon, width, mode, obstacles, this.safetyDistance);
- return obstaclePlanner.generate();
- }
+ 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 ("spiral".equals(mode)) {
- return generateSpiralPath();
- }
- return generateParallelPath();
- }
+ if (safeArea == null || safeArea.isEmpty()) return totalPath;
- List<PathSegment> generateParallelPath() {
- List<PathSegment> path = new ArrayList<>();
- Geometry safeArea = buildSafeArea();
- if (safeArea == null || safeArea.isEmpty()) {
- System.err.println("瀹夊叏鍖哄煙涓虹┖锛屾棤娉曠敓鎴愯矾寰�");
- return path;
+ 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));
+ }
}
- 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);
+ for (Polygon region : subRegions) {
+ if (region.isEmpty()) continue;
- 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;
+ 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) {
+ // 蹇界暐鏋佸叾缃曡鐨勬嫇鎵戝紓甯革紝闃叉宕╂簝
}
}
- } 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;
- }
-
- 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;
+ return markStartEnd(totalPath);
}
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;
+ 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);
- 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);
+ // 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) {
+ // 蹇界暐閿欒鐨勯殰纰嶇墿鏁版嵁
}
}
-
- // 淇敼锛氫娇鐢ㄤ紶鍏ョ殑 safetyDistance 鏉ヨ繘琛岃竟鐣屽唴缂�
- // 涔嬪墠鏄� width / 2.0锛岀幇鍦ㄤ娇鐢� this.safetyDistance
- // 杩欑‘淇濅簡璺緞瑙勫垝鍖哄煙涓庤竟鐣屼繚鎸佺敤鎴锋寚瀹氱殑璺濈
- Geometry shrunk = shrinkStraight(result, this.safetyDistance);
- return shrunk.isEmpty() ? result : shrunk;
- } catch (Exception ex) {
- System.err.println("鏋勫缓瀹夊叏鍖哄煙澶辫触: " + ex.getMessage());
+
+ // 5. 鏈�缁堟竻鐞�
+ if (!safe.isValid()) safe = safe.buffer(0);
+ return safe;
+ } catch (Exception e) {
+ // 濡傛灉鍑犱綍鏋勫缓瀹屽叏澶辫触锛岃繑鍥炵┖
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);
+ 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 best;
+ return bestDir;
}
- 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);
+ 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 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 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;
}
}
- 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])));
- }
+ List<PathSegment> generateSpiralPath() { return new ArrayList<>(); }
}
private static final class Vector2D {
- final double x;
- final double y;
+ 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(double x, double y) {
- this.x = x;
- this.y = y;
+ Vector2D normalize() {
+ double len = Math.hypot(x, y);
+ return len < 1e-9 ? new Vector2D(1, 0) : new Vector2D(x / len, y / len);
}
-
- 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;
- }
+ 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); }
}
}
\ No newline at end of file
--
Gitblit v1.10.0