From 3035b9c44c5b026e2bf6f6faa6589271f90de320 Mon Sep 17 00:00:00 2001 From: Pawel Lampe Date: Sat, 29 Jan 2022 18:36:16 +0100 Subject: [PATCH] Fix navmesh baking, fixes #57148 - improved mesh data calculation from standalone static colliders so that no VisualServer calls are performed - and thus no VS mutexes need to be locked in case of on-thread baking - improved the same for GridMap's static colliders --- .../navigation/navigation_mesh_generator.cpp | 104 +++++++++--------- .../navigation/navigation_mesh_generator.h | 1 + scene/resources/primitive_meshes.cpp | 36 ++++-- scene/resources/primitive_meshes.h | 26 +++++ 4 files changed, 107 insertions(+), 60 deletions(-) diff --git a/modules/navigation/navigation_mesh_generator.cpp b/modules/navigation/navigation_mesh_generator.cpp index 789d559c1a1..25e396983ea 100644 --- a/modules/navigation/navigation_mesh_generator.cpp +++ b/modules/navigation/navigation_mesh_generator.cpp @@ -126,6 +126,28 @@ void NavigationMeshGenerator::_add_mesh(const Ref &p_mesh, const Transform } } +void NavigationMeshGenerator::_add_mesh_array(const Array &p_array, const Transform &p_xform, Vector &p_vertices, Vector &p_indices) { + PoolVector mesh_vertices = p_array[Mesh::ARRAY_VERTEX]; + PoolVector::Read vr = mesh_vertices.read(); + + PoolVector mesh_indices = p_array[Mesh::ARRAY_INDEX]; + PoolVector::Read ir = mesh_indices.read(); + + const int face_count = mesh_indices.size() / 3; + const int current_vertex_count = p_vertices.size() / 3; + + for (int j = 0; j < mesh_vertices.size(); j++) { + _add_vertex(p_xform.xform(vr[j]), p_vertices); + } + + for (int j = 0; j < face_count; j++) { + // CCW + p_indices.push_back(current_vertex_count + (ir[j * 3 + 0])); + p_indices.push_back(current_vertex_count + (ir[j * 3 + 2])); + p_indices.push_back(current_vertex_count + (ir[j * 3 + 1])); + } +} + void NavigationMeshGenerator::_add_faces(const PoolVector3Array &p_faces, const Transform &p_xform, Vector &p_vertices, Vector &p_indices) { int face_count = p_faces.size() / 3; int current_vertex_count = p_vertices.size() / 3; @@ -189,43 +211,38 @@ void NavigationMeshGenerator::_parse_geometry(const Transform &p_navmesh_xform, Transform transform = p_navmesh_xform * static_body->get_global_transform() * col_shape->get_transform(); - Ref mesh; Ref s = col_shape->get_shape(); BoxShape *box = Object::cast_to(*s); if (box) { - Ref cube_mesh; - cube_mesh.instance(); - cube_mesh->set_size(box->get_extents() * 2.0); - mesh = cube_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + CubeMesh::create_mesh_array(arr, box->get_extents() * 2.0); + _add_mesh_array(arr, transform, p_vertices, p_indices); } CapsuleShape *capsule = Object::cast_to(*s); if (capsule) { - Ref capsule_mesh; - capsule_mesh.instance(); - capsule_mesh->set_radius(capsule->get_radius()); - capsule_mesh->set_mid_height(capsule->get_height() / 2.0); - mesh = capsule_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height() / 2.0); + _add_mesh_array(arr, transform, p_vertices, p_indices); } CylinderShape *cylinder = Object::cast_to(*s); if (cylinder) { - Ref cylinder_mesh; - cylinder_mesh.instance(); - cylinder_mesh->set_height(cylinder->get_height()); - cylinder_mesh->set_bottom_radius(cylinder->get_radius()); - cylinder_mesh->set_top_radius(cylinder->get_radius()); - mesh = cylinder_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height()); + _add_mesh_array(arr, transform, p_vertices, p_indices); } SphereShape *sphere = Object::cast_to(*s); if (sphere) { - Ref sphere_mesh; - sphere_mesh.instance(); - sphere_mesh->set_radius(sphere->get_radius()); - sphere_mesh->set_height(sphere->get_radius() * 2.0); - mesh = sphere_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0); + _add_mesh_array(arr, transform, p_vertices, p_indices); } ConcavePolygonShape *concave_polygon = Object::cast_to(*s); @@ -256,10 +273,6 @@ void NavigationMeshGenerator::_parse_geometry(const Transform &p_navmesh_xform, _add_faces(faces, transform, p_vertices, p_indices); } } - - if (mesh.is_valid()) { - _add_mesh(mesh, transform, p_vertices, p_indices); - } } } } @@ -287,44 +300,39 @@ void NavigationMeshGenerator::_parse_geometry(const Transform &p_navmesh_xform, RID shape = shapes[i + 1]; PhysicsServer::ShapeType type = PhysicsServer::get_singleton()->shape_get_type(shape); Variant data = PhysicsServer::get_singleton()->shape_get_data(shape); - Ref mesh; switch (type) { case PhysicsServer::SHAPE_SPHERE: { real_t radius = data; - Ref sphere_mesh; - sphere_mesh.instance(); - sphere_mesh->set_radius(radius); - sphere_mesh->set_height(radius * 2.0); - mesh = sphere_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + SphereMesh::create_mesh_array(arr, radius, radius * 2.0); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer::SHAPE_BOX: { Vector3 extents = data; - Ref box_mesh; - box_mesh.instance(); - box_mesh->set_size(2.0 * extents); - mesh = box_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + CubeMesh::create_mesh_array(arr, extents * 2.0); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer::SHAPE_CAPSULE: { Dictionary dict = data; real_t radius = dict["radius"]; real_t height = dict["height"]; - Ref capsule_mesh; - capsule_mesh.instance(); - capsule_mesh->set_radius(radius); - capsule_mesh->set_mid_height(0.5 * height); - mesh = capsule_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + CapsuleMesh::create_mesh_array(arr, radius, height * 0.5); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer::SHAPE_CYLINDER: { Dictionary dict = data; real_t radius = dict["radius"]; real_t height = dict["height"]; - Ref cylinder_mesh; - cylinder_mesh.instance(); - cylinder_mesh->set_height(height); - cylinder_mesh->set_bottom_radius(radius); - cylinder_mesh->set_top_radius(radius); - mesh = cylinder_mesh; + Array arr; + arr.resize(VS::ARRAY_MAX); + CylinderMesh::create_mesh_array(arr, radius, radius, height); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer::SHAPE_CONVEX_POLYGON: { PoolVector3Array vertices = data; @@ -356,10 +364,6 @@ void NavigationMeshGenerator::_parse_geometry(const Transform &p_navmesh_xform, WARN_PRINT("Unsupported collision shape type."); } break; } - - if (mesh.is_valid()) { - _add_mesh(mesh, shapes[i], p_vertices, p_indices); - } } } } diff --git a/modules/navigation/navigation_mesh_generator.h b/modules/navigation/navigation_mesh_generator.h index fa42a7173b2..e040a35e4c1 100644 --- a/modules/navigation/navigation_mesh_generator.h +++ b/modules/navigation/navigation_mesh_generator.h @@ -51,6 +51,7 @@ protected: static void _add_vertex(const Vector3 &p_vec3, Vector &p_vertices); static void _add_mesh(const Ref &p_mesh, const Transform &p_xform, Vector &p_vertices, Vector &p_indices); + static void _add_mesh_array(const Array &p_array, const Transform &p_xform, Vector &p_vertices, Vector &p_indices); static void _add_faces(const PoolVector3Array &p_faces, const Transform &p_xform, Vector &p_vertices, Vector &p_indices); static void _parse_geometry(const Transform &p_navmesh_xform, Node *p_node, Vector &p_vertices, Vector &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children); diff --git a/scene/resources/primitive_meshes.cpp b/scene/resources/primitive_meshes.cpp index 9d3b44c7b67..eb7b331cfdd 100644 --- a/scene/resources/primitive_meshes.cpp +++ b/scene/resources/primitive_meshes.cpp @@ -270,6 +270,10 @@ PrimitiveMesh::~PrimitiveMesh() { */ void CapsuleMesh::_create_mesh_array(Array &p_arr) const { + create_mesh_array(p_arr, radius, mid_height, radial_segments, rings); +} + +void CapsuleMesh::create_mesh_array(Array &p_arr, const float radius, const float mid_height, const int radial_segments, const int rings) { int i, j, prevrow, thisrow, point; float x, y, z, u, v, w; float onethird = 1.0 / 3.0; @@ -472,8 +476,8 @@ CapsuleMesh::CapsuleMesh() { // defaults radius = 1.0; mid_height = 1.0; - radial_segments = 64; - rings = 8; + radial_segments = default_radial_segments; + rings = default_rings; } /** @@ -481,6 +485,10 @@ CapsuleMesh::CapsuleMesh() { */ void CubeMesh::_create_mesh_array(Array &p_arr) const { + create_mesh_array(p_arr, size, subdivide_w, subdivide_h, subdivide_d); +} + +void CubeMesh::create_mesh_array(Array &p_arr, const Vector3 size, const int subdivide_w, const int subdivide_h, const int subdivide_d) { int i, j, prevrow, thisrow, point; float x, y, z; float onethird = 1.0 / 3.0; @@ -728,9 +736,9 @@ int CubeMesh::get_subdivide_depth() const { CubeMesh::CubeMesh() { // defaults size = Vector3(2.0, 2.0, 2.0); - subdivide_w = 0; - subdivide_h = 0; - subdivide_d = 0; + subdivide_w = default_subdivide_w; + subdivide_h = default_subdivide_h; + subdivide_d = default_subdivide_d; } /** @@ -738,6 +746,10 @@ CubeMesh::CubeMesh() { */ void CylinderMesh::_create_mesh_array(Array &p_arr) const { + create_mesh_array(p_arr, top_radius, bottom_radius, height, radial_segments, rings); +} + +void CylinderMesh::create_mesh_array(Array &p_arr, float top_radius, float bottom_radius, float height, int radial_segments, int rings) { int i, j, prevrow, thisrow, point; float x, y, z, u, v, radius; @@ -943,8 +955,8 @@ CylinderMesh::CylinderMesh() { top_radius = 1.0; bottom_radius = 1.0; height = 2.0; - radial_segments = 64; - rings = 4; + radial_segments = default_radial_segments; + rings = default_rings; } /** @@ -1453,6 +1465,10 @@ Vector3 QuadMesh::get_center_offset() const { */ void SphereMesh::_create_mesh_array(Array &p_arr) const { + create_mesh_array(p_arr, radius, height, radial_segments, rings, is_hemisphere); +} + +void SphereMesh::create_mesh_array(Array &p_arr, float radius, float height, int radial_segments, int rings, bool is_hemisphere) { int i, j, prevrow, thisrow, point; float x, y, z; @@ -1595,9 +1611,9 @@ SphereMesh::SphereMesh() { // defaults radius = 1.0; height = 2.0; - radial_segments = 64; - rings = 32; - is_hemisphere = false; + radial_segments = default_radial_segments; + rings = default_rings; + is_hemisphere = default_is_hemisphere; } /** diff --git a/scene/resources/primitive_meshes.h b/scene/resources/primitive_meshes.h index a6b6f98cfb2..6468676ef34 100644 --- a/scene/resources/primitive_meshes.h +++ b/scene/resources/primitive_meshes.h @@ -100,6 +100,10 @@ public: class CapsuleMesh : public PrimitiveMesh { GDCLASS(CapsuleMesh, PrimitiveMesh); +private: + static constexpr int default_radial_segments = 64; + static constexpr int default_rings = 8; + private: float radius; float mid_height; @@ -111,6 +115,8 @@ protected: virtual void _create_mesh_array(Array &p_arr) const; public: + static void create_mesh_array(Array &p_arr, float radius, float mid_height, int radial_segments = default_radial_segments, int rings = default_rings); + void set_radius(const float p_radius); float get_radius() const; @@ -132,6 +138,11 @@ public: class CubeMesh : public PrimitiveMesh { GDCLASS(CubeMesh, PrimitiveMesh); +private: + static constexpr int default_subdivide_w = 0; + static constexpr int default_subdivide_h = 0; + static constexpr int default_subdivide_d = 0; + private: Vector3 size; int subdivide_w; @@ -143,6 +154,8 @@ protected: virtual void _create_mesh_array(Array &p_arr) const; public: + static void create_mesh_array(Array &p_arr, Vector3 size, int subdivide_w = default_subdivide_w, int subdivide_h = default_subdivide_h, int subdivide_d = default_subdivide_d); + void set_size(const Vector3 &p_size); Vector3 get_size() const; @@ -165,6 +178,10 @@ public: class CylinderMesh : public PrimitiveMesh { GDCLASS(CylinderMesh, PrimitiveMesh); +private: + static constexpr int default_radial_segments = 64; + static constexpr int default_rings = 4; + private: float top_radius; float bottom_radius; @@ -177,6 +194,8 @@ protected: virtual void _create_mesh_array(Array &p_arr) const; public: + static void create_mesh_array(Array &p_arr, float top_radius, float bottom_radius, float height, int radial_segments = default_radial_segments, int rings = default_rings); + void set_top_radius(const float p_radius); float get_top_radius() const; @@ -294,6 +313,11 @@ public: class SphereMesh : public PrimitiveMesh { GDCLASS(SphereMesh, PrimitiveMesh); +private: + static constexpr int default_radial_segments = 64; + static constexpr int default_rings = 32; + static constexpr bool default_is_hemisphere = false; + private: float radius; float height; @@ -306,6 +330,8 @@ protected: virtual void _create_mesh_array(Array &p_arr) const; public: + static void create_mesh_array(Array &p_arr, float radius, float height, int radial_segments = default_radial_segments, int rings = default_rings, bool is_hemisphere = default_is_hemisphere); + void set_radius(const float p_radius); float get_radius() const;