#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); same &= (node_a->layer == node_b->layer); same &= (node_a->position.z == node_b->position.z); 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 z, struct dijkstra_layer *layer, int uid) { struct dijkstra_node *node; node = malloc(sizeof(*node)); if (node) { node->position.x = x; node->position.y = y; node->position.z = z; node->layer = layer; 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, int z, struct dijkstra_layer *layer) { struct dijkstra_node *node; struct dijkstra_node *found; int uid; uid = g_list_length(*list); node = dijkstra_node_new(x, y, z, layer, 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; 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; } float dijkstra_position_get_distance(struct position *position_a, struct position *position_b) { return sqrt(pow(position_a->x - position_b->x, 2) + pow(position_a->y - position_b->y, 2)); } dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a, struct dijkstra_node *node_b) { float dist; float factor; dist = dijkstra_position_get_distance(&node_a->position, &node_b->position); factor = (node_a->layer->type == TRAINLINE ? ((float) node_a->position.z) / 100.0 : 1.0); factor += (node_b->layer->type == TRAINLINE ? ((float) node_b->position.z) / 100.0 : 1.0); factor /= 2; /*if (node_a->layer->type != STREET || node_b->layer->type != STREET) { report(LL_INFO, "%d-%d Factor is %f (from %d-%d)", node_a->layer->type,node_b->layer->type, factor, node_a->position.z, node_b->position.z); }*/ dist /= factor; 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; int p0_z, p1_z, p2_z, p3_z; struct dijkstra_layer *p0_layer, *p1_layer, *p2_layer, *p3_layer; float i_x, i_y; int i_z; struct dijkstra_layer *i_layer; struct dijkstra_layer *access_layer = NULL; struct dijkstra_layer *target_layer = NULL; int target_z = -10000000; float s1_x, s1_y, s2_x, s2_y; float s, t; if (!path_a || !path_b) return -1; // Process layer p0_layer = path_a->source->layer; p1_layer = path_a->destination->layer; p2_layer = path_b->source->layer; p3_layer = path_b->destination->layer; if (p0_layer->type == ACCESS) { access_layer = p0_layer; target_layer = p2_layer; target_z = path_b->source->position.z; } else if (p1_layer->type == ACCESS) { access_layer = p1_layer; target_layer = p2_layer; target_z = path_b->source->position.z; } else if (p2_layer->type == ACCESS) { access_layer = p2_layer; target_layer = p0_layer; target_z = path_a->source->position.z; } else if (p3_layer->type == ACCESS) { access_layer = p3_layer; target_layer = p0_layer; target_z = path_a->source->position.z; } else { target_layer = p0_layer; target_z = path_a->source->position.z; // For normal intersections, all .z must be the same, so we can pick any. } if (access_layer) { if (target_layer->type == ACCESS) return 0; if (target_layer->type == TRAINLINE && strcmp(access_layer->access, target_layer->name)) return 0; // report(LL_DEBUG, "ACCESS OK"); } else { if (p0_layer->type == TRAINLINE || p1_layer->type == TRAINLINE || p2_layer->type == TRAINLINE || p3_layer->type == TRAINLINE) return 0; // Never collide trains (sounds like a good idea, doesn't it!) } if (access_layer) { target_layer = access_layer; // Without this, I will (sometimes??) get a direct link between the two layers } /* if (p0_layer->type == ACCESS || p1_layer->type == ACCESS) { //if (p2_layer->type == TRAINLINE && strcmp(p0_layer->access, p2_layer->name)) // return 0; } else if (p2_layer->type == ACCESS || p3_layer->type == ACCESS) { //if (p0_layer->type == TRAINLINE && strcmp(p2_layer->access, p0_layer->name)) // return 0; } else if (p0_layer->type == TRAINLINE || p2_layer->type == TRAINLINE) { report(LL_ERROR, "Prevent %d to %d", p0_layer->type, p2_layer->type); return 0; // Never collide trains (sounds like a good idea, doesn't it!) }*/ // Check height p0_z = path_a->source->position.z; p1_z = path_a->destination->position.z; p2_z = path_b->source->position.z; p3_z = path_b->destination->position.z; if (p0_layer->type == STREET && p2_layer->type == STREET) { if (!(p0_z == p1_z && p1_z == p2_z && p2_z == p3_z)) // We don't want to intersect on height, they must match exactly for all four points. return 0; // No collision } 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); i_layer = target_layer; i_z = target_z; report(LL_DEBUG, "Split layer to type %d", i_layer->type); node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y, i_z, i_layer); 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); // TODO: I don't have to iterate over the *entire* array here, it should be enough to only iterate over the entries after path_b. dijkstra_path_intersect_all(solver, path_new, path_new); // Since straight paths can only cross once, we don't need to check any newly created entries. } // 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); } // In case neither if matches that means we are on the corner where two paths intersect at their ends. // No need to do anything then, dijkstra_node_new_to_list will have taken care of everything. return 1; } return 0; // No collision } void dijkstra_path_intersect_all(struct dijkstra_solver *solver, struct dijkstra_path *path, struct dijkstra_path *path_last) { GList *l; struct dijkstra_path *existing_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->data == path_last) /* Avoid testing newly added paths */ break; } } struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, float x1, float y1, float x2, float y2, int z1, int z2, struct dijkstra_layer *layer, char *name, dijkstra_cost weight) { struct dijkstra_node *n1, *n2; struct dijkstra_path *path; GList *l_last; struct dijkstra_path *path_last = NULL; n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1, layer); n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2, layer); path = dijkstra_path_new(name, n1, n2, weight); l_last = g_list_last(solver->paths); if (l_last) path_last = l_last->data; solver->paths = g_list_append(solver->paths, (gpointer) path); dijkstra_path_intersect_all(solver, path, path_last); return path; } struct dijkstra_solver *dijkstra_solver_new() { struct dijkstra_solver *solver; solver = malloc(sizeof(*solver)); if (solver) { solver->nodes = NULL; solver->paths = NULL; solver->layers = 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. // NOTE: Currently, do NOT free path->access as it is part of // path->name and will be freed along with it (or not lol). 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) { report(LL_CRITICAL, "Called get_connection with NULL node (%p) or path (%p).", node, 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); } struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver, struct position *position) { GList *l = NULL; struct dijkstra_node *node = NULL; struct dijkstra_node *closest_node = NULL; float closest_distance; float distance; for (l = solver->nodes; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; distance = dijkstra_position_get_distance(position, &node->position); if (!closest_node || distance < closest_distance) { closest_node = node; closest_distance = distance; } } return closest_node; } struct dijkstra_layer *dijkstra_layer_new(enum dijkstra_layer_type type, char *name, char *access) { struct dijkstra_layer *layer; layer = malloc(sizeof(*layer)); if (layer) { layer->name = strdup(name); layer->type = type; layer->access = access; } return layer; } struct dijkstra_layer *dijkstra_layer_new_to_list(struct dijkstra_solver *solver, enum dijkstra_layer_type type, char *name, char *access) { struct dijkstra_layer *layer; GList *l = NULL; if (type == STREET) name = "all"; for (l = solver->layers; l != NULL; l = l->next) { layer = l->data; if (layer->type == type && !strcmp(layer->name, name)) { return layer; } } layer = dijkstra_layer_new(type, name, access); solver->layers = g_list_append(solver->layers, (void *) layer); return layer; }