markus/lifo-dijkstraserv
1
0
Fork 0
Code Issues 3 Pull Requests Releases Wiki Activity

[WIP] Implement trainlines

Browse Source
This commit is contained in:
Markus Koch 2021-02-07 21:04:40 +01:00
parent 6d252d7755
commit d24904c2d0
6 changed files with 261 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
src/dijkstragraph.c
View File

@ -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;
}

24
src/dijkstragraph.h
View File

@ -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

40
src/dijkstrasearch.c
View File

@ -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");

45
src/geojson.c
View File

@ -7,7 +7,7 @@
#include <json-glib/json-glib.h>
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);
}

2
src/geojson.h
View File

@ -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

34
src/main.c
View File

@ -2,6 +2,7 @@
#include <stdlib.h>
#include <getopt.h>
#include <glib.h>
#include <string.h>
#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);
}