2020-05-02 17:31:26 +02:00
|
|
|
#include "dijkstrasearch.h"
|
2020-05-03 00:00:41 +02:00
|
|
|
#include "logging.h"
|
|
|
|
#include <stdio.h>
|
2020-05-03 13:38:39 +02:00
|
|
|
#include <math.h>
|
2020-05-02 17:31:26 +02:00
|
|
|
|
|
|
|
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;
|
2020-05-03 12:09:29 +02:00
|
|
|
search->states[i]->cheapest_path = NULL;
|
2020-05-02 17:31:26 +02:00
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
if (search->start) {
|
|
|
|
search->states[search->start->uid]->cost = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (search->active_nodes) {
|
|
|
|
g_list_free(search->active_nodes);
|
|
|
|
search->active_nodes = NULL;
|
|
|
|
}
|
2020-05-02 17:31:26 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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) {
|
2020-05-03 12:09:29 +02:00
|
|
|
search->solver = solver;
|
2020-05-02 17:31:26 +02:00
|
|
|
search->states = malloc(sizeof(*search->states) * g_list_length(solver->nodes));
|
|
|
|
search->node_count = g_list_length(solver->nodes);
|
2020-05-03 00:00:41 +02:00
|
|
|
search->start = NULL;
|
2020-05-02 17:31:26 +02:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
search->active_nodes = NULL;
|
2020-05-02 17:31:26 +02:00
|
|
|
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);
|
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
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];
|
2020-05-03 12:09:29 +02:00
|
|
|
report(LL_NOISY, "Current cheapest node is %d", node->uid);
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
// 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);
|
2020-05-03 12:09:29 +02:00
|
|
|
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();
|
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
// 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);
|
2020-05-03 12:09:29 +02:00
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
// 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;
|
2020-05-03 12:09:29 +02:00
|
|
|
report(LL_NOISY, " Set cost of node %d to %d (backpath from %d to %d)", connection->uid, cost, connection->uid, node->uid);
|
2020-05-03 00:00:41 +02:00
|
|
|
if (search->states[connection->uid]->visited) {
|
2020-05-03 12:09:29 +02:00
|
|
|
report(LL_CRITICAL, " Reset cost of visited node!"); // TODO: not necessary if algorithm OK
|
2020-05-03 00:00:41 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
// 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);
|
2020-05-03 12:09:29 +02:00
|
|
|
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!");
|
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return node;
|
|
|
|
}
|
|
|
|
|
2020-05-03 13:38:39 +02:00
|
|
|
/*!
|
|
|
|
* \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
|
|
|
|
*/
|
2020-05-03 00:00:41 +02:00
|
|
|
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);
|
2020-05-03 00:09:05 +02:00
|
|
|
if (destination && search->states[destination->uid]->visited)
|
2020-05-03 00:00:41 +02:00
|
|
|
return iteration;
|
|
|
|
if (iteration >= DIJKSTRA_SEARCH_MAX_ITERATIONS) {
|
|
|
|
report(LL_ERROR, "Exceeded maximum iteration limit for query.");
|
|
|
|
return -2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-05-03 00:09:05 +02:00
|
|
|
if (!destination)
|
|
|
|
return iteration;
|
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2020-05-03 13:38:39 +02:00
|
|
|
/*!
|
|
|
|
* \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;
|
|
|
|
}
|
|
|
|
|
2020-05-03 20:13:25 +02:00
|
|
|
/*!
|
|
|
|
* \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, ...)
|
2020-05-03 19:59:05 +02:00
|
|
|
{
|
|
|
|
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)
|
2020-05-03 13:38:39 +02:00
|
|
|
{
|
|
|
|
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;
|
2020-05-03 19:59:05 +02:00
|
|
|
int offset = 0;
|
2020-05-03 13:38:39 +02:00
|
|
|
|
2020-05-03 19:59:05 +02:00
|
|
|
*buf = NULL;
|
|
|
|
|
2020-05-03 20:13:25 +02:00
|
|
|
sdprintf(buf, &offset, "[");
|
2020-05-03 13:38:39 +02:00
|
|
|
|
|
|
|
for (l = route; l != NULL; l = l->next) {
|
|
|
|
node = (struct dijkstra_node*) l->data;
|
|
|
|
if (node_last) {
|
2020-05-06 14:13:04 +02:00
|
|
|
heading = atan2f((node->position.y - node_last->position.y), (node->position.x - node_last->position.x)) / (M_PI * 2) * 360;
|
|
|
|
if (last_heading - heading > 25) {
|
2020-05-03 13:38:39 +02:00
|
|
|
relative_direction_str = "right";
|
2020-05-06 14:13:04 +02:00
|
|
|
} else if (last_heading - heading < -25) {
|
2020-05-03 13:38:39 +02:00
|
|
|
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)
|
2020-05-03 20:13:25 +02:00
|
|
|
sdprintf(buf, &offset, ",");
|
|
|
|
sdprintf(buf, &offset, "\n");
|
|
|
|
sdprintf(buf, &offset, "{\"type\": \"Feature\",\n"
|
2020-05-03 13:38:39 +02:00
|
|
|
" \"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);
|
2020-05-03 20:13:25 +02:00
|
|
|
sdprintf(buf, &offset, " \"properties\": {\n"
|
2020-05-03 13:38:39 +02:00
|
|
|
" \"heading\": \"%f\",\n"
|
|
|
|
" \"through\": \"%s\",\n",
|
|
|
|
heading, search->states[node->uid]->cheapest_path->name);
|
|
|
|
|
2020-05-03 20:13:25 +02:00
|
|
|
sdprintf(buf, &offset, " \"description\": \"");
|
|
|
|
sdprintf(buf, &offset, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name);
|
|
|
|
sdprintf(buf, &offset, "\"\n");
|
2020-05-03 13:38:39 +02:00
|
|
|
|
2020-05-03 20:13:25 +02:00
|
|
|
sdprintf(buf, &offset, " }\n");
|
|
|
|
sdprintf(buf, &offset, "}");
|
2020-05-03 13:38:39 +02:00
|
|
|
last_heading = heading;
|
|
|
|
}
|
|
|
|
node_last = node;
|
|
|
|
}
|
|
|
|
|
2020-05-03 20:13:25 +02:00
|
|
|
sdprintf(buf, &offset, "\n]\n");
|
2020-05-03 19:59:05 +02:00
|
|
|
|
|
|
|
return offset;
|
2020-05-03 13:38:39 +02:00
|
|
|
}
|
|
|
|
|
2020-05-03 12:09:29 +02:00
|
|
|
void dijkstra_print_path(struct dijkstra_search *search,
|
|
|
|
struct dijkstra_node *destination)
|
2020-05-03 00:00:41 +02:00
|
|
|
{
|
|
|
|
struct dijkstra_node *node = NULL;
|
2020-05-03 12:09:29 +02:00
|
|
|
unsigned int i = 0;
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
if (!destination || !search->states[destination->uid]->visited)
|
2020-05-03 12:09:29 +02:00
|
|
|
return;
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
node = destination;
|
2020-05-03 12:09:29 +02:00
|
|
|
while (1) {
|
2020-05-03 00:00:41 +02:00
|
|
|
if (node != destination)
|
|
|
|
printf(",");
|
|
|
|
printf("[%d,%d]", (int) node->position.x, (int) node->position.y);
|
|
|
|
|
2020-05-03 12:09:29 +02:00
|
|
|
if (node == search->start)
|
|
|
|
break;
|
2020-05-03 00:00:41 +02:00
|
|
|
node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path);
|
2020-05-03 12:09:29 +02:00
|
|
|
if (i++ > search->node_count) {
|
2020-05-03 00:00:41 +02:00
|
|
|
report(LL_CRITICAL, "Iteration limit for backsolver. This should never happen.");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2020-05-03 12:09:29 +02:00
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
printf("\n");
|
|
|
|
}
|
2020-05-03 12:09:29 +02:00
|
|
|
|
|
|
|
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");
|
|
|
|
}
|