Add height support

Optional third coordinate in position field. Streets will only
intersect when all 4 coordinates are on the same height.
This commit is contained in:
Markus Koch 2021-02-04 20:43:32 +01:00
parent 5e1ef8b340
commit 5db3ad8759
4 changed files with 43 additions and 9 deletions

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

View File

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

View File

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

View File

@ -17,12 +17,14 @@ void add_test_map(struct dijkstra_solver *solver)
dijkstra_path_new_to_list(solver, dijkstra_path_new_to_list(solver,
5,0, 5,0,
5,10, 5,10,
0,0,
"Vertical", "Vertical",
1); 1);
dijkstra_path_new_to_list(solver, dijkstra_path_new_to_list(solver,
0,5, 0,5,
10,5, 10,5,
0,0,
"Horizontal", "Horizontal",
1); 1);
} }