From 642f20dd1b068e23146b8ec96c5eac8b63c96b87 Mon Sep 17 00:00:00 2001
From: jianghaiyue <jianghaiyue@zkyt.com>
Date: 星期三, 05 十一月 2025 18:18:25 +0800
Subject: [PATCH] 优化更新
---
algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java | 390 ++++++++++++++++++++++++++++++++++++++++++++++++++-----
1 files changed, 353 insertions(+), 37 deletions(-)
diff --git a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
index 4ae0129..486c246 100644
--- a/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
+++ b/algo-zkd/src/main/java/com/algo/service/AStarPathPlanner.java
@@ -4,12 +4,15 @@
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.util.JsonUtils;
+import com.algo.util.PathTimeCalculator;
+import com.fasterxml.jackson.core.type.TypeReference;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import java.io.File;
import java.util.*;
/**
* A*璺緞瑙勫垝鍣ㄥ疄鐜�
- * 浣跨敤3D A*绠楁硶杩涜CTU鏃剁┖璺緞瑙勫垝锛屾敮鎸佺墿鐞嗙害鏉�
*/
public class AStarPathPlanner implements PathPlanner {
@@ -24,6 +27,11 @@
private final Map<String, List<Map<String, String>>> adjacencyList;
/**
+ * 鐜杩為�氭�ф暟鎹�
+ */
+ private final JsonUtils.EnvironmentConnectivity environmentConnectivity;
+
+ /**
* 鏃堕棿绮惧害锛堟绉掞級
*/
private final long timeResolution = 1000;
@@ -36,10 +44,10 @@
/**
* 鏈�澶ф悳绱㈡繁搴�
*/
- private final int maxSearchDepth = 15000;
+ private final int maxSearchDepth = 50000;
/**
- * 璺濈缂撳瓨 - 浼樺寲锛氱紦瀛樿窛绂昏绠楃粨鏋�
+ * 璺濈缂撳瓨
*/
private final Map<String, Double> distanceCache = new HashMap<>();
@@ -47,6 +55,16 @@
* 蹇�熻矾寰勭紦瀛�
*/
private final Map<String, List<String>> fastPathCache = new HashMap<>();
+
+ /**
+ * 瀹為檯鍧愭爣鏄犲皠
+ */
+ private Map<String, double[]> realCoordinateMapping;
+
+ /**
+ * 璺緞鏃堕棿璁＄畻鍣�
+ */
+ private PathTimeCalculator timeCalculator;
/**
* 鏋勯�犲嚱鏁�
@@ -55,15 +73,82 @@
*/
public AStarPathPlanner(Map<String, Map<String, Integer>> pathMapping) {
this.pathMapping = pathMapping;
- this.adjacencyList = new HashMap<>();
- // 鐩存帴鏋勫缓閭绘帴琛�
- buildAdjacencyList();
+ // 鍔犺浇鐜杩為�氭�ф暟鎹�
+ this.environmentConnectivity = JsonUtils.loadEnvironmentConnectivity("environment.json");
+
+ // 鍔犺浇閭绘帴琛�
+ Map<String, List<Map<String, String>>> loadedAdjacency = loadAdjacencyListFromJson("adjacency.json");
+
+ if (loadedAdjacency != null && !loadedAdjacency.isEmpty()) {
+ // 鎴愬姛鍔犺浇棰勭敓鎴愮殑閭绘帴琛�
+ this.adjacencyList = loadedAdjacency;
+ System.out.println("浠� adjacency.json 鍔犺浇閭绘帴琛�: " + adjacencyList.size() + " 涓妭鐐�");
+ } else {
+ this.adjacencyList = new HashMap<>();
+ buildEnvironmentAwareAdjacencyList();
+ System.out.println("浣跨敤缃戞牸鐩搁偦鎬ф瀯寤洪偦鎺ヨ〃: " + adjacencyList.size() + " 涓妭鐐�");
+ }
+
+ // 鍔犺浇瀹為檯鍧愭爣鏄犲皠
+ loadRealCoordinateMapping();
+
+ // 鍒濆鍖栨椂闂磋绠楀櫒
+ this.timeCalculator = new PathTimeCalculator(pathMapping, realCoordinateMapping);
// 棰勮绠楀父鐢ㄨ窛绂�
precomputeCommonDistances();
System.out.println("A*璺緞瑙勫垝鍣ㄥ垵濮嬪寲瀹屾垚锛岄偦鎺ヨ〃鍖呭惈 " + adjacencyList.size() + " 涓妭鐐�");
+ }
+
+ /**
+ * 浠� JSON 鏂囦欢鍔犺浇棰勭敓鎴愮殑閭绘帴琛�
+ */
+ private Map<String, List<Map<String, String>>> loadAdjacencyListFromJson(String filePath) {
+ try {
+ File file = new File(filePath);
+
+ if (!file.exists()) {
+ System.out.println("璺緞閭绘帴琛ㄦ枃浠朵笉瀛樺湪: " + filePath + "锛屼娇鐢ㄩ粯璁ゆ瀯寤烘柟娉�");
+ return null;
+ }
+
+ ObjectMapper objectMapper = new ObjectMapper();
+
+ TypeReference<Map<String, List<Map<String, String>>>> typeRef =
+ new TypeReference<Map<String, List<Map<String, String>>>>() {};
+
+ Map<String, List<Map<String, String>>> adjacency = objectMapper.readValue(file, typeRef);
+
+ if (adjacency == null || adjacency.isEmpty()) {
+ System.out.println("閭绘帴琛ㄦ枃浠朵负绌�: " + filePath);
+ return null;
+ }
+
+ return adjacency;
+
+ } catch (Exception e) {
+ System.err.println("鍔犺浇閭绘帴琛ㄥけ璐�: " + e.getMessage());
+ e.printStackTrace();
+ return null;
+ }
+ }
+
+ /**
+ * 鍔犺浇瀹為檯鍧愭爣鏄犲皠
+ */
+ private void loadRealCoordinateMapping() {
+ try {
+ this.realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
+ if (realCoordinateMapping == null || realCoordinateMapping.isEmpty()) {
+ System.out.println("鏈兘鍔犺浇瀹為檯鍧愭爣鏄犲皠锛屼娇鐢ㄧ綉鏍煎潗鏍�");
+ this.realCoordinateMapping = new HashMap<>();
+ }
+ } catch (Exception e) {
+ System.err.println("鍔犺浇瀹為檯鍧愭爣鏄犲皠澶辫触: " + e.getMessage());
+ this.realCoordinateMapping = new HashMap<>();
+ }
}
@Override
@@ -73,7 +158,8 @@
if (fastPath != null) {
return fastPath;
}
- return planSpaceTimePath(startCode, endCode, constraints, null, null);
+ long defaultStartTime = System.currentTimeMillis();
+ return planSpaceTimePath(startCode, endCode, constraints, null, null, defaultStartTime);
}
@Override
@@ -89,12 +175,14 @@
* @param constraints 闈欐�佺害鏉熸潯浠�
* @param spaceTimeOccupancyMap 鏃剁┖鍗犵敤琛�
* @param physicalConfig CTU鐗╃悊閰嶇疆
+ * @param startTimeMs 璧峰鏃堕棿锛堟绉掞級
* @return 瑙勫垝鐨勮矾寰�
*/
public PlannedPath planSpaceTimePath(String startCode, String endCode,
List<double[]> constraints,
Map<String, String> spaceTimeOccupancyMap,
- CTUPhysicalConfig physicalConfig) {
+ CTUPhysicalConfig physicalConfig,
+ long startTimeMs) {
// 楠岃瘉杈撳叆
if (!isValidPathPoint(startCode) || !isValidPathPoint(endCode)) {
System.out.println("鏃犳晥鐨勮矾寰勭偣: " + startCode + " 鎴� " + endCode);
@@ -127,7 +215,7 @@
// 鏃剁┖A*绠楁硶瀹炵幇
PlannedPath result = spaceTimeAStarSearch(
startCode, endCode, startCoord, endCoord,
- constraints, spaceTimeOccupancyMap, physicalConfig
+ constraints, spaceTimeOccupancyMap, physicalConfig, startTimeMs
);
if (result != null) {
@@ -149,13 +237,15 @@
* @param constraints 绾︽潫鏉′欢
* @param occupancyMap 鏃剁┖鍗犵敤琛�
* @param physicalConfig 鐗╃悊閰嶇疆
+ * @param startTimeMs 璧峰鏃堕棿锛堟绉掞級
* @return 瑙勫垝鐨勮矾寰�
*/
private PlannedPath spaceTimeAStarSearch(String startCode, String endCode,
int[] startCoord, int[] endCoord,
List<double[]> constraints,
Map<String, String> occupancyMap,
- CTUPhysicalConfig physicalConfig) {
+ CTUPhysicalConfig physicalConfig,
+ long startTimeMs) {
// 浣跨敤浼樺厛闃熷垪瀹炵幇寮�鏀惧垪琛�
PriorityQueue<SpaceTimeAStarNode> openSet = new PriorityQueue<>(
@@ -165,8 +255,7 @@
Map<String, Double> gScores = new HashMap<>();
Map<String, SpaceTimeAStarNode> cameFrom = new HashMap<>();
- // 璧峰鏃堕棿
- long startTime = System.currentTimeMillis();
+ long startTime = startTimeMs;
// 鍒濆鍖栬捣濮嬭妭鐐�
SpaceTimeAStarNode startNode = new SpaceTimeAStarNode(
@@ -278,8 +367,9 @@
}
}
- // 鍑忓皯绛夊緟閫夐」 - 鍙湪蹇呰鏃剁瓑寰咃紙20%姒傜巼锛�
- if (Math.random() < 0.2) {
+ // 蹇呰鏃讹紙鏈夊啿绐侊級鐢熸垚绛夊緟鑺傜偣锛涘綋鍓嶈妭鐐圭殑鎵�鏈夐偦灞呭湪鐭湡鍐呴兘琚樆鎸″垯绛夊緟
+ boolean allNeighborsBlocked = checkIfAllNeighborsBlocked(current, constraintChecker, physicalConfig);
+ if (allNeighborsBlocked) {
long waitTime = timeResolution;
long waitUntilTime = current.timePoint + waitTime;
String waitKey = createSpaceTimeKey(current.code, waitUntilTime);
@@ -306,29 +396,109 @@
}
/**
+ * 妫�鏌ュ綋鍓嶈妭鐐圭殑閭诲眳鏄惁鍦ㄧ煭鏈熷唴閮借闃绘尅
+ */
+ private boolean checkIfAllNeighborsBlocked(SpaceTimeAStarNode current,
+ EnhancedConstraintChecker constraintChecker,
+ CTUPhysicalConfig physicalConfig) {
+ // 鑾峰彇褰撳墠鑺傜偣鐨勬墍鏈夌┖闂撮偦灞�
+ List<Map<String, String>> neighbors = getNeighbors(current.code);
+
+ if (neighbors.isEmpty()) {
+ return true;
+ }
+
+ // 妫�鏌ユ湭鏉ュ嚑涓椂闂寸墖鍐呯殑鍗犵敤鎯呭喌
+ int checkTimeSteps = 3;
+ boolean allBlocked = true;
+
+ for (int step = 1; step <= checkTimeSteps; step++) {
+ long checkTime = current.timePoint + (step * timeResolution);
+
+ // 妫�鏌ユ槸鍚︽湁鑷冲皯涓�涓偦灞呭湪褰撳墠鏃堕棿鐗囧彲鐢�
+ for (Map<String, String> neighbor : neighbors) {
+ String neighborCode = neighbor.get("code");
+ long arrivalTime = checkTime;
+
+ // 瀵归綈鍒版椂闂村垎杈ㄧ巼
+ arrivalTime = alignToTimeResolution(arrivalTime);
+
+ // 妫�鏌ヨ繖涓偦灞呭湪褰撳墠鏃堕棿鐗囨槸鍚﹀彲鐢�
+ if (constraintChecker.isValidMove(current.code, neighborCode,
+ current.timePoint, arrivalTime)) {
+ // 鑷冲皯鏈変竴涓偦灞呭彲鐢紝涓嶉渶瑕佺瓑寰�
+ allBlocked = false;
+ break;
+ }
+ }
+
+ // 濡傛灉鎵惧埌鍙敤閭诲眳锛屼笉闇�瑕佺瓑寰�
+ if (!allBlocked) {
+ break;
+ }
+ }
+
+ return allBlocked;
+ }
+
+ /**
* 鏃剁┖鍚彂寮忓嚱鏁�
*
- * @param coord1 褰撳墠鍧愭爣
- * @param coord2 鐩爣鍧愭爣
+ * @param coord1 褰撳墠鍧愭爣锛堢綉鏍硷級
+ * @param coord2 鐩爣鍧愭爣锛堢綉鏍硷級
* @param currentTime 褰撳墠鏃堕棿
* @param physicalConfig 鐗╃悊閰嶇疆
* @return 鍚彂寮忓��
*/
private double spaceTimeHeuristic(int[] coord1, int[] coord2, long currentTime, CTUPhysicalConfig physicalConfig) {
- // 绌洪棿璺濈
+ String pathId1 = findPathIdByCoordinate(coord1);
+ String pathId2 = findPathIdByCoordinate(coord2);
+
+ if (pathId1 != null && pathId2 != null) {
+ double[] realCoord1 = JsonUtils.getRealCoordinate(pathId1, realCoordinateMapping);
+ double[] realCoord2 = JsonUtils.getRealCoordinate(pathId2, realCoordinateMapping);
+
+ if (realCoord1 != null && realCoord2 != null) {
+ // 浣跨敤瀹為檯璺濈璁＄畻
+ double realDistance = CTUPhysicalConfig.calculateRealDistance(realCoord1, realCoord2);
+
+ // 鏃堕棿鎴愭湰浼拌锛堝亣璁惧钩鍧囬�熷害锛�
+ double timeEstimate = realDistance / physicalConfig.getNormalSpeed();
+
+ // 鑰冭檻杞悜鎴愭湰
+ double turnPenalty = realDistance > 0.5 ? physicalConfig.getTurnTime90() / 2.0 : 0;
+
+ return timeEstimate + turnPenalty;
+ }
+ }
+
+ // 鍓嶆柟妗堬細浣跨敤缃戞牸鍧愭爣
double spatialDistance = Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);
-
- // 鏃堕棿鎴愭湰浼拌
double timeEstimate = spatialDistance * physicalConfig.getStandardPointDistance() / physicalConfig.getNormalSpeed();
-
- // 鑰冭檻杞悜鎴愭湰
double turnPenalty = spatialDistance > 1 ? physicalConfig.getTurnTime90() : 0;
return timeEstimate + turnPenalty;
}
+
+ /**
+ * 鏍规嵁缃戞牸鍧愭爣鏌ユ壘璺緞ID
+ */
+ private String findPathIdByCoordinate(int[] coord) {
+ if (coord == null) return null;
+
+ for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
+ Map<String, Integer> coordMap = entry.getValue();
+ if (coordMap != null &&
+ coord[0] == coordMap.getOrDefault("x", -1) &&
+ coord[1] == coordMap.getOrDefault("y", -1)) {
+ return entry.getKey();
+ }
+ }
+ return null;
+ }
/**
- * 璁＄畻绉诲姩鏃堕棿
+ * 璁＄畻绮剧‘鐨勭Щ鍔ㄦ椂闂达紝鍩轰簬瀹為檯鍧愭爣鍜岀墿鐞嗗弬鏁�
*
* @param fromCode 璧峰浠ｇ爜
* @param toCode 鐩爣浠ｇ爜
@@ -337,14 +507,70 @@
* @return 绉诲姩鏃堕棿锛堟绉掞級
*/
private long calculateTravelTime(String fromCode, String toCode, String direction, CTUPhysicalConfig physicalConfig) {
- // 鍩烘湰绉诲姩鏃堕棿
+ // 鑾峰彇瀹為檯鍧愭爣
+ double[] fromCoord = JsonUtils.getRealCoordinate(fromCode, realCoordinateMapping);
+ double[] toCoord = JsonUtils.getRealCoordinate(toCode, realCoordinateMapping);
+
+ if (fromCoord == null || toCoord == null) {
+ // fallback鍒扮綉鏍煎潗鏍囪绠�
+ return calculateFallbackTravelTime(fromCode, toCode, physicalConfig);
+ }
+
+ // 璁＄畻瀹為檯璺濈锛堢背锛�
+ double realDistance = CTUPhysicalConfig.calculateRealDistance(fromCoord, toCoord);
+
+ if (realDistance == 0.0) {
+ return 100; // 鏈�灏忔椂闂�100ms
+ }
+
+ // 鍋囪璧峰鍜岀粨鏉熼�熷害锛堝彲浠ユ牴鎹笂涓嬫枃杩涗竴姝ヤ紭鍖栵級
+ double startSpeed = 0.0; // 鍋囪浠庨潤姝㈠紑濮�
+ double endSpeed = 0.0; // 鍋囪缁撴潫鏃跺仠姝�
+ double targetSpeed = physicalConfig.getNormalSpeed();
+
+ // 璁＄畻绮剧‘鐨勭Щ鍔ㄦ椂闂�
+ double movementTime = physicalConfig.calculatePreciseMovementTime(
+ realDistance, startSpeed, endSpeed, targetSpeed
+ );
+
+ // 璁＄畻杞悜鏃堕棿
+ double turnTime = calculateTurnTime(fromCode, toCode, physicalConfig);
+
+ return Math.max(100, (long) ((movementTime + turnTime) * 1000));
+ }
+
+ /**
+ * 璁＄畻杞悜鏃堕棿
+ */
+ private double calculateTurnTime(String fromCode, String toCode, CTUPhysicalConfig physicalConfig) {
+
+ double[] fromCoord = JsonUtils.getRealCoordinate(fromCode, realCoordinateMapping);
+ double[] toCoord = JsonUtils.getRealCoordinate(toCode, realCoordinateMapping);
+
+ if (fromCoord == null || toCoord == null) {
+ return physicalConfig.getTurnTime90() / 4.0; // 骞冲潎杞悜鏃堕棿
+ }
+
+ // 璁＄畻鏂瑰悜鍙樺寲
+ double dx = toCoord[0] - fromCoord[0];
+ double dy = toCoord[1] - fromCoord[1];
+
+ // 鐩寸嚎杩愬姩鏃犻渶杞悜
+ if (Math.abs(dx) < 1.0 && Math.abs(dy) < 1.0) {
+ return 0.0;
+ }
+
+ return physicalConfig.getTurnTime90() / 2.0;
+ }
+
+ /**
+ * 澶囩敤鏃堕棿璁＄畻鏂规硶锛堝綋鏃犳硶鑾峰彇瀹為檯鍧愭爣鏃朵娇鐢級
+ */
+ private long calculateFallbackTravelTime(String fromCode, String toCode, CTUPhysicalConfig physicalConfig) {
double distance = physicalConfig.getStandardPointDistance();
double speed = physicalConfig.getNormalSpeed();
long moveTime = (long) ((distance / speed) * 1000);
-
- // 杞悜鏃堕棿
- long turnTime = (long) (physicalConfig.getTurnTime90() * 1000 / 4); // 鍋囪骞冲潎杞悜鏃堕棿
-
+ long turnTime = (long) (physicalConfig.getTurnTime90() * 1000 / 4);
return moveTime + turnTime;
}
@@ -384,14 +610,85 @@
}
PathCode pathCode = new PathCode(node.code, direction);
-
- // 娣诲姞鏃堕棿淇℃伅
- // pathCode.setArrivalTime(node.timePoint);
-
codeList.add(pathCode);
}
- return new PlannedPath("", "", codeList);
+ codeList = optimizePathByRemovingBacktrack(codeList);
+
+ PlannedPath plannedPath = new PlannedPath("", "", codeList);
+
+ return plannedPath;
+ }
+
+ /**
+ * 浼樺寲璺緞锛氬幓闄ょ┖闂翠笂鐨勯噸澶嶇偣锛堥伩鍏嶆潵鍥炶蛋锛�
+ * 妫�娴嬪苟绉婚櫎 A->B->A 妯″紡鐨勬潵鍥炶蛋锛岄伩鍏嶄笉蹇呰鐨勭粫琛�
+ *
+ * @param codeList 鍘熷璺緞浠ｇ爜鍒楄〃
+ * @return 浼樺寲鍚庣殑璺緞浠ｇ爜鍒楄〃
+ */
+ private List<PathCode> optimizePathByRemovingBacktrack(List<PathCode> codeList) {
+ if (codeList == null || codeList.size() <= 2) {
+ return codeList;
+ }
+
+ List<PathCode> optimized = new ArrayList<>();
+ int i = 0;
+
+ while (i < codeList.size()) {
+ PathCode current = codeList.get(i);
+
+ // 妫�鏌ユ槸鍚︽槸鏉ュ洖璧版ā寮忥細A->B->A锛堣繛缁笁涓偣锛岀涓�涓拰绗笁涓浉鍚岋級
+ // 濡傛灉鍑虹幇鏉ュ洖璧帮紝璺宠繃B鍜岀浜屼釜A锛岀洿鎺ュ埌涓嬩竴涓笉鍚岀殑鐐�
+ if (i < codeList.size() - 2) {
+ PathCode next = codeList.get(i + 1);
+ PathCode nextNext = codeList.get(i + 2);
+
+ // 妫�娴嬫潵鍥炶蛋锛氬綋鍓嶄綅缃� == 涓嬩笅涓綅缃紝涓斾腑闂翠綅缃笉鍚�
+ if (current.getCode().equals(nextNext.getCode()) &&
+ !current.getCode().equals(next.getCode())) {
+ // 鏉ュ洖璧帮細璺宠繃B鍜岀浜屼釜A锛岀洿鎺ュ鐞嗕笅涓�涓笉鍚岀殑鐐�
+ // 鎵惧埌涓嬩竴涓笌褰撳墠鐐逛笉鍚岀殑浣嶇疆
+ int j = i + 2;
+ while (j < codeList.size() && codeList.get(j).getCode().equals(current.getCode())) {
+ j++;
+ }
+ // 濡傛灉鎵惧埌浜嗕笉鍚岀殑鐐癸紝鎴栬�呭埌杈炬湯灏撅紝璺宠繃鏉ュ洖璧扮殑閮ㄥ垎
+ if (j < codeList.size()) {
+ i = j; // 璺冲埌涓嬩竴涓笉鍚岀殑鐐�
+ continue;
+ } else {
+ // 濡傛灉鍚庨潰閮芥槸鐩稿悓鐐癸紝淇濈暀褰撳墠鐐癸紙鍙兘鏄洰鏍囩偣锛�
+ break;
+ }
+ }
+ }
+
+ // 姝ｅ父娣诲姞褰撳墠鐐�
+ optimized.add(current);
+ i++;
+ }
+
+ // 纭繚鐩爣鐐硅鍖呭惈
+ if (!optimized.isEmpty() && !codeList.isEmpty()) {
+ PathCode lastOriginal = codeList.get(codeList.size() - 1);
+ PathCode lastOptimized = optimized.get(optimized.size() - 1);
+ if (!lastOriginal.getCode().equals(lastOptimized.getCode())) {
+ optimized.add(lastOriginal);
+ }
+ }
+
+ // 閲嶆柊璁＄畻鏂瑰悜锛堝洜涓轰紭鍖栧悗璺宠繃浜嗕腑闂寸偣锛屾柟鍚戦渶瑕佹洿鏂帮級
+ for (int k = 0; k < optimized.size(); k++) {
+ PathCode code = optimized.get(k);
+ if (k < optimized.size() - 1) {
+ PathCode nextCode = optimized.get(k + 1);
+ String direction = calculateDirection(code.getCode(), nextCode.getCode());
+ code.setDirection(direction);
+ }
+ }
+
+ return optimized;
}
/**
@@ -472,6 +769,15 @@
@Override
public double calculateDistance(String startCode, String endCode) {
+ // 浼樺厛浣跨敤瀹為檯鍧愭爣
+ double[] startRealCoord = JsonUtils.getRealCoordinate(startCode, realCoordinateMapping);
+ double[] endRealCoord = JsonUtils.getRealCoordinate(endCode, realCoordinateMapping);
+
+ if (startRealCoord != null && endRealCoord != null) {
+ return CTUPhysicalConfig.calculateRealDistance(startRealCoord, endRealCoord);
+ }
+
+ // 鍓嶆柟妗堬細浣跨敤缃戞牸鍧愭爣
int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);
@@ -493,9 +799,9 @@
}
/**
- * 鏋勫缓閭绘帴琛�
+ * 鏋勫缓鐜鎰熺煡鐨勯偦鎺ヨ〃
*/
- private void buildAdjacencyList() {
+ private void buildEnvironmentAwareAdjacencyList() {
// 鍒涘缓鍧愭爣鍒扮紪鍙风殑涓存椂鏄犲皠
Map<String, String> tempCoordToCode = new HashMap<>();
for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
@@ -523,6 +829,12 @@
int x = coordMap.get("x");
int y = coordMap.get("y");
+ // 鐜鎰熺煡锛氭鏌ュ綋鍓嶈妭鐐规槸鍚﹀彲閫氳
+ if (!environmentConnectivity.isTraversable(x, y)) {
+ adjacencyList.put(code, new ArrayList<>()); // 涓嶅彲閫氳鐨勮妭鐐规病鏈夐偦灞�
+ continue;
+ }
+
List<Map<String, String>> neighbors = new ArrayList<>();
// 妫�鏌ュ洓涓柟鍚戠殑閭诲眳
@@ -532,7 +844,9 @@
String coordKey = newX + "," + newY;
String neighborCode = tempCoordToCode.get(coordKey);
- if (neighborCode != null) {
+
+ // 鐜鎰熺煡锛氬彧鏈夊綋閭诲眳鑺傜偣涔熷彲閫氳鏃舵墠娣诲姞杩炴帴
+ if (neighborCode != null && environmentConnectivity.isTraversable(newX, newY)) {
Map<String, String> neighbor = new HashMap<>();
neighbor.put("code", neighborCode);
neighbor.put("direction", directionAngles[i]);
@@ -542,6 +856,8 @@
adjacencyList.put(code, neighbors);
}
+
+ System.out.println("鐜鎰熺煡閭绘帴琛ㄦ瀯寤哄畬鎴愶紝杩囨护浜嗕笉鍙�氳鐨勮繛鎺�");
}
/**
@@ -673,7 +989,7 @@
}
/**
- * 蹇�熻矾寰勮鍒� - 鍏堝皾璇曠畝鍖栫┖闂磋矾寰勮鍒�
+ * 蹇�熻矾寰勮鍒�
* 瀵逛簬杩戣窛绂昏矾寰勶紝鐩存帴杩斿洖缁撴灉閬垮厤澶嶆潅鐨勬椂绌鸿绠�
*/
private PlannedPath tryFastPathPlanning(String startCode, String endCode, List<double[]> constraints) {
@@ -722,7 +1038,7 @@
openSet.offer(startNode);
gScores.put(startCode, 0.0);
- // 绠�鍖栫殑绾︽潫妫�鏌ュ櫒
+ // 绾︽潫妫�鏌ュ櫒
FastConstraintChecker constraintChecker = new FastConstraintChecker(constraints);
int searchDepth = 0;
--
Gitblit v1.9.1