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