* (c) 2022 Martin Mares <mj@ucw.cz>
*/
+#include "leo.h"
+
#include <ucw/lib.h>
#include <ucw/clists.h>
+#include <ucw/conf.h>
#include <ucw/mempool.h>
#include <stdio.h>
#include <math.h>
-#include "leo.h"
#include "osm.h"
#include "map.h"
#include "graph.h"
+static uint bidir; // 0=unidir, 1=bidir balanced by #opened, 2=bidir balanced by distance
+static uint astar;
+
+static struct cf_section graph_cf = {
+ CF_ITEMS {
+ CF_UNS("BiDir", &bidir),
+ CF_UNS("AStar", &astar),
+ CF_END
+ }
+};
+
+static void CONSTRUCTOR map_preinit(void)
+{
+ cf_declare_section("Graph", &graph_cf, 0);
+}
+
static struct mempool *graph_pool;
static clist graph_vertices;
cnode n;
struct osm_node *node;
clist edges;
+
+ // Dijkstra forward + backward
+ double dist[2];
+ int state[2]; // 0=unseen, 1=open, 2=closed
+ struct graph_edge *via_edge[2];
};
struct graph_edge {
struct graph_vertex *dest;
struct graph_edge *twin;
double length;
- struct osm_way *way;
+ clist ways;
};
static uint num_vertices;
return n->vertex;
}
-static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
+static struct graph_vertex *graph_vertex_by_node_id(osm_id_t id)
{
- struct graph_edge *e1 = mp_alloc_zero(graph_pool, sizeof(*e1));
- struct graph_edge *e2 = mp_alloc_zero(graph_pool, sizeof(*e2));
- num_edges++;
+ struct osm_node *n = (struct osm_node *) osm_obj_find_by_id(OSM_TYPE_NODE, id);
+ if (!n)
+ die("Cannot find node #%jd", (uintmax_t) id);
+ ASSERT(n->vertex);
+ ASSERT(n->vertex->node);
+ return n->vertex;
+}
- clist_add_tail(&u->edges, &e1->n);
- e1->dest = v;
- e1->twin = e2;
- e1->length = length;
- e1->way = way;
+static uint graph_degree(struct graph_vertex *v)
+{
+ uint d = 0;
- clist_add_tail(&v->edges, &e2->n);
- e2->dest = u;
- e2->twin = e1;
- e2->length = length;
- e2->way = way;
+ CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
+ d++;
+ return d;
+}
+static struct graph_edge *add_half_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
+{
+ struct graph_edge *e = mp_alloc_zero(graph_pool, sizeof(*e));
+
+ clist_add_tail(&u->edges, &e->n);
+ e->dest = v;
+ e->length = length;
+ clist_init(&e->ways);
+
+ if (way)
+ {
+ struct osm_ref *ref = mp_alloc_zero(graph_pool, sizeof(*ref));
+ ref->o = &way->o;
+ clist_add_tail(&e->ways, &ref->n);
+ }
+
+ return e;
+}
+
+static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
+{
+ struct graph_edge *e1 = add_half_edge(u, v, way, length);
+ struct graph_edge *e2 = add_half_edge(v, u, way, length);
+ e1->twin = e2;
+ e2->twin = e1;
+ num_edges++;
return e1;
}
CLIST_WALK_DELSAFE(v, graph_vertices, tmp)
{
- uint deg = 0;
- CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
- deg++;
-
- if (deg == 2)
+ if (graph_degree(v) == 2)
{
struct graph_edge *e1 = clist_remove_head(&v->edges);
+ struct graph_edge *e2 = e1->twin;
struct graph_edge *f1 = clist_remove_head(&v->edges);
+ struct graph_edge *f2 = f1->twin;
struct graph_vertex *x = e1->dest;
struct graph_vertex *y = f1->dest;
- clist_remove(&e1->twin->n);
- clist_remove(&f1->twin->n);
+ clist_remove(&e2->n);
+ clist_remove(&f2->n);
num_edges -= 2;
clist_remove(&v->n);
num_vertices--;
- graph_add_edge(x, y, e1->way, e1->length + f1->length);
+ struct graph_edge *g1 = graph_add_edge(x, y, NULL, e1->length + f1->length);
+ struct graph_edge *g2 = g1->twin;
+ clist_add_list_tail(&g1->ways, &e1->ways);
+ clist_add_list_tail(&g1->ways, &f1->ways);
+ clist_add_list_tail(&g2->ways, &f2->ways);
+ clist_add_list_tail(&g2->ways, &e2->ways);
}
}
}
return 0;
}
+static double vertex_dist(struct graph_vertex *a, struct graph_vertex *b)
+{
+ struct osm_node *aa = a->node;
+ struct osm_node *bb = b->node;
+ return hypot(bb->x - aa->x, bb->y - aa->y);
+}
+
static void way_add_edge(struct osm_way *w)
{
- struct osm_node *prev_n = NULL;
struct graph_vertex *prev_v = NULL;
OSM_FOR_EACH_BEGIN(struct osm_node *, n, w->nodes)
{
struct graph_vertex *v = graph_lookup_vertex(n);
if (prev_v)
- {
- double length = hypot(n->x - prev_n->x, n->y - prev_n->y);
- graph_add_edge(prev_v, v, w, length);
- }
- prev_n = n;
+ graph_add_edge(prev_v, v, w, vertex_dist(v, prev_v));
prev_v = v;
}
OSM_FOR_EACH_END;
}
+static void mark_edge_ways(struct graph_edge *e, const char *key, const char *val)
+{
+ OSM_FOR_EACH_BEGIN(struct osm_way *, w, e->ways)
+ {
+ osm_obj_set_tag(&w->o, key, val);
+ }
+ OSM_FOR_EACH_END;
+}
+
+static void graph_check_edges(void)
+{
+ CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
+ CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
+ {
+ double d = vertex_dist(e->dest, v);
+ if (d > e->length)
+ 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);
+ }
+}
+
+static void visualize_states(struct graph_vertex *v_src, struct graph_vertex *v_dest)
+{
+ CLIST_FOR_EACH(struct graph_vertex *, x, graph_vertices)
+ {
+ if (x->state[0])
+ osm_obj_set_tag(&x->node->o, "pruvodce", "visited");
+ if (x->state[1])
+ osm_obj_set_tag(&x->node->o, "pruvodce2", "visited");
+ if (x->state[0] == 2)
+ {
+ CLIST_FOR_EACH(struct graph_edge *, e, x->edges)
+ mark_edge_ways(e, "pruvodce", "visited");
+ }
+ if (x->state[1] == 2)
+ {
+ CLIST_FOR_EACH(struct graph_edge *, e, x->edges)
+ mark_edge_ways(e, "pruvodce2", "visited");
+ }
+ }
+
+ osm_obj_set_tag(&v_src->node->o, "pruvodce", "src");
+ osm_obj_set_tag(&v_dest->node->o, "pruvodce", "dest");
+}
+
+static void visualize_path(struct graph_vertex *v_dest, uint dir)
+{
+ struct graph_vertex *x = v_dest;
+ while (x->via_edge[dir])
+ {
+ struct graph_edge *e = x->via_edge[dir]->twin;
+ mark_edge_ways(e, (dir ? "pruvodce2" : "pruvodce"), "path");
+ x = e->dest;
+ }
+}
+
+static void bidir_dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
+{
+ msg(L_INFO, "Finding bidirectional path from #%jd to #%jd", (uintmax_t) v_src->node->o.id, (uintmax_t) v_dest->node->o.id);
+
+ v_src->state[0] = 1;
+ v_src->dist[0] = 0;
+
+ v_dest->state[1] = 1;
+ v_dest->dist[1] = 0;
+
+ uint opened[2] = { 1, 1 }, closed[2] = { 0, 0 };
+ double last_dist[2] = { 0, 0 };
+ struct graph_vertex *w;
+
+ for (;;)
+ {
+ double best_d = 0;
+ uint dir;
+ if (bidir == 2)
+ dir = (last_dist[0] <= last_dist[1]) ? 0 : 1;
+ else
+ dir = (opened[0] <= opened[1]) ? 0 : 1;
+ w = NULL;
+ // msg(L_DEBUG, "Dijkstra: dir=%d", dir);
+
+ CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
+ {
+ if (v->state[dir] == 1)
+ {
+ double d = v->dist[dir];
+ if (astar)
+ d += vertex_dist(v, (dir ? v_src : v_dest));
+ if (!w || d < best_d)
+ w = v, best_d = d;
+ }
+ }
+
+ if (!w)
+ die("Path not found");
+
+ // msg(L_DEBUG, "Dijkstra: closing vertex #%jd (d=%f, dir %d)", w->node->o.id, best_d, dir);
+ closed[dir]++;
+ last_dist[dir] = best_d;
+
+ w->state[dir] = 2;
+ if (w->state[!dir] == 2)
+ break;
+
+ CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
+ {
+ struct graph_vertex *x = e->dest;
+ double d = w->dist[dir] + e->length;
+ // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state[dir], x->dist[dir], d);
+ if (x->state[dir] == 0 || x->dist[dir] > d)
+ {
+ if (x->state[dir] == 2)
+ msg(L_WARN, "Re-opening node #%jd in dir %d", (intmax_t) x->node->o.id, dir);
+ x->state[dir] = 1;
+ x->dist[dir] = d;
+ x->via_edge[dir] = e;
+ opened[dir]++;
+ }
+ }
+ }
+
+ // One vertex was closed in both direction
+ double d2c = w->dist[0] + w->dist[1];
+ msg(L_INFO, "Dijkstra: Path via double-closed vertex: dist=%f+%f=%f", w->dist[0], w->dist[1], d2c);
+
+ // Look for edges closed0-closed1, which might yield a shorter path
+ double d01 = d2c;
+ struct graph_vertex *ev = NULL, *ew = NULL;
+ struct graph_edge *ee = NULL;
+
+ CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
+ if (v->state[0] == 2)
+ CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
+ {
+ struct graph_vertex *w = e->dest;
+ if (w->state[1] == 2)
+ {
+ double d = v->dist[0] + e->length + w->dist[1];
+ if (d < d01)
+ d01 = d, ev = v, ew = w, ee = e;
+ }
+ }
+
+ if (d01 < d2c)
+ {
+ msg(L_INFO, "Dijkstra: Better path via extra edge: dist=%f+%f+%f=%f", ev->dist[0], ee->length, ew->dist[1], d01);
+ ev->via_edge[1] = ee->twin;
+ w = ev;
+ }
+
+ msg(L_DEBUG, "Dijkstra: Stats: %u/%u opened, %u/%u closed", opened[0], opened[1], closed[0], closed[1]);
+ visualize_states(v_src, v_dest);
+ visualize_path(w, 0);
+ visualize_path(w, 1);
+}
+
+static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
+{
+ msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v_src->node->o.id, (uintmax_t) v_dest->node->o.id);
+
+ v_src->state[0] = 1;
+ v_src->dist[0] = 0;
+
+ uint opened = 1, closed = 0;
+ struct graph_vertex *w;
+
+ for (;;)
+ {
+ double best_d = 0;
+ w = NULL;
+
+ CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
+ if (v->state[0] == 1)
+ {
+ double d = v->dist[0];
+ if (astar)
+ d += vertex_dist(v, v_dest);
+ if (!w || d < best_d)
+ w = v, best_d = d;
+ }
+
+ if (!w)
+ die("Path not found");
+
+ // msg(L_DEBUG, "Dijkstra: closing vertex #%jd", w->node->o.id);
+ closed++;
+
+ if (w == v_dest)
+ break;
+
+ w->state[0] = 2;
+ CLIST_FOR_EACH(struct graph_edge *, e, w->edges)
+ {
+ struct graph_vertex *x = e->dest;
+ double d = w->dist[0] + e->length;
+ // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state[0], x->dist[0], d);
+ if (x->state[0] == 0 || x->dist[0] > d)
+ {
+ if (x->state[0] == 2)
+ msg(L_WARN, "Re-opening node #%jd", (intmax_t) x->node->o.id);
+ x->state[0] = 1;
+ x->dist[0] = d;
+ x->via_edge[0] = e;
+ opened++;
+ }
+ }
+ }
+
+ msg(L_INFO, "Dijkstra: Found path: dist=%f", w->dist[0]);
+ msg(L_INFO, "Dijkstra: Stats: %u opened, %u closed", opened, closed);
+ visualize_states(v_src, v_dest);
+ visualize_path(v_dest, 0);
+}
+
void graph_build(void)
{
graph_pool = mp_new(65536);
clist_init(&graph_vertices);
- CLIST_FOR_EACH(struct data_source *, ds, map_sources)
- CLIST_FOR_EACH(struct osm_way *, w, ds->osm->obj_list[OSM_TYPE_WAY])
- {
- if (way_is_edge(w))
- way_add_edge(w);
- }
+ // We are considering only the first data source for the graph.
+ // Otherwise, things start becoming ugly, because node IDs are generally not unique.
+
+ struct data_source *ds = clist_head(&map_sources);
+ ASSERT(ds);
+ osm_this = ds->osm;
+ CLIST_FOR_EACH(struct osm_way *, w, osm_this->obj_list[OSM_TYPE_WAY])
+ {
+ if (way_is_edge(w))
+ way_add_edge(w);
+ }
msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
graph_optimize();
msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
+
+ graph_check_edges();
+
+ // struct graph_vertex *v1 = graph_vertex_by_node_id(30002893); // Praha-Zbraslav, K Přehradám x 101
+ struct graph_vertex *v1 = graph_vertex_by_node_id(96745573); // Beroun, Lidická x Pražská
+ // struct graph_vertex *v2 = graph_vertex_by_node_id(21289321); // Brno, Heršpická x Opuštěná
+ // struct graph_vertex *v2 = graph_vertex_by_node_id(271385400); // Znojmo, Vídeňská třída x Brněnská
+ struct graph_vertex *v2 = graph_vertex_by_node_id(26753344); // Šumperk, Jesenická x Lidická
+ // struct graph_vertex *v2 = graph_vertex_by_node_id(714908910); // Ostrava, Mariánskohorská x Plzeňská x 28. října
+ ASSERT(graph_degree(v1) && graph_degree(v2));
+
+ if (bidir)
+ bidir_dijkstra(v1, v2);
+ else
+ dijkstra(v1, v2);
}
projects / leo.git / blobdiff