#include "dijkstragraph.h" #include #include #include #include "logging.h" #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; } /*! * \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, int uid) { struct dijkstra_node *node; node = malloc(sizeof(*node)); if (node) { node->position.x = x; node->position.y = y; node->uid = uid; node->paths = NULL; } return node; } 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) { struct dijkstra_node *node; struct dijkstra_node *found; int uid; uid = g_list_length(*list); node = dijkstra_node_new(x, y, uid); if (!node) return NULL; if ((found = dijkstra_node_exists(*list, node))) { free(node); node = found; } else { *list = g_list_append(*list, node); } 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; return 0; } int dijkstra_connect_source_node_to_path(struct dijkstra_node *source, struct dijkstra_path *path) { 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; 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; } // Creates an unlinked (!) copy of the path 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)); dup->source = NULL; dup->destination = NULL; } return dup; } dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a, struct dijkstra_node *node_b) { float dist; dist = sqrt(pow(node_a->position.x - node_b->position.x, 2) + pow(node_a->position.y - node_b->position.y, 2)); if (dist < DIJKSTRA_COST_MIN) dist = DIJKSTRA_COST_MIN; return (dijkstra_cost) (dist * 100); } /*! * \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; if (weight == DIJKSTRA_WEIGHT_AUTO) path->weight = dijkstra_get_weight_from_distance(source, destination); else path->weight = weight; 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) { 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 (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; } 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, 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; struct dijkstra_node *disconnected_node; 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; 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(&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); disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination); dijkstra_connect_destination_node_to_path(node_new, path_a); path_a->weight = dijkstra_get_weight_from_distance(path_a->source, path_a->destination); dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new); path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination); } // 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); disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination); dijkstra_connect_destination_node_to_path(node_new, path_b); path_b->weight = dijkstra_get_weight_from_distance(path_b->source, path_b->destination); dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new); path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination); } return 1; } return 0; // No collision } struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, 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(&solver->nodes, x1, y1); n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2); path = dijkstra_path_new(name, n1, n2, weight); l_last = g_list_last(solver->paths); solver->paths = g_list_append(solver->paths, (gpointer) path); for (l = solver->paths; l != NULL; l = l->next) { existing_path = (struct dijkstra_path*) l->data; dijkstra_path_intersect(solver, path, existing_path); if (l == l_last) /* Avoid testing newly added paths */ break; } return path; } struct dijkstra_solver *dijkstra_solver_new() { struct dijkstra_solver *solver; solver = malloc(sizeof(*solver)); if (solver) { solver->nodes = NULL; solver->paths = NULL; } return solver; } void dijkstra_solver_free(struct dijkstra_solver *solver) { GList *l; struct dijkstra_path *path; struct dijkstra_node *node; // Free memory for (l = solver->paths; l != NULL; l = l->next) { path = (struct dijkstra_path*) l->data; // TODO: free(path->name); // Currently, we reuse the existing memory block when for the // name when cloning streets. As such, clearing it here will // cause a double free if the street has been split. free(path); } g_list_free(solver->paths); for (l = solver->nodes; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; free(node); } 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); }