Merge pull request #51354 from lawnjelly/basis_normal_xform

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); }

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;