JAVA根据A星算法规划起点到终点二维坐标的最短路径

 

工具类

AStarUtil.java

import java.util.*;
import java.util.stream.Collectors;

/**
 * A星算法工具类
 */
public class AStarUtil {


    private int[][] NODES;

    public AStarUtil(int[][] NODES) {
        this.NODES = NODES;
    }


    public AStarUtil() {
    }


    public static final int STEP = 10;

    private ArrayList<Node> openList = new ArrayList<Node>();
    private ArrayList<Node> closeList = new ArrayList<Node>();

    public Node findMinFNodeInOpneList() {
        Node tempNode = openList.get(0);
        for (Node node : openList) {
            if (node.F < tempNode.F) {
                tempNode = node;
            }
        }
        return tempNode;
    }

    public ArrayList<Node> findNeighborNodes(Node currentNode) {
        ArrayList<Node> arrayList = new ArrayList<Node>();
        // 只考虑上下左右，不考虑斜对角
        int topX = currentNode.x;
        int topY = currentNode.y - 1;
        if (canReach(topX, topY) && !exists(closeList, topX, topY)) {
            arrayList.add(new Node(topX, topY));
        }
        int bottomX = currentNode.x;
        int bottomY = currentNode.y + 1;
        if (canReach(bottomX, bottomY) && !exists(closeList, bottomX, bottomY)) {
            arrayList.add(new Node(bottomX, bottomY));
        }
        int leftX = currentNode.x - 1;
        int leftY = currentNode.y;
        if (canReach(leftX, leftY) && !exists(closeList, leftX, leftY)) {
            arrayList.add(new Node(leftX, leftY));
        }
        int rightX = currentNode.x + 1;
        int rightY = currentNode.y;
        if (canReach(rightX, rightY) && !exists(closeList, rightX, rightY)) {
            arrayList.add(new Node(rightX, rightY));
        }
        return arrayList;
    }

    /**
     * 可以行走
     *
     * @param x
     * @param y
     * @return
     */
    public boolean canReach(int x, int y) {
        if (x >= 0 && x < NODES.length && y >= 0 && y < NODES[0].length) {
            return NODES[x][y] == 0;
        }
        return false;
    }

    /**
     * 寻找路径
     *
     * @param startNode
     * @param endNode
     * @return
     */
    public Node findPath(Node startNode, Node endNode) {

        // 把起点加入 open list
        openList.add(startNode);

        while (openList.size() > 0) {
            // 遍历 open list ，查找 F值最小的节点，把它作为当前要处理的节点
            Node currentNode = findMinFNodeInOpneList();
            // 从open list中移除
            openList.remove(currentNode);
            // 把这个节点移到 close list
            closeList.add(currentNode);

            ArrayList<Node> neighborNodes = findNeighborNodes(currentNode);
            for (Node node : neighborNodes) {
                if (exists(openList, node)) {
                    foundPoint(currentNode, node);
                } else {
                    notFoundPoint(currentNode, endNode, node);
                }
            }
            if (find(openList, endNode) != null) {
                return find(openList, endNode);
            }
        }

        return find(openList, endNode);
    }

    private void foundPoint(Node tempStart, Node node) {
        int G = calcG(tempStart, node);
        if (G < node.G) {
            node.parent = tempStart;
            node.G = G;
            node.calcF();
        }
    }

    private void notFoundPoint(Node tempStart, Node end, Node node) {
        node.parent = tempStart;
        node.G = calcG(tempStart, node);
        node.H = calcH(end, node);
        node.calcF();
        openList.add(node);
    }

    private int calcG(Node start, Node node) {
        int G = STEP;
        int parentG = node.parent != null ? node.parent.G : 0;
        return G + parentG;
    }

    private int calcH(Node end, Node node) {
        int step = Math.abs(node.x - end.x) + Math.abs(node.y - end.y);
        return step * STEP;
    }


    public static Node find(List<Node> nodes, Node point) {
        for (Node n : nodes) {
            if ((n.x == point.x) && (n.y == point.y)) {
                return n;
            }
        }
        return null;
    }

    public static boolean exists(List<Node> nodes, Node node) {
        for (Node n : nodes) {
            if ((n.x == node.x) && (n.y == node.y)) {
                return true;
            }
        }
        return false;
    }

    public static boolean exists(List<Node> nodes, int x, int y) {
        for (Node n : nodes) {
            if ((n.x == x) && (n.y == y)) {
                return true;
            }
        }
        return false;
    }

    public static class Node {
        public Node(int x, int y) {
            this.x = x;
            this.y = y;
        }

        public int x;
        public int y;

        public int getX() {
            return x;
        }

        public void setX(int x) {
            this.x = x;
        }

        public int getY() {
            return y;
        }

        public void setY(int y) {
            this.y = y;
        }

        public int F;
        public int G;
        public int H;

        @Override
        public String toString() {
            return "Node{" +
                    "x=" + x +
                    ", y=" + y +
                    '}';
        }

        public void calcF() {
            this.F = this.G + this.H;
        }

        public Node parent;
    }


    /**
     * 根据坐标点获取二维数组
     *
     * @param allNodeList       所有的坐标点
     * @param obstaclesNodeList 所有的障碍物的坐标点
     * @return
     */
    public static int[][] get2DArrays(List<Node> allNodeList, List<Node> obstaclesNodeList) {
        int maxX = allNodeList.stream().sorted(Comparator.comparing(Node::getX).reversed()).collect(Collectors.toList()).get(0).x + 1;
        int maxY = allNodeList.stream().sorted(Comparator.comparing(Node::getY).reversed()).collect(Collectors.toList()).get(0).y + 1;

        int[][] ints = new int[maxX][maxY];
        for (int x = 0; x < maxX; x++) {
            for (int y = 0; y < maxY; y++) {
                for (Node o : obstaclesNodeList) {
                    if (o.x == x && o.y == y) {
                        ints[x][y] = 1;
                        break;
                    } else {
                        ints[x][y] = 0;
                    }
                }
            }
        }
        toStringInts(ints);
        System.out.println();
        return ints;
    }

    /**
     * 规划路径
     *
     * @param startNode 起始点坐标
     * @param endNode   要到达的重点坐标
     * @return 返回路径的坐标点 为空表示无法到达
     */
    public List<Node> PlanRoute(Node startNode, Node endNode) {
        AStarUtil.Node parent = new AStarUtil(NODES).findPath(startNode, endNode);
        LinkedList<AStarUtil.Node> arrayList = new LinkedList<>();

        while (parent != null) {
            arrayList.addFirst(new AStarUtil.Node(parent.x, parent.y));
            parent = parent.parent;
        }

        /**
         * 以下只是为了路线打印出来 无实际作用
         */
        for (int i = 0; i < NODES.length; i++) {
            for (int j = 0; j < NODES[0].length; j++) {
                if (AStarUtil.exists(arrayList, i, j)) {
                    System.out.print("@, ");
                } else {
                    System.out.print(NODES[i][j] + ", ");
                }

            }
            System.out.println();
        }
        return arrayList;
    }


    /**
     * 打印二维坐标数据
     *
     * @param ints
     */
    public static void toStringInts(int[][] ints) {
        for (int i = 0; i < ints.length; i++) {
            for (int j = 0; j < ints[0].length; j++) {
                System.out.print(ints[i][j] + ", ");

            }
            System.out.println();
        }
    }
}

 

 

 

使用方法

import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;

/**
 *
 *  注：左上角为原点，水平方向是 y轴,垂直方向为 x轴
 *
 *  {
 *             { 0, 0, 0, 0, 0, 0, 0, 0, 0 },  ——> y
 *             { 0, 0, 0, 0, 1, 0, 0, 0, 0 },
 *             { 0, 0, 0, 0, 0, 0, 0, 0, 0 },
 *             { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
 *             { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
 *             { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
 *             { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
 *             { 0, 0, 0, 0, 0, 0, 0, 0, 0 },
 *             { 0, 0, 0, 0, 0, 0, 0, 0, 0 },
 *
 *               ∣
 *               ∨
 *               x
 *     };
 *
 */
public class AstartMain {

    public static void main(String[] args) {

        /**
         * 模拟所有坐标
         */
        LinkedList<AStarUtil.Node> nodeLinkedList = new LinkedList<>();
        for (int i = 0; i < 9; i++) {
            for (int j = 0; j < 9; j++) {
                AStarUtil.Node ori = new AStarUtil.Node(j, i);
                nodeLinkedList.add(ori);
            }
        }


        /**
         * 模拟障碍物坐标
         */
        List<AStarUtil.Node> obstaclesNodeList =new LinkedList<>();
        obstaclesNodeList.add(new AStarUtil.Node(3, 3));
        obstaclesNodeList.add(new AStarUtil.Node(4, 3));
        obstaclesNodeList.add(new AStarUtil.Node(5, 3));
        obstaclesNodeList.add(new AStarUtil.Node(1, 4));
        obstaclesNodeList.add(new AStarUtil.Node(1, 7));

        int[][] arrays = AStarUtil.get2DArrays(nodeLinkedList, obstaclesNodeList);

        AStarUtil aStarUtil = new AStarUtil(arrays);
        /**
         * 起点坐标
         */
        AStarUtil.Node startNode = new AStarUtil.Node(0, 0);

        /**
         * 终点坐标
         */
        AStarUtil.Node endNode = new AStarUtil.Node(7, 4);


        /**
         * 路线的所有二维坐标
         */
        List<AStarUtil.Node> route = aStarUtil.PlanRoute(startNode, endNode);
        System.out.println(Arrays.toString(route.toArray()));
    }
}