+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);