diff --git a/src/dijkstrasearch.c b/src/dijkstrasearch.c index a25a72a..66de48d 100644 --- a/src/dijkstrasearch.c +++ b/src/dijkstrasearch.c @@ -1,6 +1,7 @@ #include "dijkstrasearch.h" #include "logging.h" #include +#include void dijkstra_search_reset(struct dijkstra_search *search) { @@ -139,6 +140,12 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear 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) { @@ -162,6 +169,100 @@ int dijkstra_search_find_path(struct dijkstra_search *search, 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; +} + +void dijkstra_search_route_to_geojson(struct dijkstra_search *search, + GList *route) +{ + GList *l = NULL; + struct dijkstra_node *node = NULL; + struct dijkstra_node *node_last = NULL; + float heading = -1.0; + float last_heading = 0; + char *relative_direction_str = NULL; + char *format_str = NULL; + + printf("["); + + for (l = route; l != NULL; l = l->next) { + node = (struct dijkstra_node*) l->data; + if (node_last) { + heading = atanf((node->position.y - node_last->position.y) / (node->position.x - node_last->position.x)) / (M_PI * 2) * 360; + heading = atanf((node->position.y - node_last->position.y) / (node->position.x - node_last->position.x)) / (M_PI * 2) * 360; + if (last_heading - heading > 10) { + relative_direction_str = "right"; + } else if (last_heading - heading < -10) { + relative_direction_str = "left"; + } else { + relative_direction_str = "straight"; + } + if (node_last == search->start) + format_str = "Start here."; + else if (search->states[node_last->uid]->cheapest_path->name == search->states[node->uid]->cheapest_path->name) + format_str = "Stay on %2$s by going %1$s."; + else + format_str = "Go %s onto %s."; + + if (node_last != search->start) + printf(","); + printf("\n"); + printf("{\"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); + printf(" \"properties\": {\n" + " \"heading\": \"%f\",\n" + " \"through\": \"%s\",\n", + heading, search->states[node->uid]->cheapest_path->name); + + printf(" \"description\": \""); + printf(format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name); + printf("\"\n"); + + printf(" }\n"); + printf("}"); + last_heading = heading; + } + node_last = node; + } + + printf("]\n"); +} + void dijkstra_print_path(struct dijkstra_search *search, struct dijkstra_node *destination) { diff --git a/src/dijkstrasearch.h b/src/dijkstrasearch.h index 65fb652..218672e 100644 --- a/src/dijkstrasearch.h +++ b/src/dijkstrasearch.h @@ -25,7 +25,13 @@ 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); +GList *dijkstra_search_get_route(struct dijkstra_search *search, + struct dijkstra_node *destination); + +/* Debugging functions */ void dijkstra_print_path(struct dijkstra_search *search, struct dijkstra_node *destination); void dijkstra_print_nodes(struct dijkstra_search *search); +void dijkstra_search_route_to_geojson(struct dijkstra_search *search, + GList *route); #endif // DIJKSTRASEARCH_H diff --git a/src/main.c b/src/main.c index 1cda033..404e10d 100644 --- a/src/main.c +++ b/src/main.c @@ -51,6 +51,8 @@ int main(int argc, char *argv[]) struct dijkstra_search *search; int iterations; + GList *route = NULL; + /* Init stuff */ set_log_level(LL_DEBUG); @@ -129,7 +131,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 from %d to %d after %d iterations.", start->uid, dest->uid, iterations); - dijkstra_print_path(search, dest); + route = dijkstra_search_get_route(search, dest); + dijkstra_search_route_to_geojson(search, route); + // dijkstra_print_path(search, dest); // dijkstra_print_nodes(search); } else { report(LL_WARNING, "Couldn't find path.");