A*算法:
- 把起始格添加到開啓列表。
- 重複如下的工作:
a) 尋找開啓列表中F值最低的格子。我們稱它爲當前格。
b) 把它切換到關閉列表。
c) 對相鄰的8格中的每一個
- 如果它不可通過或者已經在關閉列表中,略過它。反之如下。
- 如果它不在開啓列表中。把當前格作爲這一格的父節點。記錄這一格的F,G,和H值,把它添加開啓列表中。
- 如果它已經在開啓列表中,用G值爲參考檢查新的路徑是否更好。更低的G值意味着更好的路徑。如果是這樣,就把這一格的父節點改成當前格,並且重新計算這一格的G和F值。如果你保持你的開啓列表按F值排序,改變之後你可能需要重新對開啓列表排序。
d) 停止,當你
- 把目標格添加進了開啓列表,這時候路徑被找到,或者
- 沒有找到目標格,開啓列表已經空了。這時候,路徑不存在。
- 保存路徑。從目標格開始,沿着每一格的父節點移動直到回到起始格。這就是你的路徑。
僞代碼
while(OPEN!=NULL)
{
從OPEN表中取f(n)最小的節點n;
將n節點插入CLOSE表中;
if(n節點==目標節點)
break;
for(當前節點n的每個子節點X)
{
if(X in OPEN)
計算f(X);
if(新的f(X)<OPEN中的f(X))
{
把n設置爲X的父親;
更新OPEN表中的f(n);
}
if(X in CLOSE)
continue;
if(X not inboth)
{
把n設置爲X的父親;
求f(X);
並將X插入OPEN表中;
}
}
}
代碼實現
import android.support.annotation.NonNull;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Set;
public class FindPath {
public static final char BAR = '@';
private class Coord implements Comparable<Coord> {
char value;
int x;
int y;
Coord parent;
int g;
int h;
int f;
@Override
public int compareTo(@NonNull Coord o) {
return f - o.f;
}
}
private PriorityQueue<Coord> openMap = new PriorityQueue<>();
private Set<Coord> closeMap = new HashSet<>();
private int G(Coord start, Coord now) {
if (start.x != now.x && start.y != now.y) {
return Math.abs(start.x - now.x) * 14;
}
return (Math.abs(start.x - now.x) + Math.abs(start.y - now.y)) * 10;
}
private int H(Coord end, Coord now) {
return (Math.abs(end.x - now.x) + Math.abs(end.y - now.y)) * 10;
}
private int F(Coord start, Coord now, Coord end) {
return G(start, now) + H(end, now);
}
public void findPath(char[][] map, int startX, int startY, int endX, int endY) {
Coord[][] coords = convert(map);
Coord startCoord = coords[startX][startY];
Coord endCoord = coords[endX][endY];
startCoord.g = 0;
startCoord.h = H(endCoord, startCoord);
startCoord.f = startCoord.g + startCoord.h;
openMap.add(startCoord);
while (!openMap.isEmpty()) {
Coord nowCoord = openMap.poll();
closeMap.add(nowCoord);
if (nowCoord.equals(endCoord)) {
break;
}
for (Coord next : getNearByCoords(coords, nowCoord)) {
if (next.value == BAR || closeMap.contains(next)) {
continue;
}
if (!openMap.contains(next)) {
next.parent = nowCoord;
next.g = nowCoord.g + G(nowCoord, next);
next.h = H(endCoord, next);
next.f = next.g + next.h;
openMap.add(next);
} else {
if (next.g > nowCoord.g + G(nowCoord, next)) {
next.parent = nowCoord;
next.g = nowCoord.g + G(nowCoord, next);
next.f = next.g + next.h;
}
}
}
}
if (openMap.isEmpty()) {
System.out.println("沒有找到路徑");
} else {
printPath(endCoord);
System.out.println();
startCoord.value = 'S';
endCoord.value = 'E';
int col = map[0].length;
int row = map.length;
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
System.out.print(coords[i][j].value + " ");
}
System.out.println();
}
}
}
private void printPath(Coord end) {
if (end.parent != null) {
end.parent.value = '#';
printPath(end.parent);
}
System.out.print("(" + end.x + "," + end.y + ")-> ");
}
private List<Coord> getNearByCoords(Coord[][] coords, Coord nowCoord) {
List<Coord> coordList = new LinkedList<>();
if (nowCoord.x - 1 >= 0) {
coordList.add(coords[nowCoord.x - 1][nowCoord.y]);
}
if (nowCoord.x - 1 >= 0 && nowCoord.y + 1 < coords[0].length) {
coordList.add(coords[nowCoord.x - 1][nowCoord.y + 1]);
}
if (nowCoord.y + 1 < coords[0].length) {
coordList.add(coords[nowCoord.x][nowCoord.y + 1]);
}
if (nowCoord.x + 1 < coords.length && nowCoord.y + 1 < coords[0].length) {
coordList.add(coords[nowCoord.x + 1][nowCoord.y + 1]);
}
if (nowCoord.x + 1 < coords.length) {
coordList.add(coords[nowCoord.x + 1][nowCoord.y]);
}
if (nowCoord.x + 1 < coords.length && nowCoord.y - 1 >= 0) {
coordList.add(coords[nowCoord.x + 1][nowCoord.y - 1]);
}
if (nowCoord.y - 1 >= 0) {
coordList.add(coords[nowCoord.x][nowCoord.y - 1]);
}
if (nowCoord.x - 1 >= 0 && nowCoord.y - 1 >= 0) {
coordList.add(coords[nowCoord.x - 1][nowCoord.y - 1]);
}
return coordList;
}
private Coord[][] convert(char[][] map) {
int col = map[0].length;
int row = map.length;
Coord[][] coords = new Coord[row][col];
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
Coord coord = new Coord();
coord.x = i;
coord.y = j;
coord.value = map[i][j];
coords[i][j] = coord;
}
}
return coords;
}
@org.junit.Test
public void testFindPath() {
char[][] map = {
{'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '@', '@', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '@', '@', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
{'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
};
findPath(map, 4, 2, 5, 10);
}
}
結果
(4,2)-> (3,2)-> (2,2)-> (1,3)-> (1,4)-> (2,5)-> (3,6)-> (4,7)-> (5,8)-> (5,9)-> (5,10)->
. . . . . . . . . . . .
. . .
. .
. .
. . S . @ . .
. . . . @ . . .
. . . @ @ . . . . . . .
. . . @ . . . . . . . .
. . . @ . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .