Fix Dijkstra implementation, add debug prints

src/dijkstragraph.c

@@ -89,18 +89,6 @@ 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)
 {
@@ -230,10 +218,10 @@ struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *p
 	}
 
 	if (path->source == node) {
-			path->source = NULL;
+		path->source = NULL;
 	}
 	if (path->destination == node) {
-			path->destination = NULL;
+		path->destination = NULL;
 	}
 
 	return node;
@@ -311,8 +299,10 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
 
 			disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination);
 			dijkstra_connect_destination_node_to_path(node_new, path_a);
+			path_a->weight = dijkstra_get_weight_from_distance(path_a->source, path_a->destination);
 
 			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);
 		}
 
 		// Now do the same thing for path b
@@ -322,8 +312,10 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
 
 			disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination);
 			dijkstra_connect_destination_node_to_path(node_new, path_b);
+			path_b->weight = dijkstra_get_weight_from_distance(path_b->source, path_b->destination);
 
 			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);
 		}
 
 		return 1;
@@ -400,8 +392,12 @@ void dijkstra_solver_free(struct dijkstra_solver *solver)
 }
 
 inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node,
-						  struct dijkstra_path *path)
+							  struct dijkstra_path *path)
 {
+	if (!node || !path) {
+		report(LL_CRITICAL, "Called get_connection with NULL node (%p) or path (%p).",
+		       node, 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) {

src/dijkstrasearch.c

@@ -8,6 +8,7 @@ void dijkstra_search_reset(struct dijkstra_search *search)
 	for (i=0; i < search->node_count; i++) {
 		search->states[i]->cost = DIJKSTRA_COST_MAX;
 		search->states[i]->visited = 0;
+		search->states[i]->cheapest_path = NULL;
 	}
 
 	if (search->start) {
@@ -28,6 +29,7 @@ struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver)
 
 	search = malloc(sizeof(*search));
 	if (search) {
+		search->solver = solver;
 		search->states = malloc(sizeof(*search->states) * g_list_length(solver->nodes));
 		search->node_count = g_list_length(solver->nodes);
 		search->start = NULL;
@@ -94,11 +96,16 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear
 	}
 	node = cheapest_node;
 	state = search->states[node->uid];
-	report(LL_DEBUG, "Current cheapest node is %d", node->uid);
+	report(LL_NOISY, "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);
+	if (!state->cheapest_path && node != search->start) {
+		report(LL_CRITICAL, "DEBUG: marked current node as visited, even though it doesn't have a path to it. Cost is %d.",
+		       state->cost);
+		getchar();
+	}
 
 	// 3. Update connecting nodes
 	for (l = node->paths; l != NULL; l = l->next) {
@@ -106,13 +113,14 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear
 		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);
+			report(LL_NOISY, "  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
+				report(LL_CRITICAL, "  Reset cost of visited node!"); // TODO: not necessary if algorithm OK
 			}
 		}
 		// 3.2. Add connecting nodes to active nodes if not already visited
@@ -120,7 +128,10 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear
 			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);
+				report(LL_NOISY, "  Append %d as an active node.", connection->uid);
+				if (!search->states[connection->uid]->cheapest_path) {
+					report(LL_CRITICAL, "Appended a node without a path!");
+				}
 			}
 		}
 	}
@@ -151,28 +162,59 @@ int dijkstra_search_find_path(struct dijkstra_search *search,
 	return -1;
 }
 
-int dijkstra_print_path(struct dijkstra_search *search,
+void dijkstra_print_path(struct dijkstra_search *search,
-			struct dijkstra_node *destination)
+			 struct dijkstra_node *destination)
 {
 	struct dijkstra_node *node = NULL;
-	GList *l;
+	unsigned int i = 0;
-	int i=500; // TODO: This should not be necessary. Plus the number is arbitrary.
 
 	if (!destination || !search->states[destination->uid]->visited)
-		return 0;
+		return;
 
 	node = destination;
-	while (node != search->start) {
+	while (1) {
 		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);
+		if (node == search->start)
+			break;
 		node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path);
-		if (!i--) {
+		if (i++ > search->node_count) {
 			report(LL_CRITICAL, "Iteration limit for backsolver. This should never happen.");
 			break;
 		}
 	}
+
 	printf("\n");
 }
+
+void dijkstra_print_nodes(struct dijkstra_search *search)
+{
+	GList *l;
+	struct dijkstra_node *node;
+
+	printf("[");
+	for (l = search->solver->nodes; l != NULL; l = l->next) {
+		node = (struct dijkstra_node*) l->data;
+		printf("{ \"type\": \"Feature\",\n"
+		       "    \"geometry\" : {\n"
+		       "        \"type\" : \"Point\", \"coordinates\" : [%f, %f]\n"
+		       "    },\n"
+		       "    \"properties\" : {\n", node->position.x, node->position.y);
+		if (search->states[node->uid]->cheapest_path) {
+			printf("        \"name\" : \"Node %d (cost %d, from %d)\"\n",
+			       node->uid, search->states[node->uid]->cost, dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path)->uid);
+		} else if (node == search->start) {
+			printf("        \"name\" : \"Starting node %d\"\n",
+			       node->uid);
+		} else {
+			printf("        \"name\" : \"Unvisited node %d\"\n",
+			       node->uid);
+		}
+		printf("    }\n"
+		       "},\n");
+
+	}
+	printf("{}]\n");
+}

src/dijkstrasearch.h

@@ -12,6 +12,7 @@ struct dijkstra_state {
 };
 
 struct dijkstra_search {
+	struct dijkstra_solver *solver;
 	struct dijkstra_state **states;
 	unsigned int node_count;
 	struct dijkstra_node *start;
@@ -24,6 +25,7 @@ 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,
+void dijkstra_print_path(struct dijkstra_search *search,
 			struct dijkstra_node *destination);
+void dijkstra_print_nodes(struct dijkstra_search *search);
 #endif // DIJKSTRASEARCH_H

src/main.c

@@ -92,21 +92,24 @@ int main(int argc, char *argv[])
 
 	// add_test_map(solver);
 	// TODO: This start / end coordinates are only for testing
-	start = dijkstra_node_new_to_list(&solver->nodes, -1213, -305);
+	start = dijkstra_node_new_to_list(&solver->nodes, -856, -393);
 	dest = dijkstra_node_new_to_list(&solver->nodes, 388, 99);
+	//start = dijkstra_node_new_to_list(&solver->nodes, -957, -467);
+	//dest = dijkstra_node_new_to_list(&solver->nodes, -712, -467);
 
 	if (should_report(LL_NOISY)) {
 		for (l = solver->nodes; l != NULL; l = l->next) {
 			node = (struct dijkstra_node*) l->data;
-			printf("Node %5d @%p: %f,\t%f [%d paths]\n", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths));
+			fprintf(stderr, "Node %5d @%p: %f,\t%f [%d paths]\n", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths));
 		}
 		printf("\n");
 
 		for (l = solver->paths; l != NULL; l = l->next) {
 			path = (struct dijkstra_path*) l->data;
-			printf("Path %p: %5d (%f,%f) -> %5d (%f,%f) \t\"%s\"\n", path,
+			fprintf(stderr, "Path %p: %5d (%f,%f) -> %5d (%f,%f) cost %d \t\"%s\"\n", path,
 			       path->source->uid, path->source->position.x, path->source->position.y,
 			       path->destination->uid, path->destination->position.x, path->destination->position.y,
+			       path->weight,
 			       path->name);
 		}
 		printf("\n");
@@ -125,8 +128,9 @@ int main(int argc, char *argv[])
 	}
 	dijkstra_search_set_start(search, start);
 	if ((iterations = dijkstra_search_find_path(search, dest)) > 0) {
-		report(LL_INFO, "Found path after %d iterations.", iterations);
+		report(LL_INFO, "Found path from %d to %d after %d iterations.", start->uid, dest->uid, iterations);
 		dijkstra_print_path(search, dest);
+		// dijkstra_print_nodes(search);
 	} else {
 		report(LL_WARNING, "Couldn't find path.");
 	}