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.
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);

View File

@ -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

View File

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

View File

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