[wpimath] Implement Rotation3d interpolation as slerp instead of lerp

[calcmogul]

Also replace arithmetic operators since they're not commutative, which is confusing for users.

[KangarooKoala]

We'll see what the tests say, but I expect that a - b needs to be transformed into b.RelativeTo(a), not a.RelativeTo(b).

[calcmogul]

Why's that? a - b was intended to be equivalent to a.RelativeTo(b).

[KangarooKoala]

My bad, I misspoke- The relative order of a and b is fine, but I expect that a.RelativeTo(b) will need to be applied to other rotations in a different order than a - b was. (The old operator- was such that (a - b) + b == a. In contrast, the new a.RelativeTo(b) is such that b.RotateBy(a.RelativeTo(b)) == a (b + a.RelativeTo(b) == a), so the order you apply them to other rotations needs to be swapped.)

[KangarooKoala]

Suggested change

	// We flip the argument order if the quaternion dot product is less than
	// zero to ensure the shortest path is always taken.
	if (m_q.Dot(endValue.m_q) >= 0.0) {
	return Rotation3d{(endValue.m_q * m_q.Inverse()).Pow(t) * m_q};
	} else {
	return Rotation3d{(m_q * endValue.m_q.Inverse()).Pow(t) * endValue.m_q};
	}
	// We negate the delta if the delta's W is negative to ensure the shortest path is always taken.
	Quaternion delta = endValue.m_q * m_q.Inverse();
	if (delta.W() < 0.0) {
	delta = Quaternion{0, 0, 0, 0} - delta;
	}
	return Rotation3d{delta.Pow(t) * m_q};

From https://en.wikipedia.org/wiki/Slerp#Quaternion_slerp:

However, because the covering is double (q and −q map to the same rotation), the rotation path may turn either the "short way" (less than 180°) or the "long way" (more than 180°). Long paths can be prevented by negating one end if the dot product, cos Ω, is negative, thus ensuring that −90° ≤ Ω ≤ 90°.

Flipping the argument order just changes the direction along the same arc. In order to change whether we travel the major arc or the minor arc, we need to negate the components of the quaternion. (For example, instead of -1/2 + √3/2 î (2 cos⁻¹(-1/2) = 240° rotation around the +X axis), use 1/2 - √3/2 î (2 cos⁻¹(1/2) = 120° rotation around the -X axis).)

[calcmogul]

I wish these Wikipedia articles made their intent clearer. Ω wasn't explicitly defined to be q₁q₀⁻¹.

[calcmogul]

I don't think using the delta quaternion there is correct either. They said to negate one end, not both. Again, this article is clear as mud.

[KangarooKoala]

Hm... Negating endValue.m_q is equivalent to negating delta, though, right? (And negating m_q does the same exact with an extra negation on the top level, which doesn't affect the actual representing rotation.)
Agreed that my initial explanation wasn't quite right (and also that the article is not very clear), but I think the implementation is still okay (though we should probably update the comments).