001package squidpony.squidai;
002
003import squidpony.squidgrid.Direction;
004import squidpony.squidgrid.Measurement;
005import squidpony.squidgrid.Radius;
006import squidpony.squidgrid.mapping.DungeonUtility;
007import squidpony.squidmath.*;
008
009import java.util.ArrayList;
010import java.util.Collections;
011import java.util.Map;
012
013/**
014 * Pathfind to known connections between rooms or other "chokepoints" without needing full-map Dijkstra scans.
015 * Pre-calculates a path either from or to any given chokepoint to each other chokepoint.
016 * Created by Tommy Ettinger on 10/25/2015.
017 */
018public class WaypointPathfinder {
019    private int width;
020    private int height;
021    private DijkstraMap dm;
022    private int[][] expansionMap;
023    public StatefulRNG rng;
024    private OrderedMap<Coord, OrderedMap<Coord, Edge>> waypoints;
025
026    /**
027     * Calculates and stores the doors and doors-like connections ("chokepoints") on the given map as waypoints.
028     * Will use the given Radius enum to determine how to handle DijkstraMap measurement in future pathfinding.
029     * Uses a new RNG for all random choices, which will be seeded with {@code rng.nextLong()}, or unseeded if
030     * the parameter is null.
031     * @param map a char[][] that stores a "complete" dungeon map, with any chars as features that pathfinding needs.
032     * @param radius a Radius that should correspond to how you want path distance calculated.
033     * @param rng an RNG object or null (this will always use a new RNG, but it may be seeded by a given RNG's next result)
034     */
035    public WaypointPathfinder(char[][] map, Radius radius, IRNG rng)
036    {
037        if(rng == null)
038            this.rng = new StatefulRNG();
039        else
040            this.rng = new StatefulRNG(rng.nextLong());
041        width = map.length;
042        height = map[0].length;
043        char[][] simplified = DungeonUtility.simplifyDungeon(map);
044        OrderedSet<Coord> centers = PoissonDisk.sampleMap(simplified,
045                Math.min(width, height) * 0.4f, this.rng, '#');
046        int centerCount = centers.size();
047        expansionMap = new int[width][height];
048        waypoints = new OrderedMap<>(64);
049        dm = new DijkstraMap(simplified, Measurement.MANHATTAN);
050
051        for (Coord center : centers) {
052            dm.clearGoals();
053            dm.resetMap();
054            dm.setGoal(center);
055            dm.scan(null, null);
056            double current;
057            for (int i = 0; i < width; i++) {
058                for (int j = 0; j < height; j++) {
059                    current = dm.gradientMap[i][j];
060                    if (current >= DijkstraMap.FLOOR)
061                        continue;
062                    if (center.x == i && center.y == j)
063                        expansionMap[i][j]++;
064                    for (Direction dir : Direction.CARDINALS) {
065                        if (dm.gradientMap[i + dir.deltaX][j + dir.deltaY] == current + 1 ||
066                                dm.gradientMap[i + dir.deltaX][j + dir.deltaY] == current - 1)
067                            expansionMap[i][j]++;
068                    }
069                }
070            }
071        }
072
073        for (int i = 0; i < width; i++) {
074            for (int j = 0; j < height; j++) {
075                expansionMap[i][j] /= centerCount;
076            }
077        }
078
079        OrderedSet<Coord> chokes = new OrderedSet<>(128);
080        for (int i = 0; i < width; i++) {
081            ELEMENT_WISE:
082            for (int j = 0; j < height; j++) {
083                if(expansionMap[i][j] <= 0)
084                    continue;
085                int current = expansionMap[i][j];
086                boolean good = false;
087                for(Direction dir : Direction.CARDINALS) {
088                    if (chokes.contains(Coord.get(i + dir.deltaX, j + dir.deltaY)))
089                        continue ELEMENT_WISE;
090                    if (expansionMap[i + dir.deltaX][j + dir.deltaY] > 0 && expansionMap[i + dir.deltaX][j + dir.deltaY] > current + 1 ||
091                            (expansionMap[i + dir.deltaX][j + dir.deltaY] > current && expansionMap[i][j] <= 2)) {
092                        if (expansionMap[i - dir.deltaX][j - dir.deltaY] > 0 && expansionMap[i - dir.deltaX][j - dir.deltaY] >= current) {
093                            good = true;
094                        }
095                    }
096                }
097
098                if(good) {
099                    Coord chk = Coord.get(i, j);
100                    chokes.add(chk);
101                    waypoints.put(chk, new OrderedMap<Coord, Edge>());
102                }
103            }
104        }
105        
106        dm = new DijkstraMap(map, radius.matchingMeasurement());
107
108        for(Map.Entry<Coord, OrderedMap<Coord, Edge>> n : waypoints.entrySet())
109        {
110            chokes.remove(n.getKey());
111            if(chokes.isEmpty())
112                break;
113            dm.clearGoals();
114            dm.resetMap();
115            dm.setGoal(n.getKey());
116            dm.scan(null, null);
117            for(Coord c : chokes)
118            {
119                n.getValue().put(c, new Edge(n.getKey(), c, dm.findPathPreScanned(c), dm.gradientMap[c.x][c.y]));
120            }
121        }
122
123    }
124    /**
125     * Calculates and stores the doors and doors-like connections ("chokepoints") on the given map as waypoints.
126     * Will use the given Radius enum to determine how to handle DijkstraMap measurement in future pathfinding.
127     * Uses a new RNG for all random choices, which will be seeded with {@code rng.nextLong()}, or unseeded if
128     * the parameter is null.
129     * @param map a char[][] that stores a "complete" dungeon map, with any chars as features that pathfinding needs.
130     * @param radius a Radius that should correspond to how you want path distance calculated.
131     * @param rng an RNG object or null (this will always use a new RNG, but it may be seeded by a given RNG's next result)
132     * @param thickCorridors true if most chokepoints on the map are 2 cells wide instead of 1
133     */
134    public WaypointPathfinder(char[][] map, Radius radius, IRNG rng, boolean thickCorridors)
135    {
136        if(rng == null)
137            this.rng = new StatefulRNG();
138        else
139            this.rng = new StatefulRNG(rng.nextLong());
140        width = map.length;
141        height = map[0].length;
142        char[][] simplified = DungeonUtility.simplifyDungeon(map);
143        expansionMap = new int[width][height];
144        waypoints = new OrderedMap<>(64);
145        OrderedSet<Coord> chokes = new OrderedSet<>(128);
146
147        if(thickCorridors)
148        {
149            GreasedRegion floors = new GreasedRegion(simplified, '.'),
150                    rooms = floors.copy().retract8way().expand().and(floors).expand().and(floors);
151                    rooms.and(floors.copy().andNot(rooms).fringe()).disperse8way();
152                    chokes.addAll(rooms);
153            for (int i = 0; i < rooms.size(); i++) {
154                waypoints.put(rooms.nth(i), new OrderedMap<Coord, Edge>());
155            }
156        }
157        else {
158            OrderedSet<Coord> centers = PoissonDisk.sampleMap(simplified,
159                    Math.min(width, height) * 0.4f, this.rng, '#');
160            int centerCount = centers.size();
161            dm = new DijkstraMap(simplified, Measurement.MANHATTAN);
162
163            for (Coord center : centers) {
164                dm.clearGoals();
165                dm.resetMap();
166                dm.setGoal(center);
167                dm.scan(null, null);
168                double current;
169                for (int i = 0; i < width; i++) {
170                    for (int j = 0; j < height; j++) {
171                        current = dm.gradientMap[i][j];
172                        if (current >= DijkstraMap.FLOOR)
173                            continue;
174                        if (center.x == i && center.y == j)
175                            expansionMap[i][j]++;
176                        for (Direction dir : Direction.CARDINALS) {
177                            if (dm.gradientMap[i + dir.deltaX][j + dir.deltaY] == current + 1 ||
178                                    dm.gradientMap[i + dir.deltaX][j + dir.deltaY] == current - 1)
179                                expansionMap[i][j]++;
180                        }
181                    }
182                }
183            }
184
185            for (int i = 0; i < width; i++) {
186                for (int j = 0; j < height; j++) {
187                    expansionMap[i][j] /= centerCount;
188                }
189            }
190
191            for (int i = 0; i < width; i++) {
192                ELEMENT_WISE:
193                for (int j = 0; j < height; j++) {
194                    if (expansionMap[i][j] <= 0)
195                        continue;
196                    int current = expansionMap[i][j];
197                    boolean good = false;
198                    for (Direction dir : Direction.CARDINALS) {
199                        if (chokes.contains(Coord.get(i + dir.deltaX, j + dir.deltaY)))
200                            continue ELEMENT_WISE;
201                        if (expansionMap[i + dir.deltaX][j + dir.deltaY] > 0 && expansionMap[i + dir.deltaX][j + dir.deltaY] > current + 1 ||
202                                (expansionMap[i + dir.deltaX][j + dir.deltaY] > current && expansionMap[i][j] <= 2)) {
203                            if (expansionMap[i - dir.deltaX][j - dir.deltaY] > 0 && expansionMap[i - dir.deltaX][j - dir.deltaY] >= current) {
204                                good = true;
205                            }
206                        }
207                    }
208
209                    if (good) {
210                        Coord chk = Coord.get(i, j);
211                        chokes.add(chk);
212                        waypoints.put(chk, new OrderedMap<Coord, Edge>());
213                    }
214                }
215            }
216        }
217
218        dm = new DijkstraMap(map, radius.matchingMeasurement());
219
220        int e = 0;
221        for(Map.Entry<Coord, OrderedMap<Coord, Edge>> n : waypoints.entrySet())
222        {
223            chokes.remove(n.getKey());
224            if(chokes.isEmpty())
225                break;
226            dm.clearGoals();
227            dm.resetMap();
228            dm.setGoal(n.getKey());
229            dm.scan(null, null);
230            for(Coord c : chokes)
231            {
232                n.getValue().put(c, new Edge(n.getKey(), c, dm.findPathPreScanned(c), dm.gradientMap[c.x][c.y]));
233            }
234        }
235
236    }
237
238    /**
239     * Calculates and stores the specified fraction of walkable points from map as waypoints. Does not perform any
240     * analysis of chokepoints and acts as a more brute-force solution when maps may be unpredictable. The lack of an
241     * analysis step may mean this could have drastically less of a penalty to startup time than the other constructors,
242     * and with the right fraction parameter (29 seems ideal), may perform better as well. Will use the given Radius
243     * enum to determine how to handle DijkstraMap measurement in future pathfinding.
244     * Uses a new RNG for all random choices, which will be seeded with {@code rng.nextLong()}, or unseeded if
245     * the parameter is null.
246     * <br>
247     * Remember, a fraction value of 29 works well!
248     * @param map a char[][] that stores a "complete" dungeon map, with any chars as features that pathfinding needs.
249     * @param radius a Radius that should correspond to how you want path distance calculated.
250     * @param rng an RNG object or null (this will always use a new RNG, but it may be seeded by a given RNG's next result)
251     * @param fraction the fractional denominator of passable cells to assign as waypoints; use 29 if you aren't sure
252     */
253    public WaypointPathfinder(char[][] map, Radius radius, IRNG rng, int fraction)
254    {
255        if(rng == null)
256            this.rng = new StatefulRNG();
257        else
258            this.rng = new StatefulRNG(rng.nextLong());
259        width = map.length;
260        height = map[0].length;
261        char[][] simplified = DungeonUtility.simplifyDungeon(map);
262        expansionMap = new int[width][height];
263        waypoints = new OrderedMap<>(64);
264        OrderedSet<Coord> chokes = new OrderedSet<>(128);
265
266        Coord[] apart = new GreasedRegion(simplified, '.').quasiRandomSeparated(1.0 / fraction);
267        Collections.addAll(chokes, apart);
268        for (int i = 0; i < apart.length; i++) {
269            waypoints.put(apart[i], new OrderedMap<Coord, Edge>());
270        }
271
272        dm = new DijkstraMap(map, radius.matchingMeasurement());
273
274        int e = 0;
275        for(Map.Entry<Coord, OrderedMap<Coord, Edge>> n : waypoints.entrySet())
276        {
277            chokes.remove(n.getKey());
278            if(chokes.isEmpty())
279                break;
280            dm.clearGoals();
281            dm.resetMap();
282            dm.setGoal(n.getKey());
283            dm.scan(null, null);
284            for(Coord c : chokes)
285            {
286                n.getValue().put(c, new Edge(n.getKey(), c, dm.findPathPreScanned(c), dm.gradientMap[c.x][c.y]));
287            }
288        }
289
290    }
291
292    /**
293     * Calculates and stores the doors and doors-like connections ("chokepoints") on the given map as waypoints.
294     * Will use the given DijkstraMap for pathfinding after construction (and during some initial calculations).
295     * The dijkstra parameter will be mutated by this class, so it should not be reused elsewhere.
296     * Uses a new RNG for all random choices, which will be seeded with {@code rng.nextLong()}, or unseeded if
297     * the parameter is null.
298     * @param map a char[][] that stores a "complete" dungeon map, with any chars as features that pathfinding needs
299     * @param dijkstra a DijkstraMap that will be used to find paths; may have costs but they will not be used
300     * @param rng an RNG object or null (this will always use a new RNG, but it may be seeded by a given RNG's next result)
301     */
302    public WaypointPathfinder(char[][] map, DijkstraMap dijkstra, IRNG rng)
303    {
304        if(rng == null)
305            this.rng = new StatefulRNG();
306        else
307            this.rng = new StatefulRNG(rng.nextLong());
308        width = map.length;
309        height = map[0].length;
310        char[][] simplified = DungeonUtility.simplifyDungeon(map);
311        OrderedSet<Coord> centers = PoissonDisk.sampleMap(simplified,
312                Math.min(width, height) * 0.4f, this.rng, '#');
313        int centerCount = centers.size();
314        expansionMap = new int[width][height];
315        waypoints = new OrderedMap<>(64);
316        dm = new DijkstraMap(simplified, Measurement.MANHATTAN);
317
318        for (Coord center : centers) {
319            dm.clearGoals();
320            dm.resetMap();
321            dm.setGoal(center);
322            dm.scan(null, null);
323            double current;
324            for (int i = 0; i < width; i++) {
325                for (int j = 0; j < height; j++) {
326                    current = dm.gradientMap[i][j];
327                    if (current >= DijkstraMap.FLOOR)
328                        continue;
329                    if (center.x == i && center.y == j)
330                        expansionMap[i][j]++;
331                    for (Direction dir : Direction.CARDINALS) {
332                        if (dm.gradientMap[i + dir.deltaX][j + dir.deltaY] == current + 1 ||
333                                dm.gradientMap[i + dir.deltaX][j + dir.deltaY] == current - 1)
334                            expansionMap[i][j]++;
335                    }
336                }
337            }
338        }
339
340        for (int i = 0; i < width; i++) {
341            for (int j = 0; j < height; j++) {
342                expansionMap[i][j] /= centerCount;
343            }
344        }
345
346        OrderedSet<Coord> chokes = new OrderedSet<>(128);
347        for (int i = 0; i < width; i++) {
348            ELEMENT_WISE:
349            for (int j = 0; j < height; j++) {
350                if(expansionMap[i][j] <= 0)
351                    continue;
352                int current = expansionMap[i][j];
353                boolean good = false;
354                for(Direction dir : Direction.CARDINALS) {
355                    if (chokes.contains(Coord.get(i + dir.deltaX, j + dir.deltaY)))
356                        continue ELEMENT_WISE;
357                    if (expansionMap[i + dir.deltaX][j + dir.deltaY] > 0 && expansionMap[i + dir.deltaX][j + dir.deltaY] > current + 1 ||
358                            (expansionMap[i + dir.deltaX][j + dir.deltaY] > current && expansionMap[i][j] <= 2)) {
359                        if (expansionMap[i - dir.deltaX][j - dir.deltaY] > 0 && expansionMap[i - dir.deltaX][j - dir.deltaY] >= current) {
360                            good = true;
361                        }
362                    }
363                }
364                if(good) {
365                    Coord chk = Coord.get(i, j);
366                    chokes.add(chk);
367                    waypoints.put(chk, new OrderedMap<Coord, Edge>());
368                }
369            }
370        }
371        dm = dijkstra;
372        int e = 0;
373        for(Map.Entry<Coord, OrderedMap<Coord, Edge>> n : waypoints.entrySet())
374        {
375            chokes.remove(n.getKey());
376            if(chokes.isEmpty())
377                break;
378            dm.clearGoals();
379            dm.resetMap();
380            dm.setGoal(n.getKey());
381            dm.scan(null, null);
382            for(Coord c : chokes)
383            {
384                n.getValue().put(c, new Edge(n.getKey(), c, dm.findPathPreScanned(c), dm.gradientMap[c.x][c.y]));
385            }
386        }
387
388    }
389
390    /**
391     * Finds the appropriate one of the already-calculated, possibly-long paths this class stores to get from a waypoint
392     * to another waypoint, then quickly finds a path to get on the long path, and returns the total path. This does
393     * not need to perform any full-map scans with DijkstraMap.
394     * @param self the pathfinder's position
395     * @param approximateTarget the Coord that represents the approximate area to pathfind to; will be randomized if
396     *                          it is not walkable.
397     * @return an ArrayList of Coord that will go from a cell adjacent to self to a waypoint near approximateTarget
398     */
399    public ArrayList<Coord> getKnownPath(Coord self, Coord approximateTarget) {
400        ArrayList<Coord> near = dm.findNearestMultiple(approximateTarget, 5, waypoints.keySet());
401        Coord me = dm.findNearest(self, waypoints.keySet());
402        double bestCost = 999999.0;
403        ArrayList<Coord> path = new ArrayList<>();
404        /*if (waypoints.containsKey(me)) {
405            Edge[] ed = waypoints.get(me).values().toArray(new Edge[waypoints.get(me).size()]);
406            Arrays.sort(ed);
407            path = ed[0].path;
408        */
409        boolean reversed = false;
410        for (Coord test : near) {
411            if (waypoints.containsKey(test)) {
412                Edge ed;
413                if(waypoints.get(test).containsKey(me)) {
414                    ed = waypoints.get(test).get(me);
415                    reversed = true;
416                }
417                else if(waypoints.containsKey(me) && waypoints.get(me).containsKey(test))
418                    ed = waypoints.get(me).get(test);
419                else
420                    continue;
421                if (ed.cost < bestCost) {
422                    bestCost = ed.cost;
423                    path = new ArrayList<>(ed.path);
424                }
425            }
426        }
427        if(path.isEmpty())
428            return path;
429        if(reversed)
430            Collections.reverse(path);
431        ArrayList<Coord> getToPath = dm.findShortcutPath(self, path.toArray(new Coord[0]));
432        if (getToPath.size() > 0)
433        {
434            getToPath.remove(getToPath.size() - 1);
435            getToPath.addAll(path);
436            path = getToPath;
437        }
438        return path;
439    }
440
441    /**
442     * If a creature is interrupted or obstructed on a "highway" path, it may need to travel off the path to its goal.
443     * This method gets a straight-line path back to the path to goal. It does not contain the "highway" path, only the
444     * "on-ramp" to enter the ideal path.
445     * @param currentPosition the current position of the pathfinder, which is probably not on the ideal path
446     * @param path the ideal path, probably returned by getKnownPath
447     * @return an ArrayList of Coord that go from a cell adjacent to currentPosition to a Coord on or adjacent to path.
448     */
449    public ArrayList<Coord> goBackToPath(Coord currentPosition, ArrayList<Coord> path)
450    {
451        return dm.findShortcutPath(currentPosition, path.toArray(new Coord[0]));
452    }
453
454    public OrderedSet<Coord> getWaypoints()
455    {
456        return waypoints.keysAsOrderedSet();
457    }
458
459    private static class Edge implements Comparable<Edge>
460    {
461        public Coord from;
462        public Coord to;
463        public ArrayList<Coord> path;
464        public double cost;
465        public Edge(Coord from, Coord to, ArrayList<Coord> path, double cost)
466        {
467            this.from = from;
468            this.to = to;
469            this.path = path;
470            this.cost = cost;
471        }
472
473        @Override
474        public boolean equals(Object o) {
475            if (this == o) return true;
476            if (o == null || getClass() != o.getClass()) return false;
477
478            Edge edge = (Edge) o;
479
480            if (Double.compare(edge.cost, cost) != 0) return false;
481            if (!from.equals(edge.from)) return false;
482            return to.equals(edge.to);
483
484        }
485
486        @Override
487        public int hashCode() {
488            int result;
489            long temp;
490            result = from.hashCode();
491            result = 31 * result + to.hashCode();
492            temp = NumberTools.doubleToLongBits(cost);
493            result = 31 * result + (int) (temp ^ (temp >>> 32));
494            return result;
495        }
496
497        /**
498         * Compares this object with the specified object for order.  Returns a
499         * negative integer, zero, or a positive integer as this object is less
500         * than, equal to, or greater than the specified object.
501         *
502         * Note: this class has a natural ordering that is
503         * inconsistent with equals.
504         * @param o the object to be compared.
505         * @return a negative integer, zero, or a positive integer as this object
506         * is less than, equal to, or greater than the specified object.
507         * @throws NullPointerException if the specified object is null
508         * @throws ClassCastException   if the specified object's type prevents it
509         *                              from being compared to this object.
510         */
511        @Override
512        public int compareTo(Edge o) {
513            return (cost - o.cost > 0) ? 1 : (cost - o.cost < 0) ? -1 : 0;
514        }
515    }
516
517}