From: Martin Mares <mj@ucw.cz>
Date: Mon, 14 Mar 2022 21:41:34 +0000 (+0100)
Subject: Průvodce: Implementation of A*
X-Git-Url: http://mj.ucw.cz/gitweb/?a=commitdiff_plain;ds=sidebyside;h=06c72acb99678fc295ef7e4323cc25bed0987223;p=leo.git

Průvodce: Implementation of A*
---

diff --git a/graph.c b/graph.c
index 0df021e..338734e 100644
--- 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));