From 5db3ad8759faaea0a305272950772111b2a6f14d Mon Sep 17 00:00:00 2001 From: Markus Koch Date: Thu, 4 Feb 2021 20:43:32 +0100 Subject: [PATCH] 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 | 30 ++++++++++++++++++++++++------ src/dijkstragraph.h | 6 +++++- src/geojson.c | 14 ++++++++++++-- src/main.c | 2 ++ 4 files changed, 43 insertions(+), 9 deletions(-) diff --git a/src/dijkstragraph.c b/src/dijkstragraph.c index 07025e9..5eb49fe 100644 --- a/src/dijkstragraph.c +++ b/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; - float i_x, i_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); diff --git a/src/dijkstragraph.h b/src/dijkstragraph.h index b9e4a60..c737385 100644 --- a/src/dijkstragraph.h +++ b/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 diff --git a/src/geojson.c b/src/geojson.c index 0c66072..183b429 100644 --- a/src/geojson.c +++ b/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"); } diff --git a/src/main.c b/src/main.c index 8dc166f..6fcec7b 100644 --- a/src/main.c +++ b/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); }