Skip to content

Commit 62e8b1e

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

File tree

6 files changed

+155
-26
lines changed

6 files changed

+155
-26
lines changed

‎modules/jolt_physics/misc/jolt_math_funcs.cpp‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
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+
Vector3 x = p_basis.get_column(Vector3::AXIS_X);
53+
Vector3 y = p_basis.get_column(Vector3::AXIS_Y);
54+
Vector3 z = p_basis.get_column(Vector3::AXIS_Z);
55+
56+
const real_t x_dot_x = x.dot(x);
57+
y -= x * (y.dot(x) / x_dot_x);
58+
z -= x * (z.dot(x) / x_dot_x);
59+
const real_t y_dot_y = y.dot(y);
60+
z -= y * (z.dot(y) / y_dot_y);
61+
const real_t z_dot_z = z.dot(z);
62+
63+
const real_t det = x.cross(y).dot(z);
64+
65+
r_scale = SIGN(det) * Vector3(Math::sqrt(x_dot_x), Math::sqrt(y_dot_y), Math::sqrt(z_dot_z));
66+
67+
p_basis.set_column(Vector3::AXIS_X, x / r_scale.x);
68+
p_basis.set_column(Vector3::AXIS_Y, y / r_scale.y);
69+
p_basis.set_column(Vector3::AXIS_Z, z / r_scale.z);
70+
}

‎modules/jolt_physics/misc/jolt_math_funcs.h‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
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+
// Note that `p_basis` is mutated to be right-handed orthonormal.
39+
static void decompose(Basis &p_basis, Vector3 &r_scale);
40+
41+
// Note that `p_transform` is mutated to be right-handed orthonormal.
42+
static _FORCE_INLINE_ void decompose(Transform3D &p_transform, Vector3 &r_scale) {
43+
decompose(p_transform.basis, r_scale);
44+
}
45+
46+
static _FORCE_INLINE_ void decomposed(const Basis &p_basis, Basis &r_new_basis, Vector3 &r_scale) {
47+
Basis new_basis = p_basis;
48+
decompose(new_basis, r_scale);
49+
r_new_basis = new_basis;
50+
}
51+
52+
static _FORCE_INLINE_ void decomposed(const Transform3D &p_transform, Transform3D &r_new_transform, Vector3 &r_scale) {
53+
Transform3D new_transform;
54+
decompose(new_transform, r_scale);
55+
r_new_transform = new_transform;
56+
}
57+
};
58+
59+
#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: 13 additions & 17 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"
@@ -288,11 +289,10 @@ bool JoltPhysicsDirectSpaceState3D::_body_motion_cast(const JoltBody3D &p_body,
288289
const Transform3D transform_com_local = transform_local.translated_local(com_scaled);
289290
Transform3D transform_com = body_transform * transform_com_local;
290291

291-
Vector3 scale = transform_com.basis.get_scale();
292+
Vector3 scale;
293+
JoltMath::decompose(transform_com, scale);
292294
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.");
293295

294-
transform_com.basis.orthonormalize();
295-
296296
real_t shape_safe_fraction = 1.0;
297297
real_t shape_unsafe_fraction = 1.0;
298298

@@ -590,11 +590,10 @@ int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_para
590590
Transform3D transform = p_parameters.transform;
591591
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "intersect_shape was passed an invalid transform.");
592592

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

596-
transform.basis.orthonormalize();
597-
598597
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
599598
const Transform3D transform_com = transform.translated_local(com_scaled);
600599

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

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

653-
transform.basis.orthonormalize();
654-
655653
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
656654
Transform3D transform_com = transform.translated_local(com_scaled);
657655

@@ -684,11 +682,10 @@ bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_param
684682
Transform3D transform = p_parameters.transform;
685683
JOLT_ENSURE_SCALE_NOT_ZERO(transform, "collide_shape was passed an invalid transform.");
686684

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

690-
transform.basis.orthonormalize();
691-
692689
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
693690
const Transform3D transform_com = transform.translated_local(com_scaled);
694691

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

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

760-
transform.basis.orthonormalize();
761-
762758
const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
763759
const Transform3D transform_com = transform.translated_local(com_scaled);
764760

@@ -890,8 +886,8 @@ bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, c
890886
Transform3D transform = p_parameters.from;
891887
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()));
892888

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

896892
space->try_optimize();
897893

0 commit comments

Comments
 (0)