Rotation matrix
/>x = (Qzy-Qyz)*s
y = (Qxz-Qzx)*s
z = (Qyx-Qxy)*s
This is numerically stable so long as the trace, t, is not negative; otherwise, we risk dividing by (nearly) zero. In that case, suppose Qxx is the largest diagonal entry, so x will have the largest magnitude (the other cases are similar); then the following is safe.
r = sqrt(1+Qxx-Qyy-Qzz)
s = 0.5/r
w = (Qzy-Qyz)*s
x = 0.5*r
y = (Qxy+Qyx)*s
z = (Qzx+Qxz)*s
If the matrix contains significant error, such as accumulated numerical error, we may construct a symmetric 44 matrix,
and find the eigenvector, (w,x,y,z), of its largest magnitude eigenvalue. (If Q is truly a rotation matrix, that value will be 1.) The quaternion so obtained will correspond to the rotation matrix closest to the given matrix[dubious discuss] (Bar-Itzhack 2000).
Polar decomposition
If the nn matrix M is non-singular, its columns are linearly independent vectors; thus the Gramchmidt process can adjust them to be an orthonormal basis. Stated in terms of numerical linear algebra, we convert M to an orthogonal matrix, Q, using QR decomposition. However, we often prefer a Q “closest” to M, which this method does not accomplish. For that, the tool we want is the polar decomposition (Fan & Hoffman 1955; Higham 1989).
To measure closeness, we may use any matrix norm invariant under orthogonal transformations. A convenient choice is the Frobenius norm, ||Q||F, squared, which is the sum of the squares of the element differences. Writing this in terms of the trace, Tr, our goal is,
Find Q minimizing Tr( (Q)T(Q) ), subject to QTQ = I.
Though written in matrix terms, the objective function is just a quadratic polynomial. We can minimize it in the usual way, by finding where its derivative is zero. For a 33 matrix, the orthogonality constraint implies six scalar equalities that the entries of Q must satisfy. To incorporate the constraint(s), we may employ a standard technique, Lagrange multipliers, assembled as a symmetric matrix, Y. Thus our method is:
Differentiate Tr( (Q)T(Q) + (QTQ)Y ) with respect to (the entries of) Q, and equate to zero.
Consider a 22 example. Including constraints, we seek to minimize
Taking the derivative with respect to Qxx, Qxy, Qyx, Qyy in turn, we assemble a matrix.
In general, we obtain the equation
so that
where Q is orthogonal and S is symmetric. To ensure a minimum, the Y matrix (and hence S) must be positive definite. Linear algebra calls QS the polar decomposition of M, with S the positive square root of S2 = MTM.
When M is non-singular, the Q and S factors of the polar decomposition are uniquely determined. However, the determinant of S is positive because S is positive definite, so Q inherits the sign of the determinant of M. That is, Q is only guaranteed to be orthogonal, not a rotation matrix. This is unavoidable; an M with negative determinant has no uniquely-defined closest rotation matrix.
Axis and angle
To efficiently construct a rotation matrix from an angle and a unit axis u, we can take advantage of symmetry and skew-symmetry within the entries.
c = cos(); s = sin(); C = 1-c
xs = x*s; ys = y*s; zs = z*s
xC = x*C; yC = y*C; zC = z*C
xyC = x*yC; yzC = y*zC; zxC = z*xC
[ x*xC+c xyC-zs zxC+ys ]
[ xyC+zs y*yC+c yzC-xs ]
[ zxC-ys yzC+xs z*zC+c ]
Determining an axis and angle, like determining a quaternion, is only possible up to sign; that is, (u,) and (,) correspond to the same rotation matrix, just like q and . As well, axis-angle extraction presents additional difficulties. The angle can be restricted to be from 0 to 180, but angles are formally ambiguous by multiples of 360. When the angle is zero, the axis is undefined. When the angle is 180, the matrix becomes symmetric, which has implications in extracting the axis. Near multiples of 180, care is needed to avoid numerical problems: in extracting the angle, a two-argument arctangent with atan2(sin ,cos ) equal to avoids the insensitivity of arccosine; and in computing the axis magnitude to force unit magnitude, a brute-force approach can lose accuracy through underflow (Moler & Morrison 1983).
A partial approach is as follows.
x = Qzy-Qyz
y = Qxz-Qzx
z = Qyx-Qxy
r = hypot(x,hypot(y,z))
t = Qxx+Qyy+Qzz
= atan2(r,t1)
The x, y, and z components of the axis would then be divided by r. A fully robust approach will use different code when t is negative, as with quaternion extraction. When r is zero because the angle is zero, an axis must be provided from some source other than the matrix.
Euler angles
Complexity of conversion escalates with Euler angles (used here in the broad sense). The first difficulty is to establish which of the twenty-four variations of Cartesian axis order we will use. Suppose the three angles are 1, 2, 3; physics and