Fix broken negative scaling when using Jolt Physics

(cherry picked from commit 62e8b1e5fdeb3ffcd118fbaad68dfc22e721fc00)

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

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

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

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

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];
 

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