#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->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, 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->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_node *node; struct dijkstra_node *found; int uid; uid = g_list_length(*list); node = dijkstra_node_new(x, y, z, 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, enum dijkstra_net_type net_type) { float dist; float scale; switch (net_type) { case TRAINLINE: scale = 5.0; break; case ACCESS: scale = 0.5; break; default: scale = 1.0; break; } dist = dijkstra_position_get_distance(&node_a->position, &node_b->position); dist /= scale; 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_net_meta *net_meta) { 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, net_meta->type); else path->weight = weight; path->source = NULL; path->destination = NULL; path->net_meta = net_meta; 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; struct dijkstra_path *access_path = NULL; struct dijkstra_path *target_path = NULL; 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; float i_x, i_y; int i_z; float s1_x, s1_y, s2_x, s2_y; float s, t; if (!path_a || !path_b) return -1; if (path_a->net_meta->type == ACCESS) { access_path = path_a; target_path = path_b; } else if (path_b->net_meta->type == ACCESS) { access_path = path_b; target_path = path_a; } if (access_path) { if (target_path->net_meta->type != STREET && !strstr(access_path->net_meta->access, target_path->name)) { return 0; // Don't intersect if access != name of target } } else { if (path_a->net_meta->type != path_b->net_meta->type) return 0; // Never intersect two layers of different types, unless it's through access layers // path_a and path_b types are now always guaranteed to be the same if (path_a->net_meta->type == TRAINLINE) return 0; // Never intersect trainlines (apart from with access types) } // 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 (!access_path && // For access paths, z is invalid, so only check this if we are intersecting streets (trainlines are filtered out above) !(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_z = p0_z; // They must all on the same height when intersecting, so we can pick whichever. node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y, i_z); 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, path_new->net_meta->type); 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, path_new->net_meta->type); dijkstra_path_intersect_all(solver, path_b, 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, path_new->net_meta->type); 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, path_new->net_meta->type); } // 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, char *name, struct dijkstra_net_meta *net_meta, 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); n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2); path = dijkstra_path_new(name, n1, n2, weight, net_meta); 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->net_metas = NULL; } return solver; } void dijkstra_solver_free(struct dijkstra_solver *solver) { GList *l; struct dijkstra_path *path; struct dijkstra_node *node; struct dijkstra_net_meta *net_meta; // 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); for (l = solver->net_metas; l != NULL; l = l->next) { net_meta = (struct dijkstra_net_meta*) l->data; free(net_meta->name); if (net_meta->access) free(net_meta->access); free(net_meta); } } 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; GList *pl = NULL; struct dijkstra_node *node = NULL; struct dijkstra_node *closest_node = NULL; struct dijkstra_path *path = NULL; float closest_distance; float distance; for (l = solver->nodes; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; for (pl = node->paths; pl != NULL; pl = pl->next) { path = (struct dijkstra_path*) pl->data; if (path->net_meta->type == STREET) break; } if (!(path && path->net_meta->type == STREET)) continue; // Only start on nodes that are part of a street 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_net_meta *dijkstra_net_meta_new(enum dijkstra_net_type net_type, char *name, char *access) { struct dijkstra_net_meta *net_meta; net_meta = malloc(sizeof(*net_meta)); if (net_meta) { net_meta->type = net_type; net_meta->name = strdup(name); if (access) net_meta->access = strdup(access); else net_meta->access = NULL; } return net_meta; } struct dijkstra_net_meta *dijkstra_net_meta_new_to_list(struct dijkstra_solver *solver, enum dijkstra_net_type net_type, char *name, char *access) { struct dijkstra_net_meta *net_meta; GList *l = NULL; if (net_type == STREET) name = "all"; for (l = solver->net_metas; l != NULL; l = l->next) { net_meta = l->data; if (net_meta->type == net_type && !strcmp(net_meta->name, name) && (net_meta->access && (!strcmp(net_meta->access, access)))) { return net_meta; } } net_meta = dijkstra_net_meta_new(net_type, name, access); solver->net_metas = g_list_append(solver->net_metas, (void *) net_meta); return net_meta; }