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