Fix broken negative scaling when using Jolt Physics

[mihe]

Fixes #103408.
Supersedes #103426.

This PR effectively just brings back some code (derived in part from Jolt's own JPH::Mat44::Decompose) that I had left behind when porting the Jolt integration from the Godot Jolt extension in #99895, due to wanting to keep things as minimal as possible.

What this code is meant to do is take a potentially scaled Basis (or Transform3D) and decompose it into a rotation part and a scale part, which must be done before passing any transform into Jolt, as Jolt takes scale separately from the rest of the transform in its API, and sometimes also takes rotation separately, in the form of a quaternion.

I had mistaken the following code:

Vector3 scale = some_basis.get_scale();
Basis rotation = some_basis.orthonormalized();

... as being a valid substitute for this code:

Basis rotation;
Vector3 scale;
decomposed(some_basis, rotation, scale);

... which it is not, because while Basis::orthonormalize does also use the Gram-Schmidt process to perform the orthonormalization, much like this "new" decompose function, it does not handle reflection (i.e. negative scaling) in any way, meaning we can end up with a Basis that is orthonormal, but is not in fact a valid (right-handed) rotation, which causes problems with things like Basis::get_quaternion, as it assumes a valid (right-handed) rotation.

This "new" decompose function solves this by essentially just flipping all the axes of the Basis when we find ourselves with a negative determinant (i.e. negative scaling) which seems to line up with the convention used by other Basis methods, like Basis::get_scale and Basis::get_rotation_quaternion.

It also serves as a minor optimization, since we can avoid some of the redundant vector normalizations that are shared between Basis::get_scale and Basis::orthonormalize.

[mihe]

I'd love for someone more mathematically inclined than myself to check that I'm not talking too much rubbish here, like perhaps @jrouwe or @rburing (or anyone else who feels up to it).

[fire]

In godot engine the common conversion from Basis to quaternion is get_rotation_quaternion. I don't know why we're doing it this way.

[fire]

Please explain why we can't use get_rotation_quaternion or orthogonalize?

https://github.com/godotengine/godot/blob/master/core/math/basis.cpp#L80

[mihe]

@fire Sorry, I wasn't very clear in the PR description, but as mentioned in the issue, in a lot of these cases we are not in fact converting our Basis to a Quaternion, and instead are just making our Basis right-handed orthonormal, while extracting the scale in the process, since Jolt expects to be passed a transform (and scale separately) in these cases. So Basis::get_rotation_quaternion has no relevance in these cases, unless we want to go through the trouble of converting it back to a Basis.

I must admit I'm not sure if the handedness is strictly important to resolve before passing a transform to Jolt though, but seeing as how the extracted (potentially negative) scale that goes along with it is what's meant to represent any reflection, the extraction of which seems to be some matter of convention (flipping one versus three axes) I assume we should be stripping said reflection from the transforms as well? Perhaps @jrouwe can shed some light on whether not doing so would cause any problems in Jolt, or trigger some assertion somewhere. Either way, Basis::orthogonalize is not relevant, since it doesn't normalize, and doesn't give us the scale.

As far as I can see, if we wanted to only make use of existing Basis methods, this is what this new decompose function would look like:

void decompose(Basis &p_basis, Vector3 &r_scale) {
    // Extract the scale.
    r_scale = p_basis.get_scale();

    // Orthogonalize and normalize.
    p_basis.orthonormalize();

    // Ensure right-handed rotation.
    real_t det = p_basis.determinant();
    if (det < 0) {
        p_basis.scale(Vector3(-1, -1, -1));
    }
}

I'd be fine with merging this as well, but there is some amount of wasted computation compared to what's in the PR currently.

Also, note that since this decompose function would be applied to any transform passed into the Jolt physics server, the additional stuff done in Basis::get_rotation_quaternion, when compared to Basis::get_quaternion, is entirely redundant. Frankly the normalization of the quaternion currently happening in JPH::Quat to_jolt(const Basis &p_basis) might be redundant as well.

[fire]

Add decompose (decompose_to_scale) to Basis would be a valid fix too in my opinion.

I'll have to think about this more and ask a few friends.

We want this Basis::Decompose to do three things.

1. extract scale
2. remove scale by orthonormalize
3. extract a pure rotation as a basis

That seems correct.

The various methods should be unified in Basis in my opinion.

Edited:

I am not sure forcing right-handed rotation is correct. It feels wrong but I don't have an argument so...

[mihe]

The various methods should be unified in Basis in my opinion.

There might very well be some argument for adding/changing some stuff in Basis, but since this need seems somewhat specific to the Jolt integration at the moment, I'd prefer not to complicate this fix with core changes.

I am not sure forcing right-handed rotation is correct. It feels wrong but I don't have an argument so...

In case you haven't seen it, this is what Basis::get_rotation_quaternion does already:

godot/core/math/basis.cpp

Lines 406 to 410 in 9f68a81

	real_t det = m.determinant();
	if (det < 0) {
	// Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
	m.scale(Vector3(-1, -1, -1));
	}

And is what's being asserted in Basis::is_rotation:

godot/core/math/basis.cpp

Lines 128 to 131 in 9f68a81

	// Returns true if the basis is a pure rotation matrix, so it has no scale, skew, shear, or flip.
	bool Basis::is_rotation() const {
	return is_conformal() && Math::is_equal_approx(determinant(), 1, (real_t)UNIT_EPSILON);
	}

I'm frankly out of my depth here as well, but from what I understand it is in fact not "correct" to do this, because there is no one unique way of decomposing a negatively scaled matrix into rotation and scale, so you're forced to pick one arbitrarily.

There's some reasoning for the way Godot does it here (which goes over my head a bit) as well as a mention of it here.

[fire]

Copying

godot/core/math/basis.cpp

Lines 406 to 410 in 9f68a81

	real_t det = m.determinant();
	if (det < 0) {
	// Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
	m.scale(Vector3(-1, -1, -1));
	}

is a pretty convincing argument for your change.

[jrouwe]

Perhaps @jrouwe can shed some light on whether not doing so would cause any problems in Jolt, or trigger some assertion somewhere.

I've never tested this but I'm pretty sure that if you pass in a left-handed matrix into Jolt that everything will explode.

[jrouwe]

I'm frankly out of my depth here as well, but from what I understand it is in fact not "correct" to do this, because there is no one unique way of decomposing a negatively scaled matrix into rotation and scale, so you're forced to pick one arbitrarily.

I think there is, but you have to make sure that the sign of the scale that you 'extract' matches the flipping that you do to the rotation part. E.g. the code above doesn't do that:

void decompose(Basis &p_basis, Vector3 &r_scale) {
    // Extract the scale.
    r_scale = p_basis.get_scale();

    // Orthogonalize and normalize.
    p_basis.orthonormalize();

    // Ensure right-handed rotation.
    real_t det = p_basis.determinant();
    if (det < 0) {
        p_basis.scale(Vector3(-1, -1, -1));
    }
}

If you decide to scale p_basis you have to apply the same operation to r_scale.

In an equation:

M = R * S

Where M is p_basis at the start of the function, R is p_basis after orthonormalization and S a diagonal matrix r_scale. Note that I haven't checked the implementations of get_scale and orthonormalize to see if multiplying those two will indeed result in getting the original matrix back (if there's no shearing). I'm assuming they do so that the equation above holds.

When the determinant is negative, you're doing:

M = (R * F) * S

where F is the diagonal matrix [-1, -1, -1], so in order to keep the matrix the same you need to do:

M = (R * F) * (F^-1 * S)

And since F^-1 = F this means you need to multiply your r_scale with [-1, -1, -1] too.

Since this code can be called quite often, I would opt for not wasting CPU cycles by doing duplicate work in get_scale and orthonormalize. I would go with the code that is currently in the PR (this also frees us from having to verify the assumption on the working of get_scale and orthonormalize).

[mihe]

I think there is, but you have to make sure that the sign of the scale that you 'extract' matches the flipping that you do to the rotation part.

Right, yeah, I get that it works out so long as you stick to whatever convention you pick. I was more referring to neither convention (of negating one versus three scales/axes) necessarily being more correct than the other (if that's even true).

E.g. the code above doesn't do that

Basis::get_scale is implemented like so:

godot/core/math/basis.cpp

Lines 323 to 324 in 9f68a81

	real_t det_sign = SIGN(determinant());
	return det_sign * get_scale_abs();

Where Basis::get_scale_abs is just:

godot/core/math/basis.cpp

Lines 289 to 294 in 9f68a81

	Vector3 Basis::get_scale_abs() const {
	return Vector3(
	Vector3(rows[0][0], rows[1][0], rows[2][0]).length(),
	Vector3(rows[0][1], rows[1][1], rows[2][1]).length(),
	Vector3(rows[0][2], rows[1][2], rows[2][2]).length());
	}

So everything should be using the same convention in the code I suggested (I think), but clearly it could've used some comments to make that assumption explicit.

Since this code can be called quite often, I would opt for not wasting CPU cycles by doing duplicate work in get_scale and orthonormalize.

My vote is also (still) for keeping this PR as-is, since this is a somewhat hot code path.

[Repiteo]

We'll roll with this as-is, I think the reasons given are adequate.

Thanks!

[akien-mga]

Cherry-picked for 4.4.1.