lifo-dijkstraserv/src/dijkstrasearch.c

221 lines
6.3 KiB
C

#include "dijkstrasearch.h"
#include "logging.h"
#include <stdio.h>
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;
search->states[i]->cheapest_path = NULL;
}
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->solver = solver;
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_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) {
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_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_CRITICAL, " 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_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!");
}
}
}
}
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;
}
void dijkstra_print_path(struct dijkstra_search *search,
struct dijkstra_node *destination)
{
struct dijkstra_node *node = NULL;
unsigned int i = 0;
if (!destination || !search->states[destination->uid]->visited)
return;
node = destination;
while (1) {
if (node != destination)
printf(",");
printf("[%d,%d]", (int) node->position.x, (int) node->position.y);
if (node == search->start)
break;
node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path);
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");
}