From 2e1f6b50fb5bfe6782ce791d96d4d6d66074aef9 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Sun, 4 Aug 2024 18:04:54 +0200 Subject: [PATCH] Add bounds function to NavigationMeshSourceGeometryData Adds get_bounds() function to NavigationMeshSourceGeometryData2D/3D to get a bounding box that covers all the geometry. --- .../NavigationMeshSourceGeometryData2D.xml | 6 ++ .../NavigationMeshSourceGeometryData3D.xml | 6 ++ ...avigation_mesh_source_geometry_data_2d.cpp | 70 +++++++++++++++++++ .../navigation_mesh_source_geometry_data_2d.h | 5 ++ ...avigation_mesh_source_geometry_data_3d.cpp | 52 ++++++++++++++ .../navigation_mesh_source_geometry_data_3d.h | 5 ++ 6 files changed, 144 insertions(+) diff --git a/doc/classes/NavigationMeshSourceGeometryData2D.xml b/doc/classes/NavigationMeshSourceGeometryData2D.xml index 1d8689420bdf..82b6e077feee 100644 --- a/doc/classes/NavigationMeshSourceGeometryData2D.xml +++ b/doc/classes/NavigationMeshSourceGeometryData2D.xml @@ -57,6 +57,12 @@ Clears all projected obstructions. + + + + Returns an axis-aligned bounding box that covers all the stored geometry data. The bounds are calculated when calling this function with the result cached until further geometry changes are made. + + diff --git a/doc/classes/NavigationMeshSourceGeometryData3D.xml b/doc/classes/NavigationMeshSourceGeometryData3D.xml index 0b3126a63bf5..aa43dff52d5e 100644 --- a/doc/classes/NavigationMeshSourceGeometryData3D.xml +++ b/doc/classes/NavigationMeshSourceGeometryData3D.xml @@ -63,6 +63,12 @@ Clears all projected obstructions. + + + + Returns an axis-aligned bounding box that covers all the stored geometry data. The bounds are calculated when calling this function with the result cached until further geometry changes are made. + + diff --git a/scene/resources/2d/navigation_mesh_source_geometry_data_2d.cpp b/scene/resources/2d/navigation_mesh_source_geometry_data_2d.cpp index 686560829be2..ee6fc61af354 100644 --- a/scene/resources/2d/navigation_mesh_source_geometry_data_2d.cpp +++ b/scene/resources/2d/navigation_mesh_source_geometry_data_2d.cpp @@ -37,6 +37,7 @@ void NavigationMeshSourceGeometryData2D::clear() { traversable_outlines.clear(); obstruction_outlines.clear(); _projected_obstructions.clear(); + bounds_dirty = true; } bool NavigationMeshSourceGeometryData2D::has_data() { @@ -47,16 +48,19 @@ bool NavigationMeshSourceGeometryData2D::has_data() { void NavigationMeshSourceGeometryData2D::clear_projected_obstructions() { RWLockWrite write_lock(geometry_rwlock); _projected_obstructions.clear(); + bounds_dirty = true; } void NavigationMeshSourceGeometryData2D::_set_traversable_outlines(const Vector> &p_traversable_outlines) { RWLockWrite write_lock(geometry_rwlock); traversable_outlines = p_traversable_outlines; + bounds_dirty = true; } void NavigationMeshSourceGeometryData2D::_set_obstruction_outlines(const Vector> &p_obstruction_outlines) { RWLockWrite write_lock(geometry_rwlock); obstruction_outlines = p_obstruction_outlines; + bounds_dirty = true; } const Vector> &NavigationMeshSourceGeometryData2D::_get_traversable_outlines() const { @@ -73,6 +77,7 @@ void NavigationMeshSourceGeometryData2D::_add_traversable_outline(const Vector 1) { RWLockWrite write_lock(geometry_rwlock); traversable_outlines.push_back(p_shape_outline); + bounds_dirty = true; } } @@ -80,6 +85,7 @@ void NavigationMeshSourceGeometryData2D::_add_obstruction_outline(const Vector 1) { RWLockWrite write_lock(geometry_rwlock); obstruction_outlines.push_back(p_shape_outline); + bounds_dirty = true; } } @@ -89,6 +95,7 @@ void NavigationMeshSourceGeometryData2D::set_traversable_outlines(const TypedArr for (int i = 0; i < p_traversable_outlines.size(); i++) { traversable_outlines.write[i] = p_traversable_outlines[i]; } + bounds_dirty = true; } TypedArray> NavigationMeshSourceGeometryData2D::get_traversable_outlines() const { @@ -108,6 +115,7 @@ void NavigationMeshSourceGeometryData2D::set_obstruction_outlines(const TypedArr for (int i = 0; i < p_obstruction_outlines.size(); i++) { obstruction_outlines.write[i] = p_obstruction_outlines[i]; } + bounds_dirty = true; } TypedArray> NavigationMeshSourceGeometryData2D::get_obstruction_outlines() const { @@ -128,6 +136,7 @@ void NavigationMeshSourceGeometryData2D::append_traversable_outlines(const Typed for (int i = traversable_outlines_size; i < p_traversable_outlines.size(); i++) { traversable_outlines.write[i] = p_traversable_outlines[i]; } + bounds_dirty = true; } void NavigationMeshSourceGeometryData2D::append_obstruction_outlines(const TypedArray> &p_obstruction_outlines) { @@ -137,6 +146,7 @@ void NavigationMeshSourceGeometryData2D::append_obstruction_outlines(const Typed for (int i = obstruction_outlines_size; i < p_obstruction_outlines.size(); i++) { obstruction_outlines.write[i] = p_obstruction_outlines[i]; } + bounds_dirty = true; } void NavigationMeshSourceGeometryData2D::add_traversable_outline(const PackedVector2Array &p_shape_outline) { @@ -148,6 +158,7 @@ void NavigationMeshSourceGeometryData2D::add_traversable_outline(const PackedVec traversable_outline.write[i] = p_shape_outline[i]; } traversable_outlines.push_back(traversable_outline); + bounds_dirty = true; } } @@ -160,6 +171,7 @@ void NavigationMeshSourceGeometryData2D::add_obstruction_outline(const PackedVec obstruction_outline.write[i] = p_shape_outline[i]; } obstruction_outlines.push_back(obstruction_outline); + bounds_dirty = true; } } @@ -176,6 +188,7 @@ void NavigationMeshSourceGeometryData2D::merge(const Ref &p_vertices, bool p_carve) { @@ -195,6 +208,7 @@ void NavigationMeshSourceGeometryData2D::add_projected_obstruction(const Vector< RWLockWrite write_lock(geometry_rwlock); _projected_obstructions.push_back(projected_obstruction); + bounds_dirty = true; } void NavigationMeshSourceGeometryData2D::set_projected_obstructions(const Array &p_array) { @@ -217,6 +231,7 @@ void NavigationMeshSourceGeometryData2D::set_projected_obstructions(const Array RWLockWrite write_lock(geometry_rwlock); _projected_obstructions.push_back(projected_obstruction); + bounds_dirty = true; } } @@ -266,6 +281,7 @@ void NavigationMeshSourceGeometryData2D::set_data(const Vector> traversable_outlines = p_traversable_outlines; obstruction_outlines = p_obstruction_outlines; _projected_obstructions = p_projected_obstructions; + bounds_dirty = true; } void NavigationMeshSourceGeometryData2D::get_data(Vector> &r_traversable_outlines, Vector> &r_obstruction_outlines, Vector &r_projected_obstructions) { @@ -275,6 +291,58 @@ void NavigationMeshSourceGeometryData2D::get_data(Vector> &r_tra r_projected_obstructions = _projected_obstructions; } +Rect2 NavigationMeshSourceGeometryData2D::get_bounds() { + geometry_rwlock.read_lock(); + + if (bounds_dirty) { + geometry_rwlock.read_unlock(); + RWLockWrite write_lock(geometry_rwlock); + + bounds_dirty = false; + bounds = Rect2(); + bool first_vertex = true; + + for (const Vector &traversable_outline : traversable_outlines) { + for (const Vector2 &traversable_point : traversable_outline) { + if (first_vertex) { + first_vertex = false; + bounds.position = traversable_point; + } else { + bounds.expand_to(traversable_point); + } + } + } + + for (const Vector &obstruction_outline : obstruction_outlines) { + for (const Vector2 &obstruction_point : obstruction_outline) { + if (first_vertex) { + first_vertex = false; + bounds.position = obstruction_point; + } else { + bounds.expand_to(obstruction_point); + } + } + } + + for (const ProjectedObstruction &projected_obstruction : _projected_obstructions) { + for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) { + const Vector2 vertex = Vector2(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]); + if (first_vertex) { + first_vertex = false; + bounds.position = vertex; + } else { + bounds.expand_to(vertex); + } + } + } + } else { + geometry_rwlock.read_unlock(); + } + + RWLockRead read_lock(geometry_rwlock); + return bounds; +} + void NavigationMeshSourceGeometryData2D::_bind_methods() { ClassDB::bind_method(D_METHOD("clear"), &NavigationMeshSourceGeometryData2D::clear); ClassDB::bind_method(D_METHOD("has_data"), &NavigationMeshSourceGeometryData2D::has_data); @@ -298,6 +366,8 @@ void NavigationMeshSourceGeometryData2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_projected_obstructions", "projected_obstructions"), &NavigationMeshSourceGeometryData2D::set_projected_obstructions); ClassDB::bind_method(D_METHOD("get_projected_obstructions"), &NavigationMeshSourceGeometryData2D::get_projected_obstructions); + ClassDB::bind_method(D_METHOD("get_bounds"), &NavigationMeshSourceGeometryData2D::get_bounds); + ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "traversable_outlines", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_traversable_outlines", "get_traversable_outlines"); ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "obstruction_outlines", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_obstruction_outlines", "get_obstruction_outlines"); ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "projected_obstructions", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_projected_obstructions", "get_projected_obstructions"); diff --git a/scene/resources/2d/navigation_mesh_source_geometry_data_2d.h b/scene/resources/2d/navigation_mesh_source_geometry_data_2d.h index 01e97eee48e3..b29c106fb5e7 100644 --- a/scene/resources/2d/navigation_mesh_source_geometry_data_2d.h +++ b/scene/resources/2d/navigation_mesh_source_geometry_data_2d.h @@ -42,6 +42,9 @@ class NavigationMeshSourceGeometryData2D : public Resource { Vector> traversable_outlines; Vector> obstruction_outlines; + Rect2 bounds; + bool bounds_dirty = true; + public: struct ProjectedObstruction; @@ -103,6 +106,8 @@ class NavigationMeshSourceGeometryData2D : public Resource { void set_data(const Vector> &p_traversable_outlines, const Vector> &p_obstruction_outlines, Vector &p_projected_obstructions); void get_data(Vector> &r_traversable_outlines, Vector> &r_obstruction_outlines, Vector &r_projected_obstructions); + Rect2 get_bounds(); + NavigationMeshSourceGeometryData2D() {} ~NavigationMeshSourceGeometryData2D() { clear(); } }; diff --git a/scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp b/scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp index 4c9f381abadb..c55e25fcaeda 100644 --- a/scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp +++ b/scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp @@ -33,6 +33,7 @@ void NavigationMeshSourceGeometryData3D::set_vertices(const Vector &p_vertices) { RWLockWrite write_lock(geometry_rwlock); vertices = p_vertices; + bounds_dirty = true; } const Vector &NavigationMeshSourceGeometryData3D::get_vertices() const { @@ -44,6 +45,7 @@ void NavigationMeshSourceGeometryData3D::set_indices(const Vector &p_indice ERR_FAIL_COND(vertices.size() < p_indices.size()); RWLockWrite write_lock(geometry_rwlock); indices = p_indices; + bounds_dirty = true; } const Vector &NavigationMeshSourceGeometryData3D::get_indices() const { @@ -63,6 +65,7 @@ void NavigationMeshSourceGeometryData3D::append_arrays(const Vector &p_ve for (int64_t i = number_of_indices_before_merge; i < indices.size(); i++) { indices.set(i, indices[i] + number_of_vertices_before_merge / 3); } + bounds_dirty = true; } bool NavigationMeshSourceGeometryData3D::has_data() { @@ -75,11 +78,13 @@ void NavigationMeshSourceGeometryData3D::clear() { vertices.clear(); indices.clear(); _projected_obstructions.clear(); + bounds_dirty = true; } void NavigationMeshSourceGeometryData3D::clear_projected_obstructions() { RWLockWrite write_lock(geometry_rwlock); _projected_obstructions.clear(); + bounds_dirty = true; } void NavigationMeshSourceGeometryData3D::_add_vertex(const Vector3 &p_vec3) { @@ -207,12 +212,14 @@ void NavigationMeshSourceGeometryData3D::add_mesh_array(const Array &p_mesh_arra ERR_FAIL_COND(p_mesh_array.size() != Mesh::ARRAY_MAX); RWLockWrite write_lock(geometry_rwlock); _add_mesh_array(p_mesh_array, root_node_transform * p_xform); + bounds_dirty = true; } void NavigationMeshSourceGeometryData3D::add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform) { ERR_FAIL_COND(p_faces.size() % 3 != 0); RWLockWrite write_lock(geometry_rwlock); _add_faces(p_faces, root_node_transform * p_xform); + bounds_dirty = true; } void NavigationMeshSourceGeometryData3D::merge(const Ref &p_other_geometry) { @@ -236,6 +243,7 @@ void NavigationMeshSourceGeometryData3D::merge(const Ref &p_vertices, float p_elevation, float p_height, bool p_carve) { @@ -259,6 +267,7 @@ void NavigationMeshSourceGeometryData3D::add_projected_obstruction(const Vector< RWLockWrite write_lock(geometry_rwlock); _projected_obstructions.push_back(projected_obstruction); + bounds_dirty = true; } void NavigationMeshSourceGeometryData3D::set_projected_obstructions(const Array &p_array) { @@ -285,6 +294,7 @@ void NavigationMeshSourceGeometryData3D::set_projected_obstructions(const Array RWLockWrite write_lock(geometry_rwlock); _projected_obstructions.push_back(projected_obstruction); + bounds_dirty = true; } } @@ -336,6 +346,7 @@ void NavigationMeshSourceGeometryData3D::set_data(const Vector &p_vertice vertices = p_vertices; indices = p_indices; _projected_obstructions = p_projected_obstructions; + bounds_dirty = true; } void NavigationMeshSourceGeometryData3D::get_data(Vector &r_vertices, Vector &r_indices, Vector &r_projected_obstructions) { @@ -345,6 +356,45 @@ void NavigationMeshSourceGeometryData3D::get_data(Vector &r_vertices, Vec r_projected_obstructions = _projected_obstructions; } +AABB NavigationMeshSourceGeometryData3D::get_bounds() { + geometry_rwlock.read_lock(); + + if (bounds_dirty) { + geometry_rwlock.read_unlock(); + RWLockWrite write_lock(geometry_rwlock); + + bounds_dirty = false; + bounds = AABB(); + bool first_vertex = true; + + for (int i = 0; i < vertices.size() / 3; i++) { + const Vector3 vertex = Vector3(vertices[i * 3], vertices[i * 3 + 1], vertices[i * 3 + 2]); + if (first_vertex) { + first_vertex = false; + bounds.position = vertex; + } else { + bounds.expand_to(vertex); + } + } + for (const ProjectedObstruction &projected_obstruction : _projected_obstructions) { + for (int i = 0; i < projected_obstruction.vertices.size() / 3; i++) { + const Vector3 vertex = Vector3(projected_obstruction.vertices[i * 3], projected_obstruction.vertices[i * 3 + 1], projected_obstruction.vertices[i * 3 + 2]); + if (first_vertex) { + first_vertex = false; + bounds.position = vertex; + } else { + bounds.expand_to(vertex); + } + } + } + } else { + geometry_rwlock.read_unlock(); + } + + RWLockRead read_lock(geometry_rwlock); + return bounds; +} + void NavigationMeshSourceGeometryData3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_vertices", "vertices"), &NavigationMeshSourceGeometryData3D::set_vertices); ClassDB::bind_method(D_METHOD("get_vertices"), &NavigationMeshSourceGeometryData3D::get_vertices); @@ -367,6 +417,8 @@ void NavigationMeshSourceGeometryData3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_projected_obstructions", "projected_obstructions"), &NavigationMeshSourceGeometryData3D::set_projected_obstructions); ClassDB::bind_method(D_METHOD("get_projected_obstructions"), &NavigationMeshSourceGeometryData3D::get_projected_obstructions); + ClassDB::bind_method(D_METHOD("get_bounds"), &NavigationMeshSourceGeometryData3D::get_bounds); + ADD_PROPERTY(PropertyInfo(Variant::PACKED_VECTOR3_ARRAY, "vertices", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_vertices", "get_vertices"); ADD_PROPERTY(PropertyInfo(Variant::PACKED_INT32_ARRAY, "indices", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_indices", "get_indices"); ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "projected_obstructions", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_projected_obstructions", "get_projected_obstructions"); diff --git a/scene/resources/3d/navigation_mesh_source_geometry_data_3d.h b/scene/resources/3d/navigation_mesh_source_geometry_data_3d.h index a8e613a51dec..d7e3c3071c6c 100644 --- a/scene/resources/3d/navigation_mesh_source_geometry_data_3d.h +++ b/scene/resources/3d/navigation_mesh_source_geometry_data_3d.h @@ -41,6 +41,9 @@ class NavigationMeshSourceGeometryData3D : public Resource { Vector vertices; Vector indices; + AABB bounds; + bool bounds_dirty = true; + public: struct ProjectedObstruction; @@ -101,6 +104,8 @@ class NavigationMeshSourceGeometryData3D : public Resource { void set_data(const Vector &p_vertices, const Vector &p_indices, Vector &p_projected_obstructions); void get_data(Vector &r_vertices, Vector &r_indices, Vector &r_projected_obstructions); + AABB get_bounds(); + NavigationMeshSourceGeometryData3D() {} ~NavigationMeshSourceGeometryData3D() { clear(); } };