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

# 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]
}

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 

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) 

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))) 

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