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.)
This commit is contained in:
Markus Koch 2021-02-04 23:36:37 +01:00
parent 5db3ad8759
commit 36f4f48287
2 changed files with 30 additions and 9 deletions

View File

@ -324,6 +324,9 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new); 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); 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 // 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); 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 1;
} }
return 0; // No collision 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, struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
float x1, float x1,
float y1, 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_node *n1, *n2;
struct dijkstra_path *path; struct dijkstra_path *path;
struct dijkstra_path *existing_path; GList *l_last;
GList *l, *l_last; struct dijkstra_path *path_last = NULL;
n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1); n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1);
n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2); n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2);
path = dijkstra_path_new(name, n1, n2, weight); path = dijkstra_path_new(name, n1, n2, weight);
l_last = g_list_last(solver->paths); l_last = g_list_last(solver->paths);
if (l_last)
path_last = l_last->data;
solver->paths = g_list_append(solver->paths, (gpointer) path); solver->paths = g_list_append(solver->paths, (gpointer) path);
for (l = solver->paths; l != NULL; l = l->next) { dijkstra_path_intersect_all(solver, path, path_last);
existing_path = (struct dijkstra_path*) l->data;
dijkstra_path_intersect(solver, path, existing_path);
if (l == l_last) /* Avoid testing newly added paths */
break;
}
return path; return path;
} }

View File

@ -51,9 +51,12 @@ void dijkstra_solver_free(struct dijkstra_solver *solver);
/* Library functions */ /* Library functions */
struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node, 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 dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver,
struct position *position); struct position *position);
void dijkstra_path_intersect_all(struct dijkstra_solver *solver,
struct dijkstra_path *path,
struct dijkstra_path *path_last);
/* For debugging only */ /* For debugging only */
struct dijkstra_node *dijkstra_node_new_to_list(GList **list, struct dijkstra_node *dijkstra_node_new_to_list(GList **list,