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