-- |
-- Module: Data.Geo.Jord.Rotation
-- Copyright: (c) 2018 Cedric Liegeois
-- License: BSD3
-- Maintainer: Cedric Liegeois
-- Stability: experimental
-- Portability: portable
--
-- Rotation matrices from/to 3 angles about new axes
--
-- All functions are implemented using the vector-based approached described in
--
--
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)