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 }