Fix bugs and implement bugged version of Dijkstra
This commit is contained in:
parent
7d41c1cdf7
commit
7d6082ed68
@ -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);
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
22
src/main.c
22
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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user