From 308fdacff058fe76e9a7cafd05d9f34658645c2a Mon Sep 17 00:00:00 2001 From: Mitya Selivanov Date: Sun, 13 Oct 2024 03:50:26 +0200 Subject: Implement Dijkstra's path search example --- examples/graph.c | 219 ++++++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 191 insertions(+), 28 deletions(-) (limited to 'examples') 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; -- cgit v1.2.3