Rotation3d.interpolate lerps incorrectly

[TheComputer314]

Describe the bug
When Rotation3d.interpolate(Rotation3d, double) is called with t = 1, the output is nonsensical instead of being identical to the lerp endpoint rotation.

To Reproduce

var a = new Rotation3d(1,0,0);
var b = new Rotation3d(0,1,0);
var lerp = a.interpolate(b, 1);
System.out.println(a.toString());
System.out.println(b.toString());
System.out.println(lerp.toString());

Expected console output:

Rotation3d(Quaternion(0.8775825618903728, 0.479425538604203, 0.0, 0.0))
Rotation3d(Quaternion(0.8775825618903728, 0.0, 0.479425538604203, 0.0))
Rotation3d(Quaternion(0.8775825618903728, 0.0, 0.479425538604203, 0.0))

Actual console output:

Rotation3d(Quaternion(0.8775825618903728, 0.479425538604203, 0.0, 0.0))
Rotation3d(Quaternion(0.8775825618903728, 0.0, 0.479425538604203, 0.0))
Rotation3d(Quaternion(0.8775825618903728, 1.1102230246251568E-16, 0.2590347239999257, -0.40342268011133486))

Quaternion visualization for b:

Quaternion visualization for lerp: (Should be identical to b but isn't)

Additional context
Originally discovered by truher on CD. Credit for the above visualizations go to them.

[Oblarg]

looks like there's a nonsensical attempt at a lerp instead of a proper slerp in the java Rotation3d implementation

from a cursory inspection, the current implementation of times should be moved into interpolate, as it is the correct general formula, and then times should delegate to this. the lerp-formula-like expression is wrong, and should be removed.

[calcmogul]

Nonsensical how? If you have rotations r1 and r2, you should be able to lerp between them by doing r1 + (r2 - r1) * t. It finds the rotation between r1 and r2, scales it, then applies the result to r1.

[Oblarg]

Okay, the inversion in composition here is actually just contributing to this being obscure, rather than the cause of the bug - the real issue is that minus multiplies in the wrong order in its implementation. this commutes when the axis of the two rotations is the same, but not in general.

[calcmogul]

Which minus? There's Quaternion.minus() and Rotation3d.minus(). Neither of those do any multiplication directly. Are you referring to Rotation3d.rotateBy() instead?

[Oblarg]

Rotation3d.minus calls times with the wrong order of operands when performing the inversion. The architecture that preserves the linear metaphor in the call chain makes this sort of bug way more likely to write and harder to catch; you should still probably do what I originally said, move the SLERP implementation to the interpolate method, and rejig everything to delegate to that.

[calcmogul]

That's confusing for a different reason though. Addition and subtraction of rotations isn't interpolation.

[truher]

I agree that there's something wrong with minus().

The implementation for interpolate I would expect here is

public Rotation3d interpolate(Rotation3d b, double t) {
    return new Rotation3d(getQuaternion().exp(getQuaternion().log(b.getQuaternion()).times(t)));
}

does that seem right?

[calcmogul]

We aren't currently using Quaternion exp() and log() here because it's more expensive than would otherwise be necessary, since it's designed to work for more than just unit quaternions.

[truher]

oh ok, well, that makes sense. I can confirm, though, that exp(log()*t) does work. :-)

[truher]

Also, when i say that minus() is at issue, I really just mean that it's not the inverse of plus(). I havent looked into which one is correct.

[bhall-ctre]

By what seems to be traditional convention (after conversing in the FRC Discord), we should have q_transition * q_initial = q_final, which means that q_transition = q_final * q_initial^-1 (remember multiplication is not commutative). The order of operations there is flipped from minus(), which currently returns q_initial^-1 * q_final.