1

zhang

2025-08-29 c6ac196d7207e64b8591f84932439214561fc446

 src/main/java/com/zy/common/utils/NavigateSolution.java

@@ -13,9 +13,7 @@
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.ArrayList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.*;

/**
 * 四向库核心
@@ -45,10 +43,10 @@
    public PriorityQueue<NavigateNode> Open = new PriorityQueue<NavigateNode>();
    //Close表用普通的数组
    public ArrayList<NavigateNode> Close = new ArrayList<NavigateNode>();
    //Exist表用来存放已经出现过的结点。
    public ArrayList<NavigateNode> Exist = new ArrayList<NavigateNode>();
    //用来存放已经出现过的结点。
    Map<String, Integer> bestGMap = new HashMap<>();

    public String astarSearch(NavigateNode start, NavigateNode end, String pythonScriptPath) {
    public String astarSearchPython(NavigateNode start, NavigateNode end, String pythonScriptPath) {
        String maps = JSON.toJSONString(map);
        String startStr = start.getX() + "-" + start.getY();
        String targetStr = end.getX() + "-" + end.getY();
@@ -104,44 +102,45 @@
        return null;
    }

//    public NavigateNode astarSearch(NavigateNode start, NavigateNode end) {
//        //把第一个开始的结点加入到Open表中
//        this.Open.add(start);
//        //把出现过的结点加入到Exist表中
//        this.Exist.add(start);
//        //主循环
//        while (Open.size() > 0) {
//            //取优先队列顶部元素并且把这个元素从Open表中删除
//            NavigateNode current_node = Open.poll();
//            //将这个结点加入到Close表中
//            Close.add(current_node);
//            //对当前结点进行扩展，得到一个四周结点的数组
//            ArrayList<NavigateNode> neighbour_node = extend_current_node(current_node);
//            //对这个结点遍历，看是否有目标结点出现
//            for (NavigateNode node : neighbour_node) {
//                // G + H + E (对启发函数增加去拐点方案calcNodeExtraCost)
//                int gCost = calcNodeCost(current_node, node) * calcNodeExtraCost(current_node, node, end);
//                if (node.getX() == end.getX() && node.getY() == end.getY()) {//找到目标结点就返回
//                    //init_node操作把这个邻居结点的父节点设置为当前结点
//                    //并且计算出G， F， H等值
//                    node.setLastDistance(gCost);
//                    node.init_node(current_node, end);
//                    return node;
//                }
//
//                //进行计算对G, F, H 等值
//                node.setLastDistance(gCost);
//                node.init_node(current_node, end);
//                node.setH(calcNodeCost(node, end));
//                node.setF(node.getG() + node.getH());
//
//                Open.add(node);
//                Exist.add(node);
//            }
//        }
//        //如果遍历完所有出现的结点都没有找到最终的结点，返回null
//        return null;
//    }
    public NavigateNode astarSearchJava(NavigateNode start, NavigateNode end) {
        //把第一个开始的结点加入到Open表中
        this.Open.add(start);
        //主循环
        while (Open.size() > 0) {
            //取优先队列顶部元素并且把这个元素从Open表中删除
            NavigateNode current_node = Open.poll();

            if (current_node.getX() == end.getX() && current_node.getY() == end.getY()) {//找到目标结点就返回
                return current_node;
            }

            //将这个结点加入到Close表中
            Close.add(current_node);
            //对当前结点进行扩展，得到一个四周结点的数组
            ArrayList<NavigateNode> neighbour_node = extend_current_node(current_node);
            //对这个结点遍历，看是否有目标结点出现
            for (NavigateNode node : neighbour_node) {
                // G + H + E (对启发函数增加去拐点方案calcNodeExtraCost)
                int gCost = calcNodeExtraCost(current_node, node, end);

                //进行计算对G, F, H 等值
                node.setG(1 + gCost);
                node.init_node(current_node, end);
                node.setH(calcNodeCost(node, end));
                node.setF(node.getG() + node.getH());

                String key = node.getX() + "_" + node.getY();
                Integer recordedG = bestGMap.get(key);
                if (recordedG == null || node.getG() <= recordedG) {
                    bestGMap.put(key, node.getG());

                    Open.add(node);
                }
            }
        }
        //如果遍历完所有出现的结点都没有找到最终的结点，返回null
        return null;
    }


    public ArrayList<NavigateNode> extend_current_node(NavigateNode current_node) {
@@ -150,7 +149,7 @@
        ConfigService configService = SpringUtils.getBean(ConfigService.class);
        if (configService != null) {
            Config config = configService.selectOne(new EntityWrapper<Config>()
                    .eq("config", "direction_map")
                    .eq("code", "direction_map")
                    .eq("status", 1));
            if (config != null) {
                mapDirection = config.getValue();
@@ -196,11 +195,13 @@
                if (is_valid(x + 1, y))
                {
                    NavigateNode node = new NavigateNode(x + 1, y);
                    node.setNodeValue(map[x + 1][y]);
                    neighbour_node.add(node);
                }
                if (is_valid(x - 1, y))
                {
                    NavigateNode node = new NavigateNode(x -1, y);
                    NavigateNode node = new NavigateNode(x - 1, y);
                    node.setNodeValue(map[x - 1][y]);
                    neighbour_node.add(node);
                }
            }
@@ -210,11 +211,13 @@
                if (is_valid(x, y + 1))
                {
                    NavigateNode node = new NavigateNode(x, y + 1);
                    node.setNodeValue(map[x][y + 1]);
                    neighbour_node.add(node);
                }
                if (is_valid(x, y - 1))
                {
                    NavigateNode node = new NavigateNode(x, y - 1);
                    node.setNodeValue(map[x][y - 1]);
                    neighbour_node.add(node);
                }
            }
@@ -224,11 +227,13 @@
                if (is_valid(x, y + 1))
                {
                    NavigateNode node = new NavigateNode(x, y + 1);
                    node.setNodeValue(map[x][y + 1]);
                    neighbour_node.add(node);
                }
                if (is_valid(x, y - 1))
                {
                    NavigateNode node = new NavigateNode(x, y - 1);
                    node.setNodeValue(map[x][y - 1]);
                    neighbour_node.add(node);
                }
            }
@@ -238,11 +243,13 @@
                if (is_valid(x + 1, y))
                {
                    NavigateNode node = new NavigateNode(x + 1, y);
                    node.setNodeValue(map[x + 1][y]);
                    neighbour_node.add(node);
                }
                if (is_valid(x - 1, y))
                {
                    NavigateNode node = new NavigateNode(x -1, y);
                    NavigateNode node = new NavigateNode(x - 1, y);
                    node.setNodeValue(map[x - 1][y]);
                    neighbour_node.add(node);
                }
            }
@@ -264,22 +271,9 @@
        if (map[x][y] == MapNodeType.CAR.id) {//节点是小车
            return false;
        }
        NavigateNode navigateNode = new NavigateNode(x, y);
        if (is_exist(navigateNode)) {
            return false;
        }

        //以上情况都没有则合法
        return true;
    }

    public boolean is_exist(NavigateNode node)
    {
        for (NavigateNode exist_node : Exist) {
            if (node.getX() == exist_node.getX() && node.getY() == exist_node.getY()) {
                return true;
            }
        }
        return false;
    }

    //------------------A*启发函数------------------//
@@ -294,12 +288,12 @@
        // 第一个点或直线点
        if (currNode.getFather() == null || nextNode.getX() == currNode.getFather().getX()
                || nextNode.getY() == currNode.getFather().getY()) {
            return 1;
            return 0;
        }

        // 拐向终点的点
        if (nextNode.getX() == endNode.getX() || nextNode.getY() == endNode.getY()) {
            return 2;
            return 1;
        }

        // 普通拐点
@@ -308,7 +302,7 @@
        拿到父节点和下一节点
        通过判断父节点和下一节点的x数据和y数据都不相同时，则表明当前坐标是一个拐点
         */
        return 3;
        return 1;
    }

    //------------------A*启发函数-end------------------//