-
-
Notifications
You must be signed in to change notification settings - Fork 55
/
AStarMachine.java
173 lines (153 loc) · 5.6 KB
/
AStarMachine.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
package net.citizensnpcs.api.astar;
import java.util.Objects;
import java.util.function.Supplier;
public class AStarMachine<N extends AStarNode, P extends Plan> {
private Supplier<AStarStorage> storageSupplier;
private AStarMachine(Supplier<AStarStorage> storage) {
this.storageSupplier = storage;
}
private void f(AStarGoal<N> goal, N node, N neighbour) {
float g = node.g + goal.g(node, neighbour); // calculate the cost from the start additively
float h = goal.h(neighbour);
neighbour.g = g;
neighbour.h = h;
}
private AStarStorage getInitialisedStorage(AStarGoal<N> goal, N start) {
AStarStorage storage = storageSupplier.get();
storage.open(start);
start.g = goal.getInitialCost(start);
start.h = 0;
return storage;
}
/**
* Creates an {@link AStarState} that can be reused across multiple invocations of {{@link #run(AStarState, int)}.
*
* @see #run(AStarState, int)
* @param goal
* The {@link AStarGoal} state
* @param start
* The starting {@link AStarNode}
* @return The created state
*/
public AStarState getStateFor(AStarGoal<N> goal, N start) {
return new AStarState(goal, start, getInitialisedStorage(goal, start));
}
/**
* Runs the {@link AStarState} until a plan is found.
*
* @see #run(AStarState)
* @param state
* The state to use
* @return The generated {@link Plan}, or <code>null</code>
*/
public P run(AStarState state) {
return run(state, -1);
}
/**
* Runs the machine using the given {@link AStarState}'s {@link AStarStorage}. Can be used to provide a continuation
* style usage of the A* algorithm.
*
* @param state
* The state to use
* @param maxIterations
* The maximum number of iterations
* @return The generated {@link Plan}, or <code>null</code> if not found
*/
public P run(AStarState state, int maxIterations) {
return run(state.storage, state.goal, state.start, maxIterations);
}
@SuppressWarnings("unchecked")
private P run(AStarStorage storage, AStarGoal<N> goal, N start, int maxIterations) {
Objects.requireNonNull(goal);
Objects.requireNonNull(start);
Objects.requireNonNull(storage);
N node;
int iterations = 0;
while (true) {
node = (N) storage.removeBestNode();
if (node == null)
return null;
if (goal.isFinished(node))
return (P) node.buildPlan();
storage.close(node);
for (AStarNode neighbour : node.getNeighbours()) {
f(goal, node, (N) neighbour);
if (!storage.shouldExamine(neighbour))
continue;
storage.open(neighbour);
neighbour.parent = node;
}
if (maxIterations >= 0 && iterations++ >= maxIterations)
return null;
}
}
/**
* Runs the machine until a plan is either found or cannot be generated.
*
* @see #runFully(AStarGoal, AStarNode, int)
*/
public P runFully(AStarGoal<N> goal, N start) {
return runFully(goal, start, -1);
}
/**
* Runs the machine fully until the iteration limit has been exceeded. This will use the supplied goal and start to
* generate neighbours until the goal state has been reached using the A* algorithm.
*
* @param goal
* The {@link AStarGoal} state
* @param start
* The starting {@link AStarNode}
* @param iterations
* The number of iterations to run the machine for
* @return The generated {@link Plan}, or <code>null</code> if it was not found
*/
public P runFully(AStarGoal<N> goal, N start, int iterations) {
return run(getInitialisedStorage(goal, start), goal, start, iterations);
}
/**
* Sets the {@link Supplier} to use to generate instances of {@link AStarStorage} for use while searching.
*
* @param newSupplier
* The new supplier to use
*/
public void setStorageSupplier(Supplier<AStarStorage> newSupplier) {
storageSupplier = newSupplier;
}
public class AStarState {
private final AStarGoal<N> goal;
private final N start;
private final AStarStorage storage;
private AStarState(AStarGoal<N> goal, N start, AStarStorage storage) {
this.goal = goal;
this.start = start;
this.storage = storage;
}
@SuppressWarnings("unchecked")
public N getBestNode() {
return (N) storage.getBestNode();
}
public boolean isEmpty() {
return storage.getBestNode() == null;
}
}
/**
* Creates an AStarMachine using {@link SimpleAStarStorage} as the storage backend.
*
* @return The created instance
*/
public static <N extends AStarNode, P extends Plan> AStarMachine<N, P> createWithDefaultStorage() {
return createWithStorage(SimpleAStarStorage.FACTORY);
}
/**
* Creates an AStarMachine that uses the given {@link Supplier <AStarStorage>} to create {@link AStarStorage}
* instances.
*
* @param storageSupplier
* The storage supplier
* @return The created instance
*/
public static <N extends AStarNode, P extends Plan> AStarMachine<N, P> createWithStorage(
Supplier<AStarStorage> storageSupplier) {
return new AStarMachine<>(storageSupplier);
}
}