Converting a rotation matrix to a quaternion

Question

I found a very good link about quaternions in Mathematica , but I don't know how to create a quaternion from a rotation matrix. Can anyone help me, please?

Update

I need this:

A rotation may be converted back to a quaternion through the use of the following algorithm. The process is performed in the following stages, which are as follows:

Calculate the trace of the matrix T from the equation:

  T = 4 - 4x^2  - 4y^2  - 4z^2
    = 4( 1 - x^2  - y^2  - z^2 )
    = mat[0] + mat[5] + mat[10] + 1

If the trace of the matrix is greater than zero, then perform an "instant" calculation.

  S = 0.5 / sqrt(T)
  W = 0.25 / S
  X = ( mat[9] - mat[6] ) * S
  Y = ( mat[2] - mat[8] ) * S
  Z = ( mat[4] - mat[1] ) * S

If the trace of the matrix is less than or equal to zero then identify which major diagonal element has the greatest value.

Depending on this value, calculate the following:

Column 0:

    S  = sqrt( 1.0 + mr[0] - mr[5] - mr[10] ) * 2;
    Qx = 0.5 / S;
    Qy = (mr[1] + mr[4] ) / S;
    Qz = (mr[2] + mr[8] ) / S;
    Qw = (mr[6] + mr[9] ) / S;

Column 1:

    S  = sqrt( 1.0 + mr[5] - mr[0] - mr[10] ) * 2;
    Qx = (mr[1] + mr[4] ) / S;
    Qy = 0.5 / S;
    Qz = (mr[6] + mr[9] ) / S;
    Qw = (mr[2] + mr[8] ) / S;

Column 2:

    S  = sqrt( 1.0 + mr[10] - mr[0] - mr[5] ) * 2;
    Qx = (mr[2] + mr[8] ) / S;
    Qy = (mr[6] + mr[9] ) / S;
    Qz = 0.5 / S;
    Qw = (mr[1] + mr[4] ) / S;

The quaternion is then defined as:

   Q = | Qx Qy Qz Qw |

Answer 1

If you are just asking how to use quaternions for rotation in Mathematica, I hope the following helps. You specify the axis with a unit vector and the angle of rotation. Here is one implementation:

Needs["Quaternions`"];

qr[vec_, u_, a_] := Module[{qv, qu, r},
   qv = ReplacePart[Join[{0}, vec], 0 -> Quaternion];
   qu = ReplacePart[Join[{Cos[a/2]}, Sin[a/2] Normalize[u]], 0 -> Quaternion];
   r = qu ** qv ** Conjugate[qu];
   N @ FullSimplify[ReplacePart[r, 0 -> List][[2 ;; 4]]]]

The first argument of qr is the vector you rotate, the second argument is the axis, and the third argument is the angle of rotation.

Here is a visualization:

Manipulate[Graphics3D[
  {{Red, Line[{{0, 0, 0}, {1, 1, 1}}]},
   {Blue, Arrow[{{0, 0, 0}, qr[{1, 1, 1}, {m, n, p}, an Degree]}]},
   {Black, Arrow[{{0, 0, 0}, {m, n, p}}]},
   {Purple, Thickness[0.02], Line[Table[qr[{1, 1, 1}, {m, n, p}, j],
                                        {j, 0, 2 Pi, 2 Pi/20}]]}}],
  {{an, 0}, 0, 360, AngularGauge[##, GaugeLabels -> {"Degrees", "Value"}] &, 
   ControlPlacement -> Left}, {m, 0.1, 1}, {n, 0.1, 1}, {p, 0.1, 1}]

Answer 2

I needed to do this recently, so I broke down and decided to write routines for interconverting quaternions and rotation matrices.

I'll give the quaternion to rotation matrix routine first, since it's the shortest. I merely needed to modify my Rodrigues routine in this answer:

Needs["Quaternions`"];

quaternionToRotation[qq_] /; QuaternionQ[ToQuaternion[qq]] :=
    Module[{q = ToQuaternion[qq], aim, r},
           aim = AbsIJK[q]; r = Re[q];
           If[aim == 0, Return[IdentityMatrix[3], Module]];
           First[LinearAlgebra`Private`MatrixPolynomial[
                 {Prepend[2 aim {r, aim}/(r^2 + aim^2), 1]},
                 -LeviCivitaTensor[3, List].(Rest[List @@ q]/aim)]]]

(Use LinearAlgebra`MatrixPolynomial[] in versions before 11.2.)

Now, for the more complicated direction of converting a rotation matrix to a quaternion. I will provide two methods I found in the literature. This first one is due to Markley, which is a modification of an earlier method due to Sheppard^*:

rotationToQuaternion[m_?OrthogonalMatrixQ] := Module[{d, v, xm},
        d = Diagonal[m]; 
        v = {{m[[3, 2]] - m[[2, 3]], m[[1, 3]] - m[[3, 1]], m[[2, 1]] - m[[1, 2]]}};
        xm = IdentityMatrix[4] +
             DiagonalMatrix[{{1, 1, 1}, {1, -1, -1}, {-1, 1, -1}, {-1, -1, 1}}.d] + 
             ArrayFlatten[{{0, v}, {Transpose[v], 2 Symmetrize[m - DiagonalMatrix[d]]}}];
        Sign[Apply[Quaternion, First[MaximalBy[xm, Norm]]]]]

The second method I present is due to Bar-Itzhack:

rotationToQuaternion2[m_?OrthogonalMatrixQ] := Module[{d, v, xm},
        d = Diagonal[m]; 
        v = {{m[[3, 2]] - m[[2, 3]], m[[1, 3]] - m[[3, 1]], m[[2, 1]] - m[[1, 2]]}};
        xm = DiagonalMatrix[{{1, 1, 1}, {1, -1, -1}, {-1, 1, -1}, {-1, -1, 1}}.d] + 
             ArrayFlatten[{{0, v}, {Transpose[v], 2 Symmetrize[m - DiagonalMatrix[d]]}}];
        Sign[Apply[Quaternion, First[Eigenvectors[xm, 1]]]]]

(The observant reader ought to notice the similarities in the two methods.)

At this juncture, let me remind the leader that a quaternion $\mathbf q$ and its negative correspond to the same rotation. If needed, one can modify the conversion routines so that a quaternion with positive real part is always returned.

Now, for tests:

BlockRandom[SeedRandom[42]; (* for reproducibility *)
            tstq = Apply[Quaternion, Normalize[RandomVariate[NormalDistribution[], 4]]]];

Norm[rotationToQuaternion[quaternionToRotation[tstq]] + tstq]
   1.54074*10^-32

Norm[rotationToQuaternion2[quaternionToRotation[tstq]] - tstq]
   3.08149*10^-32

BlockRandom[SeedRandom[42]; (* for reproducibility *)
            tst = Orthogonalize[# Det[#]] &[RandomVariate[NormalDistribution[], {3, 3}]]];

Norm[quaternionToRotation[rotationToQuaternion[tst]] - tst]
   5.23541*10^-16

Norm[quaternionToRotation[rotationToQuaternion2[tst]] - tst]
   2.37306*10^-16

---

* - altho it may not look it, the article by Sheppard is at the end of the PDF from that link.

---

Update 9/15/2019

An even faster and more accurate method due to Sarabandi, Perez-Gracia, and Thomas, based on the Cayley representation of the rotation matrix, can be very simply implemented in Mathematica:

rotationToQuaternionCayley[m_?OrthogonalMatrixQ] := 
          With[{s = {{m[[3, 2]] - m[[2, 3]], m[[1, 3]] - m[[3, 1]], m[[2, 1]] - m[[1, 2]]},
                     {0, m[[2, 1]] + m[[1, 2]], m[[1, 3]] + m[[3, 1]]},
                     {0, 0, m[[3, 2]] + m[[2, 3]]}}},
               Apply[Quaternion, Prepend[Sign[First[s]], 1]
               Map[Norm, DiagonalMatrix[Prepend[NestList[RotateRight, {1, -1, -1}, 2],
                                                {1, 1, 1}].Diagonal[m] + 1] +
                         ((# + Transpose[#]) &[PadLeft[s, {-4, 4}]])]/4]]