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.
