a 寻路算法 java_A*寻路算法Java实现

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 {

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 openMap = new PriorityQueue<>();

private Set 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 getNearByCoords(Coord[][] coords, Coord nowCoord) {

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

}

}