A*寻路算法Java实现
程序员文章站
2024-03-16 22:30:52
...
A*算法：
- 把起始格添加到开启列表。
- 重复如下的工作：
a) 寻找开启列表中F值最低的格子。我们称它为当前格。
b) 把它切换到关闭列表。
c) 对相邻的8格中的每一个- 如果它不可通过或者已经在关闭列表中，略过它。反之如下。
- 如果它不在开启列表中。把当前格作为这一格的父节点。记录这一格的F,G,和H值，把它添加开启列表中。
- 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
- 把目标格添加进了开启列表，这时候路径被找到，或者
- 没有找到目标格，开启列表已经空了。这时候，路径不存在。
- 保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。这就是你的路径。
伪代码
//OPEN表可以使用优先队列实现
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表
并将X插入OPEN表中;
}
}//endfor
}//endwhile(OPEN!=NULL)
代码实现
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;
/**
* Created by 默默 on 2018/3/27.
*/
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;
}
//函数H 曼哈顿距离*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;
//1.把起始格添加到开启列表。
openMap.add(startCoord);
//2. 重复如下的工作：
while (!openMap.isEmpty()) {
//a) 寻找开启列表中F值最低的格子。我们称它为当前格。
Coord nowCoord = openMap.poll();
//b) 把它切换到关闭列表。
closeMap.add(nowCoord);
if (nowCoord.equals(endCoord)) {
break;
}
//c) 对相邻的8格中的每一个
for (Coord next : getNearByCoords(coords, nowCoord)) {
//如果它不可通过或者已经在关闭列表中，略过它。反之如下。
if (next.value == BAR || closeMap.contains(next)) {
continue;
}
//如果它不在开启列表中。把当前格作为这一格的父节点。记录这一格的F,G,和H值，把它添加开启列表中。
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 {//如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
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 . @ . . # . . . .
. . . . @ . . . # # E .
. . . @ @ . . . . . . .
. . . @ . . . . . . . .
. . . @ . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . . 上一篇： 1.15、C语言设计简易五子棋