From 062406b5551f53aff06ccf4c78fc29b43bf09acb Mon Sep 17 00:00:00 2001 From: lawnjelly Date: Sat, 7 Aug 2021 09:41:42 +0100 Subject: [PATCH] [3.x] Add Basis helper functions for transforming normals Correct transformation of normals that works with a Basis containing non-uniform scale is difficult to get correct for those not familiar with the maths, it is also rather verbose and hard to read in calling code. This PR adds helper functions which both standardize the approach and make it clearer in calling code what is being done and why. --- core/math/basis.h | 16 +++++++++++++++- servers/physics/collision_solver_sat.cpp | 20 ++++++++++---------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/core/math/basis.h b/core/math/basis.h index ebd9e7725cf..19685a958f4 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -237,9 +237,23 @@ public: bool is_symmetric() const; Basis diagonalize(); + // The following normal xform functions are correct for non-uniform scales. + // Use these two functions in combination to xform a series of normals. + // First use get_normal_xform_basis() to precalculate the inverse transpose. + // Then apply xform_normal_fast() multiple times using the inverse transpose basis. + Basis get_normal_xform_basis() const { return inverse().transposed(); } + + // N.B. This only does a normal transform if the basis used is the inverse transpose! + // Otherwise use xform_normal(). + Vector3 xform_normal_fast(const Vector3 &p_vector) const { return xform(p_vector).normalized(); } + + // This function does the above but for a single normal vector. It is considerably slower, so should usually + // only be used in cases of single normals, or when the basis changes each time. + Vector3 xform_normal(const Vector3 &p_vector) const { return get_normal_xform_basis().xform_normal_fast(p_vector); } + operator Quat() const { return get_quat(); } - Basis(const Quat &p_quat) { set_quat(p_quat); }; + Basis(const Quat &p_quat) { set_quat(p_quat); } Basis(const Quat &p_quat, const Vector3 &p_scale) { set_quat_scale(p_quat, p_scale); } Basis(const Vector3 &p_euler) { set_euler(p_euler); } diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index 5e4589a134e..7c82afde4b7 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -954,11 +954,11 @@ static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform int vertex_count = mesh.vertices.size(); // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + Basis nx_b = p_transform_b.basis.get_normal_xform_basis(); // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + Vector3 axis = nx_b.xform_normal_fast(faces[i].plane.normal); if (!separator.test_axis(axis)) { return; @@ -1373,11 +1373,11 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p } // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + Basis nx_b = p_transform_b.basis.get_normal_xform_basis(); // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + Vector3 axis = nx_b.xform_normal_fast(faces[i].plane.normal); if (!separator.test_axis(axis)) { return; @@ -1709,11 +1709,11 @@ static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transfor const Vector3 *vertices = mesh.vertices.ptr(); // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + Basis nx_b = p_transform_b.basis.get_normal_xform_basis(); // faces of B for (int i = 0; i < face_count; i++) { - Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized(); + Vector3 axis = nx_b.xform_normal_fast(faces[i].plane.normal); if (!separator.test_axis(axis)) { return; @@ -2006,11 +2006,11 @@ static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const T int vertex_count_B = mesh_B.vertices.size(); // Precalculating this makes the transforms faster. - Basis a_xform_normal = p_transform_a.basis.inverse().transposed(); + Basis nx_a = p_transform_a.basis.get_normal_xform_basis(); // faces of A for (int i = 0; i < face_count_A; i++) { - Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized(); + Vector3 axis = nx_a.xform_normal_fast(faces_A[i].plane.normal); if (!separator.test_axis(axis)) { return; @@ -2018,11 +2018,11 @@ static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const T } // Precalculating this makes the transforms faster. - Basis b_xform_normal = p_transform_b.basis.inverse().transposed(); + Basis nx_b = p_transform_b.basis.get_normal_xform_basis(); // faces of B for (int i = 0; i < face_count_B; i++) { - Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized(); + Vector3 axis = nx_b.xform_normal_fast(faces_B[i].plane.normal); if (!separator.test_axis(axis)) { return;