Repository: godotengine/godot (language: cpp)

Issue:
Negative scaling is broken when using Jolt Physics
### Tested versions


v4.4.rc2.mono.official [01545c995]

This error appeared after upgrading to 4.4rc2 from 4.3.  I was previously using the jolt physics add on.  

This error goes away if i switch to the godot physics engine instead.  It reappears if using jolt.  

### System information

Godot v4.4.rc2.mono - Windows 10 (build 19045) - Multi-window, 2 monitors - Vulkan (Forward+) - dedicated NVIDIA GeForce RTX 3070 (NVIDIA; 32.0.15.6603) - AMD Ryzen 7 3700X 8-Core Processor (16 threads)

### Issue description

With Jolt Physics enabled, when adding a static body as a child of a VehicleWheel3D I get a flood of quaternion errors:

```
E 0:00:01:120   get_quaternion: Basis [X: (1.0, 0.0, 0.0), Y: (0.0, 1.0, 0.0), Z: (0.0, 0.0, -1.0)] must be 
normalized in order to be casted to a Quaternion. Use get_rotation_quaternion() or call orthonormalized() 
if the Basis contains linearly independent vectors.
  <C++ Error>   Condition "!is_rotation()" is true. Returning: Quaternion()
  <C++ Source>  core/math/basis.cpp:730 @ get_quaternion()
```

I can get rid of the errors if I move the StaticBody3D outside of the VehicleWheel3D.  I do expect it to work as before.  I prefer adding a staticBody3D as a child of VehicleWheel3D for organizations sake. 

### Steps to reproduce

1. Enable Jolt Physics under Project -> Project Settings -> Physics -> 3D -> Physics Engine
2. Add a VehicleBody3D node with a collision mesh
3. Add a VehicleWheel3D node
4. Add a StaticBody3D node as a child of the VehicleWheel3D
5. Launch the scene and a flood of quaternion errors should appear

Including project as well.  Open the project and run.  

Thank you.

### Minimal reproduction project (MRP)

[mrpWheelStaticBodyChild.zip](https://github.com/user-attachments/files/19031403/mrpWheelStaticBodyChild.zip)


Discussion / hints:
Suggested fix here

https://github.com/godotengine/godot/pull/103426

There may be other places to reduce calculation repetition, best to get an expert to review before any merge

Fix patch:
diff --git a/modules/jolt_physics/misc/jolt_math_funcs.cpp b/modules/jolt_physics/misc/jolt_math_funcs.cpp
new file mode 100644
index 000000000000..b0ef5f45747d
--- /dev/null
+++ b/modules/jolt_physics/misc/jolt_math_funcs.cpp
@@ -0,0 +1,70 @@
+/**************************************************************************/
+/*  jolt_math_funcs.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.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Jolt Physics library.
+*/
+
+/*
+Copyright 2021 Jorrit Rouwe
+
+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 "jolt_math_funcs.h"
+
+void JoltMath::decompose(Basis &p_basis, Vector3 &r_scale) {
+	Vector3 x = p_basis.get_column(Vector3::AXIS_X);
+	Vector3 y = p_basis.get_column(Vector3::AXIS_Y);
+	Vector3 z = p_basis.get_column(Vector3::AXIS_Z);
+
+	const real_t x_dot_x = x.dot(x);
+	y -= x * (y.dot(x) / x_dot_x);
+	z -= x * (z.dot(x) / x_dot_x);
+	const real_t y_dot_y = y.dot(y);
+	z -= y * (z.dot(y) / y_dot_y);
+	const real_t z_dot_z = z.dot(z);
+
+	const real_t det = x.cross(y).dot(z);
+
+	r_scale = SIGN(det) * Vector3(Math::sqrt(x_dot_x), Math::sqrt(y_dot_y), Math::sqrt(z_dot_z));
+
+	p_basis.set_column(Vector3::AXIS_X, x / r_scale.x);
+	p_basis.set_column(Vector3::AXIS_Y, y / r_scale.y);
+	p_basis.set_column(Vector3::AXIS_Z, z / r_scale.z);
+}
diff --git a/modules/jolt_physics/misc/jolt_math_funcs.h b/modules/jolt_physics/misc/jolt_math_funcs.h
new file mode 100644
index 000000000000..ba2d290ac77c
--- /dev/null
+++ b/modules/jolt_physics/misc/jolt_math_funcs.h
@@ -0,0 +1,59 @@
+/**************************************************************************/
+/*  jolt_math_funcs.h                                                     */
+/**************************************************************************/
+/*                         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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_MATH_FUNCS_H
+#define JOLT_MATH_FUNCS_H
+
+#include "core/math/transform_3d.h"
+
+class JoltMath {
+public:
+	// Note that `p_basis` is mutated to be right-handed orthonormal.
+	static void decompose(Basis &p_basis, Vector3 &r_scale);
+
+	// Note that `p_transform` is mutated to be right-handed orthonormal.
+	static _FORCE_INLINE_ void decompose(Transform3D &p_transform, Vector3 &r_scale) {
+		decompose(p_transform.basis, r_scale);
+	}
+
+	static _FORCE_INLINE_ void decomposed(const Basis &p_basis, Basis &r_new_basis, Vector3 &r_scale) {
+		Basis new_basis = p_basis;
+		decompose(new_basis, r_scale);
+		r_new_basis = new_basis;
+	}
+
+	static _FORCE_INLINE_ void decomposed(const Transform3D &p_transform, Transform3D &r_new_transform, Vector3 &r_scale) {
+		Transform3D new_transform;
+		decompose(new_transform, r_scale);
+		r_new_transform = new_transform;
+	}
+};
+
+#endif // JOLT_MATH_FUNCS_H
diff --git a/modules/jolt_physics/objects/jolt_area_3d.cpp b/modules/jolt_physics/objects/jolt_area_3d.cpp
index b0a9ffd62225..0a2693ddb634 100644
--- a/modules/jolt_physics/objects/jolt_area_3d.cpp
+++ b/modules/jolt_physics/objects/jolt_area_3d.cpp
@@ -31,6 +31,7 @@
 #include "jolt_area_3d.h"
 
 #include "../jolt_project_settings.h"
+#include "../misc/jolt_math_funcs.h"
 #include "../misc/jolt_type_conversions.h"
 #include "../shapes/jolt_shape_3d.h"
 #include "../spaces/jolt_broad_phase_layer.h"
@@ -370,7 +371,8 @@ void JoltArea3D::set_default_area(bool p_value) {
 void JoltArea3D::set_transform(Transform3D p_transform) {
 	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to area '%s'.", to_string()));
 
-	const Vector3 new_scale = p_transform.basis.get_scale();
+	Vector3 new_scale;
+	JoltMath::decompose(p_transform, new_scale);
 
 	// Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often.
 	if (!scale.is_equal_approx(new_scale)) {
@@ -378,8 +380,6 @@ void JoltArea3D::set_transform(Transform3D p_transform) {
 		_shapes_changed();
 	}
 
-	p_transform.basis.orthonormalize();
-
 	if (!in_space()) {
 		jolt_settings->mPosition = to_jolt_r(p_transform.origin);
 		jolt_settings->mRotation = to_jolt(p_transform.basis);
diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp
index ac2b37470ef1..5bc3aee1bce7 100644
--- a/modules/jolt_physics/objects/jolt_body_3d.cpp
+++ b/modules/jolt_physics/objects/jolt_body_3d.cpp
@@ -32,6 +32,7 @@
 
 #include "../joints/jolt_joint_3d.h"
 #include "../jolt_project_settings.h"
+#include "../misc/jolt_math_funcs.h"
 #include "../misc/jolt_type_conversions.h"
 #include "../shapes/jolt_shape_3d.h"
 #include "../spaces/jolt_broad_phase_layer.h"
@@ -543,7 +544,8 @@ JoltBody3D::~JoltBody3D() {
 void JoltBody3D::set_transform(Transform3D p_transform) {
 	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to physics body '%s'.", to_string()));
 
-	const Vector3 new_scale = p_transform.basis.get_scale();
+	Vector3 new_scale;
+	JoltMath::decompose(p_transform, new_scale);
 
 	// Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often.
 	if (!scale.is_equal_approx(new_scale)) {
@@ -551,8 +553,6 @@ void JoltBody3D::set_transform(Transform3D p_transform) {
 		_shapes_changed();
 	}
 
-	p_transform.basis.orthonormalize();
-
 	if (!in_space()) {
 		jolt_settings->mPosition = to_jolt_r(p_transform.origin);
 		jolt_settings->mRotation = to_jolt(p_transform.basis);
diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
index bfe77e008d59..f32ddad0d469 100644
--- a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
+++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
@@ -30,6 +30,7 @@
 
 #include "jolt_shaped_object_3d.h"
 
+#include "../misc/jolt_math_funcs.h"
 #include "../misc/jolt_type_conversions.h"
 #include "../shapes/jolt_custom_double_sided_shape.h"
 #include "../shapes/jolt_shape_3d.h"
@@ -347,7 +348,10 @@ void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
 void JoltShapedObject3D::add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled) {
 	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed when adding shape at index %d to physics body '%s'.", shapes.size(), to_string()));
 
-	shapes.push_back(JoltShapeInstance3D(this, p_shape, p_transform.orthonormalized(), p_transform.basis.get_scale(), p_disabled));
+	Vector3 shape_scale;
+	JoltMath::decompose(p_transform, shape_scale);
+
+	shapes.push_back(JoltShapeInstance3D(this, p_shape, p_transform, shape_scale, p_disabled));
 
 	_shapes_changed();
 }
@@ -430,8 +434,8 @@ void JoltShapedObject3D::set_shape_transform(int p_index, Transform3D p_transfor
 	ERR_FAIL_INDEX(p_index, (int)shapes.size());
 	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, "Failed to correctly set transform for shape at index %d in body '%s'.");
 
-	Vector3 new_scale = p_transform.basis.get_scale();
-	p_transform.basis.orthonormalize();
+	Vector3 new_scale;
+	JoltMath::decompose(p_transform, new_scale);
 
 	JoltShapeInstance3D &shape = shapes[p_index];
 
diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
index 3852e42d51f2..d7a4afec57ba 100644
--- a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
+++ b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
@@ -32,6 +32,7 @@
 
 #include "../jolt_physics_server_3d.h"
 #include "../jolt_project_settings.h"
+#include "../misc/jolt_math_funcs.h"
 #include "../misc/jolt_type_conversions.h"
 #include "../objects/jolt_area_3d.h"
 #include "../objects/jolt_body_3d.h"
@@ -288,11 +289,10 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_cast(const JoltBody3D &p_body,
 		const Transform3D transform_com_local = transform_local.translated_local(com_scaled);
 		Transform3D transform_com = body_transform * transform_com_local;
 
-		Vector3 scale = transform_com.basis.get_scale();
+		Vector3 scale;
+		JoltMath::decompose(transform_com, scale);
 		JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "body_test_motion was passed an invalid transform along with body '%s'. This results in invalid scaling for shape at index %d.");
 
-		transform_com.basis.orthonormalize();
-
 		real_t shape_safe_fraction = 1.0;
 		real_t shape_unsafe_fraction = 1.0;
 
@@ -590,11 +590,10 @@ int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_para
 	Transform3D transform = p_parameters.transform;
 	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "intersect_shape was passed an invalid transform.");
 
-	Vector3 scale = transform.basis.get_scale();
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
 	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "intersect_shape was passed an invalid transform.");
 
-	transform.basis.orthonormalize();
-
 	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
 	const Transform3D transform_com = transform.translated_local(com_scaled);
 
@@ -647,11 +646,10 @@ bool JoltPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_paramet
 	Transform3D transform = p_parameters.transform;
 	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
 
-	Vector3 scale = transform.basis.get_scale();
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
 	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
 
-	transform.basis.orthonormalize();
-
 	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
 	Transform3D transform_com = transform.translated_local(com_scaled);
 
@@ -684,11 +682,10 @@ bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_param
 	Transform3D transform = p_parameters.transform;
 	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "collide_shape was passed an invalid transform.");
 
-	Vector3 scale = transform.basis.get_scale();
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
 	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "collide_shape was passed an invalid transform.");
 
-	transform.basis.orthonormalize();
-
 	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
 	const Transform3D transform_com = transform.translated_local(com_scaled);
 
@@ -754,11 +751,10 @@ bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameter
 	Transform3D transform = p_parameters.transform;
 	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
 
-	Vector3 scale = transform.basis.get_scale();
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
 	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
 
-	transform.basis.orthonormalize();
-
 	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
 	const Transform3D transform_com = transform.translated_local(com_scaled);
 
@@ -890,8 +886,8 @@ bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, c
 	Transform3D transform = p_parameters.from;
 	JOLT_ENSURE_SCALE_NOT_ZERO(transform, vformat("body_test_motion (maybe from move_and_slide?) was passed an invalid transform along with body '%s'.", p_body.to_string()));
 
-	Vector3 scale = transform.basis.get_scale();
-	transform.basis.orthonormalize();
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
 
 	space->try_optimize();
 


Test patch: