import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.Iterator;
import java.util.Map.Entry;
import java.util.Queue;
public class TestPath {
class Point implements Comparable<Point> {
int x; // 坐标x
int y; // 坐标y
int f = 200; // 估值数 f = g + h
int g = 100; // 与父节点的距离
int h = 100; // 与目标点的距离
boolean canWalk; // 是否可以通行
Point parent = null;
Point(int x, int y, boolean canWalk) {
this.x = x;
this.y = y;
this.canWalk = canWalk;
}
@Override
public int compareTo(Point o) {
return new Integer(f).compareTo(new Integer(o.f));
}
}
public static void main(String[] args) {
int arr[][] = { { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 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, 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, 0, 0, 0, 0, 0 } };
Point[][] paths = new Point[10][10];
// 生成路径对象数组
for (int i = 0; i < 10; i++) {
for (int j = 0; j < 10; j++) {
boolean canWalk = arr[i][j] == 1 ? false : true;
paths[i][j] = new TestPath().new Point(i, j, canWalk);
}
}
// 起始点
Point bp = paths[0][0];
// 目标点
Point ep = paths[2][4];
AStart(paths, bp, ep);
Point tmp = ep;
while (tmp.parent != bp) {
tmp = tmp.parent;
System.out.println(tmp.x + "," + tmp.y);
}
}
static void AStart(Point[][] paths, Point beginP, Point endP) {
// 待遍历列表
ArrayList<Point> openList = new ArrayList<>(100);
beginP.g = 0;
beginP.h = (Math.abs(beginP.x - endP.x) + Math.abs(beginP.y - endP.y)) * 10;
beginP.f = beginP.g + beginP.h;
endP.h = 0;
openList.add(beginP);
ArrayList<Point> closeList = new ArrayList<>(100);
//Iterator<Point> iter = openList.iterator();
while (openList.size() != 0) {
Point p = openList.get(0);
System.out.println("p: x="+p.x + ",y="+p.y + ",g="+p.g + ",h="+ p.h +",f="+p.f);
if (p.h == 0) {
System.out.println("=========end===========");
System.out.println(p.x + "," + p.y);
return;
}
// 遍历周边相邻路径
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
if (p.x + i < 0 || p.x + i > 9 || p.y + j < 0 || p.y + j > 9) { // 超出地图范围
continue;
}
if (i == 0 && j == 0) { // 本身
continue;
}
Point tmp = paths[p.x + i][p.y + j];
// 如果已经遍历过了，跳过
if (!tmp.canWalk || closeList.contains(tmp)) {
continue;
}
// 下一个路径点的g值，相邻的加10，对角的加14（10和14是自己定义的值）
int newG = 0;
if ( i != 0 && j != 0) {
newG = p.g + 14;
} else {
newG = p.g + 10;
}
System.out.println("newg="+newG);
// 下一个路径点与目标点的h值
tmp.h = (Math.abs(tmp.x - endP.x) + Math.abs(tmp.y - endP.y)) * 10;
if (openList.contains(tmp)) {
if (tmp.g > newG) { // 在待遍历列表中，并且与当前的路径点距离更近，则更换上级路径点
tmp.parent = p;
tmp.g = newG;
tmp.f = tmp.g + tmp.h;
}
} else {// 加入待遍历列表
tmp.parent = p;
tmp.g = newG;
openList.add(tmp);
}
tmp.f = tmp.g + tmp.h; // 更新f值
System.out.println(tmp.x + "," + tmp.y + "," + tmp.g+","+tmp.h+","+tmp.f+",("+tmp.parent.x + ","+ tmp.parent.y+")");
}
}
// 遍历完之后从待遍历列表中移除，加入到已遍历列表中
closeList.add(p);
openList.remove(p);
// 按f值排序
Collections.sort(openList);
//iter = openList.iterator();
}
}
}