summaryrefslogtreecommitdiff
path: root/servers/physics_3d/godot_shape_3d.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d/godot_shape_3d.cpp')
-rw-r--r--servers/physics_3d/godot_shape_3d.cpp189
1 files changed, 131 insertions, 58 deletions
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index cd61ceab62..300dca4e08 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"
@@ -521,8 +521,9 @@ void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vecto
Vector3 n = p_normal;
real_t d = n.y;
+ real_t h = height * 0.5 - radius; // half-height of the cylinder part
- if (Math::abs(d) < edge_support_threshold) {
+ if (h > 0 && Math::abs(d) < edge_support_threshold) {
// make it flat
n.y = 0.0;
n.normalize();
@@ -531,13 +532,10 @@ void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vecto
r_amount = 2;
r_type = FEATURE_EDGE;
r_supports[0] = n;
- r_supports[0].y += height * 0.5 - radius;
+ r_supports[0].y += h;
r_supports[1] = n;
- r_supports[1].y -= height * 0.5 - radius;
-
+ r_supports[1].y -= h;
} else {
- real_t h = height * 0.5 - radius;
-
n *= radius;
n.y += (d > 0) ? h : -h;
r_amount = 1;
@@ -817,48 +815,87 @@ GodotCylinderShape3D::GodotCylinderShape3D() {}
/********** CONVEX POLYGON *************/
void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
- int vertex_count = mesh.vertices.size();
+ uint32_t vertex_count = mesh.vertices.size();
if (vertex_count == 0) {
return;
}
const Vector3 *vrts = &mesh.vertices[0];
- for (int i = 0; i < vertex_count; i++) {
- real_t d = p_normal.dot(p_transform.xform(vrts[i]));
+ if (vertex_count > 3 * extreme_vertices.size()) {
+ // For a large mesh, two calls to get_support() is faster than a full
+ // scan over all vertices.
- if (i == 0 || d > r_max) {
- r_max = d;
- }
- if (i == 0 || d < r_min) {
- r_min = d;
+ Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
+ r_min = p_normal.dot(p_transform.xform(get_support(-n)));
+ r_max = p_normal.dot(p_transform.xform(get_support(n)));
+ } else {
+ for (uint32_t i = 0; i < vertex_count; i++) {
+ real_t d = p_normal.dot(p_transform.xform(vrts[i]));
+
+ if (i == 0 || d > r_max) {
+ r_max = d;
+ }
+ if (i == 0 || d < r_min) {
+ r_min = d;
+ }
}
}
}
Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
- Vector3 n = p_normal;
-
- int vert_support_idx = -1;
- real_t support_max = 0;
-
- int vertex_count = mesh.vertices.size();
- if (vertex_count == 0) {
+ // Skip if there are no vertices in the mesh
+ if (mesh.vertices.size() == 0) {
return Vector3();
}
- const Vector3 *vrts = &mesh.vertices[0];
+ // Get the array of vertices
+ const Vector3 *const vertices_array = mesh.vertices.ptr();
- for (int i = 0; i < vertex_count; i++) {
- real_t d = n.dot(vrts[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]);
- if (i == 0 || d > support_max) {
- support_max = d;
- vert_support_idx = i;
+ // 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;
}
}
- return vrts[vert_support_idx];
+ // If we checked all vertices in the mesh then we're done.
+ if (extreme_vertices.size() == mesh.vertices.size()) {
+ 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;
+
+ // 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(vertices_array[vert]);
+ if (s > max_support) {
+ next_vertex = vert;
+ max_support = s;
+ break;
+ }
+ }
+ }
+
+ // No better vertex found, we have the best
+ if (next_vertex == -1) {
+ return vertices_array[best_vertex];
+ }
+
+ // Move to the better vertex and try again
+ last_vertex = best_vertex;
+ best_vertex = next_vertex;
+ }
}
void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
@@ -1067,6 +1104,42 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
}
configure(_aabb);
+
+ // Pre-compute the extreme vertices in 26 directions. This will be used
+ // to speed up get_support() by letting us quickly get a good guess for
+ // the support vertex.
+
+ for (int x = -1; x < 2; x++) {
+ for (int y = -1; y < 2; y++) {
+ for (int z = -1; z < 2; z++) {
+ if (x != 0 || y != 0 || z != 0) {
+ Vector3 dir(x, y, z);
+ dir.normalize();
+ real_t max_support = 0.0;
+ int best_vertex = -1;
+ for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
+ real_t s = dir.dot(mesh.vertices[i]);
+ if (best_vertex == -1 || s > max_support) {
+ best_vertex = i;
+ max_support = s;
+ }
+ }
+ if (extreme_vertices.find(best_vertex) == -1)
+ extreme_vertices.push_back(best_vertex);
+ }
+ }
+ }
+ }
+
+ // Record all the neighbors of each vertex. This is used in get_support().
+
+ if (extreme_vertices.size() < mesh.vertices.size()) {
+ vertex_neighbors.resize(mesh.vertices.size());
+ 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);
+ }
+ }
}
void GodotConvexPolygonShape3D::set_data(const Variant &p_data) {
@@ -1931,7 +2004,7 @@ void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y
Vector3 clamped_point(p_point);
clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + shape_aabb.size.x);
clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + shape_aabb.size.y);
- clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + shape_aabb.size.z);
+ clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.z + shape_aabb.size.z);
r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5);
r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5);
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-16 21:55:48 +0000