From d24904c2d015524af95670762a301f1d4098a466 Mon Sep 17 00:00:00 2001 From: Markus Koch Date: Sun, 7 Feb 2021 21:04:40 +0100 Subject: [PATCH] [WIP] Implement trainlines --- src/dijkstragraph.c | 143 ++++++++++++++++++++++++++++++++++++++++--- src/dijkstragraph.h | 24 +++++++- src/dijkstrasearch.c | 40 +++++++++--- src/geojson.c | 45 +++++++++++++- src/geojson.h | 2 +- src/main.c | 34 ++++++++-- 6 files changed, 261 insertions(+), 27 deletions(-) diff --git a/src/dijkstragraph.c b/src/dijkstragraph.c index 37f6fbd..90fa482 100644 --- a/src/dijkstragraph.c +++ b/src/dijkstragraph.c @@ -14,6 +14,7 @@ int dijkstra_node_same (struct dijkstra_node *node_a, // 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; @@ -28,6 +29,7 @@ int dijkstra_node_same (struct dijkstra_node *node_a, struct dijkstra_node *dijkstra_node_new(float x, float y, int z, + struct dijkstra_layer *layer, int uid) { struct dijkstra_node *node; @@ -37,6 +39,7 @@ struct dijkstra_node *dijkstra_node_new(float x, node->position.x = x; node->position.y = y; node->position.z = z; + node->layer = layer; node->uid = uid; node->paths = NULL; } @@ -71,7 +74,8 @@ struct dijkstra_node *dijkstra_node_exists(GList *list, struct dijkstra_node *dijkstra_node_new_to_list(GList **list, float x, float y, - int z) + int z, + struct dijkstra_layer *layer) { struct dijkstra_node *node; struct dijkstra_node *found; @@ -79,7 +83,7 @@ struct dijkstra_node *dijkstra_node_new_to_list(GList **list, uid = g_list_length(*list); - node = dijkstra_node_new(x, y, z, uid); + node = dijkstra_node_new(x, y, z, layer, uid); if (!node) return NULL; @@ -171,9 +175,22 @@ 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; @@ -269,8 +286,14 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, 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; @@ -278,14 +301,73 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, 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_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 + 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; @@ -311,9 +393,11 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, // 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. + 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); + 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); @@ -373,6 +457,7 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, float y2, int z1, int z2, + struct dijkstra_layer *layer, char *name, dijkstra_cost weight) { @@ -381,8 +466,8 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, 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); + 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); @@ -403,6 +488,7 @@ struct dijkstra_solver *dijkstra_solver_new() if (solver) { solver->nodes = NULL; solver->paths = NULL; + solver->layers = NULL; } return solver; @@ -421,6 +507,8 @@ void dijkstra_solver_free(struct dijkstra_solver *solver) // 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); @@ -469,3 +557,42 @@ struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *so 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; +} diff --git a/src/dijkstragraph.h b/src/dijkstragraph.h index b4fd84e..97e0722 100644 --- a/src/dijkstragraph.h +++ b/src/dijkstragraph.h @@ -12,6 +12,7 @@ typedef int dijkstra_cost; struct dijkstra_solver { GList *nodes; GList *paths; + GList *layers; }; struct position { @@ -27,14 +28,28 @@ struct dijkstra_path { dijkstra_cost weight; }; +enum dijkstra_layer_type {STREET, TRAINLINE, ACCESS}; + +struct dijkstra_layer { + char *name; + enum dijkstra_layer_type type; + char *access; +}; + struct dijkstra_node { struct position position; GList *paths; + struct dijkstra_layer *layer; /* Layer (this will strcmp'd for matching) */ int uid; }; +struct offset_map { + char *name; + int offset; +}; + /* User Functions */ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, float x1, @@ -43,12 +58,18 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, float y2, int z1, int z2, + struct dijkstra_layer *layer, char *name, dijkstra_cost weight); struct dijkstra_solver *dijkstra_solver_new(); void dijkstra_solver_free(struct dijkstra_solver *solver); +struct dijkstra_layer *dijkstra_layer_new_to_list(struct dijkstra_solver *solver, + enum dijkstra_layer_type type, + char *name, + char *access); + /* Library functions */ struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node, struct dijkstra_path *path); @@ -62,6 +83,7 @@ void dijkstra_path_intersect_all(struct dijkstra_solver *solver, struct dijkstra_node *dijkstra_node_new_to_list(GList **list, float x, float y, - int z); + int z, + struct dijkstra_layer *layer); #endif // DIJKSTRAGRAPH_H diff --git a/src/dijkstrasearch.c b/src/dijkstrasearch.c index 2605d09..b08cadd 100644 --- a/src/dijkstrasearch.c +++ b/src/dijkstrasearch.c @@ -235,6 +235,7 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search, GList *l = NULL; struct dijkstra_node *node = NULL; struct dijkstra_node *node_last = NULL; + struct dijkstra_node *node_from = NULL; float heading = -1.0; float last_heading = 0; float heading_diff; @@ -249,6 +250,9 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search, for (l = route; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; if (node_last) { + if (node_last->layer->type != ACCESS) { + node_from = node_last; + } heading = atan2f((node->position.y - node_last->position.y), (node->position.x - node_last->position.x)) / (M_PI * 2) * 360; if (heading < 0.0) heading += 360; @@ -262,12 +266,28 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search, } else { relative_direction_str = "straight"; } - if (node_last == search->start) + if (node_last == search->start) { format_str = "Start on %2$s"; - else if (search->states[node_last->uid]->cheapest_path->name == search->states[node->uid]->cheapest_path->name) - format_str = "Stay on %2$s by going %1$s"; - else - format_str = "Go %s onto %s"; + } else if (node->layer->type == ACCESS && node_last->layer->type == ACCESS) { + + if (node_from->layer->type == STREET) + format_str = "[T] Enter the %2$s entrance on your %1$s"; + else + format_str = "[T] Get off the train at %2$s towards your %1$s"; + + } else if (search->states[node_last->uid]->cheapest_path->name == search->states[node->uid]->cheapest_path->name) { + if (node_last->layer->type == TRAINLINE) + format_str = "Waypoint for %2$s (going %1$s)"; // TODO: check whether we are passing a station + else + format_str = "Stay on %2$s by going %1$s"; + } else if (node->layer->type == TRAINLINE || node->layer->type == ACCESS) { + format_str = "[T] Take the %2$s"; + } else { + if (node_last->layer->type == TRAINLINE) + format_str = "[T] Leave the train at %2$s"; + else + format_str = "Go %s onto %s"; + } if (node_last != search->start) sdprintf(buf, &offset, ","); @@ -276,13 +296,13 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search, " \"geometry\": {\n" " \"type\": \"LineString\", \"coordinates\": [[%f,%f],[%f,%f]]\n" " },\n", - node_last->position.x, node_last->position.y, - node->position.x, node->position.y); + node_last->position.x, node_last->position.y, + node->position.x, node->position.y); sdprintf(buf, &offset, " \"properties\": {\n" " \"heading\": \"%f\",\n" " \"through\": \"%s\",\n", - heading, search->states[node->uid]->cheapest_path->name); - + heading, search->states[node->uid]->cheapest_path->name); + sdprintf(buf, &offset, " \"layertype\": \"%d->%d\",\n", node_last->layer->type, node->layer->type); sdprintf(buf, &offset, " \"description\": \""); sdprintf(buf, &offset, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name); sdprintf(buf, &offset, "\"\n"); @@ -291,7 +311,9 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search, sdprintf(buf, &offset, "}"); last_heading = heading; } + node_last = node; + } sdprintf(buf, &offset, "\n]\n"); diff --git a/src/geojson.c b/src/geojson.c index 183b429..58497be 100644 --- a/src/geojson.c +++ b/src/geojson.c @@ -7,7 +7,7 @@ #include int add_geojson_to_dijkstra(struct dijkstra_solver *solver, - char *filename) + char *filename, char *type) { JsonParser *parser; JsonNode *root; @@ -20,15 +20,30 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver, int i_max, j_max; char *name = NULL; + char *access = NULL; int x, y, z; int x_last, y_last, z_last; + char *layer_name; + enum dijkstra_layer_type layer_type; + struct dijkstra_layer *layer = NULL; struct _stats { int nodes; int paths; } stats = {.nodes = 0, .paths = 0}; - report(LL_INFO, "Loading GeoJson data from %s...", filename); + report(LL_INFO, "Loading GeoJson '%s' data from %s...", type, filename); + + if (!strcmp("streets", type)) + layer_type = STREET; + else if (!strcmp("trainlines", type)) + layer_type = TRAINLINE; + else if (!strcmp("access", type)) + layer_type = ACCESS; + else { + report(LL_ERROR, "Type '%s' not supported.\n", type); + return EXIT_FAILURE; + } parser = json_parser_new (); @@ -63,9 +78,32 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver, json_reader_end_member(reader); // name json_reader_end_member(reader); // properties + if (layer_type == ACCESS) { + access = name; + name = strchr(name, ':'); + if (name == NULL) { + report(LL_ERROR, + "Invalid name for accesss. '%s'", + access); + g_object_unref(reader); + continue; + } + *name = '\0'; + do + name++; + while (*name == ' ' && *name != '\0'); + } + json_reader_read_member(reader, "geometry"); json_reader_read_member(reader, "coordinates"); - z_last = 0; // By default, let's assume height 0 + if (layer_type == STREET) // For streets, this means elevation + z_last = 0; // By default, let's assume height 0 + else if (layer_type == ACCESS) // For access, it's the relative speed as well + z_last = 10; // 10 percent + else // For trainlines, z means relative speed to walking + z_last = 1000; // 2 times faster than walking + layer_name = name; + layer = dijkstra_layer_new_to_list(solver, layer_type, layer_name, access); if (json_reader_is_array(reader)) { j_max = json_reader_count_elements(reader); for (j = 0; j < j_max; j++) { @@ -92,6 +130,7 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver, x_last, y_last, x, y, z, z_last, + layer, name, DIJKSTRA_WEIGHT_AUTO); } diff --git a/src/geojson.h b/src/geojson.h index e767760..d89ec6c 100644 --- a/src/geojson.h +++ b/src/geojson.h @@ -5,6 +5,6 @@ #include "dijkstragraph.h" int add_geojson_to_dijkstra(struct dijkstra_solver *solver, - char *filename); + char *filename, char *type); #endif // GEOJSON_H diff --git a/src/main.c b/src/main.c index 6fcec7b..68f6d9f 100644 --- a/src/main.c +++ b/src/main.c @@ -2,6 +2,7 @@ #include #include #include +#include #include "dijkstragraph.h" #include "dijkstrasearch.h" @@ -18,6 +19,7 @@ void add_test_map(struct dijkstra_solver *solver) 5,0, 5,10, 0,0, + NULL, "Vertical", 1); @@ -25,6 +27,7 @@ void add_test_map(struct dijkstra_solver *solver) 0,5, 10,5, 0,0, + NULL, "Horizontal", 1); } @@ -50,10 +53,13 @@ int main(int argc, char *argv[]) GList *l = NULL; struct dijkstra_node *node; struct dijkstra_path *path; + struct dijkstra_layer *layer; int port = 6802; struct dijkstra_serv *serv; + char *file_name, *file_type; + /* Init stuff */ set_log_level(LL_DEBUG); @@ -93,21 +99,39 @@ int main(int argc, char *argv[]) /* Load map data -> Generate graph */ for (i = optind; i < argc; ++i) { - add_geojson_to_dijkstra(solver, argv[i]); + file_name = strchr(argv[i], ':'); + if (!file_name) { + file_name = argv[i]; + file_type = "street"; + } else { + *file_name = '\0'; + file_name++; + file_type = argv[i]; + } + add_geojson_to_dijkstra(solver, file_name, file_type); } if (should_report(LL_NOISY)) { + for (l = solver->layers; l != NULL; l = l->next) { + layer = (struct dijkstra_layer*) l->data; + fprintf(stderr, "Layer %p: %d : %s", layer, layer->type, layer->name); + if (layer->type == ACCESS) + fprintf(stderr, " (access to %s)", layer->access); + fprintf(stderr, "\n"); + } + printf("\n"); + for (l = solver->nodes; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; - fprintf(stderr, "Node %5d @%p: %f,\t%f [%d paths]\n", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths)); + fprintf(stderr, "Node %5d %d@%p: %f,\t%f [%d paths]\n", node->uid, node->layer->type, node, node->position.x, node->position.y, g_list_length(node->paths)); } printf("\n"); for (l = solver->paths; l != NULL; l = l->next) { path = (struct dijkstra_path*) l->data; - fprintf(stderr, "Path %p: %5d (%f,%f) -> %5d (%f,%f) cost %d \t\"%s\"\n", path, - path->source->uid, path->source->position.x, path->source->position.y, - path->destination->uid, path->destination->position.x, path->destination->position.y, + fprintf(stderr, "Path %p: %5d (%d,%f,%f,%d) -> %5d (%d,%f,%f,%d) cost %d \t\"%s\"\n", path, + path->source->uid, path->source->layer->type, path->source->position.x, path->source->position.y, path->source->position.z, + path->destination->uid, path->destination->layer->type, path->destination->position.x, path->destination->position.y, path->destination->position.z, path->weight, path->name); }