Fix bugs and implement bugged version of Dijkstra

src/dijkstragraph.c

@@ -2,6 +2,7 @@
 #include <stdlib.h>
 #include <glib/glist.h>
 #include <math.h>
+#include "logging.h"
 
 #define FP_THRESHOLD 0.01
 
@@ -88,12 +89,27 @@ struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
 	return node;
 }
 
+int dijkstra_connect_dangling_node_to_path(struct dijkstra_node **node,
+					      struct dijkstra_path *path)
+{
+	if (!path || !node)
+		return -1;
+
+	(*node)->paths = g_list_append((*node)->paths, (gpointer) path);
+	path->destination = *node;
+
+	return 0;
+}
+
 int dijkstra_connect_destination_node_to_path(struct dijkstra_node *destination,
 					      struct dijkstra_path *path)
 {
 	if (!path || !destination)
 		return -1;
 
+	if (path->destination) {
+		report(LL_WARNING, "Connecting an already connected path (destination).");
+	}
 	destination->paths = g_list_append(destination->paths, (gpointer) path);
 	path->destination = destination;
 
@@ -106,6 +122,10 @@ int dijkstra_connect_source_node_to_path(struct dijkstra_node *source,
 	if (!path || !source)
 		return -1;
 
+	if (path->source) {
+		report(LL_WARNING, "Connecting an already connected path (source).");
+	}
+
 	source->paths = g_list_append(source->paths, (gpointer) path);
 	path->source = source;
 
@@ -131,7 +151,7 @@ int dijkstra_connect_nodes_to_path(struct dijkstra_node *source,
 	return ret;
 }
 
-// WARNING: This does not update the references in the nodes!!!
+// Creates an unlinked (!) copy of the path
 struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path)
 {
 	struct dijkstra_path *dup;
@@ -142,6 +162,8 @@ struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path)
 	dup = malloc(sizeof(*path));
 	if (dup) {
 		memcpy(dup, path, sizeof(*path));
+		dup->source = NULL;
+		dup->destination = NULL;
 	}
 
 	return dup;
@@ -155,7 +177,10 @@ dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a,
 	dist = sqrt(pow(node_a->position.x - node_b->position.x, 2) +
 		    pow(node_a->position.y - node_b->position.y, 2));
 
-	return (dijkstra_cost) dist;
+	if (dist < DIJKSTRA_COST_MIN)
+		dist = DIJKSTRA_COST_MIN;
+
+	return (dijkstra_cost) (dist * 100);
 }
 
 /*!
@@ -173,33 +198,64 @@ struct dijkstra_path *dijkstra_path_new(char *name,
 	path = malloc(sizeof(*path));
 	if (path) {
 		path->name = name;
-		if (path->weight == DIJKSTRA_WEIGHT_AUTO)
+		if (weight == DIJKSTRA_WEIGHT_AUTO)
 			path->weight = dijkstra_get_weight_from_distance(source,
 									 destination);
 		else
 			path->weight = weight;
 
-		dijkstra_connect_nodes_to_path(source, destination, path);
+		path->source = NULL;
+		path->destination = NULL;
+
+		if (dijkstra_connect_nodes_to_path(source, destination, path) != 0) {
+			report(LL_CRITICAL, "Connect nodes failed: %p with %p as %p (%s)",
+			       source, destination, path, path->name);
+		}
 	}
 
 	return path;
 }
 
 struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *path,
-							 struct dijkstra_node *node,
+							 struct dijkstra_node *node)
-							 int ref_only)
 {
-	node->paths = g_list_remove(node->paths, (gpointer) node);
+	if (!g_list_find(node->paths, path)) {
+		report(LL_CRITICAL, "Tried disconnecting node %p from path %p (%s) that wasn't connected in the first place.",
+		       node, path, path->name);
+	}
+	node->paths = g_list_remove(node->paths, path);
 
-	if (ref_only)
+	if (path->source == path->destination) {
-		return node;
+		report(LL_CRITICAL, "Zero-length path. Destination node = source node for path %p", path);
+	}
 
-	if (path->source == node)
+	if (path->source == node) {
 			path->source = NULL;
-	if (path->destination == node)
+	}
+	if (path->destination == node) {
 			path->destination = NULL;
+	}
 
 	return node;
+
+}
+
+void checkycheckcheck(struct  dijkstra_solver *solver)
+{
+	GList *l;
+	struct dijkstra_node *node;
+	struct dijkstra_path *path;
+
+	report(LL_INFO, "");
+	for (l = solver->nodes; l != NULL; l = l->next) {
+		node = (struct dijkstra_node*) l->data;
+		report(LL_INFO, "Node %5d @%p: %f,\t%f [%d paths]", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths));
+
+		for (GList *m = node->paths; m != NULL; m = m->next) {
+			path = (struct dijkstra_path*) m->data;
+			struct dijkstra_node *foo = dijkstra_node_get_connection(node, path);
+		}
+	}
 }
 
 int dijkstra_path_intersect(struct dijkstra_solver *solver,
@@ -211,6 +267,7 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
 
 	struct dijkstra_path *path_new;
 	struct dijkstra_node *node_new;
+	struct dijkstra_node *disconnected_node;
 
 	float p0_x, p0_y, p1_x, p1_y;
 	float p2_x, p2_y, p3_x, p3_y;
@@ -248,24 +305,27 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
 		i_y = p0_y + (t * s1_y);
 
 		node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y);
-
 		if (node_new != path_a->source && node_new != path_a->destination) {
 			path_new = dijkstra_path_dup_shallow(path_a);
 			solver->paths = g_list_append(solver->paths, (gpointer) path_new);
 
-			dijkstra_disconnect_node_from_path(path_a, path_a->destination, 1);
+			disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination);
 			dijkstra_connect_destination_node_to_path(node_new, path_a);
-			dijkstra_connect_source_node_to_path(node_new, path_new);
+
+			dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
 		}
 
 		// Now do the same thing for path b
 		if (node_new != path_b->source && node_new != path_b->destination) {
 			path_new = dijkstra_path_dup_shallow(path_b);
 			solver->paths = g_list_append(solver->paths, (gpointer) path_new);
-			dijkstra_disconnect_node_from_path(path_b, path_b->destination, 1);
+
+			disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination);
 			dijkstra_connect_destination_node_to_path(node_new, path_b);
-			dijkstra_connect_source_node_to_path(node_new, path_new);
+
+			dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
 		}
+
 		return 1;
 	}
 
@@ -338,3 +398,17 @@ void dijkstra_solver_free(struct dijkstra_solver *solver)
 	}
 	g_list_free(solver->nodes);
 }
+
+inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node,
+						  struct dijkstra_path *path)
+{
+	if (node != path->destination && node != path->source) {
+		report(LL_WARNING, "get_connection requested for invalid node / path pair: %d with %p (%s).", node->uid, path, path->name);
+		for (GList *l = node->paths; l != NULL; l = l->next) {
+			struct dijkstra_path *path = (struct dijkstra_path*) l->data;
+			report(LL_WARNING, "  Available: %p (%s)", path, path->name);
+		}
+		getchar();
+	}
+	return (path->destination == node ? path->source : path->destination);
+}

src/dijkstragraph.h

@@ -5,8 +5,9 @@
 
 #define DIJKSTRA_WEIGHT_AUTO (-1)
 
-typedef unsigned int dijkstra_cost;
+typedef int dijkstra_cost;
-#define DIJKSTRA_COST_MAX UINT_MAX
+#define DIJKSTRA_COST_MAX INT_MAX
+#define DIJKSTRA_COST_MIN 1.0
 
 struct dijkstra_solver {
 	GList *nodes;
@@ -33,6 +34,7 @@ struct dijkstra_node {
 	int uid;
 };
 
+/* User Functions */
 struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
 						float x1,
 						float y1,
@@ -44,4 +46,13 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
 struct dijkstra_solver *dijkstra_solver_new();
 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);
+
+/* For debugging only */
+struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
+						float x,
+						float y);
+
 #endif // DIJKSTRAGRAPH_H

src/dijkstrasearch.c

@@ -1,4 +1,6 @@
 #include "dijkstrasearch.h"
+#include "logging.h"
+#include <stdio.h>
 
 void dijkstra_search_reset(struct dijkstra_search *search)
 {
@@ -7,6 +9,15 @@ void dijkstra_search_reset(struct dijkstra_search *search)
 		search->states[i]->cost = DIJKSTRA_COST_MAX;
 		search->states[i]->visited = 0;
 	}
+
+	if (search->start) {
+		search->states[search->start->uid]->cost = 0;
+	}
+
+	if (search->active_nodes) {
+		g_list_free(search->active_nodes);
+		search->active_nodes = NULL;
+	}
 }
 
 // WARNING: After creating this object, you must NOT modify the solver!
@@ -19,6 +30,7 @@ struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver)
 	if (search) {
 		search->states = malloc(sizeof(*search->states) * g_list_length(solver->nodes));
 		search->node_count = g_list_length(solver->nodes);
+		search->start = NULL;
 		for (i=0; i < search->node_count; i++) {
 			search->states[i] = malloc(sizeof(struct dijkstra_state));
 			if (!search->states) {
@@ -27,6 +39,7 @@ struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver)
 				return NULL;
 			}
 		}
+		search->active_nodes = NULL;
 		dijkstra_search_reset(search);
 	}
 
@@ -43,3 +56,120 @@ void dijkstra_search_free(struct dijkstra_search *search)
 	free(search->states);
 	free(search);
 }
+
+void dijkstra_search_set_start(struct dijkstra_search *search,
+			       struct dijkstra_node *start)
+{
+	if (search->start != start) {
+		search->start = start;
+		dijkstra_search_reset(search);
+	}
+}
+
+struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *search)
+{
+	struct dijkstra_path *path;
+	struct dijkstra_state *state;
+	struct dijkstra_node *connection;
+	struct dijkstra_node *cheapest_node = NULL;
+	struct dijkstra_node *node;
+
+	dijkstra_cost cost;
+	GList *l;
+
+	// 1. Find cheapest node in active nodes
+	// TODO: We can make this significantly quicker by using a sorted list
+	for (l = search->active_nodes; l != NULL; l = l->next) {
+		node = (struct dijkstra_node*) l->data;
+		state = search->states[node->uid];
+		if (!cheapest_node ||
+				state->cost < search->states[cheapest_node->uid]->cost) {
+			cheapest_node = node;
+		}
+		if (state->visited) {
+			report(LL_WARNING,
+			       "Visited node in active list (%d). This is probably an error.",
+			       node->uid); // TODO: This can be removed after I'm sure the algorithm works correctly
+		}
+	}
+	node = cheapest_node;
+	state = search->states[node->uid];
+	report(LL_DEBUG, "Current cheapest node is %d", node->uid);
+
+	// 2. Mark the current node as visited and remove it from the actives
+	state->visited = 1;
+	search->active_nodes = g_list_remove(search->active_nodes, node);
+
+	// 3. Update connecting nodes
+	for (l = node->paths; l != NULL; l = l->next) {
+		path = (struct dijkstra_path*) l->data;
+		cost = state->cost + path->weight;
+
+		connection = dijkstra_node_get_connection(node, path);
+		// 3.1 Update cost if cheaper
+		if (cost < search->states[connection->uid]->cost) {
+			search->states[connection->uid]->cost = cost;
+			search->states[connection->uid]->cheapest_path = path;
+			report(LL_DEBUG, "  Set cost of node %d to %d (backpath from %d to %d)", connection->uid, cost, connection->uid, node->uid);
+			if (search->states[connection->uid]->visited) {
+				report(LL_WARNING, "  Reset cost of visited node!"); // TODO: not necessary if algorithm OK
+			}
+		}
+		// 3.2. Add connecting nodes to active nodes if not already visited
+		if (!search->states[connection->uid]->visited) {
+			if (!g_list_find(search->active_nodes, connection)) {
+				search->active_nodes = g_list_append(search->active_nodes,
+								     (gpointer) connection);
+				report(LL_DEBUG, "  Append %d as an active node.", connection->uid);
+			}
+		}
+	}
+
+	return node;
+}
+
+int dijkstra_search_find_path(struct dijkstra_search *search,
+			      struct dijkstra_node *destination)
+{
+	int iteration = 0;
+	search->active_nodes = g_list_append(search->active_nodes, search->start);
+
+	while (g_list_length(search->active_nodes) > 0) {
+		iteration++;
+		dijkstra_search_process_queue(search);
+		if (search->states[destination->uid]->visited)
+			return iteration;
+		if (iteration >= DIJKSTRA_SEARCH_MAX_ITERATIONS) {
+			report(LL_ERROR, "Exceeded maximum iteration limit for query.");
+			return -2;
+		}
+	}
+
+	return -1;
+}
+
+int dijkstra_print_path(struct dijkstra_search *search,
+			struct dijkstra_node *destination)
+{
+	struct dijkstra_node *node = NULL;
+	GList *l;
+	int i=500; // TODO: This should not be necessary. Plus the number is arbitrary.
+
+	if (!destination || !search->states[destination->uid]->visited)
+		return 0;
+
+	node = destination;
+	while (node != search->start) {
+		if (node != destination)
+			printf(",");
+		printf("[%d,%d]", (int) node->position.x, (int) node->position.y);
+
+	//	printf("%u [%d]-> ", node->uid, search->states[node->uid]->cost);
+		node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path);
+		if (!i--) {
+			report(LL_CRITICAL, "Iteration limit for backsolver. This should never happen.");
+			break;
+		}
+	}
+	printf("\n");
+}

src/dijkstrasearch.h

@@ -3,7 +3,10 @@
 
 #include "dijkstragraph.h"
 
+#define DIJKSTRA_SEARCH_MAX_ITERATIONS 2000
+
 struct dijkstra_state {
+	struct dijkstra_path *cheapest_path;
 	dijkstra_cost cost;
 	int visited;
 };
@@ -11,9 +14,16 @@ struct dijkstra_state {
 struct dijkstra_search {
 	struct dijkstra_state **states;
 	unsigned int node_count;
+	struct dijkstra_node *start;
+	GList *active_nodes;
 };
 
 struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver);
 void dijkstra_search_free(struct dijkstra_search *search);
-
+void dijkstra_search_set_start(struct dijkstra_search *search,
+			       struct dijkstra_node *start);
+int dijkstra_search_find_path(struct dijkstra_search *search,
+			      struct dijkstra_node *destination);
+int dijkstra_print_path(struct dijkstra_search *search,
+			struct dijkstra_node *destination);
 #endif // DIJKSTRASEARCH_H

src/main.c

@@ -8,11 +8,14 @@
 #include "geojson.h"
 #include "logging.h"
 
+struct dijkstra_node *start;
+struct dijkstra_node *dest;
+
 void add_test_map(struct dijkstra_solver *solver)
 {
 	dijkstra_path_new_to_list(solver,
-				  0,0,
+				  5,0,
-				  0,10,
+				  5,10,
 				  "Vertical",
 				  1);
 
@@ -46,9 +49,10 @@ int main(int argc, char *argv[])
 	struct dijkstra_path *path;
 
 	struct dijkstra_search *search;
+	int iterations;
 
 	/* Init stuff */
-	set_log_level(LL_INFO);
+	set_log_level(LL_DEBUG);
 
 	/* Process arguments */
 	while((opt = getopt(argc, argv, "vs")) != -1) {
@@ -86,7 +90,10 @@ int main(int argc, char *argv[])
 		add_geojson_to_dijkstra(solver, argv[i]);
 	}
 
-	// add_test_map(&nodes, &paths);
+	// add_test_map(solver);
+	// TODO: This start / end coordinates are only for testing
+	start = dijkstra_node_new_to_list(&solver->nodes, -1213, -305);
+	dest = dijkstra_node_new_to_list(&solver->nodes, 388, 99);
 
 	if (should_report(LL_NOISY)) {
 		for (l = solver->nodes; l != NULL; l = l->next) {
@@ -116,6 +123,13 @@ int main(int argc, char *argv[])
 		dijkstra_solver_free(solver);
 		return -3;
 	}
+	dijkstra_search_set_start(search, start);
+	if ((iterations = dijkstra_search_find_path(search, dest)) > 0) {
+		report(LL_INFO, "Found path after %d iterations.", iterations);
+		dijkstra_print_path(search, dest);
+	} else {
+		report(LL_WARNING, "Couldn't find path.");
+	}
 
 	dijkstra_search_free(search);
 	dijkstra_solver_free(solver);