From d710af2e9742e0b452c24a4c4871b9beeed350de Mon Sep 17 00:00:00 2001 From: Black-Cat Date: Sun, 23 Apr 2023 19:03:19 +0100 Subject: [PATCH] Fix precision in physics supports generation Lower threshold for dot was (1.0 - threshold) which is incorrect. Patch changes it to correct version sqrt(1.0 - threshold * threshold) Co-authored-by: Ricardo Buring --- servers/physics_2d/godot_shape_2d.cpp | 11 +++++------ servers/physics_2d/godot_shape_2d.h | 9 ++++++--- servers/physics_3d/godot_shape_3d.cpp | 28 +++++++++++++-------------- 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/servers/physics_2d/godot_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp index 67c24e29b05..dadd8173c60 100644 --- a/servers/physics_2d/godot_shape_2d.cpp +++ b/servers/physics_2d/godot_shape_2d.cpp @@ -179,7 +179,7 @@ Variant GodotSeparationRayShape2D::get_data() const { /*********************************************************/ void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { - if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(p_normal.dot(n)) > segment_is_valid_support_threshold) { r_supports[0] = a; r_supports[1] = b; r_amount = 2; @@ -308,7 +308,7 @@ void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_sup Vector2 ag; ag[i] = 1.0; real_t dp = ag.dot(p_normal); - if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + if (Math::abs(dp) <= segment_is_valid_support_threshold) { continue; } @@ -368,10 +368,9 @@ Variant GodotRectangleShape2D::get_data() const { void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { Vector2 n = p_normal; - real_t d = n.y; real_t h = height * 0.5 - radius; // half-height of the rectangle part - if (h > 0 && Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + if (h > 0 && Math::abs(n.x) > segment_is_valid_support_threshold) { // make it flat n.y = 0.0; n.normalize(); @@ -384,7 +383,7 @@ void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_suppo r_supports[1].y -= h; } else { n *= radius; - n.y += (d > 0) ? h : -h; + n.y += (n.y > 0) ? h : -h; r_amount = 1; *r_supports = n; } @@ -506,7 +505,7 @@ void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r } //test segment - if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { + if (points[i].normal.dot(p_normal) > segment_is_valid_support_threshold) { r_amount = 2; r_supports[0] = points[i].pos; r_supports[1] = points[(i + 1) % point_count].pos; diff --git a/servers/physics_2d/godot_shape_2d.h b/servers/physics_2d/godot_shape_2d.h index 50b2bfd4e94..28c69574a09 100644 --- a/servers/physics_2d/godot_shape_2d.h +++ b/servers/physics_2d/godot_shape_2d.h @@ -32,7 +32,6 @@ #define GODOT_SHAPE_2D_H #include "servers/physics_server_2d.h" -#define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 class GodotShape2D; @@ -53,6 +52,10 @@ class GodotShape2D { HashMap owners; protected: + const double segment_is_valid_support_threshold = 0.99998; + const double segment_is_valid_support_threshold_lower = + Math::sqrt(1.0 - segment_is_valid_support_threshold * segment_is_valid_support_threshold); + void configure(const Rect2 &p_aabb); public: @@ -95,7 +98,7 @@ public: } if (r_amount == 1) { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) { //make line because they are parallel r_amount = 2; r_supports[1] = r_supports[0] + p_cast; @@ -105,7 +108,7 @@ public: } } else { - if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { + if (Math::abs(p_normal.dot(p_cast.normalized())) < segment_is_valid_support_threshold_lower) { //optimize line and make it larger because they are parallel if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) { //larger towards 1 diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index d566d612ced..50154a85ecd 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -52,11 +52,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -constexpr double edge_support_threshold = 0.0002; -constexpr double face_support_threshold = 0.9998; +const double support_threshold = 0.9998; +const double support_threshold_lower = Math::sqrt(1.0 - support_threshold * support_threshold); -constexpr double cylinder_edge_support_threshold = 0.002; -constexpr double cylinder_face_support_threshold = 0.999; +const double cylinder_support_threshold = 0.999; +const double cylinder_support_threshold_lower = Math::sqrt(1.0 - cylinder_support_threshold * cylinder_support_threshold); void GodotShape3D::configure(const AABB &p_aabb) { aabb = p_aabb; @@ -184,7 +184,7 @@ Vector3 GodotSeparationRayShape3D::get_support(const Vector3 &p_normal) const { } void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { - if (Math::abs(p_normal.z) < edge_support_threshold) { + if (Math::abs(p_normal.z) < support_threshold_lower) { r_amount = 2; r_type = FEATURE_EDGE; r_supports[0] = Vector3(0, 0, 0); @@ -335,7 +335,7 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 * Vector3 axis; axis[i] = 1.0; real_t dot = p_normal.dot(axis); - if (Math::abs(dot) > face_support_threshold) { + if (Math::abs(dot) > support_threshold) { //Vector3 axis_b; bool neg = dot < 0; @@ -376,7 +376,7 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 * Vector3 axis; axis[i] = 1.0; - if (Math::abs(p_normal.dot(axis)) < edge_support_threshold) { + if (Math::abs(p_normal.dot(axis)) < support_threshold_lower) { r_amount = 2; r_type = FEATURE_EDGE; @@ -523,7 +523,7 @@ void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vecto real_t d = n.y; real_t h = height * 0.5 - radius; // half-height of the cylinder part - if (h > 0 && Math::abs(d) < edge_support_threshold) { + if (h > 0 && Math::abs(d) < support_threshold_lower) { // make it flat n.y = 0.0; n.normalize(); @@ -701,7 +701,7 @@ Vector3 GodotCylinderShape3D::get_support(const Vector3 &p_normal) const { void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { real_t d = p_normal.y; - if (Math::abs(d) > cylinder_face_support_threshold) { + if (Math::abs(d) > cylinder_support_threshold) { real_t h = (d > 0) ? height : -height; Vector3 n = p_normal; @@ -716,7 +716,7 @@ void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vect r_supports[1].x += radius; r_supports[2] = n; r_supports[2].z += radius; - } else if (Math::abs(d) < cylinder_edge_support_threshold) { + } else if (Math::abs(d) < cylinder_support_threshold_lower) { // make it flat Vector3 n = p_normal; n.y = 0.0; @@ -925,7 +925,7 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, } for (int i = 0; i < fc; i++) { - if (faces[i].plane.normal.dot(p_normal) > face_support_threshold) { + if (faces[i].plane.normal.dot(p_normal) > support_threshold) { int ic = faces[i].indices.size(); const int *ind = faces[i].indices.ptr(); @@ -954,7 +954,7 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, for (int i = 0; i < ec; i++) { real_t dot = (vertices[edges[i].vertex_a] - vertices[edges[i].vertex_b]).normalized().dot(p_normal); dot = ABS(dot); - if (dot < edge_support_threshold && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) { + if (dot < support_threshold_lower && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) { r_amount = 2; r_type = FEATURE_EDGE; r_supports[0] = vertices[edges[i].vertex_a]; @@ -1197,7 +1197,7 @@ void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ - if (Math::abs(normal.dot(n)) > face_support_threshold) { + if (Math::abs(normal.dot(n)) > support_threshold) { r_amount = 3; r_type = FEATURE_FACE; for (int i = 0; i < 3; i++) { @@ -1231,7 +1231,7 @@ void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 // check if edge is valid as a support real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); dot = ABS(dot); - if (dot < edge_support_threshold) { + if (dot < support_threshold_lower) { r_amount = 2; r_type = FEATURE_EDGE; r_supports[0] = vertex[i];