diff options
Diffstat (limited to 'servers/physics_3d/godot_shape_3d.cpp')
| -rw-r--r-- | servers/physics_3d/godot_shape_3d.cpp | 102 |
1 files changed, 56 insertions, 46 deletions
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index 1a016fbc72..d566d612ce 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -1,32 +1,32 @@ -/*************************************************************************/ -/* godot_shape_3d.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* 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. */ -/*************************************************************************/ +/**************************************************************************/ +/* godot_shape_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 "godot_shape_3d.h" @@ -844,36 +844,41 @@ void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Tra } Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const { + // Skip if there are no vertices in the mesh if (mesh.vertices.size() == 0) { return Vector3(); } - // Find an initial guess for the support vertex by checking the ones we - // found in _setup(). + // Get the array of vertices + const Vector3 *const vertices_array = mesh.vertices.ptr(); - int best_vertex = -1; - real_t max_support = 0.0; - for (uint32_t i = 0; i < extreme_vertices.size(); i++) { - real_t s = p_normal.dot(mesh.vertices[extreme_vertices[i]]); - if (best_vertex == -1 || s > max_support) { - best_vertex = extreme_vertices[i]; + // Start with an initial assumption of the first extreme vertex. + int best_vertex = extreme_vertices[0]; + real_t max_support = p_normal.dot(vertices_array[best_vertex]); + + // Check the remaining extreme vertices for a better vertex. + for (const int &vert : extreme_vertices) { + real_t s = p_normal.dot(vertices_array[vert]); + if (s > max_support) { + best_vertex = vert; max_support = s; } } + + // If we checked all vertices in the mesh then we're done. if (extreme_vertices.size() == mesh.vertices.size()) { - // We've already checked every vertex, so we can return now. - return mesh.vertices[best_vertex]; + return vertices_array[best_vertex]; } // Move along the surface until we reach the true support vertex. - int last_vertex = -1; while (true) { int next_vertex = -1; - for (uint32_t i = 0; i < vertex_neighbors[best_vertex].size(); i++) { - int vert = vertex_neighbors[best_vertex][i]; + + // Iterate over all the neighbors checking for a better vertex. + for (const int &vert : vertex_neighbors[best_vertex]) { if (vert != last_vertex) { - real_t s = p_normal.dot(mesh.vertices[vert]); + real_t s = p_normal.dot(vertices_array[vert]); if (s > max_support) { next_vertex = vert; max_support = s; @@ -881,9 +886,13 @@ Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const { } } } + + // No better vertex found, we have the best if (next_vertex == -1) { - return mesh.vertices[best_vertex]; + return vertices_array[best_vertex]; } + + // Move to the better vertex and try again last_vertex = best_vertex; best_vertex = next_vertex; } @@ -1083,6 +1092,8 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { if (err != OK) { ERR_PRINT("Failed to build convex hull"); } + extreme_vertices.resize(0); + vertex_neighbors.resize(0); AABB _aabb; @@ -1126,8 +1137,7 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) { if (extreme_vertices.size() < mesh.vertices.size()) { vertex_neighbors.resize(mesh.vertices.size()); - for (uint32_t i = 0; i < mesh.edges.size(); i++) { - Geometry3D::MeshData::Edge &edge = mesh.edges[i]; + for (Geometry3D::MeshData::Edge &edge : mesh.edges) { vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b); vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a); } |