Skip to content

Commit 7e85cd5

Browse files
mihemihe
committed
Fix broken negative scaling when using Jolt Physics
1 parent 15ff450 commit 7e85cd5

File tree

6 files changed

+160
-28
lines changed

6 files changed

+160
-28
lines changed

‎modules/jolt_physics/misc/jolt_math_funcs.cpp

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
/**************************************************************************/
2+
/* jolt_math_funcs.cpp */
3+
/**************************************************************************/
4+
/* This file is part of: */
5+
/* GODOT ENGINE */
6+
/* https://godotengine.org */
7+
/**************************************************************************/
8+
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9+
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10+
/* */
11+
/* Permission is hereby granted, free of charge, to any person obtaining */
12+
/* a copy of this software and associated documentation files (the */
13+
/* "Software"), to deal in the Software without restriction, including */
14+
/* without limitation the rights to use, copy, modify, merge, publish, */
15+
/* distribute, sublicense, and/or sell copies of the Software, and to */
16+
/* permit persons to whom the Software is furnished to do so, subject to */
17+
/* the following conditions: */
18+
/* */
19+
/* The above copyright notice and this permission notice shall be */
20+
/* included in all copies or substantial portions of the Software. */
21+
/* */
22+
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23+
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24+
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25+
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26+
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27+
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28+
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29+
/**************************************************************************/
30+
31+
/*
32+
Adapted to Godot from the Jolt Physics library.
33+
*/
34+
35+
/*
36+
Copyright 2021 Jorrit Rouwe
37+
38+
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
39+
the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
40+
the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
41+
42+
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
43+
44+
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
45+
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
46+
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.
47+
*/
48+
49+
#include "jolt_math_funcs.h"
50+
51+
void JoltMath::decompose(Basis &p_basis, Vector3 &r_scale) {
52+
const real_t sign = SIGN(p_basis.determinant());
53+
54+
Vector3 x = p_basis.get_column(Vector3::AXIS_X);
55+
Vector3 y = p_basis.get_column(Vector3::AXIS_Y);
56+
Vector3 z = p_basis.get_column(Vector3::AXIS_Z);
57+
58+
const real_t x_dot_x = x.dot(x);
59+
60+
y -= x * (y.dot(x) / x_dot_x);
61+
z -= x * (z.dot(x) / x_dot_x);
62+
63+
const real_t y_dot_y = y.dot(y);
64+
65+
z -= y * (z.dot(y) / y_dot_y);
66+
67+
const real_t z_dot_z = z.dot(z);
68+
69+
r_scale = sign * Vector3(Math::sqrt(x_dot_x), Math::sqrt(y_dot_y), Math::sqrt(z_dot_z));
70+
71+
p_basis.set_column(Vector3::AXIS_X, x / r_scale.x);
72+
p_basis.set_column(Vector3::AXIS_Y, y / r_scale.y);
73+
p_basis.set_column(Vector3::AXIS_Z, z / r_scale.z);
74+
}

‎modules/jolt_physics/misc/jolt_math_funcs.h

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
/**************************************************************************/
2+
/* jolt_math_funcs.h */
3+
/**************************************************************************/
4+
/* This file is part of: */
5+
/* GODOT ENGINE */
6+
/* https://godotengine.org */
7+
/**************************************************************************/
8+
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9+
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10+
/* */
11+
/* Permission is hereby granted, free of charge, to any person obtaining */
12+
/* a copy of this software and associated documentation files (the */
13+
/* "Software"), to deal in the Software without restriction, including */
14+
/* without limitation the rights to use, copy, modify, merge, publish, */
15+
/* distribute, sublicense, and/or sell copies of the Software, and to */
16+
/* permit persons to whom the Software is furnished to do so, subject to */
17+
/* the following conditions: */
18+
/* */
19+
/* The above copyright notice and this permission notice shall be */
20+
/* included in all copies or substantial portions of the Software. */
21+
/* */
22+
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23+
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24+
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25+
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26+
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27+
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28+
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29+
/**************************************************************************/
30+
31+
#ifndef JOLT_MATH_FUNCS_H
32+
#define JOLT_MATH_FUNCS_H
33+
34+
#include "core/math/transform_3d.h"
35+
36+
class JoltMath {
37+
public:
38+
static void decompose(Basis &p_basis, Vector3 &r_scale);
39+
40+
static _FORCE_INLINE_ void decompose(Transform3D &p_transform, Vector3 &r_scale) {
41+
decompose(p_transform.basis, r_scale);
42+
}
43+
44+
static _FORCE_INLINE_ void decomposed(const Basis &p_basis, Basis &r_new_basis, Vector3 &r_scale) {
45+
Basis new_basis = p_basis;
46+
decompose(new_basis, r_scale);
47+
r_new_basis = new_basis;
48+
}
49+
50+
static _FORCE_INLINE_ void decomposed(const Transform3D &p_transform, Transform3D &r_new_transform, Vector3 &r_scale) {
51+
Transform3D new_transform;
52+
decompose(new_transform, r_scale);
53+
r_new_transform = new_transform;
54+
}
55+
};
56+
57+
#endif // JOLT_MATH_FUNCS_H

‎modules/jolt_physics/objects/jolt_area_3d.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
#include "jolt_area_3d.h"
3232

3333
#include "../jolt_project_settings.h"
34+
#include "../misc/jolt_math_funcs.h"
3435
#include "../misc/jolt_type_conversions.h"
3536
#include "../shapes/jolt_shape_3d.h"
3637
#include "../spaces/jolt_broad_phase_layer.h"
@@ -370,16 +371,15 @@ void JoltArea3D::set_default_area(bool p_value) {
370371
void JoltArea3D::set_transform(Transform3D p_transform) {
371372
JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to area '%s'.", to_string()));
372373

373-
const Vector3 new_scale = p_transform.basis.get_scale();
374+
Vector3 new_scale;
375+
JoltMath::decompose(p_transform, new_scale);
374376

375377
// Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often.
376378
if (!scale.is_equal_approx(new_scale)) {
377379
scale = new_scale;
378380
_shapes_changed();
379381
}
380382

381-
p_transform.basis.orthonormalize();
382-
383383
if (!in_space()) {
384384
jolt_settings->mPosition = to_jolt_r(p_transform.origin);
385385
jolt_settings->mRotation = to_jolt(p_transform.basis);

‎modules/jolt_physics/objects/jolt_body_3d.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232

3333
#include "../joints/jolt_joint_3d.h"
3434
#include "../jolt_project_settings.h"
35+
#include "../misc/jolt_math_funcs.h"
3536
#include "../misc/jolt_type_conversions.h"
3637
#include "../shapes/jolt_shape_3d.h"
3738
#include "../spaces/jolt_broad_phase_layer.h"
@@ -543,16 +544,15 @@ JoltBody3D::~JoltBody3D() {
543544
void JoltBody3D::set_transform(Transform3D p_transform) {
544545
JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to physics body '%s'.", to_string()));
545546

546-
const Vector3 new_scale = p_transform.basis.get_scale();
547+
Vector3 new_scale;
548+
JoltMath::decompose(p_transform, new_scale);
547549

548550
// Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often.
549551
if (!scale.is_equal_approx(new_scale)) {
550552
scale = new_scale;
551553
_shapes_changed();
552554
}
553555

554-
p_transform.basis.orthonormalize();
555-
556556
if (!in_space()) {
557557
jolt_settings->mPosition = to_jolt_r(p_transform.origin);
558558
jolt_settings->mRotation = to_jolt(p_transform.basis);

‎modules/jolt_physics/objects/jolt_shaped_object_3d.cpp

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030

3131
#include "jolt_shaped_object_3d.h"
3232

33+
#include "../misc/jolt_math_funcs.h"
3334
#include "../misc/jolt_type_conversions.h"
3435
#include "../shapes/jolt_custom_double_sided_shape.h"
3536
#include "../shapes/jolt_shape_3d.h"
@@ -347,7 +348,10 @@ void JoltShapedObject3D::commit_shapes(bool p_optimize_compound) {
347348
void JoltShapedObject3D::add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled) {
348349
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()));
349350

350-
shapes.push_back(JoltShapeInstance3D(this, p_shape, p_transform.orthonormalized(), p_transform.basis.get_scale(), p_disabled));
351+
Vector3 shape_scale;
352+
JoltMath::decompose(p_transform, shape_scale);
353+
354+
shapes.push_back(JoltShapeInstance3D(this, p_shape, p_transform, shape_scale, p_disabled));
351355

352356
_shapes_changed();
353357
}
@@ -430,8 +434,8 @@ void JoltShapedObject3D::set_shape_transform(int p_index, Transform3D p_transfor
430434
ERR_FAIL_INDEX(p_index, (int)shapes.size());
431435
JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, "Failed to correctly set transform for shape at index %d in body '%s'.");
432436

433-
Vector3 new_scale = p_transform.basis.get_scale();
434-
p_transform.basis.orthonormalize();
437+
Vector3 new_scale;
438+
JoltMath::decompose(p_transform, new_scale);
435439

436440
JoltShapeInstance3D &shape = shapes[p_index];
437441

‎modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp

Lines changed: 16 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232

3333
#include "../jolt_physics_server_3d.h"
3434
#include "../jolt_project_settings.h"
35+
#include "../misc/jolt_math_funcs.h"
3536
#include "../misc/jolt_type_conversions.h"
3637
#include "../objects/jolt_area_3d.h"
3738
#include "../objects/jolt_body_3d.h"
@@ -286,17 +287,17 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_cast(const JoltBody3D &p_body,
286287
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
287288
const Transform3D transform_local = p_body.get_shape_transform_scaled(i);
288289
const Transform3D transform_com_local = transform_local.translated_local(com_scaled);
289-
Transform3D transform_com = body_transform * transform_com_local;
290+
const Transform3D transform_com = body_transform * transform_com_local;
290291

291-
Vector3 scale = transform_com.basis.get_scale();
292+
Transform3D transform_com_unscaled;
293+
Vector3 scale;
294+
JoltMath::decomposed(transform_com, transform_com_unscaled, scale);
292295
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.");
293296

294-
transform_com.basis.orthonormalize();
295-
296297
real_t shape_safe_fraction = 1.0;
297298
real_t shape_unsafe_fraction = 1.0;
298299

299-
collided |= _cast_motion_impl(*jolt_shape, transform_com, scale, p_motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries(), false, settings, motion_filter, motion_filter, motion_filter, motion_filter, shape_safe_fraction, shape_unsafe_fraction);
300+
collided |= _cast_motion_impl(*jolt_shape, transform_com_unscaled, scale, p_motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries(), false, settings, motion_filter, motion_filter, motion_filter, motion_filter, shape_safe_fraction, shape_unsafe_fraction);
300301

301302
r_safe_fraction = MIN(r_safe_fraction, shape_safe_fraction);
302303
r_unsafe_fraction = MIN(r_unsafe_fraction, shape_unsafe_fraction);
@@ -590,11 +591,10 @@ int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_para
590591
Transform3D transform = p_parameters.transform;
591592
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "intersect_shape was passed an invalid transform.");
592593

593-
Vector3 scale = transform.basis.get_scale();
594+
Vector3 scale;
595+
JoltMath::decompose(transform, scale);
594596
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "intersect_shape was passed an invalid transform.");
595597

596-
transform.basis.orthonormalize();
597-
598598
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
599599
const Transform3D transform_com = transform.translated_local(com_scaled);
600600

@@ -647,11 +647,10 @@ bool JoltPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_paramet
647647
Transform3D transform = p_parameters.transform;
648648
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
649649

650-
Vector3 scale = transform.basis.get_scale();
650+
Vector3 scale;
651+
JoltMath::decompose(transform, scale);
651652
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
652653

653-
transform.basis.orthonormalize();
654-
655654
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
656655
Transform3D transform_com = transform.translated_local(com_scaled);
657656

@@ -684,11 +683,10 @@ bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_param
684683
Transform3D transform = p_parameters.transform;
685684
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "collide_shape was passed an invalid transform.");
686685

687-
Vector3 scale = transform.basis.get_scale();
686+
Vector3 scale;
687+
JoltMath::decompose(transform, scale);
688688
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "collide_shape was passed an invalid transform.");
689689

690-
transform.basis.orthonormalize();
691-
692690
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
693691
const Transform3D transform_com = transform.translated_local(com_scaled);
694692

@@ -754,11 +752,10 @@ bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameter
754752
Transform3D transform = p_parameters.transform;
755753
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
756754

757-
Vector3 scale = transform.basis.get_scale();
755+
Vector3 scale;
756+
JoltMath::decompose(transform, scale);
758757
JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
759758

760-
transform.basis.orthonormalize();
761-
762759
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
763760
const Transform3D transform_com = transform.translated_local(com_scaled);
764761

@@ -890,8 +887,8 @@ bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, c
890887
Transform3D transform = p_parameters.from;
891888
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()));
892889

893-
Vector3 scale = transform.basis.get_scale();
894-
transform.basis.orthonormalize();
890+
Vector3 scale;
891+
JoltMath::decompose(transform, scale);
895892

896893
space->try_optimize();
897894

0 commit comments

Comments
 (0)