From 7d6082ed68a3e2815e43699b814224f1b3e0e154 Mon Sep 17 00:00:00 2001 From: Markus Koch Date: Sun, 3 May 2020 00:00:41 +0200 Subject: [PATCH] Fix bugs and implement bugged version of Dijkstra --- src/dijkstragraph.c | 110 ++++++++++++++++++++++++++++++------ src/dijkstragraph.h | 15 ++++- src/dijkstrasearch.c | 130 +++++++++++++++++++++++++++++++++++++++++++ src/dijkstrasearch.h | 12 +++- src/main.c | 22 ++++++-- 5 files changed, 264 insertions(+), 25 deletions(-) diff --git a/src/dijkstragraph.c b/src/dijkstragraph.c index bb9edca..189e715 100644 --- a/src/dijkstragraph.c +++ b/src/dijkstragraph.c @@ -2,6 +2,7 @@ #include #include #include +#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); +} diff --git a/src/dijkstragraph.h b/src/dijkstragraph.h index d9dc164..58e1048 100644 --- a/src/dijkstragraph.h +++ b/src/dijkstragraph.h @@ -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 diff --git a/src/dijkstrasearch.c b/src/dijkstrasearch.c index f8ee5b5..ed746a1 100644 --- a/src/dijkstrasearch.c +++ b/src/dijkstrasearch.c @@ -1,4 +1,6 @@ #include "dijkstrasearch.h" +#include "logging.h" +#include 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"); +} diff --git a/src/dijkstrasearch.h b/src/dijkstrasearch.h index fc2f838..9440b9e 100644 --- a/src/dijkstrasearch.h +++ b/src/dijkstrasearch.h @@ -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 diff --git a/src/main.c b/src/main.c index 4dba590..da22d7f 100644 --- a/src/main.c +++ b/src/main.c @@ -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);