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 00000000000..b0ef5f45747 --- /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 00000000000..ba2d290ac77 --- /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 b0a9ffd6222..0a2693ddb63 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 ac2b37470ef..5bc3aee1bce 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 bfe77e008d5..f32ddad0d46 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 3852e42d51f..d7a4afec57b 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();