-- |
-- Module:      Data.Geo.Jord.Rotation
-- Copyright:   (c) 2018 Cedric Liegeois
-- License:     BSD3
-- Maintainer:  Cedric Liegeois <ofmooseandmen@yahoo.fr>
-- Stability:   experimental
-- Portability: portable
--
-- Rotation matrices from/to 3 angles about new axes
--
-- All functions are implemented using the vector-based approached described in
-- <http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf Gade, K. (2010). A Non-singular Horizontal Position Representation>
--
module Data.Geo.Jord.Rotation
    ( r2xyz
    , r2zyx
    , xyz2r
    , zyx2r
    ) where

import Data.Geo.Jord.Angle
import Data.Geo.Jord.Vector3d

-- | Angles about new axes in the xyz-order from a rotation matrix.
--
-- The produced list contains 3 'Angle's of rotation about new axes.
--
-- The x, y, z angles are called Euler angles or Tait-Bryan angles and are
-- defined by the following procedure of successive rotations:
-- Given two arbitrary coordinate frames A and B. Consider a temporary frame
-- T that initially coincides with A. In order to make T align with B, we
-- first rotate T an angle x about its x-axis (common axis for both A and T).
-- Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T
-- is rotated an angle z about its NEWEST z-axis. The final orientation of
-- T now coincides with the orientation of B.
-- The signs of the angles are given by the directions of the axes and the
-- right hand rule.
r2xyz :: [Vector3d] -> [Angle]
r2xyz [v0, v1, v2] = [x, y, z]
  where
    v00 = vx v0
    v01 = vy v0
    v12 = vz v1
    v22 = vz v2
    z = atan2' (-v01) v00
    x = atan2' (-v12) v22
    sy = vz v0
    -- cos y is based on as many elements as possible, to average out
    -- numerical errors. It is selected as the positive square root since
    -- y: [-pi/2 pi/2]
    cy = sqrt ((v00 * v00 + v01 * v01 + v12 * v12 + v22 * v22) / 2.0)
    y = atan2' sy cy
r2xyz m = error ("Invalid rotation matrix " ++ show m)

-- | Angles about new axes in the xyz-order from a rotation matrix.
--
-- The produced list contains 3 'Angle's of rotation about new axes.
-- The z, x, y angles are called Euler angles or Tait-Bryan angles and are
-- defined by the following procedure of successive rotations:
-- Given two arbitrary coordinate frames A and B. Consider a temporary frame
-- T that initially coincides with A. In order to make T align with B, we
-- first rotate T an angle z about its z-axis (common axis for both A and T).
-- Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T
-- is rotated an angle x about its NEWEST x-axis. The final orientation of
-- T now coincides with the orientation of B.
-- The signs of the angles are given by the directions of the axes and the
-- right hand rule.
-- Note that if A is a north-east-down frame and B is a body frame, we
-- have that z=yaw, y=pitch and x=roll.
r2zyx :: [Vector3d] -> [Angle]
r2zyx m = [z, y, x]
  where
    [x, y, z] = fmap negate' (r2xyz (transpose m))

-- | Rotation matrix (direction cosine matrix) from 3 angles about new axes in the xyz-order.
--
-- The produced (no unit) rotation matrix is such
-- that the relation between a vector v decomposed in A and B is given by:
-- @v_A = mdot R_AB v_B@
--
-- The rotation matrix R_AB is created based on 3 angles x,y,z about new axes
-- (intrinsic) in the order x-y-z. The angles are called Euler angles or
-- Tait-Bryan angles and are defined by the following procedure of successive
-- rotations:
-- Given two arbitrary coordinate frames A and B. Consider a temporary frame
-- T that initially coincides with A. In order to make T align with B, we
-- first rotate T an angle x about its x-axis (common axis for both A and T).
-- Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T
-- is rotated an angle z about its NEWEST z-axis. The final orientation of
-- T now coincides with the orientation of B.
-- The signs of the angles are given by the directions of the axes and the
-- right hand rule.
xyz2r :: Angle -> Angle -> Angle -> [Vector3d]
xyz2r x y z = [v1, v2, v3]
  where
    cx = cos' x
    sx = sin' x
    cy = cos' y
    sy = sin' y
    cz = cos' z
    sz = sin' z
    v1 = Vector3d (cy * cz) ((-cy) * sz) sy
    v2 = Vector3d (sy * sx * cz + cx * sz) ((-sy) * sx * sz + cx * cz) ((-cy) * sx)
    v3 = Vector3d ((-sy) * cx * cz + sx * sz) (sy * cx * sz + sx * cz) (cy * cx)

-- | rotation matrix (direction cosine matrix) from 3 angles about new axes in the zyx-order.
--
-- The produced (no unit) rotation matrix is such
-- that the relation between a vector v decomposed in A and B is given by:
-- @v_A = mdot R_AB v_B@
--
-- The rotation matrix R_AB is created based on 3 angles
-- z,y,x about new axes (intrinsic) in the order z-y-x. The angles are called
-- Euler angles or Tait-Bryan angles and are defined by the following
-- procedure of successive rotations:
-- Given two arbitrary coordinate frames A and B. Consider a temporary frame
-- T that initially coincides with A. In order to make T align with B, we
-- first rotate T an angle z about its z-axis (common axis for both A and T).
-- Secondly, T is rotated an angle y about the NEW y-axis of T. Finally, T
-- is rotated an angle x about its NEWEST x-axis. The final orientation of
-- T now coincides with the orientation of B.
-- The signs of the angles are given by the directions of the axes and the
-- right hand rule.
--
-- Note that if A is a north-east-down frame and B is a body frame, we
-- have that z=yaw, y=pitch and x=roll.
zyx2r :: Angle -> Angle -> Angle -> [Vector3d]
zyx2r z y x = [v1, v2, v3]
  where
    cx = cos' x
    sx = sin' x
    cy = cos' y
    sy = sin' y
    cz = cos' z
    sz = sin' z
    v1 = Vector3d (cz * cy) ((-sz) * cx + cz * sy * sx) (sz * sx + cz * sy * cx)
    v2 = Vector3d (sz * cy) (cz * cx + sz * sy * sx) ((-cz) * sx + sz * sy * cx)
    v3 = Vector3d (-sy) (cy * sx) (cy * cx)