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