A*寻路算法

Posted 2020-10-06 Qcer

 

 

java代码实现：

  1 package agstring;
  2 
  3 import java.util.ArrayList;
  4 import java.util.*;
  5 class Node{
  6     public final int x,y;
  7     public final double weight;
  8     public boolean isOpen = true;
  9     public Node parent;
 10     public int varF = 0,varG = 0,varH = 0;
 11     public Node(int x,int y,double weight){
 12         this.x = x;
 13         this.y = y;
 14         this.weight = weight;
 15     }
 16     public Node(int x,int y){
 17         this(x, y, 0.0);
 18     }
 19     public String toString(){
 20         return "("+"x:"+this.x+","+"y:"+this.y+","+"w:"+this.weight+")";
 21     }
 22 }
 23 public class AStar {
 24     
 25     public static Node[][] initData(int row,int column){
 26         Node[][] tableOf2D = new Node[row][column];
 27         for (int i = 0; i < row; i++) {
 28             for (int j = 0; j < column; j++) {
 29                 tableOf2D[i][j] = new Node(i,j);
 30             }
 31         }
 32         return tableOf2D;
 33     }
 34     private static int getDistance(Node startNode,Node endNode){
 35         return Math.abs(endNode.x - startNode.x) + Math.abs(endNode.y - startNode.y);
 36     }
 37     
 38     private  static void updateFGH(Node node,Node startNode,Node endNode){
 39         node.varG = getDistance(startNode, node);
 40         node.varH = getDistance(node, endNode);
 41         node.varF = node.varG + node.varH;
 42     }
 43     
 44     public static Node[] AStarAlgo(Node[][] tableOf2D,Node startNode,Node endNode){
 45         int row = tableOf2D.length,column = tableOf2D[0].length;
 46         ArrayList<Node> openList = new ArrayList<Node>();
 47         ArrayList<Node> closeList = new ArrayList<Node>();
 48         Node currNode = startNode;
 49         Node nextNode,checkedNode = startNode;
 50         int minF;
 51         currNode.varG = 0;
 52         currNode.varF = currNode.varH = getDistance(currNode, endNode);
 53         openList.add(currNode);
 54         int flag = 0;
 55         while(flag < 21){//!currNode.equals(endNode)
 56             minF = Integer.MAX_VALUE;
 57             for (int i = 0; i < openList.size(); i++) {
 58                 if (openList.get(i).varF < minF) {
 59                     checkedNode = openList.get(i);
 60                     minF = checkedNode.varF;
 61                 }
 62             }
 63 //            System.out.println(openList.size());
 64 //            System.out.println(checkedNode);
 65 //            System.out.println("minF:"+minF);
 66             closeList.add(checkedNode);
 67             openList.remove(checkedNode);
 68             
 69             if (checkedNode.y-1 >= 0 && tableOf2D[checkedNode.x][checkedNode.y-1].isOpen ) {
 70                 nextNode = tableOf2D[checkedNode.x][checkedNode.y-1];
 71                 if (!openList.contains(nextNode)) {
 72                     nextNode.parent = checkedNode;
 73                 }
 74                 openList.add(nextNode);
 75                 updateFGH(nextNode,startNode,endNode);
 76                 nextNode.isOpen = false;
 77             }
 78             if (checkedNode.y+1 <= column-1 && tableOf2D[checkedNode.x][checkedNode.y+1].isOpen ) {
 79                 nextNode = tableOf2D[checkedNode.x][checkedNode.y+1];
 80                 if (!openList.contains(nextNode)) {
 81                     nextNode.parent = checkedNode;
 82                 }
 83                 openList.add(nextNode);
 84                 updateFGH(nextNode,startNode,endNode);
 85                 nextNode.isOpen = false;
 86             }
 87             if (checkedNode.x-1 >= 0 && tableOf2D[checkedNode.x-1][checkedNode.y].isOpen ) {
 88                 nextNode = tableOf2D[checkedNode.x-1][checkedNode.y];
 89                 if (!openList.contains(nextNode)) {
 90                     nextNode.parent = checkedNode;
 91                 }
 92                 openList.add(nextNode);
 93                 updateFGH(nextNode,startNode,endNode);
 94                 nextNode.isOpen = false;
 95             }
 96             if (checkedNode.x+1 <= row-1 && tableOf2D[checkedNode.x+1][checkedNode.y].isOpen ) {
 97                 nextNode = tableOf2D[checkedNode.x+1][checkedNode.y];
 98                 if (!openList.contains(nextNode)) {
 99                     nextNode.parent = checkedNode;
100                 }
101                 openList.add(nextNode);
102                 updateFGH(nextNode,startNode,endNode);
103                 nextNode.isOpen = false;
104             }
105             flag++;
106         }
107         System.out.println(checkedNode);
108         return closeList.toArray(new Node[closeList.size()]);
109     }
110     public static void main(String[] args) {
111         // TODO Auto-generated method stub
112         try {
113             int row = 5,column = 7;
114             Node[][] data = initData(row, column);
115 //            for (int i = 0; i < row ; i++) {
116 //                for (int j = 0; j < column; j++) {
117 //                    System.out.println(data[i][j]);
118 //                }
119 //            }
120             data[1][3].isOpen = data[2][3].isOpen = data[3][3].isOpen = false;
121             Node startNode = data[2][1];
122             Node endNode = data[2][5];
123             Node[] pathAry = AStarAlgo(data,startNode,endNode);
124             Node tmpNode = endNode;
125 //            System.out.print(tmpNode.parent);
126             for (int i = 0; i < 8; i++) {
127                 System.out.println(tmpNode.parent);
128                 tmpNode = tmpNode.parent;
129             }
130         } catch (Exception e) {
131             // TODO: handle exception
132             e.printStackTrace();
133         }
134     }
135 
136 }