Replace quaternion to twist with new algorithm

[aacuevas]

The new algorithm is intended to work better on any orientation.

With this change, the RotationAxis property properly defines which axis on the IMU, from its physical representation, is to be used for turning computations.

[jonnew]

I stepped through the logic. Its hard for me to think about to be honest but I think i understand what this is doing. Here is my commented code

// 1. Calculate the incremental rotation
var conjugate = Quaternion.Conjugate(last); // Flips direction of axis of rotation
var delta = current * conjugate; // By multiplying by the conjugate of last, we obtain the quaternion describing the difference between between current and last instead of addition, which is current * last.  This is now in the global reference frame.

// 2. Rotate RotationAxis to the last known global coordinates
var axis = new Quaternion(RotationAxis, 0); // This axis is currently local to quaternion reference frame
var projection = (last * axis) * conjugate; // Use the last quaterion measurement to project that axis into global reference frame coordinates

// 3. Get how much the new rotation is performed through the last axis projected in global coordinates
var deltaV = new Vector3(delta.X, delta.Y, delta.Z);
var projectionV = new Vector3(projection.X, projection.Y, projection.Z);
var dotProduct = Vector3.Dot(deltaV, projectionV); // Figure out "how much" of incremental rotation delta.W is about the axis of interest e.g. if the detlta vector is perfectly aligned with the global axis of interest, then all. If orthogonal then none and if flipped then negative all.
twist = 2 * Math.Atan2(dotProduct, delta.W); // phase of complex number dotproduct + i * delta.W. So if dot product is 1, and delta.W is 0.5, then 90 deg.

• So the first step performs a subtraction, but just using Quaternion maths instead of breaking things into vector components
• The second step defines an axis of interest locally. This would normally correspond to a vector described by the tether itself. It then uses the last measurement to project this into global coordinates
• The last step just figures out how much of delta is actually about the axis of interest and applies that.

In the end, though @cjsha's testing, we have confirmed this works very well for when the headstage rogations are about the tether axis. We need to confirm it deals well with the orthoganal and flipped case.

[jonnew]

My reviews are in the comments and are really just about understanding the algorithm. @cjsha needs to finish his testing with the headstage at 90 deg. and upside down before we fully merge.

[aacuevas]

Your comments are on point.

I would just add in
var axis = new Quaternion(RotationAxis, 0); // This axis is currently local to quaternion reference frame
an e.g.: The IMU reference frame so it's more clear when we read this again in the future.

I will add these comments to the PR

[aacuevas]

@jonnew @cjsha I have updated the algorithm to account for the cases where the headstage is not completely upright. In theory, it should now follow the headstage in most cases, included when it's tilted 90 degrees but going around the commutator axis.

To do this, it now calculates two rotations:
1- The rotation around the tether (as it did before)
2- The rotation around the commutator axis

The second rotation is added weighted to the relationship between the tether vector in global coordinates and the commutator vector. For example, when the headstage is completely upright, these two vectors are the same, so the second rotation is ignored. When they are 90º they are independent and fully summed. Anything in between gets weighted.

Please use this algorithm for further testing.