A*—java代码
import java.util.ArrayList; // A*算法寻路 public class AStar2 { public static final int[][] maps = { {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, 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}, }; public static int straight = 10; public static int diagonal = 14; // 开放列表 public static ArrayList<Node> openList = new ArrayList<>(); // 闭合列表 public static ArrayList<Node> colseList = new ArrayList<>(); // 方向 public static int[][] direct = {{0, 1}, {0, -1}, {1, 0}, {-1, 0}, {1, 1}, {1, -1}, {-1, 1}, {-1, -1}}; public static void main(String[] args) { //定点:起点终点 Node start = new Node(5, 1); Node end = new Node(5, 4); Node endNode = findPath(start, end); printMap(maps, start, end); ArrayList<Node> arrayList = endNode != null ? getPaths(endNode) : null; printPaths(arrayList); } // 从起点开始，找到到终点的一条最短路径 private static Node findPath(Node start, Node end) { start.G = 0; openList.add(start); while (!openList.isEmpty()) { //从开放列表中拿到最小F节点 Node cureNode = minFINOpenList(openList); openList.remove(cureNode); // 将该节点加入到闭合列表中 colseList.add(cureNode); // 当前节点的全部合法邻居 ArrayList<Node> neighbors = getNeighbor(cureNode); for (Node nbrNode : neighbors) { // 邻居已经在openList if (exists(openList, nbrNode) != null) updateG(cureNode, nbrNode); // 邻居不在openList else joinOpenList(cureNode, nbrNode, end); } if (exists(openList, end) != null) return exists(openList, end); } return null; } private static ArrayList<Node> getPaths(Node endNode) { ArrayList<Node> arrayList = new ArrayList<>(); Node parent = endNode; while (parent != null) { arrayList.add(parent); parent = parent.parent; } return arrayList; } private static int calStep(Node node, Node cur) { if (inLine(node, cur)) return straight; else return diagonal; } private static int calH(Node endNode, Node nbrNode) { return Math.abs(endNode.y - nbrNode.y) + Math.abs(endNode.x - nbrNode.x); } // 计算距离起点的距离 private static int calG(Node cureNode, Node nbrNode) { int step = calStep(cureNode, nbrNode); return cureNode.G + step; } private static boolean inLine(Node nbr, Node cur) { if (nbr.x == cur.x || nbr.y == cur.y) return true; return false; } // 途径当前节点到达节点node的路径G会不会更短 private static void updateG(Node cureNode, Node nbrNode) { int step = calStep(cureNode, nbrNode); int G = calG(cureNode, nbrNode); if (G < nbrNode.G) { nbrNode.G = G; nbrNode.parent = cureNode; nbrNode.calcF(); } } private static void joinOpenList(Node curNode, Node nbrNode, Node endNode) { openList.add(nbrNode); nbrNode.parent = curNode; nbrNode.G = calG(curNode, nbrNode); nbrNode.H = calH(endNode, nbrNode); nbrNode.calcF(); } // 达到当前节点的可达，且不在closeList中的邻居节点 private static ArrayList<Node> getNeighbor(Node cureNode) { ArrayList<Node> arrayList = new ArrayList<>(); //从当前节点想八个方向扩散 for (int i = 0; i < 8; i++) { int newRow = cureNode.x + direct[i][0]; int newCol = cureNode.y + direct[i][1]; //当前邻居节点: 可达、不在closeList中 if (isAccesse(newRow, newCol) && !exists(colseList, newRow, newCol)) { arrayList.add(new Node(newRow, newCol)); } } return arrayList; } private static Node exists(ArrayList<Node> colseList, Node cur) { for (Node node : colseList) { if (node.x == cur.x && node.y == cur.y) return node; } return null; } private static boolean exists(ArrayList<Node> colseList, int newX, int newY) { for (Node node : colseList) { if (node.x == newX && node.y == newY) return true; } return false; } // 可达性分析(非障碍物) private static boolean isAccesse(int newX, int newY) { if (0 <= newX && newX < maps.length && 0 <= newY && newY < maps[0].length) return maps[newX][newY] == 0; return false; } // 从开放列表中找到最小F=G+H的节点 private static Node minFINOpenList(ArrayList<Node> openList) { Node min = openList.get(0); for (Node node : openList) { if (node.F < min.F) min = node; } return min; } private static void printMap(int[][] maps, Node start, Node end) { for (int col = 0; col < maps[0].length; col++) { System.out.print("\t" + col + ""); } System.out.print("\n-----------------------------------------\n"); int count = 0; for (int row = 0; row < maps.length; row++) { for (int col = 0; col < maps[0].length; col++) { if (col == 0) System.out.print(count++ + "|\t"); if (row == start.x && col == start.y || row == end.x && col == end.y) System.out.print("X\t"); else System.out.print(maps[row][col] + "\t"); } System.out.println(); } System.out.println(); } public static void printPaths(ArrayList<Node> arrayList) { if (arrayList == null) { System.out.println("无路可走"); return; } // 地图形式 for (int col = 0; col < maps[0].length; col++) { System.out.print("\t" + col + ""); } System.out.print("\n-----------------------------------------\n"); int count = 0; for (int row = 0; row < maps.length; row++) { for (int col = 0; col < maps[0].length; col++) { if (col == 0) System.out.print(count++ + "|\t"); if (exists(arrayList, row, col)) { System.out.print("X\t"); } else { System.out.print(maps[row][col] + "\t"); } } System.out.println(); } System.out.println(); // 路径形式 for (int i = arrayList.size() - 1; i >= 0; i--) { if (i == 0) System.out.print(arrayList.get(i)); else System.out.print(arrayList.get(i) + "->"); } System.out.println(); } }
结果
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 0 0 0 0 0 0
1| 0 0 0 0 0 0 0 0 0
2| 0 0 0 0 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X 0 1 X 0 0 0 0
6| 0 0 0 1 0 0 0 0 0
7| 0 0 0 1 0 0 0 0 0
8| 0 0 0 1 0 0 0 0 0
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 0 0 0 0 0 0
1| 0 0 0 0 0 0 0 0 0
2| 0 0 0 X 0 0 0 0 0
3| 0 0 X 1 X 0 0 0 0
4| 0 X 0 1 X 0 0 0 0
5| 0 X 0 1 X 0 0 0 0
6| 0 0 0 1 0 0 0 0 0
7| 0 0 0 1 0 0 0 0 0
8| 0 0 0 1 0 0 0 0 0
(5,1)->(4,1)->(3,2)->(2,3)->(3,4)->(4,4)->(5,4)
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 0 0 0 0 0 0
1| 0 0 0 0 0 0 0 0 0
2| 0 0 0 0 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X 0 0 X 0 0 0 0
6| 0 0 0 0 0 0 0 0 0
7| 0 0 0 1 0 0 0 0 0
8| 0 0 0 0 0 0 0 0 0
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 0 0 0 0 0 0
1| 0 0 0 0 0 0 0 0 0
2| 0 0 0 0 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X X X X 0 0 0 0
6| 0 0 0 0 0 0 0 0 0
7| 0 0 0 1 0 0 0 0 0
8| 0 0 0 0 0 0 0 0 0
(5,1)->(5,2)->(5,3)->(5,4)
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 0 0 0 0 0 0
1| 0 0 0 0 0 0 0 0 0
2| 0 0 0 0 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X 0 1 X 0 0 0 0
6| 0 0 0 1 0 0 0 0 0
7| 0 0 0 0 0 0 0 0 0
8| 0 0 0 0 0 0 0 0 0
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 0 0 0 0 0 0
1| 0 0 0 0 0 0 0 0 0
2| 0 0 0 0 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X 0 1 X 0 0 0 0
6| 0 0 X 1 X 0 0 0 0
7| 0 0 0 X 0 0 0 0 0
8| 0 0 0 0 0 0 0 0 0
(5,1)->(6,2)->(7,3)->(6,4)->(5,4)
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 1 0 0 0 0 0
1| 0 0 0 1 0 0 0 0 0
2| 0 0 0 1 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X 0 1 X 0 0 0 0
6| 0 0 0 1 0 0 0 0 0
7| 0 0 0 1 0 0 0 0 0
8| 0 0 0 1 0 0 0 0 0
无路可走
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 1 0 0 0 0 0
1| 0 0 0 1 0 0 0 0 0
2| 0 0 0 1 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X 0 1 X 0 0 0 0
6| 0 0 0 1 0 0 0 0 0
7| 0 0 0 1 0 0 0 0 0
8| 0 0 0 0 0 0 0 0 0
0 1 2 3 4 5 6 7 8
-----------------------------------------
0| 0 0 0 1 0 0 0 0 0
1| 0 0 0 1 0 0 0 0 0
2| 0 0 0 1 0 0 0 0 0
3| 0 0 0 1 0 0 0 0 0
4| 0 0 0 1 0 0 0 0 0
5| 0 X 0 1 X 0 0 0 0
6| 0 X 0 1 X 0 0 0 0
7| 0 0 X 1 X 0 0 0 0
8| 0 0 0 X 0 0 0 0 0
(5,1)->(6,1)->(7,2)->(8,3)->(7,4)->(6,4)->(5,4)
