Fix real part of quaternion derivatives

[unpairedbracket]

Objective

The first-order approximations to quaternion rotations were wrong in a couple of places - rather than the angular velocity * dt/2 quaternions being purely imaginary they were being given a real part related to the real part of the existing quaternion. I think this got mis-translated in commit c1bcff1 from

let q = Quaternion::from_vec4(ang_vel.0.extend(0.0)) * rot.0;
let effective_dq = locked_axes
    .apply_to_angular_velocity(delta_secs * 0.5 * q.xyz())
    .extend(delta_secs * 0.5 * q.w);
// avoid triggering bevy's change detection unnecessarily
let delta = Quaternion::from_vec4(effective_dq);
if delta != Quaternion::from_xyzw(0.0, 0.0, 0.0, 0.0) {
    rot.0 = (rot.0 + delta).normalize();
}

to

let delta_rot = Quaternion::from_vec4(
    (ang_vel * delta_seconds / 2.0).extend(rot.w * delta_seconds / 2.0),
) * rot.0;
if delta_rot.w != 0.0 && delta_rot.is_finite() {
    rot.0 = (rot.0 + delta_rot).normalize();
}

(Note in the "before" case, the .extend(delta_secs * 0.5 * q.w) is taking the w component from the rotation defined on the previous line (i.e. the 0.0 it's been extended by), not from rot!)

There are a few other places that do this quaternion maths: in the collider backend and the angular and positional constraints, and those all still extend by 0.0

Solution

• Change the two cases which extend by non-zero amounts to extend by zero. I've also rewritten the check to correctly identify zero angular velocity and not update the rotation in that case.

[Jondolf]

Thanks, good catch! This changes behavior slightly, so I just updated the Insta snapshots (currently used for cross-platform determinism tests).

There's still one more failing test however:

---- dynamics::integrator::semi_implicit_euler::tests::semi_implicit_euler stdout ----
thread 'dynamics::integrator::semi_implicit_euler::tests::semi_implicit_euler' panicked at crates\avian3d\../../src\dynamics\integrator\semi_implicit_euler.rs:287:9:
assert_relative_eq!(rotation.0, Quaternion::from_rotation_z(20.0), epsilon = 0.01)

    left  = DQuat(0.0, 0.0, -0.5159251557023111, -0.8566336636588282)
    right = DQuat(0.0, 0.0, -0.5440211108893698, -0.8390715290764524)

The error from the expected value seems somewhat large 🤔 The fix in this PR looks correct to me though

[unpairedbracket]

Ah, that result actually makes sense!

This ended up long. TL;DR changing rot = (rot + Quaternion::from_vec4((scaled_axis / 2.0).extend(0.0)) * rot).normalize(); to rot = Quaternion::from_scaled_axis(scaled_axis) * rot; should make the rotations much more accurate and make that test pass again.

It's a result of the expression q -> (q + (ωt/2) q).normalize(), which is a first-order Taylor expansion (followed by re-normalising the quaternion) of the true quaternion update rule, q -> exp(ωt/2) * q (analogous to the 2d R -> exp(iωt) R, but with quaternions in place of complex numbers).

The expression being used is equivalent to pre-multiplying with

$$\frac{I + (\mathbf{ω}t/2)}{ | I + (\mathbf{ω}t/2)|}$$

which in components is

$$\frac{[1, \mathbf{ω} t/2]}{\sqrt{1 + \frac{|ω|^2 t^2}{4}}}$$

For an intended rotation angle $ωt$, the w component of the resulting rotation (which is the cosine of half the actual rotation angle) is $\sqrt{1 + \frac{|ω|^2 t^2}{4}}^{-1}$, and by doing some trig that's equal to $\cos{\arctan{ωt/2}}$. That implies that the actual rotation angle being realised is $2 \arctan{ωt/2}$, effectively limiting actual rotation rates to π per (sub)step, and reducing the angle being rotated in the test from 20 radians to 100 steps * 2 * arctan( (ω=2) * (t=0.1) / 2) = 19.9337. Sure enough, cos(19.9337 / 2) is -0.856634 and sin(19.9337 / 2) is -0.515925, as seen in the failing test.

The accuracy of the rotation can be improved, taking advantage of the fact that the quaternion exponential itself isn't super hard to compute. Using Quaternion::from_scaled_axis(scaled_axis) (no dividing by 2 needed) should give the accurate rotation quaternion, which was previously being approximated to first order.

let scaled_axis = ang_vel * delta_seconds;
if scaled_axis != AngularVelocity::ZERO.0 && scaled_axis.is_finite() {
    let delta_rot = Quaternion::from_vec4((scaled_axis / 2.0).extend(0.0)) * rot.0;
    rot.0 = (rot.0 + delta_rot).normalize();
}

changes to

let scaled_axis = ang_vel * delta_seconds;
if scaled_axis != AngularVelocity::ZERO.0 && scaled_axis.is_finite() {
    let delta_rot = Quaternion::from_scaled_axis(scaled_axis);
    rot.0 = delta_rot * rot.0;
}

And now things should rotate more accurately. In my testing the test that was failing at epsilon=0.01 is now good even down to epsilon=1e-12

I initially didn't suggest this change here because I thought there might be a significant performance hit (was intending to ask if you were interested later on), but I've been playing with tracy this past week and it didn't look like there was much difference, if any. Both versions involve a single square root (the current version needs one in the normalise, my proposed change in separating the rotation axis from the rotation angle); the new method additionally computes a sin_cos of the angle for the quaternion exponentiation.

If that all sounds good to you, I'll commit and push some changes to that effect!

[Jondolf]

I think that sounds good! The more accurate version is probably better if there is no meaningful performance hit. It also seems to be what Rapier uses.

This does remove the normalization though, which might lead to error accumulating over time. It might be worth considering a fast renormalization afterwards, based on a first-order Taylor approximation, like Nalgebra's fast_renormalize. Rapier also uses it after the rotation integration (see here).

[unpairedbracket]

Well that was a bit of a mess. Helped once I remembered I can run tests locally to check if they'll work in CI 🤡

I've gone through and changed all the rotation updates I could find to use the accurate multiplication method, and do an approximate renormalise afterwards (I think several of the renormalisations might not be necessary, where there's no accumulation of changes across multiple time-steps, but figured I'd use it everywhere just to be safe)

Wasn't quite sure where'd be the best place to implement the approximate renormalise function - a (self) -> Self method on Quaternion probably would have made most sense, but tricky given Quaternion belongs to glam, not avian. I went with a fn renormalize(&mut self) on Rotation, but happy to change to some other signature or anything else if preferred.

[Jondolf]

Thanks, looks good now!

I'm not entirely sure if the accurate rotation update is really necessary, since the approximate linearized formulas should be pretty accurate for smaller individual rotations resulting from substepping, but it's probably still useful if there is no significant performance impact.

In the future, it might be worth trying out if we could use the linearized formulas by default and only fall back to the accurate version if the angle change is large enough that the approximation would exceed some error threshold. Not sure if it would be worth the branching though, so this would need benchmarking.