Passed
Push — master ( 3f5d98...ee84e9 )
by Vincent
08:04
created

push(Step)   A

Complexity

Conditions 3

Size

Total Lines 11
Code Lines 7

Duplication

Lines 0
Ratio 0 %

Code Coverage

Tests 6
CRAP Score 3

Importance

Changes 0
Metric Value
cc 3
eloc 7
c 0
b 0
f 0
dl 0
loc 11
ccs 6
cts 6
cp 1
crap 3
rs 10
1
/*
2
 * This file is part of ArakneUtils.
3
 *
4
 * ArakneUtils is free software: you can redistribute it and/or modify
5
 * it under the terms of the GNU Lesser General Public License as published by
6
 * the Free Software Foundation, either version 3 of the License, or
7
 * (at your option) any later version.
8
 *
9
 * ArakneUtils is distributed in the hope that it will be useful,
10
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12
 * GNU Lesser General Public License for more details.
13
 *
14
 * You should have received a copy of the GNU Lesser General Public License
15
 * along with ArakneUtils.  If not, see <https://www.gnu.org/licenses/>.
16
 *
17
 * Copyright (c) 2017-2020 Vincent Quatrevieux
18
 */
19
20
package fr.arakne.utils.maps.path;
21
22
import fr.arakne.utils.maps.CoordinateCell;
23
import fr.arakne.utils.maps.MapCell;
24
import fr.arakne.utils.maps.constant.Direction;
25
26
import java.util.ArrayList;
27
import java.util.Collections;
28
import java.util.HashMap;
29
import java.util.HashSet;
30
import java.util.List;
31
import java.util.Map;
32
import java.util.Optional;
33
import java.util.PriorityQueue;
34
import java.util.Set;
35
import java.util.SortedSet;
36
import java.util.TreeSet;
37
import java.util.function.Function;
38
import java.util.function.Predicate;
39
40
/**
41
 * Find the shortest path between two cells
42
 *
43
 * <code>
44
 *     decoder
45
 *         .pathfinder()
46
 *         .targetDistance(2)
47
 *         .directions(Direction.values())
48
 *         .findPath(fighter.cell(), target)
49
 *     ;
50
 * </code>
51
 */
52
public final class Pathfinder<C extends MapCell> {
53
    private final Decoder<C> decoder;
54
55
    /**
56
     * Minimal target distance to consider the path as reached
57
     *
58
     * @see Pathfinder#targetDistance(int)
59
     * @see Automaton#hasReachTarget()
60
     */
61 1
    private int targetDistance = 0;
62
63
    /**
64
     * Predicate for check if the cell is walkable
65
     */
66 1
    private Predicate<C> walkablePredicated = C::walkable;
67
68
    /**
69
     * Function for compute the cell cost
70
     */
71 1
    private Function<C, Integer> cellWeightFunction = cell -> 1;
72
73
    /**
74
     * Available movements directions
75
     */
76 1
    private Direction[] directions = Direction.restrictedDirections();
77
78
    /**
79
     * Maximum number of explored cells
80
     * Allow to fail when finding too complex path
81
     */
82 1
    private int exploredCellLimit = Integer.MAX_VALUE;
83
84
    /**
85
     * Does the first cell (source) should be added to the path ?
86
     */
87 1
    private boolean addFirstCell = true;
88
89 1
    public Pathfinder(Decoder<C> decoder) {
90 1
        this.decoder = decoder;
91 1
    }
92
93
    /**
94
     * Define the minimal target distance to consider the path as reached
95
     *
96
     * A distance of 0 means that the end of the path must be the target cell
97
     * A distance of 1 means that the end of the path must be an adjacent cell of the target
98
     *
99
     * By default the value is 0 (the end must be the target)
100
     *
101
     * @param distance The distance in number of cells
102
     *
103
     * @return this instance
104
     */
105
    public Pathfinder<C> targetDistance(int distance) {
106 1
        this.targetDistance = distance;
107
108 1
        return this;
109
    }
110
111
    /**
112
     * Define the predicate for check if the cell is walkable
113
     *
114
     * By default the predicate will call {@link MapCell#walkable()}
115
     *
116
     * @param predicate The predicate to use
117
     *
118
     * @return this instance
119
     */
120
    public Pathfinder<C> walkablePredicate(Predicate<C> predicate) {
121 1
        this.walkablePredicated = predicate;
122
123 1
        return this;
124
    }
125
126
    /**
127
     * Define the function for compute the cell cost
128
     * This function can add penalty on some cells. For example on adjacent cell of an enemy.
129
     *
130
     * By default the function always returns 1
131
     *
132
     * @param function The function to use
133
     *
134
     * @return this instance
135
     */
136
    public Pathfinder<C> cellWeightFunction(Function<C, Integer> function) {
137 1
        this.cellWeightFunction = function;
138
139 1
        return this;
140
    }
141
142
    /**
143
     * Define the available movements directions
144
     *
145
     * Note : Adding all directions will permit to move in diagonal,
146
     *        But the pathfinder is not optimal, because the distance is not computed using pythagoras distance
147
     *
148
     * By default, the direction are the restricted directions
149
     *
150
     * @param directions Allowed directions
151
     *
152
     * @return this instance
153
     *
154
     * @see Direction#restricted()
155
     */
156
    public Pathfinder<C> directions(Direction[] directions) {
157 1
        this.directions = directions;
158
159 1
        return this;
160
    }
161
162
    /**
163
     * Define the maximum number of cells to explore before fail
164
     *
165
     * A lower limit will fail faster, but do not permit complex path
166
     * The limit cannot be higher than walkable cells number
167
     *
168
     * @param limit The cells number. Must be a positive integer
169
     *
170
     * @return this instance
171
     */
172
    public Pathfinder<C> exploredCellLimit(int limit) {
173 1
        this.exploredCellLimit = limit;
174
175 1
        return this;
176
    }
177
178
    /**
179
     * Does the first cell (source) should be added to the path ?
180
     * The source cell should be added by path generated by the server, but it's not added by the client.
181
     *
182
     * @param addFirstCell true to add the first cell (default), or false to disable
183
     *
184
     * @return this instance
185
     */
186
    public Pathfinder<C> addFirstCell(boolean addFirstCell) {
187 1
        this.addFirstCell = addFirstCell;
188
189 1
        return this;
190
    }
191
192
    /**
193
     * Find the shortest path between source and target cells
194
     *
195
     * @param source The source (start) cell
196
     * @param target The target (end) cell
197
     *
198
     * @return The path, including source
199
     *
200
     * @throws PathException When cannot found any valid path
201
     */
202
    public Path<C> findPath(C source, C target) {
203 1
        final Automaton automaton = new Automaton(source, target);
204
205 1
        while (!automaton.hasReachTarget()) {
206 1
            automaton.pushPossibleMovements();
207 1
            automaton.move();
208
        }
209
210 1
        return new Path<>(decoder, automaton.buildPath());
211
    }
212
213
    /**
214
     * Path finding state
215
     */
216
    private class Automaton {
217
        /**
218
         * The start cell of the path
219
         */
220
        private final CoordinateCell<C> source;
221
222
        /**
223
         * The end cell of the path
224
         */
225
        private final CoordinateCell<C> target;
226
227
        /**
228
         * The current step where automaton is located
229
         */
230
        private Step current;
231
232
        /**
233
         * List of possible movements, sorted by the heuristic (distance + cost)
234
         * The head of this list is the step with the lowest cost and distance
235
         *
236
         * This list is also known as "openList" on A* algorithm
237
         *
238
         * @see Step#heuristic()
239
         */
240 1
        private final PriorityQueue<Step> movements = new PriorityQueue<>();
241
242
        /**
243
         * Get all discovered steps for reach the cell used as key.
244
         * Those steps are sorted by their score as {@link Automaton#movements}.
245
         *
246
         * This map is used to ensure that a better step is not present on available movements
247
         * when pushing all possible movements.
248
         *
249
         * This map is always synchronized with {@link Automaton#movements} :
250
         * any writes must be applied on both structures.
251
         */
252 1
        private final Map<C, SortedSet<Step>> stepsByCurrentCell = new HashMap<>();
253
254
        /**
255
         * Set of already explored steps
256
         * This set ensure that the pathfinder will not returns to an already explored cell
257
         * which can cause infinity loops
258
         *
259
         * This set is also known as "closedList" on A* algorithm
260
         */
261 1
        private final Set<C> explored = new HashSet<>();
262
263 1
        public Automaton(C source, C target) {
264 1
            this.source = new CoordinateCell<>(source);
265 1
            this.target = new CoordinateCell<>(target);
266
267 1
            this.current = new Step(this.source);
268 1
            explored.add(source);
269 1
        }
270
271
        /**
272
         * Push all possible movements from the current step
273
         *
274
         * Will push all walkable adjacent cells, which are not yet explored
275
         * The possible moves depends of the possible directions
276
         */
277
        public void pushPossibleMovements() {
278 1
            for (Direction direction : directions) {
279 1
                nextCellByDirection(direction)
280 1
                    .filter(cell -> !explored.contains(cell))
281 1
                    .filter(walkablePredicated)
282 1
                    .map(cell -> current.next(cell, direction))
283 1
                    .ifPresent(this::push)
284
                ;
285
            }
286 1
        }
287
288
        /**
289
         * Move the automaton (i.e. change the current step) to the best step
290
         * The selected step cell is added to explored cells
291
         *
292
         * @throws PathException When cannot found any valid movements
293
         */
294
        public void move() {
295 1
            if (movements.isEmpty()) {
296 1
                throw new PathException("Cannot find any valid path between " + source.cell().id() + " and " + target.cell().id());
297
            }
298
299 1
            if (explored.size() > exploredCellLimit) {
300 1
                throw new PathException("Limit exceeded for finding path");
301
            }
302
303 1
            current = movements.poll();
304
305 1
            final C cell = current.cell.cell();
306
307 1
            stepsByCurrentCell.get(cell).remove(current);
308 1
            explored.add(cell);
309 1
        }
310
311
        /**
312
         * Check if the automaton has reach the target cell, or has reach the required minimal distance
313
         */
314
        public boolean hasReachTarget() {
315 1
            return current.distance <= targetDistance;
316
        }
317
318
        /**
319
         * Build the path from the current step
320
         */
321
        public List<PathStep<C>> buildPath() {
322 1
            final List<PathStep<C>> path = new ArrayList<>();
323
324
            // Build the path from the end
325 1
            for (Step step = current; step.previous != null; step = step.previous) {
326
                // Remove all steps after an unwalkable cell
327
                // Do not use the predicate, but the real walkable method,
328
                // to ensure that the real walkable state is used
329 1
                if (!step.cell.cell().walkable()) {
330 1
                    path.clear();
331 1
                    continue;
332
                }
333
334 1
                path.add(step.toPathStep());
335
            }
336
337
            // Always add the source cell even if not walkable
338 1
            if (addFirstCell) {
339 1
                path.add(new PathStep<>(source.cell(), Direction.EAST));
340
            }
341
342
            // The path is in reverse order (starts by the end)
343 1
            Collections.reverse(path);
344
345 1
            return path;
346
        }
347
348
        /**
349
         * Resolve the adjacent cell by a direction
350
         * This method can return a null object if the cell is out of the map
351
         */
352
        private Optional<C> nextCellByDirection(Direction direction) {
353 1
            return decoder.nextCellByDirection(current.cell.cell(), direction);
354
        }
355
356
        /**
357
         * Try to push a new step on possible movements
358
         * If a better step (i.e. with lower score) exists, the new step will be ignored
359
         *
360
         * @param step The new possible step to add
361
         */
362
        private void push(Step step) {
363 1
            final C cell = step.cell.cell();
364 1
            final SortedSet<Step> availableSteps = stepsByCurrentCell.computeIfAbsent(cell, c -> new TreeSet<>());
365
366
            // A step with a lower cost is found (do not compare distance because both steps have the same distance)
367 1
            if (!availableSteps.isEmpty() && availableSteps.first().cost <= step.cost) {
368 1
                return;
369
            }
370
371 1
            availableSteps.add(step);
372 1
            movements.add(step);
373 1
        }
374
375
        /**
376
         * Step of the path
377
         *
378
         * The state contains the previous step (for building path), distance to target and its cost
379
         */
380
        private class Step implements Comparable<Step> {
381
            /**
382
             * The step cell
383
             */
384
            private final CoordinateCell<C> cell;
385
386
            /**
387
             * Direction used for reach this step
388
             */
389
            private final Direction direction;
390
391
            /**
392
             * Previous step
393
             * Can be null in case of the first step (with cell = source)
394
             */
395
            private final Step previous;
396
397
            /**
398
             * Distance to the target cell, in number of cells
399
             */
400
            private final int distance;
401
402
            /**
403
             * Cost of the step
404
             *
405
             * The cost is the sum of the previous step cost and the cell cost
406
             * The step cost is used to discourage usage of some cells (ex: cells near enemies, which can tackle),
407
             * and to store the total path cost to reach this step
408
             */
409
            private final int cost;
410
411
            /**
412
             * Creates the source cell
413
             * The cost is set to zero, and without previous step
414
             *
415
             * @param cell The source cell
416
             */
417 1
            public Step(CoordinateCell<C> cell) {
418 1
                this.cell = cell;
419 1
                this.previous = null;
420 1
                this.direction = Direction.EAST;
421 1
                this.cost = 0;
422
423 1
                distance = cell.distance(target);
424 1
            }
425
426
            /**
427
             * Creates a new step, following the current step
428
             *
429
             * @param cell The step cell
430
             * @param previous The current step
431
             * @param cost The step cell cost. Should be 1 for normal cell
432
             */
433 1
            private Step(CoordinateCell<C> cell, Direction direction, Step previous, int cost) {
434 1
                this.cell = cell;
435 1
                this.direction = direction;
436 1
                this.previous = previous;
437 1
                this.cost = previous.cost + cost;
438
439 1
                distance = cell.distance(target);
440 1
            }
441
442
            /**
443
             * Creates the next step for a given cell
444
             *
445
             * @param cell The step cell
446
             * @param direction The direction use to reach the cell
447
             */
448
            public Step next(C cell, Direction direction) {
449 1
                return new Step(new CoordinateCell<>(cell), direction, this, cellWeightFunction.apply(cell));
450
            }
451
452
            /**
453
             * The step heuristic
454
             * Steps with lower heuristics are selected first
455
             *
456
             * The heuristic is the total steps costs (from the begin, to the current step) + the remaining distance
457
             */
458
            public int heuristic() {
459 1
                return distance + cost;
460
            }
461
462
            /**
463
             * Convert current pathfinder step to a PathStep
464
             */
465
            public PathStep<C> toPathStep() {
466 1
                return new PathStep<>(cell.cell(), direction);
467
            }
468
469
            @Override
470
            public int compareTo(Step o) {
471 1
                return heuristic() - o.heuristic();
472
            }
473
        }
474
    }
475
}
476