Fix bugs and implement bugged version of Dijkstra

feature/trainlines-wip
Markus Koch 2020-05-03 00:00:41 +02:00
parent 7d41c1cdf7
commit 7d6082ed68
5 changed files with 264 additions and 25 deletions

View File

@ -2,6 +2,7 @@
#include <stdlib.h>
#include <glib/glist.h>
#include <math.h>
#include "logging.h"
#define FP_THRESHOLD 0.01
@ -88,12 +89,27 @@ struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
return node;
}
int dijkstra_connect_dangling_node_to_path(struct dijkstra_node **node,
struct dijkstra_path *path)
{
if (!path || !node)
return -1;
(*node)->paths = g_list_append((*node)->paths, (gpointer) path);
path->destination = *node;
return 0;
}
int dijkstra_connect_destination_node_to_path(struct dijkstra_node *destination,
struct dijkstra_path *path)
{
if (!path || !destination)
return -1;
if (path->destination) {
report(LL_WARNING, "Connecting an already connected path (destination).");
}
destination->paths = g_list_append(destination->paths, (gpointer) path);
path->destination = destination;
@ -106,6 +122,10 @@ int dijkstra_connect_source_node_to_path(struct dijkstra_node *source,
if (!path || !source)
return -1;
if (path->source) {
report(LL_WARNING, "Connecting an already connected path (source).");
}
source->paths = g_list_append(source->paths, (gpointer) path);
path->source = source;
@ -131,7 +151,7 @@ int dijkstra_connect_nodes_to_path(struct dijkstra_node *source,
return ret;
}
// WARNING: This does not update the references in the nodes!!!
// Creates an unlinked (!) copy of the path
struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path)
{
struct dijkstra_path *dup;
@ -142,6 +162,8 @@ struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path)
dup = malloc(sizeof(*path));
if (dup) {
memcpy(dup, path, sizeof(*path));
dup->source = NULL;
dup->destination = NULL;
}
return dup;
@ -155,7 +177,10 @@ dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a,
dist = sqrt(pow(node_a->position.x - node_b->position.x, 2) +
pow(node_a->position.y - node_b->position.y, 2));
return (dijkstra_cost) dist;
if (dist < DIJKSTRA_COST_MIN)
dist = DIJKSTRA_COST_MIN;
return (dijkstra_cost) (dist * 100);
}
/*!
@ -173,33 +198,64 @@ struct dijkstra_path *dijkstra_path_new(char *name,
path = malloc(sizeof(*path));
if (path) {
path->name = name;
if (path->weight == DIJKSTRA_WEIGHT_AUTO)
if (weight == DIJKSTRA_WEIGHT_AUTO)
path->weight = dijkstra_get_weight_from_distance(source,
destination);
else
path->weight = weight;
dijkstra_connect_nodes_to_path(source, destination, path);
path->source = NULL;
path->destination = NULL;
if (dijkstra_connect_nodes_to_path(source, destination, path) != 0) {
report(LL_CRITICAL, "Connect nodes failed: %p with %p as %p (%s)",
source, destination, path, path->name);
}
}
return path;
}
struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *path,
struct dijkstra_node *node,
int ref_only)
struct dijkstra_node *node)
{
node->paths = g_list_remove(node->paths, (gpointer) node);
if (!g_list_find(node->paths, path)) {
report(LL_CRITICAL, "Tried disconnecting node %p from path %p (%s) that wasn't connected in the first place.",
node, path, path->name);
}
node->paths = g_list_remove(node->paths, path);
if (ref_only)
return node;
if (path->source == path->destination) {
report(LL_CRITICAL, "Zero-length path. Destination node = source node for path %p", path);
}
if (path->source == node)
path->source = NULL;
if (path->destination == node)
path->destination = NULL;
if (path->source == node) {
path->source = NULL;
}
if (path->destination == node) {
path->destination = NULL;
}
return node;
}
void checkycheckcheck(struct dijkstra_solver *solver)
{
GList *l;
struct dijkstra_node *node;
struct dijkstra_path *path;
report(LL_INFO, "");
for (l = solver->nodes; l != NULL; l = l->next) {
node = (struct dijkstra_node*) l->data;
report(LL_INFO, "Node %5d @%p: %f,\t%f [%d paths]", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths));
for (GList *m = node->paths; m != NULL; m = m->next) {
path = (struct dijkstra_path*) m->data;
struct dijkstra_node *foo = dijkstra_node_get_connection(node, path);
}
}
}
int dijkstra_path_intersect(struct dijkstra_solver *solver,
@ -211,6 +267,7 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
struct dijkstra_path *path_new;
struct dijkstra_node *node_new;
struct dijkstra_node *disconnected_node;
float p0_x, p0_y, p1_x, p1_y;
float p2_x, p2_y, p3_x, p3_y;
@ -248,24 +305,27 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
i_y = p0_y + (t * s1_y);
node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y);
if (node_new != path_a->source && node_new != path_a->destination) {
path_new = dijkstra_path_dup_shallow(path_a);
solver->paths = g_list_append(solver->paths, (gpointer) path_new);
dijkstra_disconnect_node_from_path(path_a, path_a->destination, 1);
disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination);
dijkstra_connect_destination_node_to_path(node_new, path_a);
dijkstra_connect_source_node_to_path(node_new, path_new);
dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
}
// Now do the same thing for path b
if (node_new != path_b->source && node_new != path_b->destination) {
path_new = dijkstra_path_dup_shallow(path_b);
solver->paths = g_list_append(solver->paths, (gpointer) path_new);
dijkstra_disconnect_node_from_path(path_b, path_b->destination, 1);
disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination);
dijkstra_connect_destination_node_to_path(node_new, path_b);
dijkstra_connect_source_node_to_path(node_new, path_new);
dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
}
return 1;
}
@ -338,3 +398,17 @@ void dijkstra_solver_free(struct dijkstra_solver *solver)
}
g_list_free(solver->nodes);
}
inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node,
struct dijkstra_path *path)
{
if (node != path->destination && node != path->source) {
report(LL_WARNING, "get_connection requested for invalid node / path pair: %d with %p (%s).", node->uid, path, path->name);
for (GList *l = node->paths; l != NULL; l = l->next) {
struct dijkstra_path *path = (struct dijkstra_path*) l->data;
report(LL_WARNING, " Available: %p (%s)", path, path->name);
}
getchar();
}
return (path->destination == node ? path->source : path->destination);
}

View File

@ -5,8 +5,9 @@
#define DIJKSTRA_WEIGHT_AUTO (-1)
typedef unsigned int dijkstra_cost;
#define DIJKSTRA_COST_MAX UINT_MAX
typedef int dijkstra_cost;
#define DIJKSTRA_COST_MAX INT_MAX
#define DIJKSTRA_COST_MIN 1.0
struct dijkstra_solver {
GList *nodes;
@ -33,6 +34,7 @@ struct dijkstra_node {
int uid;
};
/* User Functions */
struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
float x1,
float y1,
@ -44,4 +46,13 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
struct dijkstra_solver *dijkstra_solver_new();
void dijkstra_solver_free(struct dijkstra_solver *solver);
/* Library functions */
struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node,
struct dijkstra_path *path);
/* For debugging only */
struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
float x,
float y);
#endif // DIJKSTRAGRAPH_H

View File

@ -1,4 +1,6 @@
#include "dijkstrasearch.h"
#include "logging.h"
#include <stdio.h>
void dijkstra_search_reset(struct dijkstra_search *search)
{
@ -7,6 +9,15 @@ void dijkstra_search_reset(struct dijkstra_search *search)
search->states[i]->cost = DIJKSTRA_COST_MAX;
search->states[i]->visited = 0;
}
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!
@ -19,6 +30,7 @@ struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver)
if (search) {
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) {
@ -27,6 +39,7 @@ struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver)
return NULL;
}
}
search->active_nodes = NULL;
dijkstra_search_reset(search);
}
@ -43,3 +56,120 @@ void dijkstra_search_free(struct dijkstra_search *search)
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_DEBUG, "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);
// 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_DEBUG, " 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_WARNING, " 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_DEBUG, " Append %d as an active node.", connection->uid);
}
}
}
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 (search->states[destination->uid]->visited)
return iteration;
if (iteration >= DIJKSTRA_SEARCH_MAX_ITERATIONS) {
report(LL_ERROR, "Exceeded maximum iteration limit for query.");
return -2;
}
}
return -1;
}
int dijkstra_print_path(struct dijkstra_search *search,
struct dijkstra_node *destination)
{
struct dijkstra_node *node = NULL;
GList *l;
int i=500; // TODO: This should not be necessary. Plus the number is arbitrary.
if (!destination || !search->states[destination->uid]->visited)
return 0;
node = destination;
while (node != search->start) {
if (node != destination)
printf(",");
printf("[%d,%d]", (int) node->position.x, (int) node->position.y);
// printf("%u [%d]-> ", node->uid, search->states[node->uid]->cost);
node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path);
if (!i--) {
report(LL_CRITICAL, "Iteration limit for backsolver. This should never happen.");
break;
}
}
printf("\n");
}

View File

@ -3,7 +3,10 @@
#include "dijkstragraph.h"
#define DIJKSTRA_SEARCH_MAX_ITERATIONS 2000
struct dijkstra_state {
struct dijkstra_path *cheapest_path;
dijkstra_cost cost;
int visited;
};
@ -11,9 +14,16 @@ struct dijkstra_state {
struct dijkstra_search {
struct dijkstra_state **states;
unsigned int node_count;
struct dijkstra_node *start;
GList *active_nodes;
};
struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver);
void dijkstra_search_free(struct dijkstra_search *search);
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);
int dijkstra_print_path(struct dijkstra_search *search,
struct dijkstra_node *destination);
#endif // DIJKSTRASEARCH_H

View File

@ -8,11 +8,14 @@
#include "geojson.h"
#include "logging.h"
struct dijkstra_node *start;
struct dijkstra_node *dest;
void add_test_map(struct dijkstra_solver *solver)
{
dijkstra_path_new_to_list(solver,
0,0,
0,10,
5,0,
5,10,
"Vertical",
1);
@ -46,9 +49,10 @@ int main(int argc, char *argv[])
struct dijkstra_path *path;
struct dijkstra_search *search;
int iterations;
/* Init stuff */
set_log_level(LL_INFO);
set_log_level(LL_DEBUG);
/* Process arguments */
while((opt = getopt(argc, argv, "vs")) != -1) {
@ -86,7 +90,10 @@ int main(int argc, char *argv[])
add_geojson_to_dijkstra(solver, argv[i]);
}
// add_test_map(&nodes, &paths);
// add_test_map(solver);
// TODO: This start / end coordinates are only for testing
start = dijkstra_node_new_to_list(&solver->nodes, -1213, -305);
dest = dijkstra_node_new_to_list(&solver->nodes, 388, 99);
if (should_report(LL_NOISY)) {
for (l = solver->nodes; l != NULL; l = l->next) {
@ -116,6 +123,13 @@ int main(int argc, char *argv[])
dijkstra_solver_free(solver);
return -3;
}
dijkstra_search_set_start(search, start);
if ((iterations = dijkstra_search_find_path(search, dest)) > 0) {
report(LL_INFO, "Found path after %d iterations.", iterations);
dijkstra_print_path(search, dest);
} else {
report(LL_WARNING, "Couldn't find path.");
}
dijkstra_search_free(search);
dijkstra_solver_free(solver);