A*（也叫A star, A星）寻路算法Java版

来源：互联网发布：内向工作知乎编辑：程序博客网时间：2024/05/18 03:31
转载自：http://blog.csdn.net/ruils/article/details/40780657
寻路算法有很多种，A*寻路算法被公认为最好的寻路算法。
首先要理解什么是A*寻路算法，可以参考这三篇文章：
http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003（英文）
http://www.cppblog.com/christanxw/archive/2006/04/07/5126.html（中文）
http://www.cnblogs.com/technology/archive/2011/05/26/2058842.html（中文）
下面为测试地图，0表示可以通行，1表示障碍物：
要从点(5, 1)到点(5, 5)，通过A*寻路算法找到以路径为@所示：
在代码中可以修改障碍物，起点和终点来测试算法。
最后代码：
[java] view plaincopy
import java.util.ArrayList;  
import java.util.List;  
  
public class AStar {  
  
    public static final int[][] NODES = {   
        { 0, 0, 0, 0, 0, 0, 0, 0, 0 },  
        { 0, 0, 0, 0, 0, 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 },   
    };  
  
    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;  
    }  
  
    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;  
    }  
  
    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 void main(String[] args) {  
        Node startNode = new Node(5, 1);  
        Node endNode = new Node(5, 5);  
        Node parent = new AStar().findPath(startNode, endNode);  
  
        for (int i = 0; i < NODES.length; i++) {  
            for (int j = 0; j < NODES[0].length; j++) {  
                System.out.print(NODES[i][j] + ", ");  
            }  
            System.out.println();  
        }  
        ArrayList<Node> arrayList = new ArrayList<Node>();  
  
        while (parent != null) {  
            // System.out.println(parent.x + ", " + parent.y);  
            arrayList.add(new Node(parent.x, parent.y));  
            parent = parent.parent;  
        }  
        System.out.println("\n");  
  
        for (int i = 0; i < NODES.length; i++) {  
            for (int j = 0; j < NODES[0].length; j++) {  
                if (exists(arrayList, i, j)) {  
                    System.out.print("@, ");  
                } else {  
                    System.out.print(NODES[i][j] + ", ");  
                }  
  
            }  
            System.out.println();  
        }  
  
    }  
  
    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 F;  
        public int G;  
        public int H;  
  
        public void calcF() {  
            this.F = this.G + this.H;  
        }  
  
        public Node parent;  
    }  
}  
0 0