From d77edc6de3ffe93afd5680953759fff556b5ca73 Mon Sep 17 00:00:00 2001
From: luxiaotao1123 <t1341870251@163.com>
Date: 星期五, 03 一月 2025 08:30:05 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 52 +++++++++++++++++++++++++++++++++++++++++-----------
1 files changed, 41 insertions(+), 11 deletions(-)
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
index b8e0a90..ef32661 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java
@@ -48,6 +48,13 @@
PriorityQueue<AStarNavigateNode> openQueue = new PriorityQueue<>();
Set<AStarNavigateNode> existNodes = new HashSet<>();
+ Map<String, Integer> bestGMap = new HashMap<>();
+
+ start.setG(0);
+ start.setH(calcNodeCost(start, end));
+ start.setF(start.getG() + start.getH());
+ String startKey = start.getX() + "_" + start.getY();
+ bestGMap.put(startKey, start.getG());
openQueue.add(start);
existNodes.add(start);
@@ -159,27 +166,50 @@
node.setH(calcNodeCost(node, end));
node.setF(node.getG() + node.getH());
- openQueue.add(node);
- existNodes.add(node);
+ String key = node.getX() + "_" + node.getY();
+ Integer recordedG = bestGMap.get(key);
+ if (recordedG == null || node.getG() <= recordedG) {
+ bestGMap.put(key, node.getG());
+
+ openQueue.add(node);
+ }
+
+// openQueue.add(node);
+// existNodes.add(node);
}
}
return null;
}
+ // right left up down
+ private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};
+
// 鑾峰彇鍥涘懆鑺傜偣
private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
int x = currentNode.getX();
int y = currentNode.getY();
+ AStarNavigateNode parent = currentNode.getParent();
List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
- List<AStarNavigateNode> possibleNodes = Arrays.asList(
- new AStarNavigateNode(x, y + 1), // right
- new AStarNavigateNode(x, y - 1), // left
- new AStarNavigateNode(x - 1, y), // up
- new AStarNavigateNode(x + 1, y) // down
- );
+// List<AStarNavigateNode> possibleNodes = Arrays.asList(
+// new AStarNavigateNode(x, y + 1), // right
+// new AStarNavigateNode(x, y - 1), // left
+// new AStarNavigateNode(x - 1, y), // up
+// new AStarNavigateNode(x + 1, y) // down
+// );
+
+ List<AStarNavigateNode> possibleNodes = new ArrayList<>();
+ for (int[] d: DIRECTIONS) {
+ int nx = x + d[0];
+ int ny = y + d[1];
+ // 濡傛灉鐖惰妭鐐逛笉涓虹┖锛屽苟涓� (nx,ny) 绛変簬鐖惰妭鐐瑰潗鏍囷紝鍒欒烦杩�
+ if (parent != null && nx == parent.getX() && ny == parent.getY()) {
+ continue;
+ }
+ possibleNodes.add(new AStarNavigateNode(nx, ny));
+ }
possibleNodes.parallelStream()
.map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
@@ -215,9 +245,9 @@
assert mapMatrix[x][y] == MapNodeType.ENABLE.val;
- if (existNodes.contains(nextNode)) {
- return null;
- }
+// if (existNodes.contains(nextNode)) {
+// return null;
+// }
// 鍒ゆ柇閫氳繃鎬�
String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});
--
Gitblit v1.9.1