Implement Dijkstra's path search example

1 files changed, 191 insertions, 28 deletions

diff --git a/examples/graph.c b/examples/graph.c
index 6cb8198..86ff9f3 100755
--- a/examples/graph.c
+++ b/examples/graph.c
@@ -38,6 +38,8 @@ typedef struct {
   f64 y;
   f64 radius;
   b8  hover;
+  b8  highlight;
+  f64 distance;
 } Node;
 
 typedef struct {
@@ -46,6 +48,7 @@ typedef struct {
   i64 dst;
   f64 width;
   b8  hover;
+  b8  highlight;
 } Edge;
 
 typedef struct {
@@ -76,6 +79,8 @@ void draw_node(i64 node_index) {
 
   u32 color = 0; // black color
 
+  if (n.highlight)
+    color = 0xff00ff; // pink color
   if (n.hover)
     color = 0x007f00; // green color
 
@@ -98,6 +103,8 @@ void draw_edge(i64 edge_index) {
 
   u32 color = 0x7f7f7f; // grey color
 
+  if (e.highlight)
+    color = 0xff00ff; // pink color
   if (e.hover)
     color = 0x007f00; // green color
 
@@ -129,10 +136,14 @@ void update_node(i64 node_index) {
 
   Node n = world.nodes[node_index];
 
-  f64 cx = platform.cursor_x - n.x;
-  f64 cy = platform.cursor_y - n.y;
-
-  world.nodes[node_index].hover = cx * cx + cy * cy <= n.radius * n.radius;
+  world.nodes[node_index].hover = ellipse_contains(
+    n.x - n.radius,
+    n.y - n.radius,
+    n.radius * 2,
+    n.radius * 2,
+    platform.cursor_x,
+    platform.cursor_y
+  );
 }
 
 void update_edge(i64 edge_index) {
@@ -142,29 +153,15 @@ void update_edge(i64 edge_index) {
   Node n0 = world.nodes[e.src];
   Node n1 = world.nodes[e.dst];
 
-  f64 cx = platform.cursor_x - n0.x;
-  f64 cy = platform.cursor_y - n0.y;
-
-  // normalized line direction vector
-  f64 rx = n1.x - n0.x;
-  f64 ry = n1.y - n0.y;
-  f64 r  = sqrt(rx * rx + ry * ry);
-  if (r >= EPSILON) {
-    rx /= r;
-    ry /= r;
-  }
-
-  // tangent
-  f64 tx = -ry;
-  f64 ty = rx;
-
-  // distance to the line (dot-product)
-  f64 d = cx * tx + cy * ty;
-
-  // distance to the tangent line
-  f64 l = cx * rx + cy * ry;
-
-  world.edges[edge_index].hover = d <= e.width / 2 && d >= -e.width / 2 && l >= 0 && l <= r;
+  world.edges[edge_index].hover = line_contains(
+    n0.x,
+    n0.y,
+    n1.x,
+    n1.y,
+    e.width,
+    platform.cursor_x,
+    platform.cursor_y
+  );
 }
 
 void remove_edge(i64 edge_index) {
@@ -225,6 +222,145 @@ void add_edge(i64 src, i64 dst) {
   };
 }
 
+f64 distance_between(f64 x0, f64 y0, f64 x1, f64 y1) {
+  f64 dx = x1 - x0;
+  f64 dy = y1 - y0;
+
+  return sqrt(dx * dx + dy * dy);
+}
+
+void highlight_path(i64 src, i64 dst) {
+  if (!check_node_index(src)) return;
+  if (!check_node_index(dst)) return;
+
+  for (i64 i = 0; i < MAX_NUM_NODES; ++i) {
+    world.nodes[i].highlight = 0;
+    world.nodes[i].distance  = -1;
+  }
+
+  for (i64 i = 0; i < MAX_NUM_EDGES; ++i)
+    world.edges[i].highlight = 0;
+
+  world.nodes[src].highlight = 1;
+  world.nodes[dst].highlight = 1;
+
+  world.nodes[src].distance = 0;
+
+  //  Dijkstra
+  //
+
+  for (;;) {
+    //  Find a node with minimum path distance
+    //
+
+    i64 min_index    = -1;
+    f64 min_distance = -1;
+
+    for (i64 i = 0; i < MAX_NUM_EDGES; ++i)
+      if (check_edge_index(i)) {
+        Edge e  = world.edges[i];
+        Node n0 = world.nodes[e.src];
+        Node n1 = world.nodes[e.dst];
+
+        f64 l = distance_between(n0.x, n0.y, n1.x, n1.y);
+
+        //  Assume bi-directional graph
+
+        if (n0.distance >= 0 && n1.distance < 0)
+          if (min_index < 0 || n0.distance + l < min_distance) {
+            min_index    = e.dst;
+            min_distance = n0.distance + l;
+          }
+
+        if (n1.distance >= 0 && n0.distance < 0)
+          if (min_index < 0 || n1.distance + l < min_distance) {
+            min_index    = e.src;
+            min_distance = n1.distance + l;
+          }
+      }
+
+    //  If not found, stop the search
+    //
+
+    if (min_index < 0)
+      break;
+
+    //  Save the distance for the found node
+    //
+
+    world.nodes[min_index].distance = min_distance;
+
+    //  Repeat until we got the destination node
+    //
+
+    if (min_index == dst)
+      break;
+  }
+
+  //  If path not found, stop
+  //
+
+  if (world.nodes[dst].distance < 0) {
+    printf("Path not found\n");
+    return;
+  }
+
+  //  Reconstruct the path
+  //
+
+  f64 min_distance = world.nodes[dst].distance;
+
+  for (i64 node_index = dst; node_index != src;) {
+    //  Repeat until we got the source node
+
+    //  Find neighbor node with minimal path distance
+    //
+
+    i64 min_node_index = -1;
+    i64 min_edge_index = -1;
+
+    for (i64 i = 0; i < MAX_NUM_EDGES; ++i)
+      if (check_edge_index(i)) {
+        Edge e  = world.edges[i];
+        Node n0 = world.nodes[e.src];
+        Node n1 = world.nodes[e.dst];
+
+        //  Assume bi-directional graph
+
+        if (e.src == node_index && n1.distance >= 0 && n1.distance < min_distance) {
+          min_node_index = e.dst;
+          min_edge_index = i;
+          min_distance   = n1.distance;
+        }
+
+        if (e.dst == node_index && n0.distance >= 0 && n0.distance < min_distance) {
+          min_node_index = e.src;
+          min_edge_index = i;
+          min_distance   = n0.distance;
+        }
+      }
+
+    //  If not found, stop
+    //
+
+    if (min_node_index < 0) {
+      printf("Internal error\n");
+      break;
+    }
+
+    //  Highlight found node and edge
+    //
+
+    world.nodes[min_node_index].highlight = 1;
+    world.edges[min_edge_index].highlight = 1;
+
+    //  Repeat for the found node
+    //
+
+    node_index = min_node_index;
+  }
+}
+
 i32 main(i32 argc, c8 **argv) {
   (void) argc;
   (void) argv;
@@ -249,6 +385,10 @@ i32 main(i32 argc, c8 **argv) {
   i64 adding_src  = 0;
   i64 adding_dst  = 0;
 
+  b8  path_changed = 0;
+  i64 path_src     = -1;
+  i64 path_dst     = -1;
+
   while (!platform.done) {
     p_wait_events();
 
@@ -281,6 +421,29 @@ i32 main(i32 argc, c8 **argv) {
           remove_node(i);
     }
 
+    if (platform.key_pressed['1']) {
+
+      for (i64 i = 0; i < MAX_NUM_NODES; ++i)
+        if (world.nodes[i].enabled && world.nodes[i].hover) {
+          path_src     = i;
+          path_changed = 1;
+          break;
+        }
+    }
+
+    if (platform.key_pressed['2'])
+      for (i64 i = 0; i < MAX_NUM_NODES; ++i)
+        if (world.nodes[i].enabled && world.nodes[i].hover) {
+          path_dst     = i;
+          path_changed = 1;
+          break;
+        }
+
+    if (path_changed) {
+      highlight_path(path_src, path_dst);
+      path_changed = 0;
+    }
+
     if (platform.key_pressed[BUTTON_LEFT])
       add_node(platform.cursor_x, platform.cursor_y);
 
@@ -326,7 +489,7 @@ i32 main(i32 argc, c8 **argv) {
     draw_graph();
 
     p_render_frame();
-  }  
+  }
 
   p_cleanup();
   return 0;