]> mj.ucw.cz Git - leo.git/blob - graph.c
Průvodce: Identifying endpoints
[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
29 struct graph_edge {
30   cnode n;                      // In src->edges
31   struct graph_vertex *dest;
32   struct graph_edge *twin;
33   double length;
34   struct osm_way *way;
35 };
36
37 static uint num_vertices;
38 static uint num_edges;
39
40 static struct graph_vertex *graph_lookup_vertex(struct osm_node *n)
41 {
42   if (!n->vertex)
43     {
44       struct graph_vertex *v = mp_alloc_zero(graph_pool, sizeof(*v));
45       v->node = n;
46       n->vertex = v;
47       clist_init(&v->edges);
48       clist_add_tail(&graph_vertices, &v->n);
49       num_vertices++;
50     }
51   return n->vertex;
52 }
53
54 static struct graph_vertex *graph_vertex_by_node_id(osm_id_t id)
55 {
56   struct osm_node *n = (struct osm_node *) osm_obj_find_by_id(OSM_TYPE_NODE, id);
57   if (!n)
58     die("Cannot find node #%jd", (uintmax_t) id);
59   ASSERT(n->vertex);
60   ASSERT(n->vertex->node);
61   return n->vertex;
62 }
63
64 static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
65 {
66   struct graph_edge *e1 = mp_alloc_zero(graph_pool, sizeof(*e1));
67   struct graph_edge *e2 = mp_alloc_zero(graph_pool, sizeof(*e2));
68   num_edges++;
69
70   clist_add_tail(&u->edges, &e1->n);
71   e1->dest = v;
72   e1->twin = e2;
73   e1->length = length;
74   e1->way = way;
75
76   clist_add_tail(&v->edges, &e2->n);
77   e2->dest = u;
78   e2->twin = e1;
79   e2->length = length;
80   e2->way = way;
81
82   return e1;
83 }
84
85 static void graph_optimize(void)
86 {
87   struct graph_vertex *v, *tmp;
88
89   CLIST_WALK_DELSAFE(v, graph_vertices, tmp)
90     {
91       uint deg = 0;
92       CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
93         deg++;
94
95       if (deg == 2)
96         {
97           struct graph_edge *e1 = clist_remove_head(&v->edges);
98           struct graph_edge *f1 = clist_remove_head(&v->edges);
99           struct graph_vertex *x = e1->dest;
100           struct graph_vertex *y = f1->dest;
101           clist_remove(&e1->twin->n);
102           clist_remove(&f1->twin->n);
103           num_edges -= 2;
104
105           clist_remove(&v->n);
106           num_vertices--;
107
108           graph_add_edge(x, y, e1->way, e1->length + f1->length);
109         }
110     }
111 }
112
113 static bool way_is_edge(struct osm_way *w)
114 {
115   osm_val_t hwy = osm_obj_find_tag(&w->o, KEY_HIGHWAY);
116   switch (hwy) {
117     case VALUE_MOTORWAY:
118     case VALUE_TRUNK:
119     case VALUE_PRIMARY:
120     case VALUE_SECONDARY:
121     case VALUE_MOTORWAY_LINK:
122     case VALUE_TRUNK_LINK:
123     case VALUE_PRIMARY_LINK:
124     case VALUE_SECONDARY_LINK:
125       return 1;
126   }
127
128   return 0;
129 }
130
131 static void way_add_edge(struct osm_way *w)
132 {
133   struct osm_node *prev_n = NULL;
134   struct graph_vertex *prev_v = NULL;
135
136   OSM_FOR_EACH_BEGIN(struct osm_node *, n, w->nodes)
137     {
138       struct graph_vertex *v = graph_lookup_vertex(n);
139       if (prev_v)
140         {
141           double length = hypot(n->x - prev_n->x, n->y - prev_n->y);
142           graph_add_edge(prev_v, v, w, length);
143         }
144       prev_n = n;
145       prev_v = v;
146     }
147   OSM_FOR_EACH_END;
148 }
149
150 void graph_build(void)
151 {
152   graph_pool = mp_new(65536);
153   clist_init(&graph_vertices);
154
155   // We are considering only the first data source for the graph.
156   // Otherwise, things start becoming ugly, because node IDs are generally not unique.
157
158   struct data_source *ds = clist_head(&map_sources);
159   ASSERT(ds);
160   osm_this = ds->osm;
161   CLIST_FOR_EACH(struct osm_way *, w, osm_this->obj_list[OSM_TYPE_WAY])
162     {
163       if (way_is_edge(w))
164         way_add_edge(w);
165     }
166   msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
167
168   graph_optimize();
169   msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
170
171   struct graph_vertex *v1 = graph_vertex_by_node_id(32292698);  // Praha-Zbraslav, K Přehradám
172   struct graph_vertex *v2 = graph_vertex_by_node_id(21289321);  // Brno, Heršpická x Opuštěná
173   msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v1->node->o.id, (uintmax_t) v2->node->o.id);
174 }