I made these functions using information I found from
here
With these functions, you can convert:
Euler -> Quaternion
Quaternion -> Euler
Euler -> Matrix
Matrix -> Euler
You can also multiply:
Matrices
Quaternions
(Either can be used to do both local and global rotation)
The code:
type Quaternion
w as float
x as float
y as float
z as float
endtype
type Euler
x as float
y as float
z as float
endtype
type Matrix3x3
m00 as float
m01 as float
m02 as float
m10 as float
m11 as float
m12 as float
m20 as float
m21 as float
m22 as float
endtype
global ReturnQuat as Quaternion
global ReturnEuler as Euler
global ReturnMatrix as Matrix3x3
function MatrixMult(Angle as Matrix3x3, Rotation as Matrix3x3)
ReturnMatrix.m00 = Angle.m00*Rotation.m00 + Angle.m01*Rotation.m10 + Angle.m02*Rotation.m20
ReturnMatrix.m01 = Angle.m00*Rotation.m01 + Angle.m01*Rotation.m11 + Angle.m02*Rotation.m21
ReturnMatrix.m02 = Angle.m00*Rotation.m02 + Angle.m01*Rotation.m12 + Angle.m02*Rotation.m22
ReturnMatrix.m10 = Angle.m10*Rotation.m00 + Angle.m11*Rotation.m10 + Angle.m12*Rotation.m20
ReturnMatrix.m11 = Angle.m10*Rotation.m01 + Angle.m11*Rotation.m11 + Angle.m12*Rotation.m21
ReturnMatrix.m12 = Angle.m10*Rotation.m02 + Angle.m11*Rotation.m12 + Angle.m12*Rotation.m22
ReturnMatrix.m20 = Angle.m20*Rotation.m00 + Angle.m21*Rotation.m10 + Angle.m22*Rotation.m20
ReturnMatrix.m21 = Angle.m20*Rotation.m01 + Angle.m21*Rotation.m11 + Angle.m22*Rotation.m21
ReturnMatrix.m22 = Angle.m20*Rotation.m02 + Angle.m21*Rotation.m12 + Angle.m22*Rotation.m22
endfunction
function EulerToMatrix(Rotation as Euler)
SRX as float
CRX as float
SRY as float
CRY as float
SRZ as float
CRZ as float
SRX = sin(Rotation.x);
CRX = cos(Rotation.x);
SRY = sin(Rotation.y);
CRY = cos(Rotation.y);
SRZ = sin(Rotation.z);
CRZ = cos(Rotation.z);
ReturnMatrix.m00 = CRX*CRY
ReturnMatrix.m01 = SRX*SRZ-CRX*SRY*CRZ
ReturnMatrix.m02 = CRX*SRY*SRZ+SRX*CRZ
ReturnMatrix.m10 = SRY
ReturnMatrix.m11 = CRY*CRZ
ReturnMatrix.m12 = (0-CRY)*SRZ
ReturnMatrix.m20 = (0-SRX)*CRY
ReturnMatrix.m21 = SRX*SRY*CRZ+CRX*SRZ
ReturnMatrix.m22 = (0-SRX)*SRY*SRZ+CRX*CRZ
endfunction
function MatrixToEuler(Rotation as Matrix3x3)
if Rotation.m10 > 0.999
ReturnEuler.x = atanfull(Rotation.m02,Rotation.m22)
ReturnEuler.y = 90
ReturnEuler.z = 0
exitfunction
endif
if Rotation.m10 < -0.999
ReturnEuler.x = atanfull(Rotation.m02,Rotation.m22)
ReturnEuler.y = -90
ReturnEuler.z = 0
exitfunction
endif
ReturnEuler.x = atanfull(0-Rotation.m20,Rotation.m00)
ReturnEuler.z = atanfull(0-Rotation.m12,Rotation.m11)
ReturnEuler.y = asin(Rotation.m10)
endfunction
function EulerToQuat(Rotation as Euler)
SRX as float
CRX as float
SRY as float
CRY as float
SRZ as float
CRZ as float
Rotation.x = Rotation.x/2
Rotation.y = Rotation.y/2
Rotation.z = Rotation.z/2
SRX = sin(Rotation.x)
CRX = cos(Rotation.x)
SRY = sin(Rotation.y)
CRY = cos(Rotation.y)
SRZ = sin(Rotation.z)
CRZ = cos(Rotation.z)
ReturnQuat.w = CRX*CRY*CRZ + SRX*SRY*SRZ
ReturnQuat.x = SRX*CRY*CRZ - CRX*SRY*SRZ
ReturnQuat.y = CRX*SRY*CRZ + SRX*CRY*SRZ
ReturnQuat.z = CRX*CRY*SRZ - SRX*SRY*CRZ
endfunction
function QuatToEuler(Rotation as Quaternion)
test as float
test = Rotation.w*Rotation.y - Rotation.z*Rotation.x
if test > 0.499
ReturnEuler.z = 2 * atanfull(Rotation.w,Rotation.x)
ReturnEuler.y = 90
ReturnEuler.x = 0
exitfunction
endif
if test < -0.499
ReturnEuler.z = -2 * atanfull(Rotation.w,Rotation.x)
ReturnEuler.y = -90
ReturnEuler.x = 0
exitfunction
endif
ReturnEuler.x = atanfull((2*(Rotation.w*Rotation.x + Rotation.y*Rotation.z)),(1 - 2*(Rotation.x*Rotation.x + Rotation.y*Rotation.y)))
ReturnEuler.y = asin(2*(Rotation.w*Rotation.y - Rotation.z*Rotation.x))
ReturnEuler.z = atanfull((2*(Rotation.w*Rotation.z + Rotation.x*Rotation.y)),(1 - 2*(Rotation.y*Rotation.y + Rotation.z*Rotation.z)))
endfunction
function QuatMult(Angle as Quaternion, Rotation as Quaternion)
ReturnQuat.x = Rotation.x * Angle.w + Rotation.y * Angle.z - Rotation.z * Angle.y + Rotation.w * Angle.x
ReturnQuat.y = (0-Rotation.x) * Angle.z + Rotation.y * Angle.w + Rotation.z * Angle.x + Rotation.w * Angle.y
ReturnQuat.z = Rotation.x * Angle.y - Rotation.y * Angle.x + Rotation.z * Angle.w + Rotation.w * Angle.z
ReturnQuat.w = (0-Rotation.x) * Angle.x - Rotation.y * Angle.y - Rotation.z * Angle.z + Rotation.w * Angle.w
endfunction
Function list:
EulerToMatrix(Rotation as Euler) - Converts Euler angles to Matrices
EulerToQuat(Rotation as Euler) - Converts Euler angles to Quaternions
MatrixToEuler(Rotation as Matrix3x3) - Converts Matrices to Euler angles
QuatToEuler(Rotation as Matrix3x3) - Converts Quaternions to Euler angles
MatrixMult(Angle as Matrix3x3, Rotation as Matrix3x3) - Adds two rotations. Use Angle as the current rotation, and Rotation as the rotation to apply to it, to rotate around a global axis. Switch them to do local rotation.
QuatMult(Angle as Quaternion, Rotation as Quaternion) - Same as above, but for Quaternions.
