Add height support

Optional third coordinate in position field. Streets will only
intersect when all 4 coordinates are on the same height.

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->position.z == node_b->position.z);
 
 	return same;
 }
@@ -26,6 +27,7 @@ int dijkstra_node_same (struct dijkstra_node *node_a,
  */
 struct dijkstra_node *dijkstra_node_new(float x,
 					float y,
+					int z,
 					int uid)
 {
 	struct dijkstra_node *node;
@@ -34,6 +36,7 @@ struct dijkstra_node *dijkstra_node_new(float x,
 	if (node) {
 		node->position.x = x;
 		node->position.y = y;
+		node->position.z = z;
 		node->uid = uid;
 		node->paths = NULL;
 	}
@@ -67,7 +70,8 @@ struct dijkstra_node *dijkstra_node_exists(GList *list,
  */
 struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
 						float x,
-						float y)
+						float y,
+						int z)
 {
 	struct dijkstra_node *node;
 	struct dijkstra_node *found;
@@ -75,7 +79,7 @@ struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
 
 	uid = g_list_length(*list);
 
-	node = dijkstra_node_new(x, y, uid);
+	node = dijkstra_node_new(x, y, z, uid);
 	if (!node)
 		return NULL;
 
@@ -264,7 +268,9 @@ 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;
 	float i_x, i_y;
+	int i_z;
 
 	float s1_x, s1_y, s2_x, s2_y;
 	float s, t;
@@ -272,6 +278,15 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver,
 	if (!path_a || !path_b)
 		return -1;
 
+	// 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
+
 	p0_x = path_a->source->position.x;
 	p0_y = path_a->source->position.y;
 	p1_x = path_a->destination->position.x;
@@ -296,8 +311,9 @@ 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.
 
-		node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y);
+		node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y, i_z);
 		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);
@@ -334,6 +350,8 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
 						float y1,
 						float x2,
 						float y2,
+						int z1,
+						int z2,
 						char *name,
 						dijkstra_cost weight)
 {
@@ -342,8 +360,8 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
 	struct dijkstra_path *existing_path;
 	GList *l, *l_last;
 
-	n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1);
-	n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2);
+	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);
 
 	l_last = g_list_last(solver->paths);

src/dijkstragraph.h

@@ -17,6 +17,7 @@ struct dijkstra_solver {
 struct position {
 	float x;
 	float y;
+	int z;
 };
 
 struct dijkstra_path {
@@ -40,6 +41,8 @@ struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
 						float y1,
 						float x2,
 						float y2,
+						int z1,
+						int z2,
 						char *name,
 						dijkstra_cost weight);
 
@@ -55,6 +58,7 @@ struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *so
 /* For debugging only */
 struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
 						float x,
-						float y);
+						float y,
+						int z);
 
 #endif // DIJKSTRAGRAPH_H

src/geojson.c

@@ -20,8 +20,8 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver,
 	int i_max, j_max;
 
 	char *name = NULL;
-	int x, y;
-	int x_last, y_last;
+	int x, y, z;
+	int x_last, y_last, z_last;
 
 	struct _stats {
 		int nodes;
@@ -65,6 +65,7 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver,
 
 				json_reader_read_member(reader, "geometry");
 				json_reader_read_member(reader, "coordinates");
+				z_last = 0; // By default, let's assume height 0
 				if (json_reader_is_array(reader)) {
 					j_max = json_reader_count_elements(reader);
 					for (j = 0; j < j_max; j++) {
@@ -77,18 +78,27 @@ int add_geojson_to_dijkstra(struct dijkstra_solver *solver,
 							json_reader_read_element(reader, 1);
 							y = json_reader_get_int_value(reader);
 							json_reader_end_element(reader);
+							if (json_reader_count_elements(reader) > 2) {
+								json_reader_read_element(reader, 2);
+								z = json_reader_get_int_value(reader);
+								json_reader_end_element(reader);
+							} else  {
+								z = z_last;
+							}
 
 							if (j > 0) {
 								stats.paths++;
 								dijkstra_path_new_to_list(solver,
 											  x_last, y_last,
 											  x, y,
+											  z, z_last,
 											  name,
 											  DIJKSTRA_WEIGHT_AUTO);
 							}
 
 							x_last = x;
 							y_last = y;
+							z_last = z;
 						} else {
 							report(LL_WARNING, "err 1\n");
 						}

src/main.c

@@ -17,12 +17,14 @@ 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);
 }