]> mj.ucw.cz Git - leo.git/blob - graph.c
Průvodce: More experimental sources and targets
[leo.git] / graph.c
1 /*
2  *      Hic Est Leo -- Graph and Routing
3  *
4  *      (c) 2022 Martin Mares <mj@ucw.cz>
5  */
6
7 #include "leo.h"
8
9 #include <ucw/lib.h>
10 #include <ucw/clists.h>
11 #include <ucw/mempool.h>
12
13 #include <stdio.h>
14 #include <math.h>
15
16 #include "osm.h"
17 #include "map.h"
18 #include "graph.h"
19
20 static struct mempool *graph_pool;
21 static clist graph_vertices;
22
23 struct graph_vertex {
24   cnode n;
25   struct osm_node *node;
26   clist edges;
27
28   // Dijkstra
29   double dist;
30   int state;                    // 0=unseen, 1=open, 2=closed
31   struct graph_edge *via_edge;
32 };
33
34 struct graph_edge {
35   cnode n;                      // In src->edges
36   struct graph_vertex *dest;
37   struct graph_edge *twin;
38   double length;
39   clist ways;
40 };
41
42 static uint num_vertices;
43 static uint num_edges;
44
45 static struct graph_vertex *graph_lookup_vertex(struct osm_node *n)
46 {
47   if (!n->vertex)
48     {
49       struct graph_vertex *v = mp_alloc_zero(graph_pool, sizeof(*v));
50       v->node = n;
51       n->vertex = v;
52       clist_init(&v->edges);
53       clist_add_tail(&graph_vertices, &v->n);
54       num_vertices++;
55     }
56   return n->vertex;
57 }
58
59 static struct graph_vertex *graph_vertex_by_node_id(osm_id_t id)
60 {
61   struct osm_node *n = (struct osm_node *) osm_obj_find_by_id(OSM_TYPE_NODE, id);
62   if (!n)
63     die("Cannot find node #%jd", (uintmax_t) id);
64   ASSERT(n->vertex);
65   ASSERT(n->vertex->node);
66   return n->vertex;
67 }
68
69 static uint graph_degree(struct graph_vertex *v)
70 {
71   uint d = 0;
72
73   CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
74     d++;
75   return d;
76 }
77
78 static struct graph_edge *add_half_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
79 {
80   struct graph_edge *e = mp_alloc_zero(graph_pool, sizeof(*e));
81
82   clist_add_tail(&u->edges, &e->n);
83   e->dest = v;
84   e->length = length;
85   clist_init(&e->ways);
86
87   if (way)
88     {
89       struct osm_ref *ref = mp_alloc_zero(graph_pool, sizeof(*ref));
90       ref->o = &way->o;
91       clist_add_tail(&e->ways, &ref->n);
92     }
93
94   return e;
95 }
96
97 static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
98 {
99   struct graph_edge *e1 = add_half_edge(u, v, way, length);
100   struct graph_edge *e2 = add_half_edge(v, u, way, length);
101   e1->twin = e2;
102   e2->twin = e1;
103   num_edges++;
104   return e1;
105 }
106
107 static void graph_optimize(void)
108 {
109   struct graph_vertex *v, *tmp;
110
111   CLIST_WALK_DELSAFE(v, graph_vertices, tmp)
112     {
113       if (graph_degree(v) == 2)
114         {
115           struct graph_edge *e1 = clist_remove_head(&v->edges);
116           struct graph_edge *e2 = e1->twin;
117           struct graph_edge *f1 = clist_remove_head(&v->edges);
118           struct graph_edge *f2 = f1->twin;
119           struct graph_vertex *x = e1->dest;
120           struct graph_vertex *y = f1->dest;
121           clist_remove(&e2->n);
122           clist_remove(&f2->n);
123           num_edges -= 2;
124
125           clist_remove(&v->n);
126           num_vertices--;
127
128           struct graph_edge *g1 = graph_add_edge(x, y, NULL, e1->length + f1->length);
129           struct graph_edge *g2 = g1->twin;
130           clist_add_list_tail(&g1->ways, &e1->ways);
131           clist_add_list_tail(&g1->ways, &f1->ways);
132           clist_add_list_tail(&g2->ways, &f2->ways);
133           clist_add_list_tail(&g2->ways, &e2->ways);
134         }
135     }
136 }
137
138 static bool way_is_edge(struct osm_way *w)
139 {
140   osm_val_t hwy = osm_obj_find_tag(&w->o, KEY_HIGHWAY);
141   switch (hwy) {
142     case VALUE_MOTORWAY:
143     case VALUE_TRUNK:
144     case VALUE_PRIMARY:
145     case VALUE_SECONDARY:
146     case VALUE_MOTORWAY_LINK:
147     case VALUE_TRUNK_LINK:
148     case VALUE_PRIMARY_LINK:
149     case VALUE_SECONDARY_LINK:
150       return 1;
151   }
152
153   return 0;
154 }
155
156 static double vertex_dist(struct graph_vertex *a, struct graph_vertex *b)
157 {
158   struct osm_node *aa = a->node;
159   struct osm_node *bb = b->node;
160   return hypot(bb->x - aa->x, bb->y - aa->y);
161 }
162
163 static void way_add_edge(struct osm_way *w)
164 {
165   struct graph_vertex *prev_v = NULL;
166
167   OSM_FOR_EACH_BEGIN(struct osm_node *, n, w->nodes)
168     {
169       struct graph_vertex *v = graph_lookup_vertex(n);
170       if (prev_v)
171         graph_add_edge(prev_v, v, w, vertex_dist(v, prev_v));
172       prev_v = v;
173     }
174   OSM_FOR_EACH_END;
175 }
176
177 static void mark_edge_ways(struct graph_edge *e, const char *key, const char *val)
178 {
179   OSM_FOR_EACH_BEGIN(struct osm_way *, w, e->ways)
180     {
181       osm_obj_set_tag(&w->o, key, val);
182     }
183   OSM_FOR_EACH_END;
184 }
185
186 static void graph_check_edges(void)
187 {
188   CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
189     CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
190       {
191         double d = vertex_dist(e->dest, v);
192         if (d > e->length)
193           msg(L_WARN, "Suspicious edge (#%jd, #%jd): length=%g dist=%g", (intmax_t) v->node->o.id, (intmax_t) e->dest->node->o.id, e->length, d);
194       }
195 }
196
197 static void visualize(struct graph_vertex *v_src, struct graph_vertex *v_dest)
198 {
199   CLIST_FOR_EACH(struct graph_vertex *, x, graph_vertices)
200     {
201       if (x->state)
202         osm_obj_set_tag(&x->node->o, "pruvodce", "visited");
203       if (x->state == 2)
204         CLIST_FOR_EACH(struct graph_edge *, e, x->edges)
205           mark_edge_ways(e, "pruvodce", "visited");
206     }
207
208   struct graph_vertex *x = v_dest;
209   while (x->via_edge)
210     {
211       struct graph_edge *e = x->via_edge->twin;
212       mark_edge_ways(e, "pruvodce", "path");
213       x = e->dest;
214     }
215
216   osm_obj_set_tag(&v_src->node->o, "pruvodce", "src");
217   osm_obj_set_tag(&v_dest->node->o, "pruvodce", "dest");
218 }
219
220 static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
221 {
222   v_src->state = 1;
223   v_src->dist = 0;
224
225   for (;;)
226     {
227       struct graph_vertex *w = NULL;
228       double best_d = 0;
229
230       CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
231         if (v->state == 1)
232           {
233             double d = v->dist;
234             d += vertex_dist(v, v_dest);
235             if (!w || d < best_d)
236               w = v, best_d = d;
237           }
238
239       if (!w)
240         die("Path not found");
241
242       // msg(L_DEBUG, "Dijkstra: closing vertex #%jd", w->node->o.id);
243
244       if (w == v_dest)
245         {
246           msg(L_INFO, "Found path: dist=%f", w->dist);
247           visualize(v_src, v_dest);
248           return;
249         }
250
251       w->state = 2;
252       CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
253         {
254           struct graph_vertex *x = e->dest;
255           double d = w->dist + e->length;
256           // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state, x->dist, d);
257           if (x->state == 0 || x->dist > d)
258             {
259               if (x->state == 2)
260                 msg(L_WARN, "Re-opening node #%jd", (intmax_t) x->node->o.id);
261               x->state = 1;
262               x->dist = d;
263               x->via_edge = e;
264             }
265         }
266     }
267 }
268
269 void graph_build(void)
270 {
271   graph_pool = mp_new(65536);
272   clist_init(&graph_vertices);
273
274   // We are considering only the first data source for the graph.
275   // Otherwise, things start becoming ugly, because node IDs are generally not unique.
276
277   struct data_source *ds = clist_head(&map_sources);
278   ASSERT(ds);
279   osm_this = ds->osm;
280   CLIST_FOR_EACH(struct osm_way *, w, osm_this->obj_list[OSM_TYPE_WAY])
281     {
282       if (way_is_edge(w))
283         way_add_edge(w);
284     }
285   msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
286
287   graph_optimize();
288   msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
289
290   graph_check_edges();
291
292   // struct graph_vertex *v1 = graph_vertex_by_node_id(30002893);       // Praha-Zbraslav, K Přehradám x 101
293   struct graph_vertex *v1 = graph_vertex_by_node_id(96745573);  // Beroun, Lidická x Pražská
294   // struct graph_vertex *v2 = graph_vertex_by_node_id(21289321);       // Brno, Heršpická x Opuštěná
295   // struct graph_vertex *v2 = graph_vertex_by_node_id(271385400);      // Znojmo, Vídeňská třída x Brněnská
296   // struct graph_vertex *v2 = graph_vertex_by_node_id(26753344);       // Šumperk, Jesenická x Lidická
297   struct graph_vertex *v2 = graph_vertex_by_node_id(714908910); // Ostrava, Mariánskohorská x Plzeňská x 28. října
298   ASSERT(graph_degree(v1) && graph_degree(v2));
299   msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v1->node->o.id, (uintmax_t) v2->node->o.id);
300   dijkstra(v1, v2);
301 }