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 }