寻路

首先要理解什么是A*寻路算法，可以参考这三篇文章：

下面为测试地图，0表示可以通行，1表示障碍物：

要从点(5, 1)到点(5, 5)，通过A*寻路算法找到以路径为@所示：

在代码中可以修改障碍物，起点和终点来测试算法。

最后代码：

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 openList = new ArrayList();

private ArrayList closeList = new ArrayList();

public Node findMinFNodeInOpneList() {

Node tempNode = openList.get(0);

for (Node node : openList) {

if (node.F < tempNode.F) {

tempNode = node;

}

}

return tempNode;

}

public ArrayList findNeighborNodes(Node currentNode) {

ArrayList arrayList = new ArrayList();

// 只考虑上下左右，不考虑斜对角

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 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 arrayList = new ArrayList();

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 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 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 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;

}

}