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
|
|
|
|