2 * Hic Est Leo -- Graph and Routing
4 * (c) 2022 Martin Mares <mj@ucw.cz>
10 #include <ucw/clists.h>
11 #include <ucw/mempool.h>
20 static struct mempool *graph_pool;
21 static clist graph_vertices;
25 struct osm_node *node;
30 int state; // 0=unseen, 1=open, 2=closed
31 struct graph_edge *via_edge;
35 cnode n; // In src->edges
36 struct graph_vertex *dest;
37 struct graph_edge *twin;
42 static uint num_vertices;
43 static uint num_edges;
45 static struct graph_vertex *graph_lookup_vertex(struct osm_node *n)
49 struct graph_vertex *v = mp_alloc_zero(graph_pool, sizeof(*v));
52 clist_init(&v->edges);
53 clist_add_tail(&graph_vertices, &v->n);
59 static struct graph_vertex *graph_vertex_by_node_id(osm_id_t id)
61 struct osm_node *n = (struct osm_node *) osm_obj_find_by_id(OSM_TYPE_NODE, id);
63 die("Cannot find node #%jd", (uintmax_t) id);
65 ASSERT(n->vertex->node);
69 static uint graph_degree(struct graph_vertex *v)
73 CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
78 static struct graph_edge *add_half_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
80 struct graph_edge *e = mp_alloc_zero(graph_pool, sizeof(*e));
82 clist_add_tail(&u->edges, &e->n);
89 struct osm_ref *ref = mp_alloc_zero(graph_pool, sizeof(*ref));
91 clist_add_tail(&e->ways, &ref->n);
97 static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
99 struct graph_edge *e1 = add_half_edge(u, v, way, length);
100 struct graph_edge *e2 = add_half_edge(v, u, way, length);
107 static void graph_optimize(void)
109 struct graph_vertex *v, *tmp;
111 CLIST_WALK_DELSAFE(v, graph_vertices, tmp)
113 if (graph_degree(v) == 2)
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);
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);
138 static bool way_is_edge(struct osm_way *w)
140 osm_val_t hwy = osm_obj_find_tag(&w->o, KEY_HIGHWAY);
145 case VALUE_SECONDARY:
146 case VALUE_MOTORWAY_LINK:
147 case VALUE_TRUNK_LINK:
148 case VALUE_PRIMARY_LINK:
149 case VALUE_SECONDARY_LINK:
156 static void way_add_edge(struct osm_way *w)
158 struct osm_node *prev_n = NULL;
159 struct graph_vertex *prev_v = NULL;
161 OSM_FOR_EACH_BEGIN(struct osm_node *, n, w->nodes)
163 struct graph_vertex *v = graph_lookup_vertex(n);
166 double length = hypot(n->x - prev_n->x, n->y - prev_n->y);
167 graph_add_edge(prev_v, v, w, length);
175 static void mark_edge_ways(struct graph_edge *e, const char *key, const char *val)
177 OSM_FOR_EACH_BEGIN(struct osm_way *, w, e->ways)
179 osm_obj_set_tag(&w->o, key, val);
184 static void visualize(struct graph_vertex *v_src, struct graph_vertex *v_dest)
186 CLIST_FOR_EACH(struct graph_vertex *, x, graph_vertices)
189 osm_obj_set_tag(&x->node->o, "pruvodce", "visited");
191 CLIST_FOR_EACH(struct graph_edge *, e, x->edges)
192 mark_edge_ways(e, "pruvodce", "visited");
195 struct graph_vertex *x = v_dest;
198 struct graph_edge *e = x->via_edge->twin;
199 mark_edge_ways(e, "pruvodce", "path");
203 osm_obj_set_tag(&v_src->node->o, "pruvodce", "src");
204 osm_obj_set_tag(&v_dest->node->o, "pruvodce", "dest");
207 static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
214 struct graph_vertex *w = NULL;
215 CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
216 if (v->state == 1 && (!w || v->dist < w->dist))
220 die("Path not found");
222 // msg(L_DEBUG, "Dijkstra: closing vertex #%jd", w->node->o.id);
226 msg(L_INFO, "Found path: dist=%f", w->dist);
227 visualize(v_src, v_dest);
232 CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
234 struct graph_vertex *x = e->dest;
235 double d = w->dist + e->length;
236 // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state, x->dist, d);
237 if (x->state == 0 || x->dist > d)
239 ASSERT(x->state != 2);
248 void graph_build(void)
250 graph_pool = mp_new(65536);
251 clist_init(&graph_vertices);
253 // We are considering only the first data source for the graph.
254 // Otherwise, things start becoming ugly, because node IDs are generally not unique.
256 struct data_source *ds = clist_head(&map_sources);
259 CLIST_FOR_EACH(struct osm_way *, w, osm_this->obj_list[OSM_TYPE_WAY])
264 msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
267 msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
269 struct graph_vertex *v1 = graph_vertex_by_node_id(30002893); // Praha-Zbraslav, K Přehradám x 101
270 struct graph_vertex *v2 = graph_vertex_by_node_id(21289321); // Brno, Heršpická x Opuštěná
271 ASSERT(graph_degree(v1) && graph_degree(v2));
272 msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v1->node->o.id, (uintmax_t) v2->node->o.id);