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 *v2 = graph_vertex_by_node_id(21289321);  // Brno, Heršpická x Opuštěná
 294   ASSERT(graph_degree(v1) && graph_degree(v2));
 295   msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v1->node->o.id, (uintmax_t) v2->node->o.id);
 296   dijkstra(v1, v2);
 297 }