----------------------------------------------------------
----                   Standard defn's
----------------------------------------------------------
-- by "Michael W. Martin" <aardvark@ial4.jsc.nasa.gov>

-- Functions  --------------------------------------------

atan2(y,x) = 0 when x=0 and y=0,
             atan(y/x) when x>=0,
             atan(y/x)+180 when y>=0,
             atan(y/x)-180

-- Vector Ops ------------------------------------------

   Dot(A,B) = sum(A[i]*B[i],i,1,count(A))

   MagV(A) = sqrt(Dot(A,A))

   UnitV(A) = A/MagV(A)

   Cross(A,B) = {A[2]*B[3]-A[3]*B[2],
                 A[3]*B[1]-A[1]*B[3],
                 A[1]*B[2]-A[2]*B[1]}

-- Matrix Ops ------------------------------------------

   MxM(A,B)[i,j] = sum(A[i,k]*B[k,j],k,1,count(B)) dim
                       [count(A),count(B[1])]

   MxV(A,B)[i] = sum(A[i,k]*B[k],k,1,count(B)) dim
                       [count(A)]

   VxM(A,B)[i] = sum(B[k]*A[k,i],k,1,count(B)) dim
                       [count(A)]

   Transpose(M)[i,j] = M[j,i] dim [count(M[1]),count(M)]

   Invert(A) = Adjoint(A)/det(A)

   Adjoint(A) = Transpose(Cofactor(A))

   Cofactor(A)[i,j] = (-1)^(i+j)*
                   det(Submatrix(A,i,j))

   Submatrix(A,k,l)[i,j] = A[i,j] when i<k and j<l,
                           A[i+1,j] when i³k and j<l,
                           A[i+1,j+1] when i³k and j³l,
                           A[i,  j+1] when i<k and j³l dim
                                   [count(A)-1,count(A)-1]

-- Standard Euler Angle defn's for 3x3's

   Roll(x) =  {{      1,       0,       0},
               {      0,  cos(x),  sin(x)},
               {      0, -sin(x),  cos(x)}}

   Pitch(y) = {{ cos(y),       0, -sin(y)},
               {      0,       1,       0},
               { sin(y),       0,  cos(y)}}

   Yaw(z) =   {{ cos(z),  sin(z),       0},
               {-sin(z),  cos(z),       0},
               {      0,       0,       1}}


-- Matrix to XYZ Euler sequence

   Mxyz(M) = { atan2(-M[3,2], M[3,3]),
               asin(M[3,1]),
               atan2(-M[2,1], M[1,1])}