aga,agar: Dijkstra's algorithm
[ccan] / ccan / aga / dijkstra.c
1 /* Licensed under LGPLv2+ - see LICENSE file for details */
2 #include "config.h"
3
4 #include <stdbool.h>
5 #include <stdlib.h>
6 #include <assert.h>
7
8 #include <ccan/build_assert/build_assert.h>
9 #include <ccan/check_type/check_type.h>
10 #include <ccan/order/order.h>
11 #include <ccan/lpq/lpq.h>
12
13 #include <ccan/aga/aga.h>
14 #include "private.h"
15
16 /*
17  * Dijkstra's algorithm
18  */
19
20 /* Node states */
21 #define STATE_INFINITE  0
22 #define STATE_FINITE    1
23 #define STATE_COMPLETE  2
24
25
26 static void candidate_path(struct aga_graph *g, struct aga_node *node,
27                            aga_icost_t distance,
28                            struct aga_node *prev, const void *prevedge)
29 {
30         if (aga_update_node(g, node)) {
31                 /* New node, treat has having infinite distance */
32                 node->u.dijkstra.distance = distance;
33                 node->u.dijkstra.prev = prev;
34                 node->u.dijkstra.prevedge = prevedge;
35                 node->u.dijkstra.complete = false;
36
37                 lpq_enqueue(&g->state.dijkstra, node);
38         } else if (distance < node->u.dijkstra.distance) {
39                 assert(!node->u.dijkstra.complete);
40
41                 node->u.dijkstra.distance = distance;
42                 node->u.dijkstra.prev = prev;
43                 node->u.dijkstra.prevedge = prevedge;
44
45                 lpq_reorder(&g->state.dijkstra, node);
46         }
47 }
48
49 int aga_dijkstra_start(struct aga_graph *g, struct aga_node *source)
50 {
51         total_order_by_field(order, long_reverse,
52                              struct aga_node, u.dijkstra.distance);
53         int rc;
54
55         /* Make sure we're actually using the right ordering for
56          * aga_icost_t */
57         BUILD_ASSERT(check_types_match(long, aga_icost_t) == 0);
58
59         rc = aga_start(g);
60         if (rc < 0)
61                 return rc;
62
63         lpq_init(&g->state.dijkstra, order.cb, order.ctx);
64
65         candidate_path(g, source, 0, NULL, NULL);
66
67         return 0;
68 }
69
70 struct aga_node *aga_dijkstra_step(struct aga_graph *g)
71 {
72         struct aga_node *n = lpq_dequeue(&g->state.dijkstra);
73         const void *e;
74         struct aga_edge_info ei;
75         int err;
76
77         if (!aga_check_state(g))
78                 return NULL;
79
80         if (!n)
81                 return NULL;
82
83         aga_for_each_edge_info(e, ei, err, g, n) {
84                 if (ei.icost < 0) {
85                         aga_fail(g, AGA_ERR_NEGATIVE_COST);
86                         return NULL;
87                 }
88                 candidate_path(g, ei.to,
89                                n->u.dijkstra.distance + ei.icost, n, e);
90         }
91         if (err) {
92                 aga_fail(g, err);
93                 return NULL;
94         }
95
96         n->u.dijkstra.complete = true;
97
98         return n;
99 }
100
101 bool aga_dijkstra_path(struct aga_graph *g, struct aga_node *node,
102                        aga_icost_t *total_cost,
103                        struct aga_node **prev, const void **prevedge)
104 {
105         if (!aga_check_state(g))
106                 return false;
107
108         while (aga_node_needs_update(g, node) || !node->u.dijkstra.complete) {
109                 if (!aga_dijkstra_step(g))
110                         return false;
111         };
112
113         if (total_cost)
114                 *total_cost = node->u.dijkstra.distance;
115         if (prev)
116                 *prev = node->u.dijkstra.prev;
117         if (prevedge)
118                 *prevedge = node->u.dijkstra.prevedge;
119
120         return true;
121 }
122
123 void aga_dijkstra_all_paths(struct aga_graph *g)
124 {
125         if (!aga_check_state(g))
126                 return;
127
128         while (aga_dijkstra_step(g))
129                 ;
130 }
131