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/conf.h>
  12 #include <ucw/mempool.h>
  13
  14 #include <stdio.h>
  15 #include <math.h>
  16
  17 #include "osm.h"
  18 #include "map.h"
  19 #include "graph.h"
  20
  21 static uint bidir;
  22 static uint astar;
  23
  24 static struct cf_section graph_cf = {
  25   CF_ITEMS {
  26     CF_UNS("BiDir", &bidir),
  27     CF_UNS("AStar", &astar),
  28     CF_END
  29   }
  30 };
  31
  32 static void CONSTRUCTOR map_preinit(void)
  33 {
  34   cf_declare_section("Graph", &graph_cf, 0);
  35 }
  36
  37 static struct mempool *graph_pool;
  38 static clist graph_vertices;
  39
  40 struct graph_vertex {
  41   cnode n;
  42   struct osm_node *node;
  43   clist edges;
  44
  45   // Dijkstra forward + backward
  46   double dist[2];
  47   int state[2];                 // 0=unseen, 1=open, 2=closed
  48   struct graph_edge *via_edge[2];
  49 };
  50
  51 struct graph_edge {
  52   cnode n;                      // In src->edges
  53   struct graph_vertex *dest;
  54   struct graph_edge *twin;
  55   double length;
  56   clist ways;
  57 };
  58
  59 static uint num_vertices;
  60 static uint num_edges;
  61
  62 static struct graph_vertex *graph_lookup_vertex(struct osm_node *n)
  63 {
  64   if (!n->vertex)
  65     {
  66       struct graph_vertex *v = mp_alloc_zero(graph_pool, sizeof(*v));
  67       v->node = n;
  68       n->vertex = v;
  69       clist_init(&v->edges);
  70       clist_add_tail(&graph_vertices, &v->n);
  71       num_vertices++;
  72     }
  73   return n->vertex;
  74 }
  75
  76 static struct graph_vertex *graph_vertex_by_node_id(osm_id_t id)
  77 {
  78   struct osm_node *n = (struct osm_node *) osm_obj_find_by_id(OSM_TYPE_NODE, id);
  79   if (!n)
  80     die("Cannot find node #%jd", (uintmax_t) id);
  81   ASSERT(n->vertex);
  82   ASSERT(n->vertex->node);
  83   return n->vertex;
  84 }
  85
  86 static uint graph_degree(struct graph_vertex *v)
  87 {
  88   uint d = 0;
  89
  90   CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
  91     d++;
  92   return d;
  93 }
  94
  95 static struct graph_edge *add_half_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
  96 {
  97   struct graph_edge *e = mp_alloc_zero(graph_pool, sizeof(*e));
  98
  99   clist_add_tail(&u->edges, &e->n);
 100   e->dest = v;
 101   e->length = length;
 102   clist_init(&e->ways);
 103
 104   if (way)
 105     {
 106       struct osm_ref *ref = mp_alloc_zero(graph_pool, sizeof(*ref));
 107       ref->o = &way->o;
 108       clist_add_tail(&e->ways, &ref->n);
 109     }
 110
 111   return e;
 112 }
 113
 114 static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
 115 {
 116   struct graph_edge *e1 = add_half_edge(u, v, way, length);
 117   struct graph_edge *e2 = add_half_edge(v, u, way, length);
 118   e1->twin = e2;
 119   e2->twin = e1;
 120   num_edges++;
 121   return e1;
 122 }
 123
 124 static void graph_optimize(void)
 125 {
 126   struct graph_vertex *v, *tmp;
 127
 128   CLIST_WALK_DELSAFE(v, graph_vertices, tmp)
 129     {
 130       if (graph_degree(v) == 2)
 131         {
 132           struct graph_edge *e1 = clist_remove_head(&v->edges);
 133           struct graph_edge *e2 = e1->twin;
 134           struct graph_edge *f1 = clist_remove_head(&v->edges);
 135           struct graph_edge *f2 = f1->twin;
 136           struct graph_vertex *x = e1->dest;
 137           struct graph_vertex *y = f1->dest;
 138           clist_remove(&e2->n);
 139           clist_remove(&f2->n);
 140           num_edges -= 2;
 141
 142           clist_remove(&v->n);
 143           num_vertices--;
 144
 145           struct graph_edge *g1 = graph_add_edge(x, y, NULL, e1->length + f1->length);
 146           struct graph_edge *g2 = g1->twin;
 147           clist_add_list_tail(&g1->ways, &e1->ways);
 148           clist_add_list_tail(&g1->ways, &f1->ways);
 149           clist_add_list_tail(&g2->ways, &f2->ways);
 150           clist_add_list_tail(&g2->ways, &e2->ways);
 151         }
 152     }
 153 }
 154
 155 static bool way_is_edge(struct osm_way *w)
 156 {
 157   osm_val_t hwy = osm_obj_find_tag(&w->o, KEY_HIGHWAY);
 158   switch (hwy) {
 159     case VALUE_MOTORWAY:
 160     case VALUE_TRUNK:
 161     case VALUE_PRIMARY:
 162     case VALUE_SECONDARY:
 163     case VALUE_MOTORWAY_LINK:
 164     case VALUE_TRUNK_LINK:
 165     case VALUE_PRIMARY_LINK:
 166     case VALUE_SECONDARY_LINK:
 167       return 1;
 168   }
 169
 170   return 0;
 171 }
 172
 173 static double vertex_dist(struct graph_vertex *a, struct graph_vertex *b)
 174 {
 175   struct osm_node *aa = a->node;
 176   struct osm_node *bb = b->node;
 177   return hypot(bb->x - aa->x, bb->y - aa->y);
 178 }
 179
 180 static void way_add_edge(struct osm_way *w)
 181 {
 182   struct graph_vertex *prev_v = NULL;
 183
 184   OSM_FOR_EACH_BEGIN(struct osm_node *, n, w->nodes)
 185     {
 186       struct graph_vertex *v = graph_lookup_vertex(n);
 187       if (prev_v)
 188         graph_add_edge(prev_v, v, w, vertex_dist(v, prev_v));
 189       prev_v = v;
 190     }
 191   OSM_FOR_EACH_END;
 192 }
 193
 194 static void mark_edge_ways(struct graph_edge *e, const char *key, const char *val)
 195 {
 196   OSM_FOR_EACH_BEGIN(struct osm_way *, w, e->ways)
 197     {
 198       osm_obj_set_tag(&w->o, key, val);
 199     }
 200   OSM_FOR_EACH_END;
 201 }
 202
 203 static void graph_check_edges(void)
 204 {
 205   CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
 206     CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
 207       {
 208         double d = vertex_dist(e->dest, v);
 209         if (d > e->length)
 210           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);
 211       }
 212 }
 213
 214 static void visualize_states(struct graph_vertex *v_src, struct graph_vertex *v_dest)
 215 {
 216   CLIST_FOR_EACH(struct graph_vertex *, x, graph_vertices)
 217     {
 218       if (x->state[0])
 219         osm_obj_set_tag(&x->node->o, "pruvodce", "visited");
 220       if (x->state[1])
 221         osm_obj_set_tag(&x->node->o, "pruvodce2", "visited");
 222       if (x->state[0] == 2)
 223         {
 224           CLIST_FOR_EACH(struct graph_edge *, e, x->edges)
 225             mark_edge_ways(e, "pruvodce", "visited");
 226         }
 227       if (x->state[1] == 2)
 228         {
 229           CLIST_FOR_EACH(struct graph_edge *, e, x->edges)
 230             mark_edge_ways(e, "pruvodce2", "visited");
 231         }
 232     }
 233
 234   osm_obj_set_tag(&v_src->node->o, "pruvodce", "src");
 235   osm_obj_set_tag(&v_dest->node->o, "pruvodce", "dest");
 236 }
 237
 238 static void visualize_path(struct graph_vertex *v_dest, uint dir)
 239 {
 240   struct graph_vertex *x = v_dest;
 241   while (x->via_edge[dir])
 242     {
 243       struct graph_edge *e = x->via_edge[dir]->twin;
 244       mark_edge_ways(e, (dir ? "pruvodce2" : "pruvodce"), "path");
 245       x = e->dest;
 246     }
 247 }
 248
 249 static void bidir_dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
 250 {
 251   msg(L_INFO, "Finding bidirectional path from #%jd to #%jd", (uintmax_t) v_src->node->o.id, (uintmax_t) v_dest->node->o.id);
 252
 253   v_src->state[0] = 1;
 254   v_src->dist[0] = 0;
 255
 256   v_dest->state[1] = 1;
 257   v_dest->dist[1] = 0;
 258
 259   uint open[2] = { 1, 1 };
 260   struct graph_vertex *w;
 261
 262   for (;;)
 263     {
 264       double best_d = 0;
 265       uint dir = (open[0] <= open[1]) ? 0 : 1;
 266       w = NULL;
 267       msg(L_DEBUG, "Dijkstra: dir=%d", dir);
 268
 269       CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
 270         {
 271           if (v->state[dir] == 1)
 272             {
 273               double d = v->dist[dir];
 274               if (astar)
 275                 d += vertex_dist(v, (dir ? v_src : v_dest));
 276               if (!w || d < best_d)
 277                 w = v, best_d = d;
 278             }
 279         }
 280
 281       if (!w)
 282         die("Path not found");
 283
 284       msg(L_DEBUG, "Dijkstra: closing vertex #%jd (d=%f, dir %d)", w->node->o.id, best_d, dir);
 285
 286       w->state[dir] = 2;
 287       if (w->state[!dir] == 2)
 288         break;
 289
 290       CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
 291         {
 292           struct graph_vertex *x = e->dest;
 293           double d = w->dist[dir] + e->length;
 294           msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state[dir], x->dist[dir], d);
 295           if (x->state[dir] == 0 || x->dist[dir] > d)
 296             {
 297               if (x->state[dir] == 2)
 298                 msg(L_WARN, "Re-opening node #%jd in dir %d", (intmax_t) x->node->o.id, dir);
 299               x->state[dir] = 1;
 300               x->dist[dir] = d;
 301               x->via_edge[dir] = e;
 302               open[dir]++;
 303             }
 304         }
 305     }
 306
 307   // One vertex was closed in both direction
 308   double d2c = w->dist[0] + w->dist[1];
 309   msg(L_INFO, "Dijkstra: Path via double-closed vertex: dist=%f+%f=%f", w->dist[0], w->dist[1], d2c);
 310
 311   // Look for edges closed0-closed1, which might yield a shorter path
 312   double d01 = d2c;
 313   struct graph_vertex *ev = NULL, *ew = NULL;
 314   struct graph_edge *ee = NULL;
 315
 316   CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
 317     if (v->state[0] == 2)
 318       CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
 319         {
 320           struct graph_vertex *w = e->dest;
 321           if (w->state[1] == 2)
 322             {
 323               double d = v->dist[0] + e->length + w->dist[1];
 324               if (d < d01)
 325                 d01 = d, ev = v, ew = w, ee = e;
 326             }
 327         }
 328
 329   if (d01 < d2c)
 330     {
 331       msg(L_INFO, "Dijkstra: Better path via extra edge: dist=%f+%f+%f=%f", ev->dist[0], ee->length, ew->dist[1], d01);
 332       ev->via_edge[1] = ee->twin;
 333       w = ev;
 334     }
 335
 336   visualize_states(v_src, v_dest);
 337   visualize_path(w, 0);
 338   visualize_path(w, 1);
 339 }
 340
 341 static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
 342 {
 343   msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v_src->node->o.id, (uintmax_t) v_dest->node->o.id);
 344
 345   v_src->state[0] = 1;
 346   v_src->dist[0] = 0;
 347
 348   for (;;)
 349     {
 350       struct graph_vertex *w = NULL;
 351       double best_d = 0;
 352
 353       CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
 354         if (v->state[0] == 1)
 355           {
 356             double d = v->dist[0];
 357             if (astar)
 358               d += vertex_dist(v, v_dest);
 359             if (!w || d < best_d)
 360               w = v, best_d = d;
 361           }
 362
 363       if (!w)
 364         die("Path not found");
 365
 366       // msg(L_DEBUG, "Dijkstra: closing vertex #%jd", w->node->o.id);
 367
 368       if (w == v_dest)
 369         {
 370           msg(L_INFO, "Found path: dist=%f", w->dist[0]);
 371           visualize_states(v_src, v_dest);
 372           visualize_path(v_dest, 0);
 373           return;
 374         }
 375
 376       w->state[0] = 2;
 377       CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
 378         {
 379           struct graph_vertex *x = e->dest;
 380           double d = w->dist[0] + e->length;
 381           // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state[0], x->dist[0], d);
 382           if (x->state[0] == 0 || x->dist[0] > d)
 383             {
 384               if (x->state[0] == 2)
 385                 msg(L_WARN, "Re-opening node #%jd", (intmax_t) x->node->o.id);
 386               x->state[0] = 1;
 387               x->dist[0] = d;
 388               x->via_edge[0] = e;
 389             }
 390         }
 391     }
 392 }
 393
 394 void graph_build(void)
 395 {
 396   graph_pool = mp_new(65536);
 397   clist_init(&graph_vertices);
 398
 399   // We are considering only the first data source for the graph.
 400   // Otherwise, things start becoming ugly, because node IDs are generally not unique.
 401
 402   struct data_source *ds = clist_head(&map_sources);
 403   ASSERT(ds);
 404   osm_this = ds->osm;
 405   CLIST_FOR_EACH(struct osm_way *, w, osm_this->obj_list[OSM_TYPE_WAY])
 406     {
 407       if (way_is_edge(w))
 408         way_add_edge(w);
 409     }
 410   msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
 411
 412   graph_optimize();
 413   msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
 414
 415   graph_check_edges();
 416
 417   // struct graph_vertex *v1 = graph_vertex_by_node_id(30002893);       // Praha-Zbraslav, K Přehradám x 101
 418   struct graph_vertex *v1 = graph_vertex_by_node_id(96745573);  // Beroun, Lidická x Pražská
 419   // struct graph_vertex *v2 = graph_vertex_by_node_id(21289321);       // Brno, Heršpická x Opuštěná
 420   // struct graph_vertex *v2 = graph_vertex_by_node_id(271385400);      // Znojmo, Vídeňská třída x Brněnská
 421   // struct graph_vertex *v2 = graph_vertex_by_node_id(26753344);       // Šumperk, Jesenická x Lidická
 422   struct graph_vertex *v2 = graph_vertex_by_node_id(714908910); // Ostrava, Mariánskohorská x Plzeňská x 28. října
 423   ASSERT(graph_degree(v1) && graph_degree(v2));
 424
 425   if (bidir)
 426     bidir_dijkstra(v1, v2);
 427   else
 428     dijkstra(v1, v2);
 429 }