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/domain/AStarNavigateNode.java | 21 +++++++++-
zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/AStarNavigateService.java | 52 ++++++++++++++++++++-----
2 files changed, 60 insertions(+), 13 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()});
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java
index e203a45..382f889 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/AStarNavigateNode.java
@@ -4,8 +4,8 @@
import lombok.extern.slf4j.Slf4j;
import java.io.Serializable;
-import java.util.Objects;
-import java.util.Optional;
+import java.util.*;
+import java.util.stream.Collectors;
/**
* A*瀵昏矾绠楁硶Node鑺傜偣
@@ -100,4 +100,21 @@
);
}
+
+ private void print() {
+ AStarNavigateNode finishNode = this.clone();
+ ArrayList<AStarNavigateNode> navigateNodes = new ArrayList<>();
+
+ while (finishNode != null) {
+ navigateNodes.add(finishNode);
+ System.out.println("node:" + finishNode.getCodeData() + ", g:" + finishNode.getG() + ", h:" + finishNode.getH() + ", f:" + finishNode.getF());
+ finishNode = finishNode.getParent();
+ }
+
+ Collections.reverse(navigateNodes);
+
+ List<String> navigatePath = navigateNodes.stream().map(AStarNavigateNode::getCodeData).collect(Collectors.toList());
+ System.out.println(navigatePath.toString());
+ }
+
}
--
Gitblit v1.9.1