From fcd26b8841524c6cc19e467797fc2d8f7647d5c1 Mon Sep 17 00:00:00 2001 From: Pawel Lampe Date: Thu, 7 Apr 2022 20:54:21 +0200 Subject: Fix navmesh baking - 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 --- modules/navigation/navigation_mesh_generator.cpp | 104 ++++++++++++----------- modules/navigation/navigation_mesh_generator.h | 1 + 2 files changed, 55 insertions(+), 50 deletions(-) (limited to 'modules/navigation') diff --git a/modules/navigation/navigation_mesh_generator.cpp b/modules/navigation/navigation_mesh_generator.cpp index 61c3cefc7a..9e2daf3a99 100644 --- a/modules/navigation/navigation_mesh_generator.cpp +++ b/modules/navigation/navigation_mesh_generator.cpp @@ -124,6 +124,28 @@ void NavigationMeshGenerator::_add_mesh(const Ref &p_mesh, const Transform } } +void NavigationMeshGenerator::_add_mesh_array(const Array &p_array, const Transform3D &p_xform, Vector &p_vertices, Vector &p_indices) { + Vector mesh_vertices = p_array[Mesh::ARRAY_VERTEX]; + const Vector3 *vr = mesh_vertices.ptr(); + + Vector mesh_indices = p_array[Mesh::ARRAY_INDEX]; + const int *ir = mesh_indices.ptr(); + + 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 PackedVector3Array &p_faces, const Transform3D &p_xform, Vector &p_vertices, Vector &p_indices) { int face_count = p_faces.size() / 3; int current_vertex_count = p_vertices.size() / 3; @@ -187,43 +209,38 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans Transform3D transform = p_navmesh_transform * static_body->get_global_transform() * col_shape->get_transform(); - Ref mesh; Ref s = col_shape->get_shape(); BoxShape3D *box = Object::cast_to(*s); if (box) { - Ref box_mesh; - box_mesh.instantiate(); - box_mesh->set_size(box->get_size()); - mesh = box_mesh; + Array arr; + arr.resize(RS::ARRAY_MAX); + BoxMesh::create_mesh_array(arr, box->get_size()); + _add_mesh_array(arr, transform, p_vertices, p_indices); } CapsuleShape3D *capsule = Object::cast_to(*s); if (capsule) { - Ref capsule_mesh; - capsule_mesh.instantiate(); - capsule_mesh->set_radius(capsule->get_radius()); - capsule_mesh->set_height(capsule->get_height()); - mesh = capsule_mesh; + Array arr; + arr.resize(RS::ARRAY_MAX); + CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height()); + _add_mesh_array(arr, transform, p_vertices, p_indices); } CylinderShape3D *cylinder = Object::cast_to(*s); if (cylinder) { - Ref cylinder_mesh; - cylinder_mesh.instantiate(); - 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(RS::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); } SphereShape3D *sphere = Object::cast_to(*s); if (sphere) { - Ref sphere_mesh; - sphere_mesh.instantiate(); - sphere_mesh->set_radius(sphere->get_radius()); - sphere_mesh->set_height(sphere->get_radius() * 2.0); - mesh = sphere_mesh; + Array arr; + arr.resize(RS::ARRAY_MAX); + SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0); + _add_mesh_array(arr, transform, p_vertices, p_indices); } ConcavePolygonShape3D *concave_polygon = Object::cast_to(*s); @@ -254,10 +271,6 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans _add_faces(faces, transform, p_vertices, p_indices); } } - - if (mesh.is_valid()) { - _add_mesh(mesh, transform, p_vertices, p_indices); - } } } } @@ -284,44 +297,39 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans RID shape = shapes[i + 1]; PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape); Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape); - Ref mesh; switch (type) { case PhysicsServer3D::SHAPE_SPHERE: { real_t radius = data; - Ref sphere_mesh; - sphere_mesh.instantiate(); - sphere_mesh->set_radius(radius); - sphere_mesh->set_height(radius * 2.0); - mesh = sphere_mesh; + Array arr; + arr.resize(RS::ARRAY_MAX); + SphereMesh::create_mesh_array(arr, radius, radius * 2.0); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer3D::SHAPE_BOX: { Vector3 extents = data; - Ref box_mesh; - box_mesh.instantiate(); - box_mesh->set_size(2.0 * extents); - mesh = box_mesh; + Array arr; + arr.resize(RS::ARRAY_MAX); + BoxMesh::create_mesh_array(arr, extents * 2.0); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer3D::SHAPE_CAPSULE: { Dictionary dict = data; real_t radius = dict["radius"]; real_t height = dict["height"]; - Ref capsule_mesh; - capsule_mesh.instantiate(); - capsule_mesh->set_radius(radius); - capsule_mesh->set_height(height); - mesh = capsule_mesh; + Array arr; + arr.resize(RS::ARRAY_MAX); + CapsuleMesh::create_mesh_array(arr, radius, height); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer3D::SHAPE_CYLINDER: { Dictionary dict = data; real_t radius = dict["radius"]; real_t height = dict["height"]; - Ref cylinder_mesh; - cylinder_mesh.instantiate(); - cylinder_mesh->set_height(height); - cylinder_mesh->set_bottom_radius(radius); - cylinder_mesh->set_top_radius(radius); - mesh = cylinder_mesh; + Array arr; + arr.resize(RS::ARRAY_MAX); + CylinderMesh::create_mesh_array(arr, radius, radius, height); + _add_mesh_array(arr, shapes[i], p_vertices, p_indices); } break; case PhysicsServer3D::SHAPE_CONVEX_POLYGON: { PackedVector3Array vertices = data; @@ -354,10 +362,6 @@ void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_trans 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 21f7a4941b..8cc1531b53 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 Transform3D &p_xform, Vector &p_vertices, Vector &p_indices); + static void _add_mesh_array(const Array &p_array, const Transform3D &p_xform, Vector &p_vertices, Vector &p_indices); static void _add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector &p_vertices, Vector &p_indices); static void _parse_geometry(const Transform3D &p_navmesh_transform, Node *p_node, Vector &p_vertices, Vector &p_indices, NavigationMesh::ParsedGeometryType p_generate_from, uint32_t p_collision_mask, bool p_recurse_children); -- cgit v1.2.3