From 36f4f482878eef35b195097385bbae49503fec2c Mon Sep 17 00:00:00 2001 From: Markus Koch Date: Thu, 4 Feb 2021 23:36:37 +0100 Subject: [PATCH] Fix path intersection algorithm I forgot that, when splitting a path, I need to also check the newly generated ("right") segment against all (following) existing path segments because the original segment is now shrunk to the size of the new "left" one, and will therefore not intersect every possible line. (But you still need to keep checking with it because there is not guarantee that the line we just intersected with was the physi- cally closest to the starting point, it was just the first in memory.) --- src/dijkstragraph.c | 34 ++++++++++++++++++++++++++-------- src/dijkstragraph.h | 5 ++++- 2 files changed, 30 insertions(+), 9 deletions(-) diff --git a/src/dijkstragraph.c b/src/dijkstragraph.c index 5eb49fe..37f6fbd 100644 --- a/src/dijkstragraph.c +++ b/src/dijkstragraph.c @@ -324,6 +324,9 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new); path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination); + + // TODO: I don't have to iterate over the *entire* array here, it should be enough to only iterate over the entries after path_b. + dijkstra_path_intersect_all(solver, path_new, path_new); // Since straight paths can only cross once, we don't need to check any newly created entries. } // Now do the same thing for path b @@ -339,12 +342,30 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination); } + // In case neither if matches that means we are on the corner where two paths intersect at their ends. + // No need to do anything then, dijkstra_node_new_to_list will have taken care of everything. + return 1; } return 0; // No collision } +void dijkstra_path_intersect_all(struct dijkstra_solver *solver, + struct dijkstra_path *path, + struct dijkstra_path *path_last) +{ + GList *l; + struct dijkstra_path *existing_path; + + for (l = solver->paths; l != NULL; l = l->next) { + existing_path = (struct dijkstra_path*) l->data; + dijkstra_path_intersect(solver, path, existing_path); + if (l->data == path_last) /* Avoid testing newly added paths */ + break; + } +} + struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, float x1, float y1, @@ -357,22 +378,19 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, { struct dijkstra_node *n1, *n2; struct dijkstra_path *path; - struct dijkstra_path *existing_path; - GList *l, *l_last; + GList *l_last; + struct dijkstra_path *path_last = NULL; n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1); n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2); path = dijkstra_path_new(name, n1, n2, weight); l_last = g_list_last(solver->paths); + if (l_last) + path_last = l_last->data; solver->paths = g_list_append(solver->paths, (gpointer) path); - for (l = solver->paths; l != NULL; l = l->next) { - existing_path = (struct dijkstra_path*) l->data; - dijkstra_path_intersect(solver, path, existing_path); - if (l == l_last) /* Avoid testing newly added paths */ - break; - } + dijkstra_path_intersect_all(solver, path, path_last); return path; } diff --git a/src/dijkstragraph.h b/src/dijkstragraph.h index c737385..b4fd84e 100644 --- a/src/dijkstragraph.h +++ b/src/dijkstragraph.h @@ -51,9 +51,12 @@ void dijkstra_solver_free(struct dijkstra_solver *solver); /* Library functions */ struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node, - struct dijkstra_path *path); + struct dijkstra_path *path); struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver, struct position *position); +void dijkstra_path_intersect_all(struct dijkstra_solver *solver, + struct dijkstra_path *path, + struct dijkstra_path *path_last); /* For debugging only */ struct dijkstra_node *dijkstra_node_new_to_list(GList **list,