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 edit
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.
