Euler Angles

GWM Euler angles are often used to express the rotation of an object. There are at least 12 different methods of expressing them, but in each case there are just 3 angles. http://mathworld.wolfram.com/EulerAngles.html

One common system is used by aircraft, involving rotation about the vertical (heading), axis through the wings of the aircraft (pitch) and rotation about the length of the craft (roll).

Each rotation can be expressed as a matrix:

Rheading = [ cos(h)  -sin(h) 0 ]
           [ sin(h)   cos(h) 0 ]
           [     0      0    1 ]

then Rheading * (x,y,z) results in a vector that has been rotated about the Z axis (heading if you allow Z to be vertical). for example Rh*(1,0,0) -> (cos(h), -sin(h), 0).

2 more matrices can be written for rotation about Y and X, multiplying the matrices gives a total rotation matrix for the Euler angles.

A simple package to perform symbolic manipulation of Euler matrices (and others)

# PROCS
# rotmat {axis angle} - create a 3*3 array of elements for rotation about xyz
# identitymat - returns an identity matrix.
# matmultiply multiply symbols, returns 3*3 symbolic matrix.
# matprint - prints symbolic matrix
# getmatvalue m args = substitute values for parameters and return list of rows in matrix
#

proc identitymat {} { ;# return identity matrix
    variable m
    foreach i {1 2 3} {
        foreach j {1 2 3} {
            set m($i,$j) [expr {$i == $j}]
        }
    }
    return [array get m]
}

proc rotmat {axis angle} {
    # make a 3 by 3 rotation matrix for angle about axis (x,y or z)
    # angle can be a number or a variable name.
    # Returned matrix is symbolic NOT numeric.
    # Use "getmatvalue" to get numerical values for the matrix.
    # eg array set m [rotmat Z p] - returns m(1,1) = m(2,2)=cos(p) etc
  variable m
  array set m [identitymat]
  switch -nocase -- $axis {
  x {
      array set m [list {2,2} cos($angle) {3,3} cos($angle) \
                   {2,3} sin($angle) {3,2} -sin($angle)]
  } y {
      array set m [list {1,1} cos($angle) {3,3} cos($angle) \
          {1,3} -sin($angle) {3,1} sin($angle)]
  } z {
      array set m [list {1,1} cos($angle) {2,2} cos($angle) \
          {1,2} sin($angle) {2,1} -sin($angle)]
  }}
  return [array get m]
}

proc matmultiply {left right} {
    # conventional matrix multiplication 3 by 3 matrix
    # using symbolic not numeric evaluation.
    # left & right are arrays in the calling routine with elements 1,1...3,3
    upvar 1 $left l
    upvar 1 $right r
    variable m
    foreach i {1 2 3} {
        foreach j {1 2 3} {
            set m($i,$j) ""
            foreach k {1 2 3} { ;# add up the inner product terms.
                if {$l($i,$k)=="" || $r($k,$j)==""} { ;# add nothing
                    append m($i,$j) ""
                } elseif {$l($i,$k)==0 || $r($k,$j)==0} { ;# add nothing
                    append m($i,$j) ""
                } elseif {$l($i,$k)==1} {
                    if {[string length $m($i,$j)]>0 && [string index $m($i,$j) end]!="+"} {append m($i,$j) "+"}
                    append m($i,$j) "$r($k,$j)"
                } elseif {$r($k,$j)==1} {
                    if {[string length $m($i,$j)]>0 && [string index $m($i,$j) end]!="+"} {append m($i,$j) "+"}
                    append m($i,$j) "$l($i,$k)"
                } else {
                    if {[string length $m($i,$j)]>0 && [string index $m($i,$j) end]!="+"} {append m($i,$j) "+"}
                    append m($i,$j) "($l($i,$k)*($r($k,$j)))"
                }
            }
        }
    }
    foreach i {1 2 3} {
        foreach j {1 2 3} {
          # check replace "" with 0
          if {$m($i,$j) eq ""} {set m($i,$j) 0}
        }
    }
    return [array get m]
}

proc matprint {m} {
    # prints the 3 by 3 matrix m.
    # m is an array in the calling routine with elements 1,1...3,3
    upvar 1 $m mat
    foreach i {1 2 3} {
        puts "$mat($i,1) | $mat($i,2) | $mat($i,3) "
    }
}

proc evaluate {term args} {
    # evaluate term substituting name-value pairs from args
    # EG evaluate "sin(theta)" theta 1.1
    # evaluates sin(1.1). There may be multiple values to substitute
    # useful for complex substitutions as seen in the rotation matrix example
    # NB this can also use formulae such as
    #   evaluate "sin(theta)" theta 2*atan(1)
    # which becomes sin(2*atan(1))
    # and the final expr in this proc evaluates the 2*atan as well(!)
    # NB the expr cannot be braced as it is not composed of numerical variables but is a complex string.
    foreach {name value} $args {
        set term [regsub -all $name $term $value]
    }
    # nb no brackets as term modifies itself so cannot be compiled?. 
    return [expr $term]
}

proc getmatvalue {m args} {
    # evaluate matrix substituting name-value pairs from args
    # EG getmatvalue result theta 1.1 phi 2.4 psi 3.1
    # evaluates each element of the matrix with theta = 1.1, phi 2.4 etc.
    # NB can also evaluate as getmatvalue result theta 2*atan(1)
    upvar 1 $m mat
    variable mv
    foreach i {1 2 3} {
        foreach j {1 2 3} {
            # evaluate each element with numeric substitutions
            set mv($i,$j) [format "%8.5f" [eval evaluate $mat($i,$j) $args]]
        }
    }
    return [array get mv]
}

Examples

simple rotation matrix

array set mx [rotmat z psi]
puts "\n=====\nResult of rotation by psi about Z axis"
matprint mx

 cos(psi) | sin(psi) | 0 
-sin(psi) | cos(psi) | 0 
 0 | 0 | 1 

Product of 2 matrices

array set my [rotmat z theta]
array set result [matmultiply mx my]
puts "\n=====\nResult of rotation by psi about Z axis then by theta about z"
matprint result

cos(theta) | sin(theta) | 0 
(cos(psi)*(-sin(theta))) | (cos(psi)*(cos(theta))) | sin(psi) 
(-sin(psi)*(-sin(theta))) | (-sin(psi)*(cos(theta))) | cos(psi) 

Full 3 angle Euler matrix

array set mx [rotmat X psi]
array set my [rotmat Y theta]
array set mz [rotmat Z phi]
array set result [matmultiply mx my]
array set result [matmultiply result mz]
puts "\n=====\nResult of multiplying in order XYZ = psi theta phi"
matprint result

(cos(theta)*(cos(phi))) | (cos(theta)*(sin(phi))) | -sin(theta) 
((sin(psi)*(sin(theta)))*(cos(phi)))+(cos(psi)*(-sin(phi))) | ((sin(psi)*(sin(theta)))*(sin(phi)))+(cos(psi)*(cos(phi))) | (sin(psi)*(cos(theta))) 
((cos(psi)*(sin(theta)))*(cos(phi)))+(-sin(psi)*(-sin(phi))) | ((cos(psi)*(sin(theta)))*(sin(phi)))+(-sin(psi)*(cos(phi))) | (cos(psi)*(cos(theta))) 

Compare this with Wolfram Encyclopedia lines 51-59. A means of simplifying the terms to remove brackets would be useful.

Evaluating this matrix with theta, phi, psi defined

set phi .2
set theta .3
set psi .4
puts " Matrix theta = $theta*atan(1) psi $psi, phi $phi"
array set matvalues [getmatvalue result theta $theta*atan(1) psi $psi phi $phi]
matprint matvalues

 0.95299 |  0.19318 | -0.23345 
-0.09389 |  0.92076 |  0.37866 
 0.28810 | -0.33894 |  0.89561 

AMG: I suggest putting your matrices in nested lists instead of arrays. You can use [lindex] and [lset] to efficiently get and set elements inside nested lists. This will improve performance a little bit because there's no need for shimmering the indices to strings to get the hash table key.