diff --git a/src/dijkstrasearch.c b/src/dijkstrasearch.c new file mode 100644 index 0000000..c3ea2cb --- /dev/null +++ b/src/dijkstrasearch.c @@ -0,0 +1,291 @@ +#include "dijkstrasearch.h" +#include +#include +#include + +#define FP_THRESHOLD 0.01 +int dijkstra_node_same (struct dijkstra_node *node_a, + struct dijkstra_node *node_b) +{ + int same = 1; + + // Not the proper way to do it, but it works for now. + same &= (fabs(node_a->position.x - node_b->position.x) < FP_THRESHOLD); + same &= (fabs(node_a->position.y - node_b->position.y) < FP_THRESHOLD); + + return same; +} + +struct dijkstra_node *dijkstra_node_exists(GList *list, + struct dijkstra_node *node) +{ + GList *l; + struct dijkstra_node *node_existing; + + for (l = list; l != NULL; l = l->next) { + node_existing = (struct dijkstra_node*) l->data; + if (dijkstra_node_same(node, node_existing)) { + return node_existing; + } + } + + return NULL; +} + +/*! + * \brief dijkstra_node_new_to_list creates a new node entry if it does not already exist in the list + * \param list list of dijkstra_node + * \param x + * \param y + * \param is_new When != NULL: is_new will be set to true if a new node had to be created + * \return a dijkstra_node with the specified position (either newly created or from the list). Or NULL on error. + */ +struct dijkstra_node *dijkstra_node_new_to_list(GList **list, + float x, + float y, + int *is_new) +{ + struct dijkstra_node *node; + struct dijkstra_node *found; + + node = dijkstra_node_new(x, y); + if (!node) + return NULL; + + if ((found = dijkstra_node_exists(*list, node))) { + free(node); + node = found; + if (is_new) + *is_new = 0; + } else { + *list = g_list_append(*list, node); + if (is_new) + *is_new = 1; + } + + return node; +} + +int dijkstra_connect_destination_node_to_path(struct dijkstra_node *destination, + struct dijkstra_path *path) +{ + if (!path || !destination) + return -1; + + destination->paths = g_list_append(destination->paths, (gpointer) path); + path->destination = destination; + + return 0; +} + +int dijkstra_connect_source_node_to_path(struct dijkstra_node *source, + struct dijkstra_path *path) +{ + if (!path || !source) + return -1; + + source->paths = g_list_append(source->paths, (gpointer) path); + path->source = source; + + return 0; +} + +/*! + * \brief dijkstra_connect_nodes_to_path connect the two nodes using the path + * \param source + * \param destination + * \param path + * \return + */ +int dijkstra_connect_nodes_to_path(struct dijkstra_node *source, + struct dijkstra_node *destination, + struct dijkstra_path *path) +{ + int ret = 0; + + ret |= dijkstra_connect_source_node_to_path(source, path); + ret |= dijkstra_connect_destination_node_to_path(destination, path); + + return ret; +} + +// WARNING: This does not update the references in the nodes!!! +struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path) +{ + struct dijkstra_path *dup; + + if (!path) + return NULL; + + dup = malloc(sizeof(*path)); + if (dup) { + memcpy(dup, path, sizeof(*path)); + } + + return dup; +} + +struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *path, + struct dijkstra_node *node, + int ref_only) +{ + node->paths = g_list_remove(node->paths, (gpointer) node); + + if (ref_only) + return node; + + if (path->source == node) + path->source = NULL; + if (path->destination == node) + path->destination = NULL; + + return node; +} + +int dijkstra_path_intersect(GList **nodes, + GList **paths, + struct dijkstra_path *path_a, + struct dijkstra_path *path_b) +{ + // Yes, I was too lazy to code this myself: + // https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect + + struct dijkstra_path *path_new; + struct dijkstra_node *node_new; + + float p0_x, p0_y, p1_x, p1_y; + float p2_x, p2_y, p3_x, p3_y; + float i_x, i_y; + + float s1_x, s1_y, s2_x, s2_y; + float s, t; + + int is_new; + + if (!path_a || !path_b) + return -1; + + p0_x = path_a->source->position.x; + p0_y = path_a->source->position.y; + p1_x = path_a->destination->position.x; + p1_y = path_a->destination->position.y; + + p2_x = path_b->source->position.x; + p2_y = path_b->source->position.y; + p3_x = path_b->destination->position.x; + p3_y = path_b->destination->position.y; + + s1_x = p1_x - p0_x; + s1_y = p1_y - p0_y; + + s2_x = p3_x - p2_x; + s2_y = p3_y - p2_y; + + s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y); + t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y); + + if (s >= 0 && s <= 1 && t >= 0 && t <= 1) + { + // Collision detected + i_x = p0_x + (t * s1_x); + i_y = p0_y + (t * s1_y); + + node_new = dijkstra_node_new_to_list(nodes, i_x, i_y, &is_new); + + if (node_new != path_a->source && node_new != path_a->destination) { + path_new = dijkstra_path_dup_shallow(path_a); + *paths = g_list_append(*paths, (gpointer) path_new); + + dijkstra_disconnect_node_from_path(path_a, path_a->destination, 1); + dijkstra_connect_destination_node_to_path(node_new, path_a); + dijkstra_connect_source_node_to_path(node_new, 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); + *paths = g_list_append(*paths, (gpointer) path_new); + dijkstra_disconnect_node_from_path(path_b, path_b->destination, 1); + dijkstra_connect_destination_node_to_path(node_new, path_b); + dijkstra_connect_source_node_to_path(node_new, path_new); + } + return 1; + } + + return 0; // No collision +} + +struct dijkstra_path *dijkstra_path_new_to_list(GList **nodes, + GList **paths, + float x1, + float y1, + float x2, + float y2, + char *name, + dijkstra_cost weight) +{ + struct dijkstra_node *n1, *n2; + struct dijkstra_path *path; + struct dijkstra_path *existing_path; + GList *l, *l_last; + + n1 = dijkstra_node_new_to_list(nodes, x1, y1, NULL); + n2 = dijkstra_node_new_to_list(nodes, x2, y2, NULL); + path = dijkstra_path_new(name, n1, n2, weight); + + l_last = g_list_last(*paths); + *paths = g_list_append(*paths, (gpointer) path); + + for (l = *paths; l != NULL; l = l->next) { + existing_path = (struct dijkstra_path*) l->data; + dijkstra_path_intersect(nodes, paths, path, existing_path); + if (l == l_last) /* Avoid testing newly added paths */ + break; + } + + return path; +} + +/*! + * \brief dijkstra_node_new creates a new dijkstra_node + * \param x x position + * \param y y position + * \return a new dijkstra node or NULL. Needs to be freed manually. + */ +struct dijkstra_node *dijkstra_node_new(float x, + float y) +{ + struct dijkstra_node *node; + + node = malloc(sizeof(*node)); + if (node) { + node->position.x = x; + node->position.y = y; + node->paths = NULL; + } + + return node; +} + +/*! + * \brief dijkstra_path_new creates a new dijkstra_path between two nodes. + * \param name the name of the path (pointer is not copied and must stay valid!) + * \return a new dijkstra path or NULL. Needs to be freed manually. + */ +struct dijkstra_path *dijkstra_path_new(char *name, + struct dijkstra_node *source, + struct dijkstra_node *destination, + dijkstra_cost weight) +{ + struct dijkstra_path *path; + + path = malloc(sizeof(*path)); + if (path) { + path->name = name; + path->weight = weight; + + dijkstra_connect_nodes_to_path(source, destination, path); + } + + return path; +} diff --git a/src/dijkstrasearch.h b/src/dijkstrasearch.h new file mode 100644 index 0000000..a959e5a --- /dev/null +++ b/src/dijkstrasearch.h @@ -0,0 +1,48 @@ +#ifndef DIJKSTRASEARCH_H +#define DIJKSTRASEARCH_H + +#include + +typedef int dijkstra_cost; + +struct position { + float x; + float y; +}; + +struct dijkstra_path { + struct dijkstra_node *source; + struct dijkstra_node *destination; + char *name; + dijkstra_cost weight; +}; + +struct dijkstra_node { + struct position position; + + GList *paths; + + dijkstra_cost cost; +}; + +struct dijkstra_node *dijkstra_node_new(float x, + float y); +struct dijkstra_path *dijkstra_path_new(char *name, + struct dijkstra_node *source, + struct dijkstra_node *destination, + dijkstra_cost weight); + +int dijkstra_path_intersect(GList **nodes, + GList **paths, + struct dijkstra_path *path_a, + struct dijkstra_path *path_b); + +struct dijkstra_path *dijkstra_path_new_to_list(GList **nodes, + GList **paths, + float x1, + float y1, + float x2, + float y2, + char *name, + dijkstra_cost weight); +#endif // DIJKSTRASEARCH_H diff --git a/src/main.c b/src/main.c index 754935c..3b5bca0 100644 --- a/src/main.c +++ b/src/main.c @@ -1,7 +1,47 @@ #include +#include "dijkstrasearch.h" +#include + int main() { - printf("Hello World!\n"); + GList *nodes = NULL; + GList *paths = NULL; + + GList *l = NULL; + struct dijkstra_node *node; + struct dijkstra_path *path; + + dijkstra_path_new_to_list(&nodes, + &paths, + 0,0, + 0,10, + "Vertical", + 1); + + dijkstra_path_new_to_list(&nodes, + &paths, + 0,5, + 10,5, + "Horizontal", + 1); + + for (l = nodes; l != NULL; l = l->next) { + node = (struct dijkstra_node*) l->data; + printf("Node %p: %f,\t%f [%d paths]\n", node, node->position.x, node->position.y, g_list_length(node->paths)); + } + printf("\n"); + + for (l = paths; l != NULL; l = l->next) { + path = (struct dijkstra_path*) l->data; + printf("Path %p: %p (%f,%f) -> %p (%f,%f) \t\"%s\"\n", path, + path->source, path->source->position.x, path->source->position.y, + path->destination, path->destination->position.x, path->destination->position.y, + path->name); + } + + printf("\nDone.\n"); + + // TODO: Free memory return 0; }