#include "dijkstragraph.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; } /*! * \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_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; } 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)); return (dijkstra_cost) dist; } /*! * \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 (path->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); } return path; } 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(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; 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); 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); solver->paths = g_list_append(solver->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(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); }