#include "dijkstrasearch.h" #include "logging.h" #include void dijkstra_search_reset(struct dijkstra_search *search) { unsigned int i; for (i=0; i < search->node_count; i++) { 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! struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver) { struct dijkstra_search *search; unsigned int i; search = malloc(sizeof(*search)); 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) { // TODO: I should dealloc all allocated memory before returning, // but whatever, we'll crash and burn one way of the other. return NULL; } } search->active_nodes = NULL; dijkstra_search_reset(search); } return search; } void dijkstra_search_free(struct dijkstra_search *search) { unsigned int i; for (i=0; i < search->node_count; i++) { free(search->states[i]); } 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 (destination && search->states[destination->uid]->visited) return iteration; if (iteration >= DIJKSTRA_SEARCH_MAX_ITERATIONS) { report(LL_ERROR, "Exceeded maximum iteration limit for query."); return -2; } } if (!destination) return iteration; 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"); }