Quaternion math

This notebook confirms some important math identities used in the class discussion on quaternions.

Generic definitions

Turn off annoying warnings messages

Off[General::spell1];
Off[General::spell];

Load in necessary packages

<< LinearAlgebra`MatrixManipulation`;

Nice view of matrices and vectors

Nice[m_] := m // MatrixForm  //  TraditionalForm

Generate generic vector with subscripts

GenericVector[v_] := {Subscript[v, x],  Subscript[v, y],  
        Subscript[v, z]}

Elementary rotation and transform definitions

Definition of elementary rotation matrices

rX[
          a_] := {{1, 0, 0}, {0,  Cos[a],  -Sin[a]}, {0, Sin[a],  
            Cos[a]}};
rY[
          a_] := {{Cos[a], 0, Sin[a]}, {0, 1, 0},  {-Sin[a], 0, 
            Cos[a]}};
rZ[
          a_] := {{Cos[a],  -Sin[a], 0}, {Sin[a], Cos[a], 0}, {0, 0, 
            1}};

Convert a 3x3 rotation matrix and a 3x1 position vector to a 4x4 homogeneous operator

RAndPToT[r_,  p_] := 
Join[
          Transpose[Join[Transpose[r],  {p}]], {{0, 0, 0, 1}}];

Convert a 3x3 rotation matrix to 4x4 homogeneous rotational operator

RToT[r_] := RAndPToT[r,  {0, 0, 0}];

Elementary homogeneous rotational operators

RX[a_] := RToT[rX[a]];
RY[a_] := RToT[rY[a]];
RZ[a_] := RToT[rZ[a]];

Angle-axis rotation [(Subscript[R, k])(θ)]

RK[θ_,  
          k_] := 
Module[{kx,  ky, kz,  v, c, 
            s}, 
{kx,  ky,  kz}  =  
            k/Sqrt[k . k]; {{kx^2 v  +  c, kx ky v  -  kz s,  
                kx kz v  +  ky s}, {kx ky v  +  kz s,  
                ky^2 v +  c,  
                ky kz v  -  kx s}, {kx kz v  -  ky s,  
                ky kz v  +  kx s,  
                kz^2 v  +  c}}  /.  {v -> (1 - Cos[θ]),  
              c -> Cos[θ],  s -> Sin[θ]}];

Convert a 3x1 position vector to a 4x4 homogeneous translational operator

PToT[p_] := RAndPToT[IdentityMatrix[3],  p];

Elementary homogeneous translational operators

DX[x_] := PToT[{x, 0, 0}];
DY[y_] := PToT[{0, y, 0}];
DZ[z_] := PToT[{0, 0, z}];

Extract 3x3 rotation matrix from 4x4 homogeneous transform

RFromT[t_] := TakeMatrix[t, {1, 1}, {3, 3}];

Extract 3x1 position vector from 4x4 homogeneous transform

PFromT[t_] := 
        TakeMatrix[t, {1, 4}, {3, 4}]  //  Flatten;

Compute inverse of a 4x4 homogeneous transform

InverseT[
          t_] := 
Module[{r,  
            p}, 
r  =  
            RFromT[t]; 
p  =  
            PFromT[t]; 
RAndPToT[
            Transpose[
              r],  -Transpose[r] . p]
];

Derivation of required math identities in quaternion notes

(v x p) . v = 0

vv = GenericVector[v];
pv = GenericVector[p];
vv  //  Nice
pv  //  Nice

tst  =  Cross[vv, pv] . vv;
tst  //  Nice

Simplify[tst]

(v x p) x v = (v . v)  p - (v . p) v

vv  =  GenericVector[v];
pv  =  GenericVector[p];
vv  //  Nice
pv  //  Nice

tst1  =  Cross[Cross[vv, pv], vv]; 
tst1  //  Nice

tst2  =  (vv . vv) pv  -  (vv . 
                pv) vv;
tst2  //  Nice

tst  =  Simplify[tst1  ==  tst2]

Trigonometric half-angle identities

eq1 = (Cos[t]  ==  
            Cos[t/2]^2  -  Sin[t/2]^2);
eq1  //  Nice
eq1  //  Simplify

eq2 = (Sin[t]  ==  
            2 Cos[t/2] Sin[t/2]);
eq2  //  Nice
eq2  //  Simplify

eq3 = (1  -  Cos[t]  ==  
            2 Sin[t/2]^2);
eq3 // Nice
eq3 // Simplify

Quaternion to rotation matrix

Cross product to matrix

vv  =  {x, y, z};
pv  =  GenericVector[p];
vv  //  Nice
pv  //  Nice

tst1  =  Cross[vv, pv];
tst1  //  Nice

vmat  =  {{0,  -z, y}, {z, 0, -x}, {-y, x, 
            0}};
vmat  //  Nice

tst2  =  vmat . pv;
tst2  //  Nice

tst2  ==  tst1

(v . p) v = (v . v') . p

vv  =  {x, y, z};
pv  =  GenericVector[p];
vv  //  Nice
pv  //  Nice

tst1  = (vv . pv) vv  //  
          ExpandAll;
tst1  //  Nice

vmat  =  Outer[Times, vv, vv]; 
vmat  //  Nice

tst2  =  vmat . pv;
tst2  //  Nice

tst1 == tst2

Equivalence of equivalent angle-axis and quaternion rotations

Derivation of equivalent angle-axis rotation matrix

Multiply elementary rotation matrices

rk  = 
        rZ[α] . rY[β] . rZ[θ] . rY[-β] . 
          rZ[-α];
rk  //  Nice

Substitute angles in terms of \(TraditionalForm\`k\&^\)

\(\(substitutionRules\  = \ {Cos[α]\  -> 
            kx/Sqrt[kx^2\  + \ ky^2], 
		Sin[α]\  -> \ 
            ky/Sqrt[kx^2\  + \ ky^2], 
		Cos[β]\  -> kz, \ 
          Sin[β] -> 
            Sqrt[kx^2\  + \ ky^2]};\)\)
substitutionRules  //  Nice

rk2  = rk  /.  substitutionRules;
rk2  //  Nice

Simplify, for cryin' out loud!

rk3  =  Simplify[rk2];
rk3  //  Nice

Fine tune...

subRule2  =  
        kz^2  ->  (1 - kx^2 - ky^2);
subRule3  =  (kx^2  +  ky^2)  ->  (1 - 
              kz^2);
rk4  =  
        rk3  /. subRule2  //  Simplify;
rksub33  =  
        rk3[[3, 3]]  /.  subRule3  //  
          Simplify;
rk4  =  
        ReplacePart[rk4,  rksub33,  {3, 3}];
rk4  //  Nice

Equivalence of quaternion rotation

Compute rotated point (via quaternions)

pp  =  {x, y, z};
k  =  {kx,  ky,  kz};
p1  =  
        Cos[θ] pp  +  
            Sin[θ] Cross[k,  
                pp]  +  (1  -  Cos[θ]) (k . pp) k  // 
          ExpandAll;
p1  //  Nice

Compute rotated point (via equivalent angle-axis rotation matrix)

p2  =  rk4 . pp  //  ExpandAll;
p2  //  Nice

Check that they are equivalent

p1  ==  p2