From 67bd2974c596eb4bf10d448cda931759ec8270d0 Mon Sep 17 00:00:00 2001 From: Martin Mares Date: Mon, 14 Mar 2022 17:19:23 +0100 Subject: [PATCH] =?utf8?q?Pr=C5=AFvodce:=20Dijkstra=20and=20its=20visualiz?= =?utf8?q?ation?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- graph.c | 148 ++++++++++++++++++++++++++++++++++++++++++--------- map.c | 2 +- pruvodce.css | 26 +++++++++ 3 files changed, 151 insertions(+), 25 deletions(-) diff --git a/graph.c b/graph.c index f82f06a..0df021e 100644 --- a/graph.c +++ b/graph.c @@ -24,6 +24,11 @@ struct graph_vertex { cnode n; struct osm_node *node; clist edges; + + // Dijkstra + double dist; + int state; // 0=unseen, 1=open, 2=closed + struct graph_edge *via_edge; }; struct graph_edge { @@ -31,7 +36,7 @@ struct graph_edge { struct graph_vertex *dest; struct graph_edge *twin; double length; - struct osm_way *way; + clist ways; }; static uint num_vertices; @@ -61,24 +66,41 @@ static struct graph_vertex *graph_vertex_by_node_id(osm_id_t id) 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 uint graph_degree(struct graph_vertex *v) { - struct graph_edge *e1 = mp_alloc_zero(graph_pool, sizeof(*e1)); - struct graph_edge *e2 = mp_alloc_zero(graph_pool, sizeof(*e2)); - num_edges++; + uint d = 0; - clist_add_tail(&u->edges, &e1->n); - e1->dest = v; - e1->twin = e2; - e1->length = length; - e1->way = way; + CLIST_FOR_EACH(struct graph_edge *, e, v->edges) + d++; + return d; +} - clist_add_tail(&v->edges, &e2->n); - e2->dest = u; - e2->twin = e1; - e2->length = length; - e2->way = way; +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; } @@ -88,24 +110,27 @@ static void graph_optimize(void) 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); } } } @@ -147,6 +172,79 @@ static void way_add_edge(struct osm_way *w) 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 visualize(struct graph_vertex *v_src, struct graph_vertex *v_dest) +{ + CLIST_FOR_EACH(struct graph_vertex *, x, graph_vertices) + { + if (x->state) + osm_obj_set_tag(&x->node->o, "pruvodce", "visited"); + if (x->state == 2) + CLIST_FOR_EACH(struct graph_edge *, e, x->edges) + mark_edge_ways(e, "pruvodce", "visited"); + } + + struct graph_vertex *x = v_dest; + while (x->via_edge) + { + struct graph_edge *e = x->via_edge->twin; + mark_edge_ways(e, "pruvodce", "path"); + x = e->dest; + } + + osm_obj_set_tag(&v_src->node->o, "pruvodce", "src"); + osm_obj_set_tag(&v_dest->node->o, "pruvodce", "dest"); +} + +static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest) +{ + v_src->state = 1; + v_src->dist = 0; + + for (;;) + { + struct graph_vertex *w = NULL; + CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices) + if (v->state == 1 && (!w || v->dist < w->dist)) + w = v; + + if (!w) + die("Path not found"); + + // msg(L_DEBUG, "Dijkstra: closing vertex #%jd", w->node->o.id); + + if (w == v_dest) + { + msg(L_INFO, "Found path: dist=%f", w->dist); + visualize(v_src, v_dest); + return; + } + + w->state = 2; + CLIST_FOR_EACH(struct graph_edge *, e, w->edges) + { + struct graph_vertex *x = e->dest; + double d = w->dist + e->length; + // msg(L_DEBUG, "Neighbor: #%jd, state=%d, dist=%f vs. %f", x->node->o.id, x->state, x->dist, d); + if (x->state == 0 || x->dist > d) + { + ASSERT(x->state != 2); + x->state = 1; + x->dist = d; + x->via_edge = e; + } + } + } +} + void graph_build(void) { graph_pool = mp_new(65536); @@ -168,7 +266,9 @@ void graph_build(void) graph_optimize(); msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges); - struct graph_vertex *v1 = graph_vertex_by_node_id(32292698); // Praha-Zbraslav, K Přehradám + struct graph_vertex *v1 = graph_vertex_by_node_id(30002893); // Praha-Zbraslav, K Přehradám x 101 struct graph_vertex *v2 = graph_vertex_by_node_id(21289321); // Brno, Heršpická x Opuštěná + ASSERT(graph_degree(v1) && graph_degree(v2)); msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v1->node->o.id, (uintmax_t) v2->node->o.id); + dijkstra(v1, v2); } diff --git a/map.c b/map.c index 366d17a..5b8e92b 100644 --- a/map.c +++ b/map.c @@ -347,7 +347,7 @@ static void generalize_recursively(struct gen_context *gc, int first, int last) max_err = MAX(max_err, e); } - double close = 5; + double close = 1; if (max_err > close*close) { int mid = (first + last) / 2; diff --git a/pruvodce.css b/pruvodce.css index efcd38b..ce8e590 100644 --- a/pruvodce.css +++ b/pruvodce.css @@ -92,3 +92,29 @@ way[highway=service] { way[highway=proposed] { width: 0; } + +/*** Dijkstra ***/ + +way[pruvodce=visited] { + color: #00f; +} + +way[pruvodce=path] { + color: #f00; + width: 1; + z-index: 10; +} + +node[pruvodce=visited] { + symbol-shape: circle; + symbol-size: 0.7; + symbol-fill-color: #00f; + z-index: 10; +} + +node[pruvodce=src], node[pruvodce=dest] { + symbol-shape: circle; + symbol-size: 2; + symbol-fill-color: #f0f; + z-index: 10; +} -- 2.39.2