-- |

-- Module:      Data.Geo.Jord.Rotation

-- Copyright:   (c) 2020 Cedric Liegeois

-- License:     BSD3

-- Maintainer:  Cedric Liegeois <ofmooseandmen@yahoo.fr>

-- Stability:   experimental

-- Portability: portable

--

-- Rotation matrices from/to 3 angles about new axes.

--

-- In order to use this module you should start with the following imports:

--

-- @

-- import qualified Data.Geo.Jord.Angle as Angle

-- import Data.Geo.Jord.Math3d (V3(..))

-- import qualified Data.Geo.Jord.Rotation as Rotation

-- @

--

-- 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 (Angle)
import qualified Data.Geo.Jord.Angle as Angle (atan2, cos, negate, sin)
import qualified Data.Geo.Jord.Math3d as Math3d (V3, transposeM, v3x, v3y, v3z, vec3)

-- | 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 :: [Math3d.V3] -> [Angle]
r2xyz :: [V3] -> [Angle]
r2xyz [V3
v0, V3
v1, V3
v2] = [Angle
x, Angle
y, Angle
z]
  where
    v00 :: Double
v00 = V3 -> Double
Math3d.v3x V3
v0
    v01 :: Double
v01 = V3 -> Double
Math3d.v3y V3
v0
    v12 :: Double
v12 = V3 -> Double
Math3d.v3z V3
v1
    v22 :: Double
v22 = V3 -> Double
Math3d.v3z V3
v2
    z :: Angle
z = Double -> Double -> Angle
Angle.atan2 (-Double
v01) Double
v00
    x :: Angle
x = Double -> Double -> Angle
Angle.atan2 (-Double
v12) Double
v22
    sy :: Double
sy = V3 -> Double
Math3d.v3z V3
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 :: Double
cy = Double -> Double
forall a. Floating a => a -> a
sqrt ((Double
v00 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v00 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v01 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v01 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v12 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v12 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v22 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v22) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2.0)
    y :: Angle
y = Double -> Double -> Angle
Angle.atan2 Double
sy Double
cy
r2xyz [V3]
m = [Char] -> [Angle]
forall a. HasCallStack => [Char] -> a
error ([Char]
"Invalid rotation matrix " [Char] -> [Char] -> [Char]
forall a. [a] -> [a] -> [a]
++ [V3] -> [Char]
forall a. Show a => a -> [Char]
show [V3]
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 :: [Math3d.V3] -> [Angle]
r2zyx :: [V3] -> [Angle]
r2zyx [V3]
m = [Angle
z, Angle
y, Angle
x]
  where
    [Angle
x, Angle
y, Angle
z] = (Angle -> Angle) -> [Angle] -> [Angle]
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Angle -> Angle
Angle.negate ([V3] -> [Angle]
r2xyz ([V3] -> [V3]
Math3d.transposeM [V3]
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 'Angle's 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 -> [Math3d.V3]
xyz2r :: Angle -> Angle -> Angle -> [V3]
xyz2r Angle
x Angle
y Angle
z = [V3
v1, V3
v2, V3
v3]
  where
    cx :: Double
cx = Angle -> Double
Angle.cos Angle
x
    sx :: Double
sx = Angle -> Double
Angle.sin Angle
x
    cy :: Double
cy = Angle -> Double
Angle.cos Angle
y
    sy :: Double
sy = Angle -> Double
Angle.sin Angle
y
    cz :: Double
cz = Angle -> Double
Angle.cos Angle
z
    sz :: Double
sz = Angle -> Double
Angle.sin Angle
z
    v1 :: V3
v1 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz) ((-Double
cy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz) Double
sy
    v2 :: V3
v2 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz) ((-Double
sy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz) ((-Double
cy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx)
    v3 :: V3
v3 = Double -> Double -> Double -> V3
Math3d.vec3 ((-Double
sy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz) (Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz) (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
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 'Angle's

-- 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 -> [Math3d.V3]
zyx2r :: Angle -> Angle -> Angle -> [V3]
zyx2r Angle
z Angle
y Angle
x = [V3
v1, V3
v2, V3
v3]
  where
    cx :: Double
cx = Angle -> Double
Angle.cos Angle
x
    sx :: Double
sx = Angle -> Double
Angle.sin Angle
x
    cy :: Double
cy = Angle -> Double
Angle.cos Angle
y
    sy :: Double
sy = Angle -> Double
Angle.sin Angle
y
    cz :: Double
cz = Angle -> Double
Angle.cos Angle
z
    sz :: Double
sz = Angle -> Double
Angle.sin Angle
z
    v1 :: V3
v1 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy) ((-Double
sz) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx) (Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx)
    v2 :: V3
v2 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy) (Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx) ((-Double
cz) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx)
    v3 :: V3
v3 = Double -> Double -> Double -> V3
Math3d.vec3 (-Double
sy) (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx) (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx)