From 9416f0c2664662925f085b3ec3866d31081532f8 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Sun, 22 Oct 2023 17:07:26 +0200 Subject: [PATCH] Fix hole in heightmap navigation mesh baking Fixes hole in heightmap navigation mesh baking. --- modules/navigation/nav_mesh_generator_3d.cpp | 72 +++++++------------- 1 file changed, 26 insertions(+), 46 deletions(-) diff --git a/modules/navigation/nav_mesh_generator_3d.cpp b/modules/navigation/nav_mesh_generator_3d.cpp index 5de1c4cba94..8719801c72d 100644 --- a/modules/navigation/nav_mesh_generator_3d.cpp +++ b/modules/navigation/nav_mesh_generator_3d.cpp @@ -384,33 +384,23 @@ void NavMeshGenerator3D::generator_parse_staticbody3d_node(const Ref &map_data = heightmap_shape->get_map_data(); Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1); - Vector2 start = heightmap_gridsize * -0.5; + Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5; Vector vertex_array; vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6); - int map_data_current_index = 0; + Vector3 *vertex_array_ptrw = vertex_array.ptrw(); + const real_t *map_data_ptr = map_data.ptr(); + int vertex_index = 0; - for (int d = 0; d < heightmap_depth; d++) { - for (int w = 0; w < heightmap_width; w++) { - if (map_data_current_index + 1 + heightmap_depth < map_data.size()) { - float top_left_height = map_data[map_data_current_index]; - float top_right_height = map_data[map_data_current_index + 1]; - float bottom_left_height = map_data[map_data_current_index + heightmap_depth]; - float bottom_right_height = map_data[map_data_current_index + 1 + heightmap_depth]; - - Vector3 top_left = Vector3(start.x + w, top_left_height, start.y + d); - Vector3 top_right = Vector3(start.x + w + 1.0, top_right_height, start.y + d); - Vector3 bottom_left = Vector3(start.x + w, bottom_left_height, start.y + d + 1.0); - Vector3 bottom_right = Vector3(start.x + w + 1.0, bottom_right_height, start.y + d + 1.0); - - vertex_array.push_back(top_right); - vertex_array.push_back(bottom_left); - vertex_array.push_back(top_left); - vertex_array.push_back(top_right); - vertex_array.push_back(bottom_right); - vertex_array.push_back(bottom_left); - } - map_data_current_index += 1; + for (int d = 0; d < heightmap_depth - 1; d++) { + for (int w = 0; w < heightmap_width - 1; w++) { + vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d); + vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1); + vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_index += 6; } } if (vertex_array.size() > 0) { @@ -540,33 +530,23 @@ void NavMeshGenerator3D::generator_parse_gridmap_node(const Ref const Vector &map_data = dict["heights"]; Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1); - Vector2 start = heightmap_gridsize * -0.5; + Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5; Vector vertex_array; vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6); - int map_data_current_index = 0; + Vector3 *vertex_array_ptrw = vertex_array.ptrw(); + const real_t *map_data_ptr = map_data.ptr(); + int vertex_index = 0; - for (int d = 0; d < heightmap_depth; d++) { - for (int w = 0; w < heightmap_width; w++) { - if (map_data_current_index + 1 + heightmap_depth < map_data.size()) { - float top_left_height = map_data[map_data_current_index]; - float top_right_height = map_data[map_data_current_index + 1]; - float bottom_left_height = map_data[map_data_current_index + heightmap_depth]; - float bottom_right_height = map_data[map_data_current_index + 1 + heightmap_depth]; - - Vector3 top_left = Vector3(start.x + w, top_left_height, start.y + d); - Vector3 top_right = Vector3(start.x + w + 1.0, top_right_height, start.y + d); - Vector3 bottom_left = Vector3(start.x + w, bottom_left_height, start.y + d + 1.0); - Vector3 bottom_right = Vector3(start.x + w + 1.0, bottom_right_height, start.y + d + 1.0); - - vertex_array.push_back(top_right); - vertex_array.push_back(bottom_left); - vertex_array.push_back(top_left); - vertex_array.push_back(top_right); - vertex_array.push_back(bottom_right); - vertex_array.push_back(bottom_left); - } - map_data_current_index += 1; + for (int d = 0; d < heightmap_depth - 1; d++) { + for (int w = 0; w < heightmap_width - 1; w++) { + vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d); + vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d); + vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1); + vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1); + vertex_index += 6; } } if (vertex_array.size() > 0) {