From 6ee084472a48399d0d0b6d10a656d9ef8fae47de Mon Sep 17 00:00:00 2001 From: Markus Koch Date: Fri, 26 Feb 2021 19:15:48 +0100 Subject: [PATCH] Implement trainline support --- src/dijkstragraph.c | 116 +++++++++++++++++++++++++++++++++++++++---- src/dijkstragraph.h | 15 ++++++ src/dijkstrasearch.c | 47 ++++++++++++++---- src/geojson.c | 49 ++++++++++++++++-- src/geojson.h | 3 +- src/main.c | 47 ++++++++---------- 6 files changed, 229 insertions(+), 48 deletions(-) diff --git a/src/dijkstragraph.c b/src/dijkstragraph.c index 37f6fbd..e893cfe 100644 --- a/src/dijkstragraph.c +++ b/src/dijkstragraph.c @@ -168,11 +168,25 @@ float dijkstra_position_get_distance(struct position *position_a, } dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a, - struct dijkstra_node *node_b) + struct dijkstra_node *node_b, + enum dijkstra_net_type net_type) { float dist; + float scale; + + switch (net_type) { + case TRAINLINE: + scale = (float)node_a->position.z; + scale += (float)node_b->position.z; + scale /= 200; + 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; @@ -188,7 +202,8 @@ dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a, struct dijkstra_path *dijkstra_path_new(char *name, struct dijkstra_node *source, struct dijkstra_node *destination, - dijkstra_cost weight) + dijkstra_cost weight, + struct dijkstra_net_meta *net_meta) { struct dijkstra_path *path; @@ -197,13 +212,16 @@ struct dijkstra_path *dijkstra_path_new(char *name, path->name = name; if (weight == DIJKSTRA_WEIGHT_AUTO) path->weight = dijkstra_get_weight_from_distance(source, - destination); + 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); @@ -265,6 +283,8 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, 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; @@ -278,13 +298,32 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, 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 + } + // 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. + 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; @@ -320,10 +359,16 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, 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_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->weight = dijkstra_get_weight_from_distance( + path_new->source, + path_new->destination, + path_new->net_meta->type); // 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. @@ -336,10 +381,15 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, 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_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->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. @@ -374,6 +424,7 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, int z1, int z2, char *name, + struct dijkstra_net_meta *net_meta, dijkstra_cost weight) { struct dijkstra_node *n1, *n2; @@ -383,7 +434,7 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, 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); + path = dijkstra_path_new(name, n1, n2, weight, net_meta); l_last = g_list_last(solver->paths); if (l_last) @@ -403,6 +454,7 @@ struct dijkstra_solver *dijkstra_solver_new() if (solver) { solver->nodes = NULL; solver->paths = NULL; + solver->net_metas = NULL; } return solver; @@ -413,6 +465,7 @@ 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) { @@ -430,6 +483,12 @@ void dijkstra_solver_free(struct dijkstra_solver *solver) 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); + free(net_meta); + } } inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node, @@ -469,3 +528,42 @@ struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *so 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); + net_meta->access = access; + } + + 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)) { + 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; +} diff --git a/src/dijkstragraph.h b/src/dijkstragraph.h index b4fd84e..a294e70 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 *net_metas; }; struct position { @@ -20,11 +21,20 @@ struct position { int z; }; +enum dijkstra_net_type {STREET, TRAINLINE, ACCESS}; + +struct dijkstra_net_meta { + char *name; + enum dijkstra_net_type type; + char *access; +}; + struct dijkstra_path { struct dijkstra_node *source; struct dijkstra_node *destination; char *name; dijkstra_cost weight; + struct dijkstra_net_meta *net_meta; }; struct dijkstra_node { @@ -44,6 +54,7 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver, int z1, int z2, char *name, + struct dijkstra_net_meta *net_meta, dijkstra_cost weight); struct dijkstra_solver *dijkstra_solver_new(); @@ -57,6 +68,10 @@ struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *so void dijkstra_path_intersect_all(struct dijkstra_solver *solver, struct dijkstra_path *path, struct dijkstra_path *path_last); +struct dijkstra_net_meta *dijkstra_net_meta_new_to_list(struct dijkstra_solver *solver, + enum dijkstra_net_type net_type, + char *name, + char *access); /* For debugging only */ struct dijkstra_node *dijkstra_node_new_to_list(GList **list, diff --git a/src/dijkstrasearch.c b/src/dijkstrasearch.c index 2605d09..39e9a68 100644 --- a/src/dijkstrasearch.c +++ b/src/dijkstrasearch.c @@ -235,6 +235,9 @@ 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_path *path_to_this = NULL; + struct dijkstra_path *path_to_next = NULL; + float heading = -1.0; float last_heading = 0; float heading_diff; @@ -249,6 +252,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) { + path_to_this = search->states[node_last->uid]->cheapest_path; + path_to_next = search->states[node->uid]->cheapest_path; + 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 +268,32 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search, } else { relative_direction_str = "straight"; } - 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"; + + format_str = "Internal error in format_str in dijkstra_search_route_to_geojson()"; + switch (path_to_next->net_meta->type) { + case STREET: + if (node_last == search->start) + format_str = "Start on %2$s"; + else if (path_to_this->name == path_to_next->name) + format_str = "Stay on %2$s by going %1$s"; + else + format_str = "Go %s onto %s"; + break; + + case TRAINLINE: + if (path_to_this->net_meta->type == ACCESS) // Enter train + format_str = "[T] Get on the %2$s"; + else // Stay on train + format_str = "[T] Stay on the %2$s"; + break; + + case ACCESS: + if (path_to_this->net_meta->type == TRAINLINE) // Enter station from train + format_str = "[T] Leave the train at %2$s to your %1$s"; + else // Enter station from street + format_str = "[T] Enter %2$s on your %1$s"; + break; + } if (node_last != search->start) sdprintf(buf, &offset, ","); @@ -276,12 +302,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); + " \"through\": \"%s\",\n" + " \"type\": \"%d\",\n", + heading, path_to_next->name, path_to_next->net_meta->type); sdprintf(buf, &offset, " \"description\": \""); sdprintf(buf, &offset, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name); diff --git a/src/geojson.c b/src/geojson.c index 183b429..0db8f15 100644 --- a/src/geojson.c +++ b/src/geojson.c @@ -7,7 +7,8 @@ #include int add_geojson_to_dijkstra(struct dijkstra_solver *solver, - char *filename) + char *filename, + char *type) { JsonParser *parser; JsonNode *root; @@ -20,15 +21,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; + enum dijkstra_net_type net_type; + struct dijkstra_net_meta *net_meta; + 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("street", type)) + net_type = STREET; + else if (!strcmp("trainline", type)) + net_type = TRAINLINE; + else if (!strcmp("access", type)) + net_type = ACCESS; + else { + report(LL_ERROR, "Type '%s' not supported.\n", type); + return EXIT_FAILURE; + } parser = json_parser_new (); @@ -63,9 +79,35 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver, json_reader_end_member(reader); // name json_reader_end_member(reader); // properties + if (net_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 + switch (net_type) { + case STREET: + z_last = 0; // By default, let's assume height 0 + break; + case TRAINLINE: + z_last = 1000; // By default, let's assume 5x normal speed + break; + default: + z_last = -1; + } + net_meta = dijkstra_net_meta_new_to_list(solver, net_type, name, access); if (json_reader_is_array(reader)) { j_max = json_reader_count_elements(reader); for (j = 0; j < j_max; j++) { @@ -93,6 +135,7 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver, x, y, z, z_last, name, + net_meta, DIJKSTRA_WEIGHT_AUTO); } diff --git a/src/geojson.h b/src/geojson.h index e767760..6a1451f 100644 --- a/src/geojson.h +++ b/src/geojson.h @@ -5,6 +5,7 @@ #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..1e389d2 100644 --- a/src/main.c +++ b/src/main.c @@ -12,31 +12,18 @@ struct dijkstra_node *start; struct dijkstra_node *dest; -void add_test_map(struct dijkstra_solver *solver) -{ - dijkstra_path_new_to_list(solver, - 5,0, - 5,10, - 0,0, - "Vertical", - 1); - - dijkstra_path_new_to_list(solver, - 0,5, - 10,5, - 0,0, - "Horizontal", - 1); -} - void help(const char *exec) { fprintf(stderr, - "Usage: %s [OPTION] [FILE]...\n" + "Usage: %s [OPTION] [TYPE:FILE]...\n" "Start the lifo-dijkstraserv routing server.\n\n" + "Available options:\n" " -s\tset output to silent\n" " -v\tincrease verbosity. Maybe used multiple times.\n" - " -p\tport number (default 6802)\n", + " -p\tport number (default 6802)\n\n" + "TYPE can be any of:\n" + " street (default), trainline, access\n\n" + "At least one GeoJson FILE must be presented.\n", exec); } @@ -50,6 +37,7 @@ int main(int argc, char *argv[]) GList *l = NULL; struct dijkstra_node *node; struct dijkstra_path *path; + char *file_name, *file_type; int port = 6802; struct dijkstra_serv *serv; @@ -68,7 +56,7 @@ int main(int argc, char *argv[]) set_log_level(0); break; case 'p': - port = atoi(argv[i]); + port = atoi(optarg); break; case ':': case '?': @@ -93,7 +81,16 @@ 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)) { @@ -106,10 +103,10 @@ int main(int argc, char *argv[]) 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, - path->weight, - path->name); + path->source->uid, path->source->position.x, path->source->position.y, + path->destination->uid, path->destination->position.x, path->destination->position.y, + path->weight, + path->name); } printf("\n"); }