1 import java.util.ArrayDeque;
2 import java.util.ArrayList;
3 import java.util.Collections;
4 import java.util.HashMap;
5 import java.util.Iterator;
6 import java.util.Map.Entry;
7 import java.util.Queue;
8
9 public class TestPath {
10 class Point implements Comparable<Point> {
11 int x; // 坐标x
12 int y; // 坐标y
13 int f = 200; // 估值数 f = g + h
14 int g = 100; // 与父节点的距离
15 int h = 100; // 与目标点的距离
16 boolean canWalk; // 是否可以通行
17 Point parent = null;
18
19 Point(int x, int y, boolean canWalk) {
20 this.x = x;
21 this.y = y;
22 this.canWalk = canWalk;
23 }
24
25 @Override
26 public int compareTo(Point o) {
27 return new Integer(f).compareTo(new Integer(o.f));
28 }
29
30 }
31
32 public static void main(String[] args) {
33 int arr[][] = { { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
34 { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
35 { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
36 { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
37 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
38 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
39 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
40 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
41 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
42 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } };
43 Point[][] paths = new Point[10][10];
// 生成路径对象数组
44 for (int i = 0; i < 10; i++) {
45 for (int j = 0; j < 10; j++) {
46 boolean canWalk = arr[i][j] == 1 ? false : true;
47 paths[i][j] = new TestPath().new Point(i, j, canWalk);
48 }
49 }
// 起始点
50 Point bp = paths[0][0];
// 目标点
51 Point ep = paths[2][4];
52 AStart(paths, bp, ep);
53 Point tmp = ep;
54 while (tmp.parent != bp) {
55 tmp = tmp.parent;
56 System.out.println(tmp.x + "," + tmp.y);
57 }
58
59 }
60
61 static void AStart(Point[][] paths, Point beginP, Point endP) {
// 待遍历列表
62 ArrayList<Point> openList = new ArrayList<>(100);
63 beginP.g = 0;
64 beginP.h = (Math.abs(beginP.x - endP.x) + Math.abs(beginP.y - endP.y)) * 10;
65 beginP.f = beginP.g + beginP.h;
66 endP.h = 0;
67 openList.add(beginP);
68 ArrayList<Point> closeList = new ArrayList<>(100);
69 //Iterator<Point> iter = openList.iterator();
70 while (openList.size() != 0) {
71 Point p = openList.get(0);
72 System.out.println("p: x="+p.x + ",y="+p.y + ",g="+p.g + ",h="+ p.h +",f="+p.f);
73 if (p.h == 0) {
74 System.out.println("=========end===========");
75 System.out.println(p.x + "," + p.y);
76 return;
77 }
// 遍历周边相邻路径
78 for (int i = -1; i <= 1; i++) {
79 for (int j = -1; j <= 1; j++) {
80 if (p.x + i < 0 || p.x + i > 9 || p.y + j < 0 || p.y + j > 9) { // 超出地图范围
81 continue;
82 }
83 if (i == 0 && j == 0) { // 本身
84 continue;
85 }
86 Point tmp = paths[p.x + i][p.y + j];
// 如果已经遍历过了，跳过
87 if (!tmp.canWalk || closeList.contains(tmp)) {
88 continue;
89 }
// 下一个路径点的g值，相邻的加10，对角的加14（10和14是自己定义的值）
90 int newG = 0;
91 if ( i != 0 && j != 0) {
92 newG = p.g + 14;
93 } else {
94 newG = p.g + 10;
95 }
96 System.out.println("newg="+newG);
// 下一个路径点与目标点的h值
97 tmp.h = (Math.abs(tmp.x - endP.x) + Math.abs(tmp.y - endP.y)) * 10;
98 if (openList.contains(tmp)) {
99 if (tmp.g > newG) { // 在待遍历列表中，并且与当前的路径点距离更近，则更换上级路径点
100 tmp.parent = p;
101 tmp.g = newG;
102 tmp.f = tmp.g + tmp.h;
103 }
104 } else {// 加入待遍历列表
105 tmp.parent = p;
106 tmp.g = newG;
107 openList.add(tmp);
108 }
109 tmp.f = tmp.g + tmp.h; // 更新f值
110 System.out.println(tmp.x + "," + tmp.y + "," + tmp.g+","+tmp.h+","+tmp.f+",("+tmp.parent.x + ","+ tmp.parent.y+")");
111 }
112 }
// 遍历完之后从待遍历列表中移除，加入到已遍历列表中
113 closeList.add(p);
114 openList.remove(p);
// 按f值排序
115 Collections.sort(openList);
116 //iter = openList.iterator();
117 }
118 }
119
120 }
