Implement trainline support

This commit is contained in:
Markus Koch 2021-02-26 19:15:48 +01:00
parent 6d252d7755
commit 6ee084472a
6 changed files with 229 additions and 48 deletions

View File

@ -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, 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 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 = dijkstra_position_get_distance(&node_a->position, &node_b->position);
dist /= scale;
if (dist < DIJKSTRA_COST_MIN) if (dist < DIJKSTRA_COST_MIN)
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_path *dijkstra_path_new(char *name,
struct dijkstra_node *source, struct dijkstra_node *source,
struct dijkstra_node *destination, struct dijkstra_node *destination,
dijkstra_cost weight) dijkstra_cost weight,
struct dijkstra_net_meta *net_meta)
{ {
struct dijkstra_path *path; struct dijkstra_path *path;
@ -197,13 +212,16 @@ struct dijkstra_path *dijkstra_path_new(char *name,
path->name = name; path->name = name;
if (weight == DIJKSTRA_WEIGHT_AUTO) if (weight == DIJKSTRA_WEIGHT_AUTO)
path->weight = dijkstra_get_weight_from_distance(source, path->weight = dijkstra_get_weight_from_distance(source,
destination); destination,
net_meta->type);
else else
path->weight = weight; path->weight = weight;
path->source = NULL; path->source = NULL;
path->destination = NULL; path->destination = NULL;
path->net_meta = net_meta;
if (dijkstra_connect_nodes_to_path(source, destination, path) != 0) { if (dijkstra_connect_nodes_to_path(source, destination, path) != 0) {
report(LL_CRITICAL, "Connect nodes failed: %p with %p as %p (%s)", report(LL_CRITICAL, "Connect nodes failed: %p with %p as %p (%s)",
source, destination, path, path->name); source, destination, path, path->name);
@ -265,6 +283,8 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
struct dijkstra_path *path_new; struct dijkstra_path *path_new;
struct dijkstra_node *node_new; struct dijkstra_node *node_new;
struct dijkstra_node *disconnected_node; 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 p0_x, p0_y, p1_x, p1_y;
float p2_x, p2_y, p3_x, p3_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) if (!path_a || !path_b)
return -1; 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 // Check height
p0_z = path_a->source->position.z; p0_z = path_a->source->position.z;
p1_z = path_a->destination->position.z; p1_z = path_a->destination->position.z;
p2_z = path_b->source->position.z; p2_z = path_b->source->position.z;
p3_z = path_b->destination->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 return 0; // No collision
p0_x = path_a->source->position.x; 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); disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination);
dijkstra_connect_destination_node_to_path(node_new, path_a); 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); 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. // 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. 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); disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination);
dijkstra_connect_destination_node_to_path(node_new, path_b); 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); 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. // 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 z1,
int z2, int z2,
char *name, char *name,
struct dijkstra_net_meta *net_meta,
dijkstra_cost weight) dijkstra_cost weight)
{ {
struct dijkstra_node *n1, *n2; 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); n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1);
n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2); 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); l_last = g_list_last(solver->paths);
if (l_last) if (l_last)
@ -403,6 +454,7 @@ struct dijkstra_solver *dijkstra_solver_new()
if (solver) { if (solver) {
solver->nodes = NULL; solver->nodes = NULL;
solver->paths = NULL; solver->paths = NULL;
solver->net_metas = NULL;
} }
return solver; return solver;
@ -413,6 +465,7 @@ void dijkstra_solver_free(struct dijkstra_solver *solver)
GList *l; GList *l;
struct dijkstra_path *path; struct dijkstra_path *path;
struct dijkstra_node *node; struct dijkstra_node *node;
struct dijkstra_net_meta *net_meta;
// Free memory // Free memory
for (l = solver->paths; l != NULL; l = l->next) { for (l = solver->paths; l != NULL; l = l->next) {
@ -430,6 +483,12 @@ void dijkstra_solver_free(struct dijkstra_solver *solver)
free(node); free(node);
} }
g_list_free(solver->nodes); 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, 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; 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;
}

View File

@ -12,6 +12,7 @@ typedef int dijkstra_cost;
struct dijkstra_solver { struct dijkstra_solver {
GList *nodes; GList *nodes;
GList *paths; GList *paths;
GList *net_metas;
}; };
struct position { struct position {
@ -20,11 +21,20 @@ struct position {
int z; 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_path {
struct dijkstra_node *source; struct dijkstra_node *source;
struct dijkstra_node *destination; struct dijkstra_node *destination;
char *name; char *name;
dijkstra_cost weight; dijkstra_cost weight;
struct dijkstra_net_meta *net_meta;
}; };
struct dijkstra_node { struct dijkstra_node {
@ -44,6 +54,7 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
int z1, int z1,
int z2, int z2,
char *name, char *name,
struct dijkstra_net_meta *net_meta,
dijkstra_cost weight); dijkstra_cost weight);
struct dijkstra_solver *dijkstra_solver_new(); 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, void dijkstra_path_intersect_all(struct dijkstra_solver *solver,
struct dijkstra_path *path, struct dijkstra_path *path,
struct dijkstra_path *path_last); 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 */ /* For debugging only */
struct dijkstra_node *dijkstra_node_new_to_list(GList **list, struct dijkstra_node *dijkstra_node_new_to_list(GList **list,

View File

@ -235,6 +235,9 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search,
GList *l = NULL; GList *l = NULL;
struct dijkstra_node *node = NULL; struct dijkstra_node *node = NULL;
struct dijkstra_node *node_last = 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 heading = -1.0;
float last_heading = 0; float last_heading = 0;
float heading_diff; 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) { for (l = route; l != NULL; l = l->next) {
node = (struct dijkstra_node*) l->data; node = (struct dijkstra_node*) l->data;
if (node_last) { 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; heading = atan2f((node->position.y - node_last->position.y), (node->position.x - node_last->position.x)) / (M_PI * 2) * 360;
if (heading < 0.0) if (heading < 0.0)
heading += 360; heading += 360;
@ -262,12 +268,32 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search,
} else { } else {
relative_direction_str = "straight"; relative_direction_str = "straight";
} }
if (node_last == search->start)
format_str = "Start on %2$s"; format_str = "Internal error in format_str in dijkstra_search_route_to_geojson()";
else if (search->states[node_last->uid]->cheapest_path->name == search->states[node->uid]->cheapest_path->name) switch (path_to_next->net_meta->type) {
format_str = "Stay on %2$s by going %1$s"; case STREET:
else if (node_last == search->start)
format_str = "Go %s onto %s"; 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) if (node_last != search->start)
sdprintf(buf, &offset, ","); sdprintf(buf, &offset, ",");
@ -276,12 +302,13 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search,
" \"geometry\": {\n" " \"geometry\": {\n"
" \"type\": \"LineString\", \"coordinates\": [[%f,%f],[%f,%f]]\n" " \"type\": \"LineString\", \"coordinates\": [[%f,%f],[%f,%f]]\n"
" },\n", " },\n",
node_last->position.x, node_last->position.y, node_last->position.x, node_last->position.y,
node->position.x, node->position.y); node->position.x, node->position.y);
sdprintf(buf, &offset, " \"properties\": {\n" sdprintf(buf, &offset, " \"properties\": {\n"
" \"heading\": \"%f\",\n" " \"heading\": \"%f\",\n"
" \"through\": \"%s\",\n", " \"through\": \"%s\",\n"
heading, search->states[node->uid]->cheapest_path->name); " \"type\": \"%d\",\n",
heading, path_to_next->name, path_to_next->net_meta->type);
sdprintf(buf, &offset, " \"description\": \""); sdprintf(buf, &offset, " \"description\": \"");
sdprintf(buf, &offset, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name); sdprintf(buf, &offset, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name);

View File

@ -7,7 +7,8 @@
#include <json-glib/json-glib.h> #include <json-glib/json-glib.h>
int add_geojson_to_dijkstra(struct dijkstra_solver *solver, int add_geojson_to_dijkstra(struct dijkstra_solver *solver,
char *filename) char *filename,
char *type)
{ {
JsonParser *parser; JsonParser *parser;
JsonNode *root; JsonNode *root;
@ -20,15 +21,30 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver,
int i_max, j_max; int i_max, j_max;
char *name = NULL; char *name = NULL;
char *access = NULL;
int x, y, z; int x, y, z;
int x_last, y_last, z_last; int x_last, y_last, z_last;
enum dijkstra_net_type net_type;
struct dijkstra_net_meta *net_meta;
struct _stats { struct _stats {
int nodes; int nodes;
int paths; int paths;
} stats = {.nodes = 0, .paths = 0}; } 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 (); 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); // name
json_reader_end_member(reader); // properties 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, "geometry");
json_reader_read_member(reader, "coordinates"); 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)) { if (json_reader_is_array(reader)) {
j_max = json_reader_count_elements(reader); j_max = json_reader_count_elements(reader);
for (j = 0; j < j_max; j++) { for (j = 0; j < j_max; j++) {
@ -93,6 +135,7 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver,
x, y, x, y,
z, z_last, z, z_last,
name, name,
net_meta,
DIJKSTRA_WEIGHT_AUTO); DIJKSTRA_WEIGHT_AUTO);
} }

View File

@ -5,6 +5,7 @@
#include "dijkstragraph.h" #include "dijkstragraph.h"
int add_geojson_to_dijkstra(struct dijkstra_solver *solver, int add_geojson_to_dijkstra(struct dijkstra_solver *solver,
char *filename); char *filename,
char *type);
#endif // GEOJSON_H #endif // GEOJSON_H

View File

@ -12,31 +12,18 @@
struct dijkstra_node *start; struct dijkstra_node *start;
struct dijkstra_node *dest; 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) void help(const char *exec)
{ {
fprintf(stderr, fprintf(stderr,
"Usage: %s [OPTION] [FILE]...\n" "Usage: %s [OPTION] [TYPE:FILE]...\n"
"Start the lifo-dijkstraserv routing server.\n\n" "Start the lifo-dijkstraserv routing server.\n\n"
"Available options:\n"
" -s\tset output to silent\n" " -s\tset output to silent\n"
" -v\tincrease verbosity. Maybe used multiple times.\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); exec);
} }
@ -50,6 +37,7 @@ int main(int argc, char *argv[])
GList *l = NULL; GList *l = NULL;
struct dijkstra_node *node; struct dijkstra_node *node;
struct dijkstra_path *path; struct dijkstra_path *path;
char *file_name, *file_type;
int port = 6802; int port = 6802;
struct dijkstra_serv *serv; struct dijkstra_serv *serv;
@ -68,7 +56,7 @@ int main(int argc, char *argv[])
set_log_level(0); set_log_level(0);
break; break;
case 'p': case 'p':
port = atoi(argv[i]); port = atoi(optarg);
break; break;
case ':': case ':':
case '?': case '?':
@ -93,7 +81,16 @@ int main(int argc, char *argv[])
/* Load map data -> Generate graph */ /* Load map data -> Generate graph */
for (i = optind; i < argc; ++i) { 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)) { if (should_report(LL_NOISY)) {
@ -106,10 +103,10 @@ int main(int argc, char *argv[])
for (l = solver->paths; l != NULL; l = l->next) { for (l = solver->paths; l != NULL; l = l->next) {
path = (struct dijkstra_path*) l->data; path = (struct dijkstra_path*) l->data;
fprintf(stderr, "Path %p: %5d (%f,%f) -> %5d (%f,%f) cost %d \t\"%s\"\n", path, 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->source->uid, path->source->position.x, path->source->position.y,
path->destination->uid, path->destination->position.x, path->destination->position.y, path->destination->uid, path->destination->position.x, path->destination->position.y,
path->weight, path->weight,
path->name); path->name);
} }
printf("\n"); printf("\n");
} }