Průvodce: Implementation of A*

diff --git a/graph.c b/graph.c
index 0df021e344cbc3b02ecb1843912982a40f6bd42f..338734eacd2e3c4031388760a4398be852cbae1f 100644 (file)
--- a/graph.c
+++ b/graph.c
@@ -153,20 +153,22 @@ static bool way_is_edge(struct osm_way *w)
   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;
@@ -181,6 +183,17 @@ static void mark_edge_ways(struct graph_edge *e, const char *key, const char *va
   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)
@@ -212,9 +225,16 @@ static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
   for (;;)
     {
       struct graph_vertex *w = NULL;
+      double best_d = 0;
+
       CLIST_FOR_EACH(struct graph_vertex *, v, graph_vertices)
-       if (v->state == 1 && (!w || v->dist < w->dist))
-         w = v;
+       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");
@@ -236,7 +256,8 @@ static void dijkstra(struct graph_vertex *v_src, struct graph_vertex *v_dest)
          // 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);
+             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;
@@ -266,6 +287,8 @@ void graph_build(void)
   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));