[WIP] Implement trainlines

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_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);
+	n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1, layer);
-	n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2);
+	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;
+}

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

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)
+			} 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, ",");
@@ -282,7 +302,7 @@ int dijkstra_search_route_to_geojson(struct dijkstra_search *search,
 			       "        \"heading\": \"%f\",\n"
 			       "        \"through\": \"%s\",\n",
 				 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");

src/geojson.c

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

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

src/main.c

@@ -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,
+			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->position.x, path->source->position.y,
+			       path->source->uid, path->source->layer->type, path->source->position.x, path->source->position.y, path->source->position.z,
-			       path->destination->uid, path->destination->position.x, path->destination->position.y,
+			       path->destination->uid, path->destination->layer->type, path->destination->position.x, path->destination->position.y, path->destination->position.z,
 			       path->weight,
 			       path->name);
 		}