Průvodce: Bi-directional Dijkstra

diff --git a/graph.c b/graph.c
index 8112a44a8fc59a543f30a00d657c29da73aff41e..e258af8d9bf5b23e7956a74c6da766c1a5ba2186 100644 (file)
--- a/graph.c
+++ b/graph.c
@@ -8,6 +8,7 @@
 
 #include <ucw/lib.h>
 #include <ucw/clists.h>
+#include <ucw/conf.h>
 #include <ucw/mempool.h>
 
 #include <stdio.h>
@@ -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 (executable)
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 (executable)
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 ce8e590a0d43f9284fd0b798b0c005b2fd0b33d5..d384b2d0913e9d8000960c408e797dfc9c8a3338 100644 (file)
--- 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;
 }