Fix Transform::xform(Plane) functions

The Transform::xform and xform_inv are made safe for Planes when using non-uniform scaling.
Optimization of calling sites to prevent loss of performance from the changes to xform(Plane).

1 files changed, 20 insertions, 7 deletions

diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index 6a7f2b73c5..852e9ecd74 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -956,9 +956,12 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
 	const Vector3 *vertices = mesh.vertices.ptr();
 	int vertex_count = mesh.vertices.size();
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count; i++) {
-		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
+		Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -1379,9 +1382,12 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3
 		}
 	}
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count; i++) {
-		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
+		Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -1733,9 +1739,12 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
 	int edge_count = mesh.edges.size();
 	const Vector3 *vertices = mesh.vertices.ptr();
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count; i++) {
-		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
+		Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -2057,20 +2066,24 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
 	const Vector3 *vertices_B = mesh_B.vertices.ptr();
 	int vertex_count_B = mesh_B.vertices.size();
 
+	// Precalculating this makes the transforms faster.
+	Basis a_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of A
 	for (int i = 0; i < face_count_A; i++) {
-		Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal;
-		//Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized();
+		Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
 		}
 	}
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count_B; i++) {
-		Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal;
-		//Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized();
+		Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;