From aaa515911eef34e2263ce8baa6f860025fd6060d Mon Sep 17 00:00:00 2001
From: vincentlu <t1341870251@gmail.com>
Date: 星期二, 14 一月 2025 11:21:02 +0800
Subject: [PATCH] #

---
 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java |   14 ++-
 zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java             |   16 ++++
 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java      |    8 +
 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java               |  169 ++++++++++++++++++++---------------------
 4 files changed, 112 insertions(+), 95 deletions(-)

diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java
index 41ea3f3..b7c7104 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java
@@ -10,6 +10,22 @@
  */
 public class MapDataUtils {
 
+    public static Double[][][] preComputeCdaMatrix(String[][] cdaStrMatrix) {
+        int rows = cdaStrMatrix.length;
+        int cols = cdaStrMatrix[0].length;
+
+        Double[][][] cdaMatrix = new Double[rows][cols][2];
+        for (int i = 0; i < rows; i++) {
+            for (int j = 0; j < cols; j++) {
+                List<Double> cda = MapDataUtils.parseCdaNode(cdaStrMatrix[i][j]);
+                cdaMatrix[i][j][0] = cda.get(0);
+                cdaMatrix[i][j][1] = cda.get(1);
+            }
+        }
+
+        return cdaMatrix;
+    }
+
     public static List<String> parseWaveNode(String waveNodeStr) {
         List<String> waveNodeList = new ArrayList<>();
         if (Cools.isEmpty(waveNodeStr)) {
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java
index 08e7042..9fcf125 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java
@@ -179,6 +179,7 @@
     }
 
     private boolean calcWaveScopeByJava(Integer lev) throws Exception {
+
         AgvModel agvModel = agvModelService.selectByType(AgvModelType.CTU_BOX_TRANSPORT_AGV.toString());    // can be optimized
         Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
 
@@ -187,15 +188,16 @@
         String[][] waveMatrix = mapDataDispatcher.initWaveMatrix(lev);
 
         // lock path
+        long time = 0;
         DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev);
         for (int i = 0; i < dynamicMatrix.length; i++) {
             for (int j = 0; j < dynamicMatrix[i].length; j++) {
                 DynamicNode dynamicNode = dynamicMatrix[i][j];
                 String vehicle = dynamicNode.getVehicle();
                 if (!DynamicNodeType.ACCESS.val.equals(vehicle) && !DynamicNodeType.BLOCK.val.equals(vehicle)) {
-
+                    long startTime = System.currentTimeMillis();
                     List<NavigateNode> includeList = mapService.getWaveScopeByCode(lev, codeMatrix[i][j], avoidDistance);
-
+                    time += System.currentTimeMillis() - startTime;
                     for (NavigateNode navigateNode : includeList) {
                         String waveNode = waveMatrix[navigateNode.getX()][navigateNode.getY()]; // overlay
                         waveMatrix[navigateNode.getX()][navigateNode.getY()] = MapDataUtils.generateWaveNode(waveNode, vehicle);
@@ -206,7 +208,7 @@
 
 //            mapDataDispatcher.printMatrix(waveMatrix);
         mapDataDispatcher.setWaveMatrix(lev, waveMatrix);
-
+        System.out.println("calcWaveScopeByJava " + time);
         return true;
     }
 
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
index faa379e..2d71496 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java
@@ -2,7 +2,6 @@
 
 import com.zy.acs.common.enums.AgvDirectionType;
 import com.zy.acs.framework.common.Cools;
-import com.zy.acs.manager.common.utils.MapDataUtils;
 import com.zy.acs.manager.core.constant.MapDataConstant;
 import com.zy.acs.manager.core.domain.SortCodeDto;
 import com.zy.acs.manager.core.domain.UnlockPathTask;
@@ -212,7 +211,7 @@
     // v1 BFS ------------------------------------------------------------------------------
     public List<NavigateNode> getWaveScopeByCode(Integer lev, String code, Double radiusLen) {
         String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
-        String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
+        Double[][][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
 
         int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code);
         NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code);
@@ -229,7 +228,7 @@
     }
 
     public void spreadWaveNode(NavigateNode originNode, NavigateNode currNode
-            , String[][] codeMatrix, String[][] cdaMatrix, Double radiusLen
+            , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLen
             , List<NavigateNode> includeList, Set<NavigateNode> existNodes) {
         int x = currNode.getX();
         int y = currNode.getY();
@@ -241,7 +240,7 @@
     }
 
     public void extendNeighborNodes(NavigateNode originNode, NavigateNode nextNode
-            , String[][] codeMatrix, String[][] cdaMatrix, Double radiusLen
+            , String[][] codeMatrix, Double[][][] cdaMatrix, Double radiusLen
             , List<NavigateNode> includeList, Set<NavigateNode> existNodes) {
 
         int x = nextNode.getX();
@@ -258,11 +257,9 @@
 
         existNodes.add(nextNode);
 
-
-        List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]);
-        List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[x][y]);
-
-        if (Math.pow(o1Cda.get(0) - o2Cda.get(0), 2) + Math.pow(o1Cda.get(1) - o2Cda.get(1), 2) <= Math.pow(radiusLen, 2)) {
+        Double[] o1Cda = cdaMatrix[originNode.getX()][originNode.getY()];
+        Double[] o2Cda = cdaMatrix[x][y];
+        if (Math.pow(o1Cda[0] - o2Cda[0], 2) + Math.pow(o1Cda[1] - o2Cda[1], 2) <= Math.pow(radiusLen, 2)) {
             nextNode.setCodeData(codeMatrix[x][y]);
 
             if (!nextNode.getCodeData().equals(CodeNodeType.NONE.val)) {
@@ -275,83 +272,83 @@
     }
 
     // v2 BFS ------------------------------------------------------------------------------
-    public List<NavigateNode> getWaveScopeByCode0(Integer lev, String code, Double radiusLen) {
-        String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
-
-        int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code);
-        NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code);
-
-        List<NavigateNode> includeList = new ArrayList<>();
-        Set<NavigateNode> visited = new HashSet<>(); // Track visited nodes to avoid re-processing
-        Queue<NavigateNode> queue = new LinkedList<>();
-
-        includeList.add(originNode);
-        visited.add(originNode);
-        queue.offer(originNode);
-
-        while (!queue.isEmpty()) {
-            NavigateNode currNode = queue.poll();
-            this.spreadWaveNode0(originNode, currNode, codeMatrix, radiusLen, includeList, visited, queue);
-        }
-
-        return includeList;
-    }
-
-    public void spreadWaveNode0(NavigateNode originNode, NavigateNode currNode,
-                               String[][] codeMatrix, Double radiusLen,
-                               List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) {
-
-        int x = currNode.getX();
-        int y = currNode.getY();
-
-        // Expand neighbors in all four directions (up, down, left, right)
-        this.extendNeighborNodes0(originNode, new NavigateNode(x, y + 1), codeMatrix, radiusLen, includeList, visited, queue);
-        this.extendNeighborNodes0(originNode, new NavigateNode(x, y - 1), codeMatrix, radiusLen, includeList, visited, queue);
-        this.extendNeighborNodes0(originNode, new NavigateNode(x - 1, y), codeMatrix, radiusLen, includeList, visited, queue);
-        this.extendNeighborNodes0(originNode, new NavigateNode(x + 1, y), codeMatrix, radiusLen, includeList, visited, queue);
-    }
-
-    public void extendNeighborNodes0(NavigateNode originNode, NavigateNode nextNode,
-                                    String[][] codeMatrix, Double radiusLen,
-                                    List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) {
-        int x = nextNode.getX();
-        int y = nextNode.getY();
-
-        // Check if the node is out of bounds
-        if (x < 0 || x >= codeMatrix.length || y < 0 || y >= codeMatrix[0].length) {
-            return;
-        }
-
-        // If the node has already been visited, skip it
-        if (visited.contains(nextNode)) {
-            return;
-        }
-
-        visited.add(nextNode);
-
-        String nextNodeCodeData = codeMatrix[x][y];
-
-        // If it's a NONE node, we still need to check its surroundings
-        if (nextNodeCodeData.equals(CodeNodeType.NONE.val)) {
-            this.spreadWaveNode0(originNode, nextNode, codeMatrix, radiusLen, includeList, visited, queue);
-        } else {
-            Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV;
-            String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
-
-            // Check if the distance between nodes is within the radius length
-            List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]);
-            List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[nextNode.getX()][nextNode.getY()]);
-
-            // Calculate Euclidean distance between the nodes
-            if (Math.pow(o1Cda.get(0) - o2Cda.get(0), 2) + Math.pow(o1Cda.get(1) - o2Cda.get(1), 2) <= Math.pow(radiusLen, 2)) {
-                nextNode.setCodeData(nextNodeCodeData);
-                includeList.add(nextNode);
-
-                // Add the node to the queue to expand its neighbors
-                queue.offer(nextNode);
-            }
-        }
-    }
+//    public List<NavigateNode> getWaveScopeByCode0(Integer lev, String code, Double radiusLen) {
+//        String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev);
+//
+//        int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code);
+//        NavigateNode originNode = new NavigateNode(codeMatrixIdx[0], codeMatrixIdx[1], code);
+//
+//        List<NavigateNode> includeList = new ArrayList<>();
+//        Set<NavigateNode> visited = new HashSet<>(); // Track visited nodes to avoid re-processing
+//        Queue<NavigateNode> queue = new LinkedList<>();
+//
+//        includeList.add(originNode);
+//        visited.add(originNode);
+//        queue.offer(originNode);
+//
+//        while (!queue.isEmpty()) {
+//            NavigateNode currNode = queue.poll();
+//            this.spreadWaveNode0(originNode, currNode, codeMatrix, radiusLen, includeList, visited, queue);
+//        }
+//
+//        return includeList;
+//    }
+//
+//    public void spreadWaveNode0(NavigateNode originNode, NavigateNode currNode,
+//                               String[][] codeMatrix, Double radiusLen,
+//                               List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) {
+//
+//        int x = currNode.getX();
+//        int y = currNode.getY();
+//
+//        // Expand neighbors in all four directions (up, down, left, right)
+//        this.extendNeighborNodes0(originNode, new NavigateNode(x, y + 1), codeMatrix, radiusLen, includeList, visited, queue);
+//        this.extendNeighborNodes0(originNode, new NavigateNode(x, y - 1), codeMatrix, radiusLen, includeList, visited, queue);
+//        this.extendNeighborNodes0(originNode, new NavigateNode(x - 1, y), codeMatrix, radiusLen, includeList, visited, queue);
+//        this.extendNeighborNodes0(originNode, new NavigateNode(x + 1, y), codeMatrix, radiusLen, includeList, visited, queue);
+//    }
+//
+//    public void extendNeighborNodes0(NavigateNode originNode, NavigateNode nextNode,
+//                                    String[][] codeMatrix, Double radiusLen,
+//                                    List<NavigateNode> includeList, Set<NavigateNode> visited, Queue<NavigateNode> queue) {
+//        int x = nextNode.getX();
+//        int y = nextNode.getY();
+//
+//        // Check if the node is out of bounds
+//        if (x < 0 || x >= codeMatrix.length || y < 0 || y >= codeMatrix[0].length) {
+//            return;
+//        }
+//
+//        // If the node has already been visited, skip it
+//        if (visited.contains(nextNode)) {
+//            return;
+//        }
+//
+//        visited.add(nextNode);
+//
+//        String nextNodeCodeData = codeMatrix[x][y];
+//
+//        // If it's a NONE node, we still need to check its surroundings
+//        if (nextNodeCodeData.equals(CodeNodeType.NONE.val)) {
+//            this.spreadWaveNode0(originNode, nextNode, codeMatrix, radiusLen, includeList, visited, queue);
+//        } else {
+//            Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV;
+//            String[][] cdaMatrix = mapDataDispatcher.getCdaMatrix(lev);
+//
+//            // Check if the distance between nodes is within the radius length
+//            List<Double> o1Cda = MapDataUtils.parseCdaNode(cdaMatrix[originNode.getX()][originNode.getY()]);
+//            List<Double> o2Cda = MapDataUtils.parseCdaNode(cdaMatrix[nextNode.getX()][nextNode.getY()]);
+//
+//            // Calculate Euclidean distance between the nodes
+//            if (Math.pow(o1Cda.get(0) - o2Cda.get(0), 2) + Math.pow(o1Cda.get(1) - o2Cda.get(1), 2) <= Math.pow(radiusLen, 2)) {
+//                nextNode.setCodeData(nextNodeCodeData);
+//                includeList.add(nextNode);
+//
+//                // Add the node to the queue to expand its neighbors
+//                queue.offer(nextNode);
+//            }
+//        }
+//    }
 
 
     public Boolean isTurnCorner(String codeData) {
diff --git a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java
index 72b5182..04be5ac 100644
--- a/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java
+++ b/zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java
@@ -6,6 +6,7 @@
 import com.zy.acs.common.utils.RedisSupport;
 import com.zy.acs.framework.common.Cools;
 import com.zy.acs.framework.exception.CoolException;
+import com.zy.acs.manager.common.utils.MapDataUtils;
 import com.zy.acs.manager.core.service.astart.domain.DynamicNode;
 import com.zy.acs.manager.core.utils.RouteGenerator;
 import com.zy.acs.manager.manager.entity.Code;
@@ -36,7 +37,7 @@
 
     private int[][] turnMatrix;
 
-    private String[][] cdaMatrix;
+    private Double[][][] cdaMatrix;
 
     public Map<String, Boolean> routeCdaMap = new HashMap<>();
 
@@ -89,13 +90,14 @@
         redis.setValue(RedisConstant.AGV_MAP_ASTAR_DYNAMIC_FLAG, String.valueOf(lev), JSON.toJSONString(dynamicMatrix));
     }
 
-    public String[][] getCdaMatrix(Integer lev) {
+    public Double[][][] getCdaMatrix(Integer lev) {
         lev = Optional.ofNullable(lev).orElse(MAP_DEFAULT_LEV);
         // redis
         if (null == this.cdaMatrix) {
             String cdaMatrixStr = redis.getValue(RedisConstant.AGV_MAP_ASTAR_CDA_FLAG, String.valueOf(lev));
             if (!Cools.isEmpty(cdaMatrixStr)) {
-                this.cdaMatrix = JSON.parseObject(cdaMatrixStr, String[][].class);
+                String[][] cdaStrMatrix = JSON.parseObject(cdaMatrixStr, String[][].class);
+                this.cdaMatrix = MapDataUtils.preComputeCdaMatrix(cdaStrMatrix);
             }
         }
         // init
@@ -110,9 +112,9 @@
         return this.cdaMatrix;
     }
 
-    public void setCdaMatrix(Integer lev, String[][] cdaMatrix) {
-        redis.setValue(RedisConstant.AGV_MAP_ASTAR_CDA_FLAG, String.valueOf(lev), JSON.toJSONString(cdaMatrix));
-        this.cdaMatrix = cdaMatrix;
+    public void setCdaMatrix(Integer lev, String[][] cdaStrMatrix) {
+        redis.setValue(RedisConstant.AGV_MAP_ASTAR_CDA_FLAG, String.valueOf(lev), JSON.toJSONString(cdaStrMatrix));
+        this.cdaMatrix = MapDataUtils.preComputeCdaMatrix(cdaStrMatrix);
     }
 
     public int[][] getTurnMatrix(Integer lev) {

--
Gitblit v1.9.1