godotengine/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-11-19 14:31:59 +00:00
Code Releases Wiki Activity

Fix broken negative scaling when using Jolt Physics

Browse Source
This commit is contained in:
Mikael Hermansson
2025-03-01 18:47:49 +01:00
parent 15ff450680
commit 62e8b1e5fd
6 changed files with 155 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
modules/jolt_physics/misc/jolt_math_funcs.cpp Normal file
View File

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

59
modules/jolt_physics/misc/jolt_math_funcs.h Normal file
View File

@@ -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

6
modules/jolt_physics/objects/jolt_area_3d.cpp
View File

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

6
modules/jolt_physics/objects/jolt_body_3d.cpp
View File

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

10
modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
View File

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

30
modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
View File

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