• A* Algorithm
  • Breadth First Search

简单实现BFS和A* 算法

更多实现参考 https://www.redblobgames.com/pathfinding/a-star/implementation.html

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
import java.util.*;

class Node implements Comparable<Node>{
	int x;
	int y;
	Node parent;
	int gScore;
	int hScore;
	int fScore;

	Node(){};
	Node(int x, int y){
		this.x = x;
		this.y = y;
	}


	public void setParent(Node parent) {
		this.parent = parent;
	}

	public void setgScore(int gScore) {
		this.gScore = gScore;
	}

	public void sethScore(int hScore) {
		this.hScore = hScore;
	}

	public void setfScore(int fScore) {
		this.fScore = fScore;
	}

	@Override
	public int compareTo(Node o) {
		return this.fScore - o.fScore;
	}

	@Override
	public boolean equals(Object o){
		if(this == o)
			return true;
		if(!(o instanceof Node))
			return false;
		Node n = (Node)o;
		return n.x == this.x && n.y == this.y;
	}

	@Override
	public int hashCode(){
		return 0; // bad hashCode but correct
	}

	@Override
	public String toString(){
		return String.format("x = %s\ty = %s\tgScore = %s\thScore = %s\tfScore = %s\t", this.x, this.y, this.gScore,this.hScore, this.fScore);
	}
}

public class AStar {
	public static void main(String[] args) {
		int[][]matrix = {{0,0,0,0,1,1,1,1},{0,0,0,0,0,1,1,1},{0,1,1,1,1,1,0,0},{0,0,0,0,0,0,0,0}};
		System.out.println(AStar(matrix,4,8));
		System.out.println(bfs(matrix,4,8));
	}
  
  
	/**
	 * From (0,0) to (m-1,n-1)
	 * @param matrix : 0 means empty and 1 means obstacle
	 */  
	//breadth-first search
	public static int bfs(int[][]matrix, int m, int n){
		Queue<Node> queue = new PriorityQueue<>();
		Set<Node> visited = new HashSet<>();
		Node initStatus = new Node(0,0);
		queue.offer(initStatus);
		visited.add(initStatus);
		int[][] adj = {{0,1},{0,-1},{1,0},{-1,0},{-1,-1},{1,-1},{-1,1},{1,1}};
		while(!queue.isEmpty()){
			Node parentNode = queue.poll();
			System.out.println(parentNode.toString());
			if(parentNode.x == m-1 && parentNode.y == n-1)
				return parentNode.gScore;

			for(int[]tmp: adj) {
				int x = parentNode.x + tmp[0];
				int y = parentNode.y + tmp[1];
				if (x < 0 || x >= m || y < 0 || y >= n || matrix[x][y] == 1)
					continue;
				Node curNode = new Node(x,y);
				curNode.setgScore(parentNode.gScore + 1); //Specific scenario
				if(!visited.contains(curNode)){
					visited.add(curNode);
					queue.offer(curNode);
				}
			}
		}
		return -1;
	}

	/**
	 * From (0,0) to (m-1,n-1)
	 * @param matrix : 0 means empty and 1 means obstacle
	 */
	public static int AStar(int[][]matrix, int m, int n){
		Map<Node,Integer> openList = new HashMap<>();
		// TreeMap does not work, For TreeMap ContainsKey using compareTo() not equals()
		// The deep reason is the implement of TreeMap is RBT
		Queue<Node> openQueue = new PriorityQueue<>();
		Map<Node,Integer> closeList = new HashMap<>();
		Node initStatus = new Node(0,0);
		openList.put(initStatus,0);
		openQueue.add(initStatus);
		int[][] adj = {{0,1},{0,-1},{1,0},{-1,0},{-1,-1},{1,-1},{-1,1},{1,1}};
		while(!openQueue.isEmpty()){
			Node parentNode = openQueue.poll();
			System.out.println(parentNode.toString());
			openList.remove(parentNode);
			if(parentNode.x == m-1 && parentNode.y == n-1)
				return parentNode.gScore;

			for(int[]tmp: adj){
				int x = parentNode.x + tmp[0];
				int y = parentNode.y + tmp[1];
				if(x < 0 || x >= m || y < 0 || y >= n || matrix[x][y] == 1)
					continue;
				Node curNode = new Node(x,y);
				int gScore = parentNode.gScore + 1; //Specific scenario
				int hScore = Math.abs(m-1-x) + Math.abs(n-1-y); //Manhattan distance
				int fScore = gScore + hScore;
				curNode.setgScore(gScore);
				curNode.sethScore(hScore);
				curNode.setfScore(fScore);
				curNode.setParent(parentNode);

				if(openList.containsKey(curNode) &&  openList.get(curNode) <= fScore)
					continue;
				if(closeList.containsKey(curNode) && closeList.get(curNode) <= fScore)
					continue;
//              //不需要remove，因为加入open之后，还是要再次进入close，更新新的fscore
//                if(closeList.containsKey(curNode)) {
//                    if (closeList.get(curNode) <= fScore)
//                        continue;
//                    else
//                        closeList.remove(curNode);
//                    // 最好remove，防止同时出现open，close
//                }
				openList.put(curNode,fScore);
				openQueue.add(curNode);
				closeList.put(parentNode,parentNode.fScore);
			}
		}
		return -1;
	}
}