Implement preliminary GeoJson route output

This commit is contained in:
Markus Koch 2020-05-03 13:38:39 +02:00
parent adb94f8be8
commit fe1547b37d
3 changed files with 112 additions and 1 deletions

View File

@ -1,6 +1,7 @@
#include "dijkstrasearch.h"
#include "logging.h"
#include <stdio.h>
#include <math.h>
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)
{

View File

@ -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

View File

@ -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.");