001package squidpony.squidmath; 002 003 004import squidpony.squidgrid.Direction; 005 006import java.io.Serializable; 007import java.util.HashSet; 008import java.util.LinkedList; 009import java.util.Queue; 010 011/** 012 * Performs A* search. 013 * 014 * A* is a best-first search algorithm for pathfinding. It uses a heuristic 015 * value to reduce the total search space. If the heuristic is too large then 016 * the optimal path is not guaranteed to be returned. 017 * <br> 018 * NOTE: Due to implementation problems, this class is atypically slow for A* and 34x slower than DijkstraMap on the 019 * same inputs. It may be improved in future versions, but for now you should strongly prefer DijkstraMap. 020 * @see squidpony.squidai.DijkstraMap a faster pathfinding algorithm with more features. 021 * @author Eben Howard - http://squidpony.com - howard@squidpony.com 022 */ 023public class AStarSearch implements Serializable{ 024 private static final long serialVersionUID = -6315495888417856297L; 025 /** 026 * The type of heuristic to use. 027 */ 028 public enum SearchType { 029 030 /** 031 * The distance it takes when only the four primary directions can be 032 * moved in. 033 */ 034 MANHATTAN, 035 /** 036 * The distance it takes when diagonal movement costs the same as 037 * cardinal movement. 038 */ 039 CHEBYSHEV, 040 /** 041 * The distance it takes as the crow flies. 042 */ 043 EUCLIDEAN, 044 /** 045 * Full space search. Least efficient but guaranteed to return a path if 046 * one exists. See also DijkstraMap class. 047 */ 048 DIJKSTRA 049 } 050 051 protected final double[][] map; 052 protected final HashSet<Coord> open = new HashSet<>(); 053 protected final int width, height; 054 protected Coord[][] parent; 055 protected Coord start, target; 056 protected final SearchType type; 057 058 protected AStarSearch() 059 { 060 width = 0; 061 height = 0; 062 type = SearchType.MANHATTAN; 063 map = new double[width][height]; 064 } 065 /** 066 * Builds a pathing object to run searches on. 067 * 068 * Values in the map are treated as positive values (and 0) being legal 069 * weights, with higher values being harder to pass through. Any negative 070 * value is treated as being an impassible space. 071 * 072 * If the type is Manhattan, only the cardinal directions will be used. All 073 * other search types will return results based on intercardinal and 074 * cardinal pathing. 075 * 076 * @param map 077 * the search map. It is not modified by this class, hence you can 078 * share this map among multiple instances. 079 * @param type the manner of search 080 */ 081 public AStarSearch(double[][] map, SearchType type) { 082 if (map == null) 083 throw new NullPointerException("map should not be null when building an AStarSearch"); 084 this.map = map; 085 width = map.length; 086 height = width == 0 ? 0 : map[0].length; 087 if (type == null) 088 throw new NullPointerException("SearchType should not be null when building an AStarSearch"); 089 this.type = type; 090 } 091 092 /** 093 * Finds an A* path to the target from the start. If no path is possible, 094 * returns null. 095 * 096 * @param startx the x coordinate of the start location 097 * @param starty the y coordinate of the start location 098 * @param targetx the x coordinate of the target location 099 * @param targety the y coordinate of the target location 100 * @return the shortest path, or null 101 */ 102 public Queue<Coord> path(int startx, int starty, int targetx, int targety) { 103 return path(Coord.get(startx, starty), Coord.get(targetx, targety)); 104 } 105 /** 106 * Finds an A* path to the target from the start. If no path is possible, 107 * returns null. 108 * 109 * @param start the start location 110 * @param target the target location 111 * @return the shortest path, or null 112 */ 113 public Queue<Coord> path(Coord start, Coord target) { 114 this.start = start; 115 this.target = target; 116 open.clear(); 117 boolean[][] finished = new boolean[width][height]; 118 parent = new Coord[width][height]; 119 120 Direction[] dirs; 121 switch (type) { 122 case MANHATTAN: 123 dirs = Direction.CARDINALS; 124 break; 125 case CHEBYSHEV: 126 case EUCLIDEAN: 127 case DIJKSTRA: 128 default: 129 dirs = Direction.OUTWARDS; 130 break; 131 } 132 133 Coord p = start; 134 open.add(p); 135 136 while (!p.equals(target)) { 137 finished[p.x][p.y] = true; 138 open.remove(p); 139 for (Direction dir : dirs) { 140 141 int x = p.x + dir.deltaX; 142 if (x < 0 || x >= width) { 143 continue;//out of bounds so skip ahead 144 } 145 146 int y = p.y + dir.deltaY; 147 if (y < 0 || y >= height) { 148 continue;//out of bounds so skip ahead 149 } 150 151 if (!finished[x][y]) { 152 Coord test = Coord.get(x, y); 153 if (open.contains(test)) { 154 double parentG = g(parent[x][y].x, parent[x][y].y); 155 if (parentG < 0) { 156 continue;//not a valid point so skip ahead 157 } 158 double g = g(p.x, p.y); 159 if (g < 0) { 160 continue;//not a valid point so skip ahead 161 } 162 if (parent[x][y] == null || parentG > g) { 163 parent[x][y] = p; 164 } 165 } else { 166 open.add(test); 167 parent[x][y] = p; 168 } 169 } 170 } 171 p = smallestF(); 172 if (p == null) { 173 return null;//no path possible 174 } 175 } 176 177 /* Not using Deque nor ArrayDeque, they aren't Gwt compatible */ 178 final LinkedList<Coord> deq = new LinkedList<>(); 179 while (!p.equals(start)) { 180 deq.addFirst(p); 181 p = parent[p.x][p.y]; 182 } 183 return deq; 184 } 185 186 /** 187 * Finds the g value (start to current) for the given location. 188 * 189 * If the given location is not valid or not attached to the pathfinding 190 * then -1 is returned. 191 * 192 * @param x coordinate 193 * @param y coordinate 194 */ 195 protected double g(int x, int y) { 196 if (x == start.x && y == start.y) { 197 return 0; 198 } 199 if (x < 0 || y < 0 || x >= width || y >= height || map[x][y] < 0 || parent[x][y] == null) { 200 return -1;//not a valid location 201 } 202 203 double parentG = g(parent[x][y].x, parent[x][y].y); 204 if (parentG < 0) { 205 return -1;//if any part of the path is not valid, this part is not valid 206 } 207 208 return map[x][y] + parentG + 1;//follow path back to start 209 } 210 211 /** 212 * Returns the heuristic distance from the current cell to the goal location\ 213 * using the current calculation type. 214 * 215 * @param x coordinate 216 * @param y coordinate 217 * @return distance 218 */ 219 protected double h(int x, int y) { 220 switch (type) { 221 case MANHATTAN: 222 return Math.abs(x - target.x) + Math.abs(y - target.y); 223 case CHEBYSHEV: 224 return Math.max(Math.abs(x - target.x), Math.abs(y - target.y)); 225 case EUCLIDEAN: 226 int xDist = Math.abs(x - target.x); 227 xDist *= xDist; 228 int yDist = Math.abs(y - target.y); 229 yDist *= yDist; 230 return Math.sqrt(xDist + yDist); 231 case DIJKSTRA: 232 default: 233 return 0; 234 235 } 236 } 237 238 /** 239 * Combines g and h to get the estimated distance from start to goal going on the current route. 240 * @param x coordinate 241 * @param y coordinate 242 * @return The current known shortest distance to the start position from 243 * the given position. If the current position cannot reach the 244 * start position or is invalid, -1 is returned. 245 */ 246 protected double f(int x, int y) { 247 double foundG = g(x, y); 248 if (foundG < 0) { 249 return -1; 250 } 251 return h(x, y) + foundG; 252 } 253 254 /** 255 * @return the current open point with the smallest F 256 */ 257 protected Coord smallestF() { 258 Coord smallest = null; 259 double smallF = Double.POSITIVE_INFINITY; 260 double f; 261 for (Coord p : open) { 262 f = f(p.x, p.y); 263 if (f < 0) { 264 continue;//current tested point is not valid so skip it 265 } 266 if (smallest == null || f < smallF) { 267 smallest = p; 268 smallF = f; 269 } 270 } 271 272 return smallest; 273 } 274}