diff --git a/src/dijkstragraph.c b/src/dijkstragraph.c index b2c99b0..db7258b 100644 --- a/src/dijkstragraph.c +++ b/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; @@ -404,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) { diff --git a/src/dijkstrasearch.c b/src/dijkstrasearch.c index 80c085f..a25a72a 100644 --- a/src/dijkstrasearch.c +++ b/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, - struct dijkstra_node *destination) +void 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. + unsigned int i = 0; 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"); +} diff --git a/src/dijkstrasearch.h b/src/dijkstrasearch.h index 9440b9e..65fb652 100644 --- a/src/dijkstrasearch.h +++ b/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 diff --git a/src/main.c b/src/main.c index da22d7f..1cda033 100644 --- a/src/main.c +++ b/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."); }