diff options
Diffstat (limited to 'scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp')
| -rw-r--r-- | scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp | 325 |
1 files changed, 325 insertions, 0 deletions
diff --git a/scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp b/scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp new file mode 100644 index 0000000000..39a17946fa --- /dev/null +++ b/scene/resources/3d/navigation_mesh_source_geometry_data_3d.cpp @@ -0,0 +1,325 @@ +/**************************************************************************/ +/* navigation_mesh_source_geometry_data_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "navigation_mesh_source_geometry_data_3d.h" + +void NavigationMeshSourceGeometryData3D::set_vertices(const Vector<float> &p_vertices) { + vertices = p_vertices; +} + +void NavigationMeshSourceGeometryData3D::set_indices(const Vector<int> &p_indices) { + indices = p_indices; +} + +void NavigationMeshSourceGeometryData3D::clear() { + vertices.clear(); + indices.clear(); + clear_projected_obstructions(); +} + +void NavigationMeshSourceGeometryData3D::clear_projected_obstructions() { + RWLockWrite write_lock(geometry_rwlock); + _projected_obstructions.clear(); +} + +void NavigationMeshSourceGeometryData3D::_add_vertex(const Vector3 &p_vec3) { + vertices.push_back(p_vec3.x); + vertices.push_back(p_vec3.y); + vertices.push_back(p_vec3.z); +} + +void NavigationMeshSourceGeometryData3D::_add_mesh(const Ref<Mesh> &p_mesh, const Transform3D &p_xform) { + int current_vertex_count; + for (int i = 0; i < p_mesh->get_surface_count(); i++) { + current_vertex_count = vertices.size() / 3; + + if (p_mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) { + continue; + } + + int index_count = 0; + if (p_mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { + index_count = p_mesh->surface_get_array_index_len(i); + } else { + index_count = p_mesh->surface_get_array_len(i); + } + + ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0)); + + int face_count = index_count / 3; + + Array a = p_mesh->surface_get_arrays(i); + ERR_CONTINUE(a.is_empty() || (a.size() != Mesh::ARRAY_MAX)); + + Vector<Vector3> mesh_vertices = a[Mesh::ARRAY_VERTEX]; + ERR_CONTINUE(mesh_vertices.is_empty()); + const Vector3 *vr = mesh_vertices.ptr(); + + if (p_mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { + Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX]; + ERR_CONTINUE(mesh_indices.is_empty() || (mesh_indices.size() != index_count)); + const int *ir = mesh_indices.ptr(); + + for (int j = 0; j < mesh_vertices.size(); j++) { + _add_vertex(p_xform.xform(vr[j])); + } + + for (int j = 0; j < face_count; j++) { + // CCW + indices.push_back(current_vertex_count + (ir[j * 3 + 0])); + indices.push_back(current_vertex_count + (ir[j * 3 + 2])); + indices.push_back(current_vertex_count + (ir[j * 3 + 1])); + } + } else { + ERR_CONTINUE(mesh_vertices.size() != index_count); + face_count = mesh_vertices.size() / 3; + for (int j = 0; j < face_count; j++) { + _add_vertex(p_xform.xform(vr[j * 3 + 0])); + _add_vertex(p_xform.xform(vr[j * 3 + 2])); + _add_vertex(p_xform.xform(vr[j * 3 + 1])); + + indices.push_back(current_vertex_count + (j * 3 + 0)); + indices.push_back(current_vertex_count + (j * 3 + 1)); + indices.push_back(current_vertex_count + (j * 3 + 2)); + } + } + } +} + +void NavigationMeshSourceGeometryData3D::_add_mesh_array(const Array &p_mesh_array, const Transform3D &p_xform) { + ERR_FAIL_COND(p_mesh_array.size() != Mesh::ARRAY_MAX); + + Vector<Vector3> mesh_vertices = p_mesh_array[Mesh::ARRAY_VERTEX]; + ERR_FAIL_COND(mesh_vertices.is_empty()); + const Vector3 *vr = mesh_vertices.ptr(); + + Vector<int> mesh_indices = p_mesh_array[Mesh::ARRAY_INDEX]; + ERR_FAIL_COND(mesh_indices.is_empty()); + const int *ir = mesh_indices.ptr(); + + const int face_count = mesh_indices.size() / 3; + const int current_vertex_count = vertices.size() / 3; + + for (int j = 0; j < mesh_vertices.size(); j++) { + _add_vertex(p_xform.xform(vr[j])); + } + + for (int j = 0; j < face_count; j++) { + // CCW + indices.push_back(current_vertex_count + (ir[j * 3 + 0])); + indices.push_back(current_vertex_count + (ir[j * 3 + 2])); + indices.push_back(current_vertex_count + (ir[j * 3 + 1])); + } +} + +void NavigationMeshSourceGeometryData3D::_add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform) { + ERR_FAIL_COND(p_faces.is_empty()); + ERR_FAIL_COND(p_faces.size() % 3 != 0); + int face_count = p_faces.size() / 3; + int current_vertex_count = vertices.size() / 3; + + for (int j = 0; j < face_count; j++) { + _add_vertex(p_xform.xform(p_faces[j * 3 + 0])); + _add_vertex(p_xform.xform(p_faces[j * 3 + 1])); + _add_vertex(p_xform.xform(p_faces[j * 3 + 2])); + + indices.push_back(current_vertex_count + (j * 3 + 0)); + indices.push_back(current_vertex_count + (j * 3 + 2)); + indices.push_back(current_vertex_count + (j * 3 + 1)); + } +} + +void NavigationMeshSourceGeometryData3D::add_mesh(const Ref<Mesh> &p_mesh, const Transform3D &p_xform) { + ERR_FAIL_COND(!p_mesh.is_valid()); + _add_mesh(p_mesh, root_node_transform * p_xform); +} + +void NavigationMeshSourceGeometryData3D::add_mesh_array(const Array &p_mesh_array, const Transform3D &p_xform) { + ERR_FAIL_COND(p_mesh_array.size() != Mesh::ARRAY_MAX); + _add_mesh_array(p_mesh_array, root_node_transform * p_xform); +} + +void NavigationMeshSourceGeometryData3D::add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform) { + ERR_FAIL_COND(p_faces.size() % 3 != 0); + _add_faces(p_faces, root_node_transform * p_xform); +} + +void NavigationMeshSourceGeometryData3D::merge(const Ref<NavigationMeshSourceGeometryData3D> &p_other_geometry) { + ERR_FAIL_NULL(p_other_geometry); + + // No need to worry about `root_node_transform` here as the vertices are already xformed. + const int64_t number_of_vertices_before_merge = vertices.size(); + const int64_t number_of_indices_before_merge = indices.size(); + vertices.append_array(p_other_geometry->vertices); + indices.append_array(p_other_geometry->indices); + for (int64_t i = number_of_indices_before_merge; i < indices.size(); i++) { + indices.set(i, indices[i] + number_of_vertices_before_merge / 3); + } + + if (p_other_geometry->_projected_obstructions.size() > 0) { + RWLockWrite write_lock(geometry_rwlock); + + for (const ProjectedObstruction &other_projected_obstruction : p_other_geometry->_projected_obstructions) { + ProjectedObstruction projected_obstruction; + projected_obstruction.vertices.resize(other_projected_obstruction.vertices.size()); + + const float *other_obstruction_vertices_ptr = other_projected_obstruction.vertices.ptr(); + float *obstruction_vertices_ptrw = projected_obstruction.vertices.ptrw(); + + for (int j = 0; j < other_projected_obstruction.vertices.size(); j++) { + obstruction_vertices_ptrw[j] = other_obstruction_vertices_ptr[j]; + } + + projected_obstruction.elevation = other_projected_obstruction.elevation; + projected_obstruction.height = other_projected_obstruction.height; + projected_obstruction.carve = other_projected_obstruction.carve; + + _projected_obstructions.push_back(projected_obstruction); + } + } +} + +void NavigationMeshSourceGeometryData3D::add_projected_obstruction(const Vector<Vector3> &p_vertices, float p_elevation, float p_height, bool p_carve) { + ERR_FAIL_COND(p_vertices.size() < 3); + ERR_FAIL_COND(p_height < 0.0); + + ProjectedObstruction projected_obstruction; + projected_obstruction.vertices.resize(p_vertices.size() * 3); + projected_obstruction.elevation = p_elevation; + projected_obstruction.height = p_height; + projected_obstruction.carve = p_carve; + + float *obstruction_vertices_ptrw = projected_obstruction.vertices.ptrw(); + + int vertex_index = 0; + for (const Vector3 &vertex : p_vertices) { + obstruction_vertices_ptrw[vertex_index++] = vertex.x; + obstruction_vertices_ptrw[vertex_index++] = vertex.y; + obstruction_vertices_ptrw[vertex_index++] = vertex.z; + } + + RWLockWrite write_lock(geometry_rwlock); + _projected_obstructions.push_back(projected_obstruction); +} + +void NavigationMeshSourceGeometryData3D::set_projected_obstructions(const Array &p_array) { + clear_projected_obstructions(); + + for (int i = 0; i < p_array.size(); i++) { + Dictionary data = p_array[i]; + ERR_FAIL_COND(!data.has("version")); + + uint32_t po_version = data["version"]; + + if (po_version == 1) { + ERR_FAIL_COND(!data.has("vertices")); + ERR_FAIL_COND(!data.has("elevation")); + ERR_FAIL_COND(!data.has("height")); + ERR_FAIL_COND(!data.has("carve")); + } + + ProjectedObstruction projected_obstruction; + projected_obstruction.vertices = Vector<float>(data["vertices"]); + projected_obstruction.elevation = data["elevation"]; + projected_obstruction.height = data["height"]; + projected_obstruction.carve = data["carve"]; + + RWLockWrite write_lock(geometry_rwlock); + _projected_obstructions.push_back(projected_obstruction); + } +} + +Vector<NavigationMeshSourceGeometryData3D::ProjectedObstruction> NavigationMeshSourceGeometryData3D::_get_projected_obstructions() const { + RWLockRead read_lock(geometry_rwlock); + return _projected_obstructions; +} + +Array NavigationMeshSourceGeometryData3D::get_projected_obstructions() const { + RWLockRead read_lock(geometry_rwlock); + + Array ret; + ret.resize(_projected_obstructions.size()); + + for (int i = 0; i < _projected_obstructions.size(); i++) { + const ProjectedObstruction &projected_obstruction = _projected_obstructions[i]; + + Dictionary data; + data["version"] = (int)ProjectedObstruction::VERSION; + data["vertices"] = projected_obstruction.vertices; + data["elevation"] = projected_obstruction.elevation; + data["height"] = projected_obstruction.height; + data["carve"] = projected_obstruction.carve; + + ret[i] = data; + } + + return ret; +} + +bool NavigationMeshSourceGeometryData3D::_set(const StringName &p_name, const Variant &p_value) { + if (p_name == "projected_obstructions") { + set_projected_obstructions(p_value); + return true; + } + return false; +} + +bool NavigationMeshSourceGeometryData3D::_get(const StringName &p_name, Variant &r_ret) const { + if (p_name == "projected_obstructions") { + r_ret = get_projected_obstructions(); + return true; + } + return false; +} + +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); + + ClassDB::bind_method(D_METHOD("set_indices", "indices"), &NavigationMeshSourceGeometryData3D::set_indices); + ClassDB::bind_method(D_METHOD("get_indices"), &NavigationMeshSourceGeometryData3D::get_indices); + + ClassDB::bind_method(D_METHOD("clear"), &NavigationMeshSourceGeometryData3D::clear); + ClassDB::bind_method(D_METHOD("has_data"), &NavigationMeshSourceGeometryData3D::has_data); + + ClassDB::bind_method(D_METHOD("add_mesh", "mesh", "xform"), &NavigationMeshSourceGeometryData3D::add_mesh); + ClassDB::bind_method(D_METHOD("add_mesh_array", "mesh_array", "xform"), &NavigationMeshSourceGeometryData3D::add_mesh_array); + ClassDB::bind_method(D_METHOD("add_faces", "faces", "xform"), &NavigationMeshSourceGeometryData3D::add_faces); + ClassDB::bind_method(D_METHOD("merge", "other_geometry"), &NavigationMeshSourceGeometryData3D::merge); + + ClassDB::bind_method(D_METHOD("add_projected_obstruction", "vertices", "elevation", "height", "carve"), &NavigationMeshSourceGeometryData3D::add_projected_obstruction); + ClassDB::bind_method(D_METHOD("clear_projected_obstructions"), &NavigationMeshSourceGeometryData3D::clear_projected_obstructions); + 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); + + 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"); +} |