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}