But i've ran into a problem, i'm getting some weird behavior at some orientations, some of the axes jump between values in a way i didn't expect, including axes i thought would remain relatively stable even near the pole.
The basic idea for this "monopole rotation representation", is sorta like this: you take the surface of a sphere and flatten it into a disc (obviously to do that for real would involve non-euclidean space), pitch is measured up and down, and yaw horizontally, with zero rotation at the center, and 180 at the pole, the edge of the circle. And the roll is calculated in a way that sorta make it so that with zero roll the orientation is sorta like what your head spontaneously does when looking at the direction indicated by the yaw and pitch (extrapolating a bit, since human heads can't turn much more than about 90 degrees in any way). The roll value is expected to get a bit crazy when pitch and/or yaw are close to the pole, but i don't think yaw and pitch would misbehave all that much.
I should note that i'm not really all that familiar with Python ( to get an idea: i almost, just almost, can make a "Hello World" without consulting the documentation); so i probably broke a bunch of conventions, and might have done some things the harder way, and perhaps even missed obvious errors. So, besides trying to fix my math, please feel free to provide critic on my coding skills as well if you see need.
So without further ado, here is what i got so far:
Code: Select all
from math import * #To be able to just type stuff like pi and atan2
def update(): #The main stuff
matrix = quat2mat(hydra[0].q0, hydra[0].q1, hydra[0].q2, hydra[0].q3) #Gets the matrix from the quaternion; i understand rotation matrixes better than quaternions (which doesn't mean much since i tend to treat quaternions as blackboxes)
diagnostics.watch(matrix[0])
diagnostics.watch(matrix[1])
diagnostics.watch(matrix[2])
diagnostics.watch(matrix[3])
diagnostics.watch(matrix[4])
diagnostics.watch(matrix[5])
diagnostics.watch(matrix[6])
diagnostics.watch(matrix[7])
diagnostics.watch(matrix[8])
diagnostics.watch(hydra[0].x)
diagnostics.watch(hydra[0].y)
diagnostics.watch(hydra[0].z)
monoeulrot = mat2monopeul(matrix) #gets the monopole rotation representation
diagnostics.watch(monoeulrot[0] / pi * 180)
diagnostics.watch(monoeulrot[1] / pi * 180)
diagnostics.watch(monoeulrot[2] / pi * 180)
#just putting the position axes to use, just because; not relevant for the monopole rotation representation
ppJoy[0].setAxis(AxisTypes.X, EnsureMapRange(hydra[0].x, -250, 250, -1000, 1000))
ppJoy[0].setAxis(AxisTypes.Y, EnsureMapRange(hydra[0].y, 250, -250, -1000, 1000))
ppJoy[0].setAxis(AxisTypes.ZAxis, EnsureMapRange(hydra[0].z, 600, 100, -1000, 1000))
#Applying the calculated Yaw, Pitch and Roll to the rotation axes of the virtual joystick
ppJoy[0].setAxis(AxisTypes.ZRotation, monoeulrot[0] / pi * 1000)
ppJoy[0].setAxis(AxisTypes.XRotation, monoeulrot[1] / pi * 1000)
ppJoy[0].setAxis(AxisTypes.YRotation, monoeulrot[2] / pi * 1000)
#putting the joystick axes to use, just because
ppJoy[0].setAxis(AxisTypes.Dial, hydra[0].joyx * 1000)
ppJoy[0].setAxis(AxisTypes.Slider, hydra[0].joyy * 1000)
def quat2mat(w, x, y, z): #magically transmogrifies the quaternion into a rotation matrix, with the same frame of reference as the position data from the Hydra: X = side to side, Y = Up and Down, Z = Forward and Backward. Formula originally from Wikipedia, but i twiddled stuff around till things were pointing the right way.
return [ -(2 * x * z + 2 * y * w), -(2 * x * y - 2 * z *w), 1 - 2 * (y ** 2) - 2 * (z**2),
-(2 * y * z - 2 * x * w), -(1 -2 * (x ** 2) - 2 * (z ** 2)), 2 * x * y + 2 * z * w,
-(1 - 2 * (x ** 2) - 2 * (y**2)), -(2 * y * z + 2 * x * w), 2 * x * z - 2 * y * w]
def mat2monopeul(mat): #The crazy s██t i came up with to try to make a monopole rotation representation
#Forward vector's X, Y and Z
fx = mat[0]
fy = mat[1]
fz = mat[2]
#Up vector's X, Y, Z
ux = mat[3]
uy = mat[4]
uz = mat[5]
#Left vector's X, Y and Z
lx = mat[6]
ly = mat[7]
lz = mat[8]
#This bit was supposed to be easy
pitch = atan2(-fy, -fz) * (1 - abs(fx))
yaw = atan2( fx, -fz) * (1 - abs(fy))
#Calculating the roll along the vertical, horizontal and... i dunno the word, "depthal" axes, and applying some weights so the roll should remain unchanged if you're just changing the other axes (keeping the roll angle in relation to the pole the same)
sideroll = atan2(uz, uy) * fx * (1 - abs(fz))
vertroll = atan2(ux, uz * sign(fy)) * abs(fy) * (1 - abs(fz))
frontroll = atan2(ly, -lx) * abs(fz)
diagnostics.watch(sideroll / pi * 180)
diagnostics.watch(vertroll / pi * 180)
diagnostics.watch(frontroll / pi * 180)
roll = frontroll + sideroll + vertroll #Puts it all together to make one roll
return [yaw, pitch, roll]
def sign(val): #Returns -1 for negatives, 0 for zero and 1 for positives
return (1 * (val > 0)) + (-1 * (val < 0))
def EnsureMapRange(val, IB, IE, OB, OE): # a recreation of GPIE's EnsureMapRange; parameters are Value, Input Begin, Input End, Output Begin and Output End
return max(min(((val - IB) / (IE - IB)) * (OE - OB) + OB, OE), OB)
update() #keeps running the that main stuff from the beginning
ps: I think the forum might be messing with my tab formatting in that code, lemme know if it is too confusing this way and i'll try to find a better way to share the code