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,