Again I have played with magicks beyond my understanding.
I have a unique problem: I'm trying to interface three programs that no one has ever tried, in an effort to make a cheap VR controller.
I am using PSMoveServiceEX (https://github.com/Timocop/PSMoveServiceEx) which gets position and orientation data from Playstation Move Controllers. I then use PSMoveFreePIEBridge (https://github.com/HipsterSloth/PSMoveFreepieBridge) to transfer position and orientation data to FreePIE (https://github.com/AndersMalmgren/FreePIE) I then map various buttons and axes, as well as manipulate the orientation data. That data gets fed to the Razer Hydra plugin, which I'm using SteamVR Driver for Razer Hydra to feed the data into SteamVR.
And to top it all off, the pitch of the controller when seen in SteamVR is off by +45 degrees.
I have observed the output from PSMoveServiceEX as described in its orientation tests as "altitude", "heading, and "bank". These are obviously Pitch, Yaw, and Roll. However, the output of the bank/roll and is unique, as you rotate the controller clockwise from 0-90 degrees, the value goes from 0 to -90. However, as you continue from 90-180 degrees roll, the value decreases back to zero! Yaw/Heading and Pitch/Altitude varies +/- 180 degrees as expected. The 3d representation of the controller in the config utility is accurately portrayed in the window. However, with my current FreePIE script, I can observe some weirdness when the roll gets around +/-90 degrees.
I have deduced from Googling and barely understanding Wikipedia articles that what PSMoveServiceEX is outputting are "Tait-Bryan Angles", a convention commonly used by aircraft. https://en.wikipedia.org/wiki/Euler_angles#Tait%E2%80%93Bryan_angles
I also believe what SteamVR is expecting are "Euler angles" https://en.wikipedia.org/wiki/Euler_angles Although there is a good possibility I could be wrong.
So the solution is obvious: We convert Tait-Bryan angles to Euler angles.
So some more Googling and bad Wikipedia reading tells me there are two possible ways:
Tait-Bryan -> Quaternion -> Euler
Tait-Bryan -> Rotation Matrix -> Euler
Hoo boy. Some serious maths here.
Couldn't find an easily translatable example of Tait-Bryan Angles to Quaternation. All I was able to find was Tait-Bryan to Rotation Matrix, then Rotation Matrix to Euler Angles on Wikipedia. At least we aren't doing Matrix multiplication...yet...
Edit 2: Ok, so we copied Tait-Bryan Angles to Rotation matrix from Wikipedia (ZXY) https://en.wikipedia.org/wiki/Euler_angles#Tait%E2%80%93Bryan_angles and Rotation Matrix to Euler-Angles ZXZ: So FreePIE uses Python for its scripts, and so I wrote the following:
def tait_to_rmatrix(head,alt,bank):
c1 = math.cos(head)
c2 = math.cos(alt)
c3 = math.cos(bank)
s1 = math.sin(head)
s2 = math.sin(alt)
s3 = math.sin(bank)
R = [(c1*c3)-(s1*s2*s3) , -(c2*s1) , (c1*s3)+(c3*s1*s2) , (c3*s1)+(c1*s2*s3) , c1*c2 , (s1*s3)-(c1*c3*s2) , -(c2*s3) , s2 , c2*c3]
return R
def rmatrix_to_euler(R):
yaw = math.atan(R[2]/-R[5])
pitch = math.atan(math.sqrt(1-(R[8]*R[8]))/R[8])
roll = math.atan(R[6]/R[7])
return [yaw,pitch,roll]
RmatrixR = tait_to_rmatrix(-freePieIO[idR].yaw,(freePieIO[idR].pitch+45),freePieIO[idR].roll)
eulerRout = rmatrix_to_euler(RmatrixR)
diagnostics.watch(math.degrees(eulerRout[0]))
diagnostics.watch(math.degrees(eulerRout[1]))
diagnostics.watch(math.degrees(eulerRout[2]))
hydra[indexR].x = (freePieIO[idR].x * 10)
hydra[indexR].y = (freePieIO[idR].y * 10)
hydra[indexR].z = (freePieIO[idR].z * 10)
hydra[indexR].yaw = eulerRout[0]
hydra[indexR].pitch = eulerRout[1]
hydra[indexR].roll = eulerRout[2]
Edit: I found the problem, but unfortunately I don't know how to solve it: Here's the offending code giving me the odd roll outputs:
freepie_io_6dof_data poseData;
PSMQuatf normalizedQuat = PSM_QuatfNormalizeWithDefault(&controllerPose.Orientation, k_psm_quaternion_identity);
//glm::quat glmOrientation = glm::quat(normalizedQuat.w, normalizedQuat.x, normalizedQuat.y, normalizedQuat.z);
poseData.x = controllerPose.Position.x;
poseData.y = controllerPose.Position.y;
poseData.z = controllerPose.Position.z;
//data.pitch = glm::pitch(glmOrientation);
//data.yaw = glm::yaw(glmOrientation);
//data.roll = glm::roll(glmOrientation);
//Calcuate rotation here, glm library doesn't work for yaw
//Both glm and this seem to work fine when each axis is independent, but issues when combined.
poseData.yaw = std::atan2(2 * normalizedQuat.y * normalizedQuat.w - 2 * normalizedQuat.x * normalizedQuat.z, 1 - 2 * normalizedQuat.y * normalizedQuat.y - 2 * normalizedQuat.z * normalizedQuat.z);
poseData.roll = std::asin(2 * normalizedQuat.x * normalizedQuat.y + 2 * normalizedQuat.z * normalizedQuat.w);
poseData.pitch = std::atan2(2 * normalizedQuat.x * normalizedQuat.w - 2 * normalizedQuat.y * normalizedQuat.z, 1 - 2 * normalizedQuat.x * normalizedQuat.x - 2 * normalizedQuat.z * normalizedQuat.z);
WriteToFreepie(poseData, trackedFreepieIndicies[i]);
As we can see here, the roll is using an asin() function where pitch and yaw are using atan2().
"Surely its not that hard to fork the code and make the changes myself?"
Wow, dev environments are about obtuse as software gets. Its going to take me forever to learn how to just get the existing code to build, let alone make changes.
And to top it all off, even if I COULD edit the code to fix the rotations, its still going to suffer from the phenomenon of "gimbal lock". So I'd have to get a quaternion out of PSMoveFreePIEBridge. Then that leaves one problem that I probably will never figure out, the FreePIE Razer Hydra plugin can only READ quaternions from a real Razer Hydra controller, it can't WRITE them to SteamVR.
I suppose the best I could do would be to write a cludge in the script to filter out the extreme values when I get close to gimbal lock.
sigh So close, and yet so far.