* (c) 2022 Martin Mares <mj@ucw.cz>
*/
+#include "leo.h"
+
#include <ucw/lib.h>
#include <ucw/clists.h>
#include <ucw/mempool.h>
#include <stdio.h>
#include <math.h>
-#include "leo.h"
#include "osm.h"
#include "map.h"
#include "graph.h"
cnode n;
struct osm_node *node;
clist edges;
+
+ // Dijkstra
+ double dist;
+ int state; // 0=unseen, 1=open, 2=closed
+ struct graph_edge *via_edge;
};
struct graph_edge {
struct graph_vertex *dest;
struct graph_edge *twin;
double length;
- struct osm_way *way;
+ clist ways;
};
static uint num_vertices;
return n->vertex;
}
-static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
+static struct graph_vertex *graph_vertex_by_node_id(osm_id_t id)
{
- struct graph_edge *e1 = mp_alloc_zero(graph_pool, sizeof(*e1));
- struct graph_edge *e2 = mp_alloc_zero(graph_pool, sizeof(*e2));
- num_edges++;
+ struct osm_node *n = (struct osm_node *) osm_obj_find_by_id(OSM_TYPE_NODE, id);
+ if (!n)
+ die("Cannot find node #%jd", (uintmax_t) id);
+ ASSERT(n->vertex);
+ ASSERT(n->vertex->node);
+ return n->vertex;
+}
- clist_add_tail(&u->edges, &e1->n);
- e1->dest = v;
- e1->twin = e2;
- e1->length = length;
- e1->way = way;
+static uint graph_degree(struct graph_vertex *v)
+{
+ uint d = 0;
- clist_add_tail(&v->edges, &e2->n);
- e2->dest = u;
- e2->twin = e1;
- e2->length = length;
- e2->way = way;
+ CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
+ d++;
+ return d;
+}
+
+static struct graph_edge *add_half_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
+{
+ struct graph_edge *e = mp_alloc_zero(graph_pool, sizeof(*e));
+
+ clist_add_tail(&u->edges, &e->n);
+ e->dest = v;
+ e->length = length;
+ clist_init(&e->ways);
+ if (way)
+ {
+ struct osm_ref *ref = mp_alloc_zero(graph_pool, sizeof(*ref));
+ ref->o = &way->o;
+ clist_add_tail(&e->ways, &ref->n);
+ }
+
+ return e;
+}
+
+static struct graph_edge *graph_add_edge(struct graph_vertex *u, struct graph_vertex *v, struct osm_way *way, double length)
+{
+ struct graph_edge *e1 = add_half_edge(u, v, way, length);
+ struct graph_edge *e2 = add_half_edge(v, u, way, length);
+ e1->twin = e2;
+ e2->twin = e1;
+ num_edges++;
return e1;
}
CLIST_WALK_DELSAFE(v, graph_vertices, tmp)
{
- uint deg = 0;
- CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
- deg++;
-
- if (deg == 2)
+ if (graph_degree(v) == 2)
{
struct graph_edge *e1 = clist_remove_head(&v->edges);
+ struct graph_edge *e2 = e1->twin;
struct graph_edge *f1 = clist_remove_head(&v->edges);
+ struct graph_edge *f2 = f1->twin;
struct graph_vertex *x = e1->dest;
struct graph_vertex *y = f1->dest;
- clist_remove(&e1->twin->n);
- clist_remove(&f1->twin->n);
+ clist_remove(&e2->n);
+ clist_remove(&f2->n);
num_edges -= 2;
clist_remove(&v->n);
num_vertices--;
- graph_add_edge(x, y, e1->way, e1->length + f1->length);
+ struct graph_edge *g1 = graph_add_edge(x, y, NULL, e1->length + f1->length);
+ struct graph_edge *g2 = g1->twin;
+ clist_add_list_tail(&g1->ways, &e1->ways);
+ clist_add_list_tail(&g1->ways, &f1->ways);
+ clist_add_list_tail(&g2->ways, &f2->ways);
+ clist_add_list_tail(&g2->ways, &e2->ways);
}
}
}
return 0;
}
+static double vertex_dist(struct graph_vertex *a, struct graph_vertex *b)
+{
+ struct osm_node *aa = a->node;
+ struct osm_node *bb = b->node;
+ return hypot(bb->x - aa->x, bb->y - aa->y);
+}
+
static void way_add_edge(struct osm_way *w)
{
- struct osm_node *prev_n = NULL;
struct graph_vertex *prev_v = NULL;
OSM_FOR_EACH_BEGIN(struct osm_node *, n, w->nodes)
{
struct graph_vertex *v = graph_lookup_vertex(n);
if (prev_v)
- {
- double length = hypot(n->x - prev_n->x, n->y - prev_n->y);
- graph_add_edge(prev_v, v, w, length);
- }
- prev_n = n;
+ graph_add_edge(prev_v, v, w, vertex_dist(v, prev_v));
prev_v = v;
}
OSM_FOR_EACH_END;
}
+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 graph_check_edges(void)
+{
+ CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
+ CLIST_FOR_EACH(struct graph_edge *, e, v->edges)
+ {
+ double d = vertex_dist(e->dest, v);
+ if (d > e->length)
+ msg(L_WARN, "Suspicious edge (#%jd, #%jd): length=%g dist=%g", (intmax_t) v->node->o.id, (intmax_t) e->dest->node->o.id, e->length, d);
+ }
+}
+
+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;
+ double best_d = 0;
+
+ CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
+ if (v->state == 1)
+ {
+ double d = v->dist;
+ d += vertex_dist(v, 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", 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)
+ {
+ if (x->state == 2)
+ msg(L_WARN, "Re-opening node #%jd", (intmax_t) x->node->o.id);
+ x->state = 1;
+ x->dist = d;
+ x->via_edge = e;
+ }
+ }
+ }
+}
+
void graph_build(void)
{
graph_pool = mp_new(65536);
clist_init(&graph_vertices);
- CLIST_FOR_EACH(struct data_source *, ds, map_sources)
- CLIST_FOR_EACH(struct osm_way *, w, ds->osm->obj_list[OSM_TYPE_WAY])
- {
- if (way_is_edge(w))
- way_add_edge(w);
- }
+ // We are considering only the first data source for the graph.
+ // Otherwise, things start becoming ugly, because node IDs are generally not unique.
+
+ struct data_source *ds = clist_head(&map_sources);
+ ASSERT(ds);
+ osm_this = ds->osm;
+ CLIST_FOR_EACH(struct osm_way *, w, osm_this->obj_list[OSM_TYPE_WAY])
+ {
+ if (way_is_edge(w))
+ way_add_edge(w);
+ }
msg(L_INFO, "Built road graph: %u vertices, %u edges", num_vertices, num_edges);
graph_optimize();
msg(L_INFO, "Optimized road graph: %u vertices, %u edges", num_vertices, num_edges);
+
+ graph_check_edges();
+
+ struct graph_vertex *v1 = graph_vertex_by_node_id(30002893); // Praha-Zbraslav, K Přehradám x 101
+ struct graph_vertex *v2 = graph_vertex_by_node_id(21289321); // Brno, Heršpická x Opuštěná
+ 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);
}
projects / leo.git / blobdiff