From: Martin Mares Date: Sat, 26 Mar 2022 23:20:00 +0000 (+0100) Subject: Průvodce: Bi-directional Dijkstra X-Git-Url: http://mj.ucw.cz/gitweb/?a=commitdiff_plain;h=46f4a491a789be8f125c4152defff0bacf6c1c9a;p=leo.git Průvodce: Bi-directional Dijkstra --- diff --git a/graph.c b/graph.c index 8112a44..e258af8 100644 --- a/graph.c +++ b/graph.c @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -17,6 +18,22 @@ #include "map.h" #include "graph.h" +static uint bidir; +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; @@ -25,10 +42,10 @@ struct graph_vertex { struct osm_node *node; clist edges; - // Dijkstra - double dist; - int state; // 0=unseen, 1=open, 2=closed - struct graph_edge *via_edge; + // Dijkstra forward + backward + double dist[2]; + int state[2]; // 0=unseen, 1=open, 2=closed + struct graph_edge *via_edge[2]; }; struct graph_edge { @@ -194,33 +211,139 @@ static void graph_check_edges(void) } } -static void visualize(struct graph_vertex *v_src, struct graph_vertex *v_dest) +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) + if (x->state[0]) 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"); + 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) + while (x->via_edge[dir]) { - struct graph_edge *e = x->via_edge->twin; - mark_edge_ways(e, "pruvodce", "path"); + struct graph_edge *e = x->via_edge[dir]->twin; + mark_edge_ways(e, (dir ? "pruvodce2" : "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 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 open[2] = { 1, 1 }; + struct graph_vertex *w; + + for (;;) + { + double best_d = 0; + uint dir = (open[0] <= open[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); + + 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; + open[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; + } + + 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) { - v_src->state = 1; - v_src->dist = 0; + 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; for (;;) { @@ -228,10 +351,11 @@ static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest) double best_d = 0; CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices) - if (v->state == 1) + if (v->state[0] == 1) { - double d = v->dist; - d += vertex_dist(v, v_dest); + double d = v->dist[0]; + if (astar) + d += vertex_dist(v, v_dest); if (!w || d < best_d) w = v, best_d = d; } @@ -243,24 +367,25 @@ static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest) if (w == v_dest) { - msg(L_INFO, "Found path: dist=%f", w->dist); - visualize(v_src, v_dest); + msg(L_INFO, "Found path: dist=%f", w->dist[0]); + visualize_states(v_src, v_dest); + visualize_path(v_dest, 0); return; } - w->state = 2; + w->state[0] = 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) + 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 == 2) + if (x->state[0] == 2) msg(L_WARN, "Re-opening node #%jd", (intmax_t) x->node->o.id); - x->state = 1; - x->dist = d; - x->via_edge = e; + x->state[0] = 1; + x->dist[0] = d; + x->via_edge[0] = e; } } } @@ -296,6 +421,9 @@ void graph_build(void) // 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)); - msg(L_INFO, "Finding path from #%jd to #%jd", (uintmax_t) v1->node->o.id, (uintmax_t) v2->node->o.id); - dijkstra(v1, v2); + + if (bidir) + bidir_dijkstra(v1, v2); + else + dijkstra(v1, v2); } diff --git a/mk b/mk new file mode 100755 index 0000000..42aa180 --- /dev/null +++ b/mk @@ -0,0 +1,5 @@ +#!/bin/sh +set -e +make +run/bin/leo +inkscape -Do output.pdf output.svg diff --git a/mk-all b/mk-all new file mode 100755 index 0000000..1f540d4 --- /dev/null +++ b/mk-all @@ -0,0 +1,21 @@ +#!/bin/sh +set -e +make +for a in 0 1 ; do + for b in 0 1 ; do + out="output-" + case $a in + 0) out="$out-dijk" ;; + 1) out="$out-astar" ;; + *) exit 1 ;; + esac + case $b in + 0) out="$out-unidir" ;; + 1) out="$out-bidir" ;; + *) exit 1 ;; + esac + echo "### Running with astar=$a bidir=$b ($out) ###" + run/bin/leo -SGraph.AStar=$a -SGraph.BiDir=$b -SMap.SVGOutput=$out.svg + inkscape -Do output-$out.pdf output-$out.svg + done +done diff --git a/pruvodce.css b/pruvodce.css index ce8e590..d384b2d 100644 --- a/pruvodce.css +++ b/pruvodce.css @@ -115,6 +115,25 @@ node[pruvodce=visited] { node[pruvodce=src], node[pruvodce=dest] { symbol-shape: circle; symbol-size: 2; - symbol-fill-color: #f0f; + symbol-fill-color: #c0c; + z-index: 10; +} + +/*** Backward Dijkstra ***/ + +way[pruvodce2=visited] { + color: #0ff; +} + +way[pruvodce2=path] { + color: #0f0; + width: 1; + z-index: 10; +} + +node[pruvodce2=visited] { + symbol-shape: circle; + symbol-size: 0.7; + symbol-fill-color: #c0c; z-index: 10; }