Implement trainline support

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

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,

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";
+			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)
 				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_last->position.x, node_last->position.y,
-			       node->position.x, node->position.y);
+				 node->position.x, node->position.y);
 			sdprintf(buf, &offset, "    \"properties\": {\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, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name);

src/geojson.c

@@ -7,7 +7,8 @@
 #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 +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);
 							}
 

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

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