From 1cf1ecbc75c6d14b40efb3161e7db0b8b64f7de2 Mon Sep 17 00:00:00 2001
From: 张世豪 <979909237@qq.com>
Date: 星期三, 17 十二月 2025 12:05:27 +0800
Subject: [PATCH] 新增有障碍物的路径规划算法和优化没有障碍物的路径算法
---
src/lujing/Lunjingguihua.java | 95 +++++++++++++++++++++++++++++++++++++----------
1 files changed, 75 insertions(+), 20 deletions(-)
diff --git a/src/lujing/Lunjingguihua.java b/src/lujing/Lunjingguihua.java
index 907a4f7..3e54d7b 100644
--- a/src/lujing/Lunjingguihua.java
+++ b/src/lujing/Lunjingguihua.java
@@ -30,16 +30,18 @@
/**
* 鐢熸垚鍓茶崏璺緞娈靛垪琛ㄣ��
*
- * @param polygonCoords 澶氳竟褰㈣竟鐣屽潗鏍囷紝鏍煎紡濡� "x1,y1;x2,y2;..."锛堢背锛�
+ * @param polygonCoords 澶氳竟褰㈣竟鐣屽潗鏍囷紝鏍煎紡濡� "x1,y1;x2,y2;..."锛堢背锛�
* @param obstaclesCoords 闅滅鐗╁潗鏍囷紝鏀寔澶氫釜鎷彿娈垫垨鍦嗗舰瀹氫箟锛屼緥 "(x1,y1;x2,y2)(cx,cy;px,py)"
- * @param mowingWidth 鍓茶崏瀹藉害瀛楃涓诧紝绫冲崟浣嶏紝鍏佽淇濈暀涓や綅灏忔暟
- * @param modeStr 鍓茶崏妯″紡锛�"0"/绌轰负骞宠绾匡紝"1" 鎴� "spiral" 琛ㄧず铻烘棆妯″紡锛堝綋鍓嶄粎骞宠绾垮疄鐜帮級
+ * @param mowingWidth 鍓茶崏瀹藉害瀛楃涓诧紝绫冲崟浣嶏紝鍏佽淇濈暀涓や綅灏忔暟
+ * @param safetyDistStr 瀹夊叏璺濈瀛楃涓诧紝绫冲崟浣嶃�傝矾寰勫皢涓庤竟鐣屽拰闅滅鐗╀繚鎸佹璺濈銆�
+ * @param modeStr 鍓茶崏妯″紡锛�"0"/绌轰负骞宠绾匡紝"1" 鎴� "spiral" 琛ㄧず铻烘棆妯″紡锛堝綋鍓嶄粎骞宠绾垮疄鐜帮級
* @return 璺緞娈靛垪琛紝鎸夎椹堕『搴忔帓鍒�
*/
public static List<PathSegment> generatePathSegments(String polygonCoords,
- String obstaclesCoords,
- String mowingWidth,
- String modeStr) {
+ String obstaclesCoords,
+ String mowingWidth,
+ String safetyDistStr,
+ String modeStr) {
List<Coordinate> polygon = parseCoordinates(polygonCoords);
if (polygon.size() < 4) {
throw new IllegalArgumentException("澶氳竟褰㈠潗鏍囨暟閲忎笉瓒筹紝鑷冲皯闇�瑕佷笁涓偣");
@@ -49,30 +51,55 @@
if (width <= 0) {
throw new IllegalArgumentException("鍓茶崏瀹藉害蹇呴』澶т簬 0");
}
+
+ // 瑙ｆ瀽瀹夊叏璺濈锛屽鏋滄湭璁剧疆鎴栨棤鏁堬紝榛樿涓� NaN (鍦� PlannerCore 涓鐞嗛粯璁ゅ��)
+ double safetyDistance = parseDoubleOrDefault(safetyDistStr, Double.NaN);
List<List<Coordinate>> obstacles = parseObstacles(obstaclesCoords);
String mode = normalizeMode(modeStr);
- PlannerCore planner = new PlannerCore(polygon, width, mode, obstacles);
+ PlannerCore planner = new PlannerCore(polygon, width, safetyDistance, mode, obstacles);
return planner.generate();
}
/**
+ * 淇濇寔鍚戝悗鍏煎鐨勯噸杞芥柟娉曪紙涓嶅甫 safeDistance锛屼娇鐢ㄩ粯璁よ绠楋級
+ */
+ public static List<PathSegment> generatePathSegments(String polygonCoords,
+ String obstaclesCoords,
+ String mowingWidth,
+ String modeStr) {
+ return generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
+ }
+
+ /**
* 閫氳繃瀛楃涓插弬鏁扮敓鎴愬壊鑽夎矾寰勫潗鏍囥��
*
- * @param polygonCoords 澶氳竟褰㈣竟鐣屽潗鏍囷紝鏍煎紡濡� "x1,y1;x2,y2;..."锛堢背锛�
+ * @param polygonCoords 澶氳竟褰㈣竟鐣屽潗鏍囷紝鏍煎紡濡� "x1,y1;x2,y2;..."锛堢背锛�
* @param obstaclesCoords 闅滅鐗╁潗鏍囷紝鏀寔澶氫釜鎷彿娈垫垨鍦嗗舰瀹氫箟锛屼緥 "(x1,y1;x2,y2)(cx,cy;px,py)"
- * @param mowingWidth 鍓茶崏瀹藉害瀛楃涓诧紝绫冲崟浣嶏紝鍏佽淇濈暀涓や綅灏忔暟
- * @param modeStr 鍓茶崏妯″紡锛�"0"/绌轰负骞宠绾匡紝"1" 鎴� "spiral" 琛ㄧず铻烘棆妯″紡锛堝綋鍓嶄粎骞宠绾垮疄鐜帮級
+ * @param mowingWidth 鍓茶崏瀹藉害瀛楃涓诧紝绫冲崟浣嶏紝鍏佽淇濈暀涓や綅灏忔暟
+ * @param safetyDistStr 瀹夊叏璺濈瀛楃涓诧紝绫冲崟浣嶃��
+ * @param modeStr 鍓茶崏妯″紡锛�"0"/绌轰负骞宠绾匡紝"1" 鎴� "spiral" 琛ㄧず铻烘棆妯″紡锛堝綋鍓嶄粎骞宠绾垮疄鐜帮級
* @return 杩炵画璺緞鍧愭爣瀛楃涓诧紝椤哄簭绱ц窡鍓茶崏鏈鸿杩涜矾绾�
*/
public static String generatePathFromStrings(String polygonCoords,
String obstaclesCoords,
String mowingWidth,
+ String safetyDistStr,
String modeStr) {
- List<PathSegment> segments = generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, modeStr);
+ List<PathSegment> segments = generatePathSegments(polygonCoords, obstaclesCoords, mowingWidth, safetyDistStr, modeStr);
return formatPathSegments(segments);
}
+
+ /**
+ * 淇濇寔鍚戝悗鍏煎鐨勯噸杞芥柟娉�
+ */
+ public static String generatePathFromStrings(String polygonCoords,
+ String obstaclesCoords,
+ String mowingWidth,
+ String modeStr) {
+ return generatePathFromStrings(polygonCoords, obstaclesCoords, mowingWidth, null, modeStr);
+ }
/**
* 灏嗚矾寰勬鍒楄〃杞崲涓哄潗鏍囧瓧绗︿覆銆�
@@ -168,7 +195,7 @@
try {
return Double.parseDouble(value.trim());
} catch (NumberFormatException ex) {
- throw new IllegalArgumentException("鍓茶崏瀹藉害鏍煎紡涓嶆纭�: " + value, ex);
+ throw new IllegalArgumentException("鏍煎紡涓嶆纭�: " + value, ex);
}
}
@@ -227,7 +254,7 @@
public boolean isStartPoint;
public boolean isEndPoint;
- PathSegment(Coordinate start, Coordinate end, boolean isMowing) {
+ public PathSegment(Coordinate start, Coordinate end, boolean isMowing) {
this.start = start;
this.end = end;
this.isMowing = isMowing;
@@ -251,28 +278,53 @@
/**
* 鍐呴儴鏍稿績瑙勫垝鍣紝瀹炵幇涓� MowingPathPlanner 绛夋晥鐨勯�昏緫銆�
*/
- private static final class PlannerCore {
+ 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();
- PlannerCore(List<Coordinate> polygon, double width, String mode, List<List<Coordinate>> obstacles) {
+ 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;
+ } else {
+ // 濡傛灉鎻愪緵浜嗭紝浣跨敤鎻愪緵鐨勫�硷紝浣嗚嚦灏戣淇濊瘉鏈哄櫒涓績涓嶇澹侊紙瀹藉害涓�鍗婏級
+ // 鍏佽鐢ㄦ埛璁剧疆姣� width/2 鏇村ぇ鐨勫�兼潵杩滅杈圭晫
+ this.safetyDistance = Math.max(safetyDistance, width / 2.0);
+ }
+ }
+
+ // 鍏煎鏃ф瀯閫犲嚱鏁�
+ 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();
+ }
+
+ // 娌℃湁闅滅鐗╂椂浣跨敤鍘熸湁閫昏緫
if ("spiral".equals(mode)) {
return generateSpiralPath();
}
return generateParallelPath();
}
- private List<PathSegment> generateParallelPath() {
+ List<PathSegment> generateParallelPath() {
List<PathSegment> path = new ArrayList<>();
Geometry safeArea = buildSafeArea();
if (safeArea == null || safeArea.isEmpty()) {
@@ -285,7 +337,7 @@
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 baseProjection = perp.dot(baseStartVec);
double minProj = Double.POSITIVE_INFINITY;
double maxProj = Double.NEGATIVE_INFINITY;
@@ -361,7 +413,7 @@
return path;
}
- private List<PathSegment> generateSpiralPath() {
+ List<PathSegment> generateSpiralPath() {
Geometry safeArea = buildSafeArea();
if (safeArea == null || safeArea.isEmpty()) {
System.err.println("瀹夊叏鍖哄煙涓虹┖锛屾棤娉曠敓鎴愯灪鏃嬭矾寰�");
@@ -418,7 +470,10 @@
}
}
- Geometry shrunk = shrinkStraight(result, width / 2.0);
+ // 淇敼锛氫娇鐢ㄤ紶鍏ョ殑 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());
@@ -620,4 +675,4 @@
this.index = index;
}
}
-}
+}
\ No newline at end of file
--
Gitblit v1.10.0