anansi.algorithms.astar source code

1 /**
2  * Definitions for running an A* search over a graph
3  */
4 module anansi.algorithms.astar;
5 
6 import std.traits;
7 import std.exception;
8 
9 import anansi.algorithms.bfs;
10 import anansi.algorithms.relax : relax;
11 import anansi.algorithms.vertex_queue : VertexQueue;
12 import anansi.types;
13 import anansi.traits;
14 
15 version(unittest) {
16     import std.stdio;
17     import anansi.adjacencylist;
18 }
19 
20 template isAStarHeuristic (CallableT, VertexT) {
21     enum bool isAStarHeuristic = is(typeof(
22     (ref CallableT fn, VertexT v) {
23         real x = fn(v);
24     }));
25 }
26 
27 public template NullAstarVisitor(GraphT) {
28     alias Vertex = GraphT.VertexDescriptor;
29     alias Edge = GraphT.EdgeDescriptor;
30 
31     void initVertex(ref const(GraphT) g, Vertex v) {}
32     void discoverVertex(ref const(GraphT) g, Vertex v) {}
33     void examineVertex(ref const(GraphT) g, Vertex v) {}
34     void examineEdge(ref const(GraphT) g, Edge e) {}
35     void edgeRelaxed(ref const(GraphT) g, Edge e) {}
36     void edgeNotRelaxed(ref const(GraphT) g, Edge e) {}
37     void blackTarget(ref const(GraphT), Edge e) {}
38     void finishVertex(ref const(GraphT) g, Vertex e) {}
39 }
40 
41 package struct AStarBfsVisitor(GraphT,
42                                QueueT,
43                                AStarVisitorT,
44                                DistanceMapT,
45                                PredecessorMapT,
46                                WeightMapT,
47                                CostMapT,
48                                ColourMapT,
49                                HeuristicT) {
50     alias Vertex = GraphT.VertexDescriptor;
51     alias Edge = GraphT.EdgeDescriptor;
52 
53     static assert(isReadablePropertyMap!(WeightMapT, Edge, real));
54     static assert(isPropertyMap!(DistanceMapT, Vertex, real));
55     static assert(isPropertyMap!(PredecessorMapT, Vertex, Vertex));
56 
57     static assert(isCallable!HeuristicT);
58 
59 this(ref AStarVisitorT visitor,
60      ref DistanceMapT distanceMap,
61      ref const(WeightMapT) weightMap,
62      ref PredecessorMapT predecessorMap,
63      ref ColourMapT colourMap,
64      ref QueueT queue,
65      ref CostMapT costMap,
66      ref HeuristicT heuristic) {
67         _visitor = &visitor;
68         _distanceMap = &distanceMap;
69         _weightMap = &weightMap;
70         _predecessorMap = &predecessorMap;
71         _queue = &queue;
72         _costMap = &costMap;
73         _colourMap = &colourMap;
74         _heuristic = heuristic;
75     }
76 
77     void initVertex(ref const(GraphT) g, Vertex v) {
78         _visitor.initVertex(g, v);
79     }
80 
81     void discoverVertex(ref const(GraphT) g, Vertex v) {
82         _visitor.discoverVertex(g, v);
83     }
84 
85     void examineVertex(ref const(GraphT) g, Vertex v) {
86         _visitor.examineVertex(g, v);
87     }
88 
89     void examineEdge(ref const(GraphT) g, Edge e) {
90         auto weight = (*_weightMap)[e];
91         enforce (weight >= 0.0);
92         _visitor.examineEdge(g, e);
93     }
94 
95     void treeEdge(ref const(GraphT) g, Edge e) {
96         bool decreased = relax(g, *_weightMap,
97                                   *_distanceMap,
98                                   *_predecessorMap, e);
99         if (decreased) {
100             auto t = g.target(e);
101             (*_costMap)[t] = (*_distanceMap)[t] + _heuristic(t);
102             _visitor.edgeRelaxed(g, e);
103         } else {
104             _visitor.edgeNotRelaxed(g, e);
105         }
106     }
107 
108     void nonTreeEdge(ref const(GraphT) g, Edge e) {
109 
110     }
111 
112     void greyTarget(ref const(GraphT) g, Edge e) {
113         bool decreased = relax(g, *_weightMap,
114                                   *_distanceMap,
115                                   *_predecessorMap, e);
116         if (decreased) {
117             auto t = g.target(e);
118             (*_costMap)[t] = (*_distanceMap)[t] + _heuristic(t);
119             (*_queue).updateVertex(t);
120             _visitor.edgeRelaxed(g, e);
121         } else {
122             _visitor.edgeNotRelaxed(g, e);
123         }
124     }
125 
126     void blackTarget(ref const(GraphT) g, Edge e) {
127         bool decreased = relax(g, *_weightMap,
128                                   *_distanceMap,
129                                   *_predecessorMap, e);
130         if (decreased) {
131             auto t = g.target(e);
132             _visitor.edgeRelaxed(g, e);
133             (*_costMap)[t] = (*_distanceMap)[t] + _heuristic(t);
134             (*_colourMap)[t] = Colour.Grey;
135             _visitor.blackTarget(g, e);
136         } else {
137             _visitor.edgeNotRelaxed(g, e);
138         }
139     }
140 
141     void finishVertex(ref const(GraphT) g, Vertex e) {
142         _visitor.finishVertex(g, e);
143     }
144 
145     private AStarVisitorT* _visitor;
146     private DistanceMapT* _distanceMap;
147     private const(WeightMapT*) _weightMap;
148     private PredecessorMapT* _predecessorMap;
149     private QueueT* _queue;
150     private ColourMapT* _colourMap;
151     private CostMapT* _costMap;
152     private HeuristicT _heuristic;
153 }
154 
155 template aStarSearch(GraphT,
156                      VertexDescriptorT,
157                      HeuristicT,
158                      VisitorT = NullAstarVisitor!GraphT,
159                      WeightMapT = real[VertexDescriptorT],
160                      PredecessorMapT = VertexDescriptorT[VertexDescriptorT],
161                      DistanceMapT = real[VertexDescriptorT]) {
162     void aStarSearch(ref const(GraphT) g,
163                      VertexDescriptorT src,
164                      HeuristicT heuristic,
165                      ref const (WeightMapT) weights,
166                      ref PredecessorMapT predecessorMap,
167                      VisitorT visitor = VisitorT.init) {
168         static if (is(VertexDescriptorT == size_t)) {
169             auto colourMap = new Colour[g.vertexCount];
170             auto distanceMap = new real[g.vertexCount];
171             auto costMap = new real[g.vertexCount];
172         }
173         else {
174             real[VertexDescriptorT] distanceMap;
175             real[VertexDescriptorT] costMap;
176             Colour[VertexDescriptorT] colourMap;
177         }
178 
179         aStarSearch(g,
180                     src,
181                     heuristic,
182                     weights,
183                     predecessorMap,
184                     visitor,
185                     colourMap,
186                     distanceMap,
187                     costMap);
188     }
189 }
190 
191 template aStarSearch(GraphT,
192                      VertexDescriptorT,
193                      HeuristicT,
194                      VisitorT = NullAstarVisitor!GraphT,
195                      ColourMapT = Colour[VertexDescriptorT],
196                      PredecessorMapT = VertexDescriptorT[VertexDescriptorT],
197                      WeightMapT = real[VertexDescriptorT],
198                      DistanceMapT = real[VertexDescriptorT],
199                      CostMapT = real[VertexDescriptorT]) {
200     void aStarSearch(ref const(GraphT) g,
201                      VertexDescriptorT src,
202                      HeuristicT heuristic,
203                      ref const(WeightMapT) weights,
204                      ref PredecessorMapT predecessorMap,
205                      VisitorT visitor,
206                      ref ColourMapT colourMap,
207                      ref DistanceMapT distanceMap,
208                      ref CostMapT costMap) {
209         foreach(v; g.vertices) {
210             visitor.initVertex(g, v);
211             distanceMap[v] = real.infinity;
212             costMap[v] = real.infinity;
213             predecessorMap[v] = v;
214             colourMap[v] = Colour.White;
215         }
216         distanceMap[src] = 0.0;
217 
218         aStarSearchNoInit(g,
219                           src,
220                           heuristic,
221                           weights,
222                           predecessorMap,
223                           visitor,
224                           colourMap,
225                           distanceMap,
226                           costMap);
227     }
228 }
229 
230 template aStarSearchNoInit(GraphT,
231                            VertexDescriptorT,
232                            HeuristicT,
233                            VisitorT = NullAstarVisitor!GraphT,
234                            ColourMapT = Colour[VertexDescriptorT],
235                            PredecessorMapT = VertexDescriptorT[VertexDescriptorT],
236                            WeightMapT = real[VertexDescriptorT],
237                            DistanceMapT = real[VertexDescriptorT],
238                            CostMapT = real[VertexDescriptorT]) {
239     static assert(isGraph!GraphT);
240 
241     alias EdgeDescriptorT = GraphT.EdgeDescriptor;
242 
243     static assert(isPropertyMap!(ColourMapT, VertexDescriptorT, Colour));
244     static assert(isPropertyMap!(PredecessorMapT, VertexDescriptorT, VertexDescriptorT));
245     static assert(isReadablePropertyMap!(WeightMapT, EdgeDescriptorT, real));
246     static assert(isPropertyMap!(CostMapT, VertexDescriptorT, real));
247 
248     static assert(isCallable!HeuristicT);
249 
250 
251     void aStarSearchNoInit(ref const(GraphT) g,
252                            VertexDescriptorT src,
253                            HeuristicT heuristic,
254                            ref const(WeightMapT) weights,
255                            ref PredecessorMapT predecessorMap,
256                            VisitorT visitor,
257                            ref ColourMapT colourMap,
258                            ref DistanceMapT distanceMap,
259                            ref CostMapT costMap) {
260         alias QueueT = VertexQueue!(GraphT, CostMapT);
261         auto queue = QueueT(costMap);
262 
263         alias AStar = AStarBfsVisitor!(GraphT,
264                                        QueueT,
265                                        VisitorT,
266                                        DistanceMapT,
267                                        PredecessorMapT,
268                                        WeightMapT,
269                                        CostMapT,
270                                        ColourMapT,
271                                        HeuristicT);
272         auto astar = AStar(visitor,
273                            distanceMap,
274                            weights,
275                            predecessorMap,
276                            colourMap,
277                            queue,
278                            costMap,
279                            heuristic);
280         breadthFirstVisit(g, src, colourMap, queue, astar);
281     }
282 }
283 
284 template extractPath(VertexDescriptorT,
285                      PredecessorMapT = VertexDescriptorT[VertexDescriptorT],
286                      
287 ) {
288     import std.algorithm : reverse;
289     import std.container : Array;
290 
291     static assert(isPropertyMap!(PredecessorMapT, VertexDescriptorT, VertexDescriptorT));
292 
293     Array!VertexDescriptorT extractPath(ref PredecessorMapT predecessorMap,
294                                         VertexDescriptorT dst) {
295         auto path = Array!VertexDescriptorT();
296         auto v = dst;
297         while (true) {
298             path.insertBack(v);
299             auto prev = predecessorMap[v];
300             if (prev == v) {
301                 break;
302             }
303             v = prev;
304         }
305         reverse(path[]);
306         return path;
307     }
308 }
309 
310 version(unittest) {
311     import std.algorithm : reverse;
312     import std.container : Array;
313 
314     private struct Node {
315         char id;
316         int weight;
317     };
318 
319     private struct TestGraph(GraphT) {
320         GraphT graph;
321         GraphT.VertexDescriptor[char] vertices;
322 
323         alias graph this;
324 
325         GraphT.VertexDescriptor opIndex(char id) {
326             return vertices[id];
327         }
328     };
329 
330     private TestGraph!GraphT MakeTestGraph(GraphT)() {
331         // Test data from: 
332         //  https://www.slideshare.net/hemak15/lecture-14-heuristic-searcha-star-algorithm
333         GraphT graph;
334         auto s = graph.addVertex(Node('s', 17));
335         auto a = graph.addVertex(Node('a', 10));
336         auto b = graph.addVertex(Node('b', 13));
337         auto c = graph.addVertex(Node('c',  4));
338         auto d = graph.addVertex(Node('d',  2));
339         auto e = graph.addVertex(Node('e',  4));
340         auto f = graph.addVertex(Node('f',  1));
341         auto g = graph.addVertex(Node('g',  0));
342         auto h = graph.addVertex(Node('h', 99));
343 
344         //     /-> a 
345         //    /     \
346         //   /       e     h 
347         //  /       / \
348         // s ----> b   f -> g  
349         //  \       \ /
350         //   \       d
351         //    \     /
352         //     \-> c
353 
354         graph.addEdge(s, a,  6);
355         graph.addEdge(s, b,  5);
356         graph.addEdge(s, c, 10);
357         graph.addEdge(a, e,  6);
358         graph.addEdge(b, e,  6);
359         graph.addEdge(c, d,  6);
360         graph.addEdge(b, d,  7);
361         graph.addEdge(d, f,  6);
362         graph.addEdge(e, f,  4);
363         graph.addEdge(f, g,  3);
364 
365         GraphT.VertexDescriptor[char] vertices;
366         foreach (v; graph.vertices()) {
367             vertices[graph[v].id] = v;
368         }
369         return TestGraph!GraphT(graph, vertices);
370     }
371 
372     private alias G = AdjacencyList!(VecS, VecS, DirectedS, Node, int); 
373     private alias Vertex = G.VertexDescriptor;
374     private alias Edge = G.EdgeDescriptor;
375 
376     class FoundTarget : Throwable {
377         this() { super("found it"); }
378     }
379 
380     struct Visitor {
381         mixin NullAstarVisitor!G;
382 
383         Vertex _target;
384 
385         this(Vertex target) {
386             _target = target;
387         }        
388 
389         void examineVertex(ref const(G) g, Vertex v) {
390             if (v == _target) {
391                 throw new FoundTarget;
392             }
393         }
394     }
395 }
396 
397 unittest {
398     writeln("A*: Finds a path");
399 
400     auto testGraph = MakeTestGraph!G();
401     auto predecessors = new Vertex[testGraph.vertexCount];
402 
403     auto start = testGraph['s'];
404     auto destination = testGraph['g'];
405 
406     real heuristic(Vertex v) { return testGraph.graph[v].weight; }
407 
408     try {
409         aStarSearch(testGraph,
410                     start,
411                     &heuristic,
412                     testGraph.graph,
413                     predecessors,
414                     Visitor(destination));
415         assert (false, "Path not found");
416     }
417     catch(FoundTarget) {
418     }
419     auto path = extractPath(predecessors, destination);
420     auto v = testGraph.vertices;
421     const auto expectedPath = Array!Vertex([v['s'], v['b'], v['e'], v['f'], v['g']]);
422     assert (path == expectedPath);
423 }
424 
425 unittest {
426     writeln("A*: Terminates on unreachable node");
427 
428     auto testGraph = MakeTestGraph!G();
429     auto g = &testGraph.graph;
430     auto predecessor = new Vertex[g.vertexCount];
431 
432     auto start = testGraph.vertices['s'];
433     auto destination = testGraph.vertices['h'];
434 
435     real heuristic(Vertex v) { return (*g)[v].weight; }
436     try {
437         aStarSearch(*g,
438                     start,
439                     &heuristic,
440                     *g,
441                     predecessor,
442                     Visitor(destination));
443     }
444     catch(FoundTarget) {
445         assert (false, "Should never find an answer");
446     }
447 }