-- Rotations
-- by Hank.Dolben@UNH.edu 1994 May 18
----------------------------------------------------------------------------
-- a is a vector in coordinate system A.
-- a' is the vector in coordinate system A'.
-- A' is rotated by yaw around z, pitch around y, and roll around x, from A.
-- R = rotation(yaw,pitch,roll)
-- a' = Ra = transform(R,a)
-- the rows of R are the basis vectors of A' in A.
----------------------------------------------------------------------------
rotation(y,p,r) = {{cos(p)*       cos(y),
                    cos(p)*       sin(y),
                   -sin(p)                            },
                   {sin(p)*sin(r)*cos(y)-cos(r)*sin(y),
                    sin(p)*sin(r)*sin(y)+cos(r)*cos(y),
                    cos(p)*sin(r)                     },
                   {sin(p)*cos(r)*cos(y)+sin(r)*sin(y),
                    sin(p)*cos(r)*sin(y)-sin(r)*cos(y),
                    cos(p)*cos(r)                     }}

-- when |pitch| = 90 (cos(pitch)=0), the partition of yaw and roll is arbitrary,
-- say roll = 0, then:
--   R[2,1] = sin(p)*sin(r)*cos(y)-cos(r)*sin(y) = -sin(y)
--   R[2,2] = sin(p)*sin(r)*sin(y)+cos(r)*cos(y) =  cos(y)

Yaw(R)   = atan2(-R[2,1],R[2,2]) when R[1,2]=0 and R[1,1]=0,
           atan2(R[1,2],R[1,1]) otherwise

Pitch(R) = asin(-R[1,3])        --   -90  Pitch(R)  90

Roll(R)  = 0 when R[2,3]=0 and R[3,3]=0,
           atan2(R[2,3],R[3,3]) otherwise

rotate(yaw,pitch,roll,v) = transform(rotation(yaw,pitch,roll),v)

transform(R,b)[i] = dot(R[i],b) dim [count(R)]

-- compose the transformations such that R2 applies after R1
multiply(R2,R1)[i,j] = dot(R2[i],transpose(R1)[j]) dim [count(R2),count(R1[1])]

transpose(R)[i,j] = R[j,i] dim [count(R[1]),count(R)]

dot(a,b) = sum(a[i]*b[i],i,1,count(a))
