#

luxiaotao1123

2025-01-03 d77edc6de3ffe93afd5680953759fff556b5ca73

 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()});