#include "dijkstrasearch.h" #include "logging.h" #include #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; 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; } /*! * \brief dijkstra_search_find_path Calculates the path between start and destination * \param search * \param destination the destination. May also be set to NULL to calculate the entire map costs. * \return */ 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; } /*! * \brief dijkstra_search_get_path get list of nodes (must call dijkstra_search_find_path before this) * \param search * \param destination * \return a list of nodes or NULL if the route has not been found */ GList *dijkstra_search_get_route(struct dijkstra_search *search, struct dijkstra_node *destination) { GList *l = NULL; struct dijkstra_node *node = NULL; unsigned int i = 0; if (!destination || !search->states[destination->uid]->visited) return NULL; node = destination; while (1) { l = g_list_prepend(l, node); 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; } } return l; } /*! * \brief sdprintf printf to a dynamic buffer * \param buf The character buffer. Will append if not NULL; * \param offset Should be set to strlen when continuing a string. Will be incremented automatically. * \param __format printf format string * \return the number of bytes written */ int sdprintf(char **buf, int *offset, const char *__restrict __format, ...) { va_list vargs; int len; va_start(vargs, __format); len = vsnprintf(NULL, 0, __format, vargs); va_end(vargs); *buf = realloc(*buf, *offset + len + 1); va_start(vargs, __format); len = vsprintf(*buf + *offset, __format, vargs); va_end(vargs); *offset += len; return len; } // TODO: Rewrite to use glib-json... On the other hand, this is probably faster. int dijkstra_search_route_to_geojson(struct dijkstra_search *search, GList *route, char **buf) { GList *l = NULL; struct dijkstra_node *node = NULL; struct dijkstra_node *node_last = NULL; struct dijkstra_path *path_to_this = NULL; struct dijkstra_path *path_to_next = NULL; float heading = -1.0; float last_heading = 0; float heading_diff; char *relative_direction_str = NULL; char *format_str = NULL; int offset = 0; *buf = NULL; sdprintf(buf, &offset, "["); for (l = route; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; if (node_last) { path_to_this = search->states[node_last->uid]->cheapest_path; path_to_next = search->states[node->uid]->cheapest_path; heading = atan2f((node->position.y - node_last->position.y), (node->position.x - node_last->position.x)) / (M_PI * 2) * 360; if (heading < 0.0) heading += 360; heading_diff = last_heading - heading; if (fabsf(heading_diff) > 180) heading_diff = heading - last_heading; if (heading_diff > 25) { relative_direction_str = "right"; } else if (heading_diff < -25) { relative_direction_str = "left"; } else { relative_direction_str = "straight"; } format_str = "Internal error in format_str in dijkstra_search_route_to_geojson()"; switch (path_to_next->net_meta->type) { case STREET: if (node_last == search->start) format_str = "Start on %2$s"; else if (path_to_this->name == path_to_next->name) format_str = "Stay on %2$s by going %1$s"; else format_str = "Go %s onto %s"; break; case TRAINLINE: if (path_to_this->net_meta->type == ACCESS) // Enter train format_str = "[T] Get on the %2$s"; else // Stay on train format_str = "[T] Stay on the %2$s"; break; case ACCESS: if (path_to_this->net_meta->type == TRAINLINE) // Enter station from train format_str = "[T] Leave the train at %2$s to your %1$s"; else // Enter station from street format_str = "[T] Enter %2$s on your %1$s"; break; } if (node_last != search->start) sdprintf(buf, &offset, ","); sdprintf(buf, &offset, "\n"); sdprintf(buf, &offset, "{\"type\": \"Feature\",\n" " \"geometry\": {\n" " \"type\": \"LineString\", \"coordinates\": [[%f,%f],[%f,%f]]\n" " },\n", node_last->position.x, node_last->position.y, node->position.x, node->position.y); sdprintf(buf, &offset, " \"properties\": {\n" " \"heading\": \"%f\",\n" " \"through\": \"%s\",\n" " \"type\": \"%d\",\n", heading, path_to_next->name, path_to_next->net_meta->type); sdprintf(buf, &offset, " \"description\": \""); sdprintf(buf, &offset, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name); sdprintf(buf, &offset, "\"\n"); sdprintf(buf, &offset, " }\n"); sdprintf(buf, &offset, "}"); last_heading = heading; } node_last = node; } sdprintf(buf, &offset, "\n]\n"); return offset; } 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"); }