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