We wish to define a rotation matrix by using euler angles psi, theta, and phi that correspond to rotations about the principal axes x, y, and z respectively. Each principal axis rotation forms a 3x3 matrix as follows where the direction of rotation for a positive angle is defined by the right-hand rule about the axis of rotation:
Rx = RotAboutXaxis(RadAngle psi) = [ 1 0 0 ] [ 0 cos(psi) -sin(psi) ] [ 0 sin(psi) cos(psi) ] Ry = RotAboutYaxis(RadAngle theta) = [ cos(theta) 0 sin(theta) ] [ 0 1 0 ] [ -sin(theta) 0 cos(theta) ] Rz = RotAboutZaxis(RadAngle phi) = [ cos(phi) -sin(phi) 0 ] [ sin(phi) cos(phi) 0 ] [ 0 0 1 ]So a positive psi, theta, and phi correspond to rotations about the principal axes from Y-to-Z, Z-to-X, and X-to-Y respectively corresponding to the right-hand rule. We form the composite rotation matrix C by applying the rotation in order about x, about y, and then about z (using post-multiplication/column vectors):
C = Rz * Ry * RxSo a point defined in the rotated object's coordinates Pobj, can be rotated into world coordinates as follows:
Pworld = C * Pobj
Given the time rate of change (time derivatives) of psi, theta, and phi (dpsi, dtheta, dphi), we must compute the resulting angular velocity Omega as the sum of the angular velocities about the principal axes. Omega is a vector whose direction represents the axis of rotation and whose magnitude represents the rotation angle.
To compute the angular velocity for Xaxis rotation given dpsi, we are given the magnitude of the rotation about X, dpsi, but we must compute the axis of rotation. Naively, we would just assume the Xaxis [1 0 0], but this rotation is followed by a rotation about y and then about z. The axis of rotation is also rotated! We have to account for this. Without the two successive rotations, the angular velocity is simply [1 0 0] * dpsi. We have to transform [1 0 0] with rotations Ry and then Rz to get the true axis of rotation. This result is scaled by dpsi to get the resulting angular velocity Xomega:
Xomega = ( Rz * Ry * [1 0 0]T ) * dpsi = ( (Rz*Ry) * [1 0 0]T ) * dpsi = [ cos(phi)*cos(theta) -sin(phi) cos(phi)*sin(theta) ] [ 1 ] [ sin(phi)*cos(theta) cos(phi) sin(phi)*sin(theta) ] [ 0 ] * dpsi [ -sin(theta) 0 cos(theta) ] [ 0 ] = [ cos(phi)*cos(theta) ] [ sin(phi)*cos(theta) ] * dpsi [ -sin(theta) ]The calculation of the angular velocities about Y and Z proceed similarly except that the Y axis need only be rotated by the Z rotation (Rz), and the Z axis need not be rotated at all:
Yomega = ( Rz * [0 1 0]T ) * dtheta = [ -sin(phi) ] [ cos(phi) ] * dtheta [ 0 ]
Zomega = [0 0 1]T * dphiThe resulting angular velocity Omega is simply the sum of the principal axis rotation angular velocities:
Omega = Xomega + Yomega + ZomegaWe can rewrite Omega as a matrix tranformation of the vector formed by the Euler angle time derivatives:
= [ cos(phi)*cos(theta) -sin(phi)
0 ] [ dpsi ]
[ sin(phi)*cos(theta)
cos(phi) 0 ] [ dtheta ]
[ -sin(theta)
0 1 ] [ dphi ]
= M(theta,phi) * [ dpsi dtheta dphi ]T
Using the previous total angular velocity function of the time derivatives and the current YZ Euler Angles, we can compute the Euler angle time derivatives, given the current angular velocity (Omega) and the YZ Euler Angles (theta and phi) by inverting the matrix M:
[ dpsi dtheta dphi ]T = M-1(theta,phi) * OmegaWe now need only compute the inverse of M:
M-1(theta,phi) = [ cos(phi)/cos(theta) sin(phi)/cos(theta) 0 ] [ -sin(phi) cos(phi) 0 ] [ cos(phi)*sin(theta)/cos(theta) sin(phi)*sin(theta)/cos(theta) 1 ]