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

@@ -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_last;
-	GList *l, *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) {
+	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;
 }

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,