graph.c

   1 /*
   2  *      Hic Est Leo -- Graph and Routing
   3  *
   4  *      (c) 2022 Martin Mares <mj@ucw.cz>
   5  */
   6
   7 #include <ucw/lib.h>
   8 #include <ucw/clists.h>
   9 #include <ucw/mempool.h>
  10
  11 #include <stdio.h>
  12 #include <math.h>
  13
  14 #include "leo.h"
  15 #include "osm.h"
  16 #include "map.h"
  17 #include "graph.h"
  18
  19 static struct mempool *graph_pool;
  20 static clist graph_vertices;
  21
  22 struct graph_vertex {
  23   cnode n;
  24   struct osm_node *node;
  25   clist edges;
  26 };
  27
  28 struct graph_edge {
  29   cnode n;                      // In src->edges
  30   struct graph_vertex *dest;
  31   struct graph_edge *twin;
  32   double length;
  33   struct osm_way *way;
  34 };
  35
  36 static uint num_vertices;
  37 static uint num_edges;
  38
  39 static struct graph_vertex *graph_lookup_vertex(struct osm_node *n)
  40 {
  41   if (!n->vertex)
  42     {
  43       struct graph_vertex *v = mp_alloc_zero(graph_pool, sizeof(*v));
  44       v->node = n;
  45       n->vertex = v;
  46       clist_init(&v->edges);
  47       clist_add_tail(&graph_vertices, &v->n);
  48       num_vertices++;
  49     }
  50   return n->vertex;
  51 }
  52
  53 static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
  54 {
  55   struct graph_edge *e1 = mp_alloc_zero(graph_pool, sizeof(*e1));
  56   struct graph_edge *e2 = mp_alloc_zero(graph_pool, sizeof(*e2));
  57   num_edges++;
  58
  59   clist_add_tail(&u->edges, &e1->n);
  60   e1->dest = v;
  61   e1->twin = e2;
  62   e1->length = length;
  63   e1->way = way;
  64
  65   clist_add_tail(&v->edges, &e2->n);
  66   e2->dest = u;
  67   e2->twin = e1;
  68   e2->length = length;
  69   e2->way = way;
  70
  71   return e1;
  72 }
  73
  74 static void graph_optimize(void)
  75 {
  76   struct graph_vertex *v, *tmp;
  77
  78   CLIST_WALK_DELSAFE(v, graph_vertices, tmp)
  79     {
  80       uint deg = 0;
  81       CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
  82         deg++;
  83
  84       if (deg == 2)
  85         {
  86           struct graph_edge *e1 = clist_remove_head(&v->edges);
  87           struct graph_edge *f1 = clist_remove_head(&v->edges);
  88           struct graph_vertex *x = e1->dest;
  89           struct graph_vertex *y = f1->dest;
  90           clist_remove(&e1->twin->n);
  91           clist_remove(&f1->twin->n);
  92           num_edges -= 2;
  93
  94           clist_remove(&v->n);
  95           num_vertices--;
  96
  97           graph_add_edge(x, y, e1->way, e1->length + f1->length);
  98         }
  99     }
 100 }
 101
 102 static bool way_is_edge(struct osm_way *w)
 103 {
 104   osm_val_t hwy = osm_obj_find_tag(&w->o, KEY_HIGHWAY);
 105   switch (hwy) {
 106     case VALUE_MOTORWAY:
 107     case VALUE_TRUNK:
 108     case VALUE_PRIMARY:
 109     case VALUE_SECONDARY:
 110     case VALUE_MOTORWAY_LINK:
 111     case VALUE_TRUNK_LINK:
 112     case VALUE_PRIMARY_LINK:
 113     case VALUE_SECONDARY_LINK:
 114       return 1;
 115   }
 116
 117   return 0;
 118 }
 119
 120 static void way_add_edge(struct osm_way *w)
 121 {
 122   struct osm_node *prev_n = NULL;
 123   struct graph_vertex *prev_v = NULL;
 124
 125   OSM_FOR_EACH_BEGIN(struct osm_node *, n, w->nodes)
 126     {
 127       struct graph_vertex *v = graph_lookup_vertex(n);
 128       if (prev_v)
 129         {
 130           double length = hypot(n->x - prev_n->x, n->y - prev_n->y);
 131           graph_add_edge(prev_v, v, w, length);
 132         }
 133       prev_n = n;
 134       prev_v = v;
 135     }
 136   OSM_FOR_EACH_END;
 137 }
 138
 139 void graph_build(void)
 140 {
 141   graph_pool = mp_new(65536);
 142   clist_init(&graph_vertices);
 143
 144   CLIST_FOR_EACH(struct data_source *, ds, map_sources)
 145     CLIST_FOR_EACH(struct osm_way *, w, ds->osm->obj_list[OSM_TYPE_WAY])
 146       {
 147         if (way_is_edge(w))
 148           way_add_edge(w);
 149       }
 150   msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
 151
 152   graph_optimize();
 153   msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
 154 }