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;      // 0=unidir, 1=bidir balanced by #opened, 2=bidir balanced by distance
  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 opened[2] = { 1, 1 }, closed[2] = { 0, 0 };
 260   double last_dist[2] = { 0, 0 };
 261   struct graph_vertex *w;
 262
 263   for (;;)
 264     {
 265       double best_d = 0;
 266       uint dir;
 267       if (bidir == 2)
 268         dir = (last_dist[0] <= last_dist[1]) ? 0 : 1;
 269       else
 270         dir = (opened[0] <= opened[1]) ? 0 : 1;
 271       w = NULL;
 272       // msg(L_DEBUG, "Dijkstra: dir=%d", dir);
 273
 274       CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
 275         {
 276           if (v->state[dir] == 1)
 277             {
 278               double d = v->dist[dir];
 279               if (astar)
 280                 d += vertex_dist(v, (dir ? v_src : v_dest));
 281               if (!w || d < best_d)
 282                 w = v, best_d = d;
 283             }
 284         }
 285
 286       if (!w)
 287         die("Path not found");
 288
 289       // msg(L_DEBUG, "Dijkstra: closing vertex #%jd (d=%f, dir %d)", w->node->o.id, best_d, dir);
 290       closed[dir]++;
 291       last_dist[dir] = best_d;
 292
 293       w->state[dir] = 2;
 294       if (w->state[!dir] == 2)
 295         break;
 296
 297       CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
 298         {
 299           struct graph_vertex *x = e->dest;
 300           double d = w->dist[dir] + e->length;
 301           // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state[dir], x->dist[dir], d);
 302           if (x->state[dir] == 0 || x->dist[dir] > d)
 303             {
 304               if (x->state[dir] == 2)
 305                 msg(L_WARN, "Re-opening node #%jd in dir %d", (intmax_t) x->node->o.id, dir);
 306               x->state[dir] = 1;
 307               x->dist[dir] = d;
 308               x->via_edge[dir] = e;
 309               opened[dir]++;
 310             }
 311         }
 312     }
 313
 314   // One vertex was closed in both direction
 315   double d2c = w->dist[0] + w->dist[1];
 316   msg(L_INFO, "Dijkstra: Path via double-closed vertex: dist=%f+%f=%f", w->dist[0], w->dist[1], d2c);
 317
 318   // Look for edges closed0-closed1, which might yield a shorter path
 319   double d01 = d2c;
 320   struct graph_vertex *ev = NULL, *ew = NULL;
 321   struct graph_edge *ee = NULL;
 322
 323   CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
 324     if (v->state[0] == 2)
 325       CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
 326         {
 327           struct graph_vertex *w = e->dest;
 328           if (w->state[1] == 2)
 329             {
 330               double d = v->dist[0] + e->length + w->dist[1];
 331               if (d < d01)
 332                 d01 = d, ev = v, ew = w, ee = e;
 333             }
 334         }
 335
 336   if (d01 < d2c)
 337     {
 338       msg(L_INFO, "Dijkstra: Better path via extra edge: dist=%f+%f+%f=%f", ev->dist[0], ee->length, ew->dist[1], d01);
 339       ev->via_edge[1] = ee->twin;
 340       w = ev;
 341     }
 342
 343   msg(L_DEBUG, "Dijkstra: Stats: %u/%u opened, %u/%u closed", opened[0], opened[1], closed[0], closed[1]);
 344   visualize_states(v_src, v_dest);
 345   visualize_path(w, 0);
 346   visualize_path(w, 1);
 347 }
 348
 349 static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
 350 {
 351   msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v_src->node->o.id, (uintmax_t) v_dest->node->o.id);
 352
 353   v_src->state[0] = 1;
 354   v_src->dist[0] = 0;
 355
 356   uint opened = 1, closed = 0;
 357   struct graph_vertex *w;
 358
 359   for (;;)
 360     {
 361       double best_d = 0;
 362       w = NULL;
 363
 364       CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
 365         if (v->state[0] == 1)
 366           {
 367             double d = v->dist[0];
 368             if (astar)
 369               d += vertex_dist(v, v_dest);
 370             if (!w || d < best_d)
 371               w = v, best_d = d;
 372           }
 373
 374       if (!w)
 375         die("Path not found");
 376
 377       // msg(L_DEBUG, "Dijkstra: closing vertex #%jd", w->node->o.id);
 378       closed++;
 379
 380       if (w == v_dest)
 381         break;
 382
 383       w->state[0] = 2;
 384       CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
 385         {
 386           struct graph_vertex *x = e->dest;
 387           double d = w->dist[0] + e->length;
 388           // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state[0], x->dist[0], d);
 389           if (x->state[0] == 0 || x->dist[0] > d)
 390             {
 391               if (x->state[0] == 2)
 392                 msg(L_WARN, "Re-opening node #%jd", (intmax_t) x->node->o.id);
 393               x->state[0] = 1;
 394               x->dist[0] = d;
 395               x->via_edge[0] = e;
 396               opened++;
 397             }
 398         }
 399     }
 400
 401   msg(L_INFO, "Dijkstra: Found path: dist=%f", w->dist[0]);
 402   msg(L_INFO, "Dijkstra: Stats: %u opened, %u closed", opened, closed);
 403   visualize_states(v_src, v_dest);
 404   visualize_path(v_dest, 0);
 405 }
 406
 407 void graph_build(void)
 408 {
 409   graph_pool = mp_new(65536);
 410   clist_init(&graph_vertices);
 411
 412   // We are considering only the first data source for the graph.
 413   // Otherwise, things start becoming ugly, because node IDs are generally not unique.
 414
 415   struct data_source *ds = clist_head(&map_sources);
 416   ASSERT(ds);
 417   osm_this = ds->osm;
 418   CLIST_FOR_EACH(struct osm_way *, w, osm_this->obj_list[OSM_TYPE_WAY])
 419     {
 420       if (way_is_edge(w))
 421         way_add_edge(w);
 422     }
 423   msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
 424
 425   graph_optimize();
 426   msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
 427
 428   graph_check_edges();
 429
 430   // struct graph_vertex *v1 = graph_vertex_by_node_id(30002893);       // Praha-Zbraslav, K Přehradám x 101
 431   struct graph_vertex *v1 = graph_vertex_by_node_id(96745573);  // Beroun, Lidická x Pražská
 432   // struct graph_vertex *v2 = graph_vertex_by_node_id(21289321);       // Brno, Heršpická x Opuštěná
 433   // struct graph_vertex *v2 = graph_vertex_by_node_id(271385400);      // Znojmo, Vídeňská třída x Brněnská
 434   // struct graph_vertex *v2 = graph_vertex_by_node_id(26753344);       // Šumperk, Jesenická x Lidická
 435   struct graph_vertex *v2 = graph_vertex_by_node_id(714908910); // Ostrava, Mariánskohorská x Plzeňská x 28. října
 436   ASSERT(graph_degree(v1) && graph_degree(v2));
 437
 438   if (bidir)
 439     bidir_dijkstra(v1, v2);
 440   else
 441     dijkstra(v1, v2);
 442 }