X-Git-Url: http://mj.ucw.cz/gitweb/?a=blobdiff_plain;f=graph.c;h=b7eeabf73f7a92c6e3711ebd9fe6ac71089611c8;hb=refs%2Fheads%2Fpruvodce;hp=8112a44a8fc59a543f30a00d657c29da73aff41e;hpb=8e2cca5c154623c72d2b4a0301e41eb22d732a26;p=leo.git diff --git a/graph.c b/graph.c index 8112a44..b7eeabf 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; // 0=unidir, 1=bidir balanced by #opened, 2=bidir balanced by distance +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,44 +211,162 @@ 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 opened[2] = { 1, 1 }, closed[2] = { 0, 0 }; + double last_dist[2] = { 0, 0 }; + struct graph_vertex *w; + + for (;;) + { + double best_d = 0; + uint dir; + if (bidir == 2) + dir = (last_dist[0] <= last_dist[1]) ? 0 : 1; + else + dir = (opened[0] <= opened[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); + closed[dir]++; + last_dist[dir] = best_d; + + 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; + opened[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; + } + + msg(L_DEBUG, "Dijkstra: Stats: %u/%u opened, %u/%u closed", opened[0], opened[1], closed[0], closed[1]); + 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; + + uint opened = 1, closed = 0; + struct graph_vertex *w; for (;;) { - struct graph_vertex *w = NULL; double best_d = 0; + w = NULL; 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; } @@ -240,30 +375,33 @@ static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest) die("Path not found"); // msg(L_DEBUG, "Dijkstra: closing vertex #%jd", w->node->o.id); + closed++; if (w == v_dest) - { - msg(L_INFO, "Found path: dist=%f", w->dist); - visualize(v_src, v_dest); - return; - } + break; - 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; + opened++; } } } + + msg(L_INFO, "Dijkstra: Found path: dist=%f", w->dist[0]); + msg(L_INFO, "Dijkstra: Stats: %u opened, %u closed", opened, closed); + visualize_states(v_src, v_dest); + visualize_path(v_dest, 0); } void graph_build(void) @@ -293,9 +431,12 @@ void graph_build(void) struct graph_vertex *v1 = graph_vertex_by_node_id(96745573); // Beroun, Lidická x Pražská // struct graph_vertex *v2 = graph_vertex_by_node_id(21289321); // Brno, Heršpická x Opuštěná // struct graph_vertex *v2 = graph_vertex_by_node_id(271385400); // Znojmo, Vídeňská třída x Brněnská - // 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 + 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); }