{-# OPTIONS_GHC -Wall #-}
{-# Language ScopedTypeVariables #-}

module SpatialMath
       ( Euler(..)
       , ArcTan2(..)
       , rotateXyzAboutX
       , rotateXyzAboutY
       , rotateXyzAboutZ
       , euler321OfQuat
       , euler321OfDcm
       , unsafeEuler321OfDcm
       , quatOfEuler321
       , dcmOfQuat
       , dcmOfQuatB2A
       , dcmOfEuler321
       , quatOfDcm
       , quatOfDcmB2A
       , rotVecByDcm
       , rotVecByDcmB2A
       , rotVecByQuat
       , rotVecByQuatB2A
       , rotVecByEuler
       , rotVecByEulerB2A
         -- * re-exported from linear
       , M33
       , V3(..)
       , Quaternion(..)
       ) where

import Linear

import Types

-- | doesn't require RealFloat, used for overloading symbolics
class Floating a => ArcTan2 a where
  arctan2 :: a -> a -> a
instance ArcTan2 Double where arctan2 :: Double -> Double -> Double
arctan2 = forall a. RealFloat a => a -> a -> a
atan2
instance ArcTan2 Float where arctan2 :: Float -> Float -> Float
arctan2 = forall a. RealFloat a => a -> a -> a
atan2

-- $setup
-- |
-- >>> :{
--     let trunc :: Functor f => f Double -> f Double
--         trunc = fmap trunc'
--           where
--             trunc' x
--               | nearZero x = 0
--               | nearZero (x - 1) = 1
--               | nearZero (x + 1) = -1
--               | otherwise = x
-- :}

normalize' :: Floating a => Quaternion a -> Quaternion a
normalize' :: forall a. Floating a => Quaternion a -> Quaternion a
normalize' Quaternion a
q = forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (forall a. Num a => a -> a -> a
* a
normInv) Quaternion a
q
  where
    normInv :: a
normInv = a
1forall a. Fractional a => a -> a -> a
/(forall (f :: * -> *) a. (Metric f, Floating a) => f a -> a
norm Quaternion a
q)

--normalize' :: (Floating a, Epsilon a) => Quaternion a -> Quaternion a
--normalize' = normalize

-- | Rotate a vector about the X axis
--
-- >>> trunc $ rotateXyzAboutX (V3 0 1 0) (pi/2)
-- V3 0.0 0.0 1.0
--
-- >>> trunc $ rotateXyzAboutX (V3 0 0 1) (pi/2)
-- V3 0.0 (-1.0) 0.0
rotateXyzAboutX :: Floating a => V3 a -> a -> V3 a
rotateXyzAboutX :: forall a. Floating a => V3 a -> a -> V3 a
rotateXyzAboutX (V3 a
ax a
ay a
az) a
rotAngle = forall a. a -> a -> a -> V3 a
V3 a
bx a
by a
bz
  where
    cosTheta :: a
cosTheta = forall a. Floating a => a -> a
cos a
rotAngle
    sinTheta :: a
sinTheta = forall a. Floating a => a -> a
sin a
rotAngle

    bx :: a
bx =  a
ax
    by :: a
by =  a
ayforall a. Num a => a -> a -> a
*a
cosTheta forall a. Num a => a -> a -> a
- a
azforall a. Num a => a -> a -> a
*a
sinTheta
    bz :: a
bz =  a
ayforall a. Num a => a -> a -> a
*a
sinTheta forall a. Num a => a -> a -> a
+ a
azforall a. Num a => a -> a -> a
*a
cosTheta

-- | Rotate a vector about the Y axis
--
-- >>> trunc $ rotateXyzAboutY (V3 0 0 1) (pi/2)
-- V3 1.0 0.0 0.0
--
-- >>> trunc $ rotateXyzAboutY (V3 1 0 0) (pi/2)
-- V3 0.0 0.0 (-1.0)
rotateXyzAboutY :: Floating a => V3 a -> a -> V3 a
rotateXyzAboutY :: forall a. Floating a => V3 a -> a -> V3 a
rotateXyzAboutY (V3 a
ax a
ay a
az) a
rotAngle = forall a. a -> a -> a -> V3 a
V3 a
bx a
by a
bz
  where
    cosTheta :: a
cosTheta = forall a. Floating a => a -> a
cos a
rotAngle
    sinTheta :: a
sinTheta = forall a. Floating a => a -> a
sin a
rotAngle

    bx :: a
bx =  a
axforall a. Num a => a -> a -> a
*a
cosTheta forall a. Num a => a -> a -> a
+ a
azforall a. Num a => a -> a -> a
*a
sinTheta
    by :: a
by =  a
ay
    bz :: a
bz = -a
axforall a. Num a => a -> a -> a
*a
sinTheta forall a. Num a => a -> a -> a
+ a
azforall a. Num a => a -> a -> a
*a
cosTheta

-- | Rotate a vector about the Z axis
--
-- >>> trunc $ rotateXyzAboutZ (V3 1 0 0) (pi/2)
-- V3 0.0 1.0 0.0
--
-- >>> trunc $ rotateXyzAboutZ (V3 0 1 0) (pi/2)
-- V3 (-1.0) 0.0 0.0
--
rotateXyzAboutZ :: Floating a => V3 a -> a -> V3 a
rotateXyzAboutZ :: forall a. Floating a => V3 a -> a -> V3 a
rotateXyzAboutZ (V3 a
ax a
ay a
az) a
rotAngle = forall a. a -> a -> a -> V3 a
V3 a
bx a
by a
bz
  where
    cosTheta :: a
cosTheta = forall a. Floating a => a -> a
cos a
rotAngle
    sinTheta :: a
sinTheta = forall a. Floating a => a -> a
sin a
rotAngle

    bx :: a
bx =  a
axforall a. Num a => a -> a -> a
*a
cosTheta forall a. Num a => a -> a -> a
- a
ayforall a. Num a => a -> a -> a
*a
sinTheta
    by :: a
by =  a
axforall a. Num a => a -> a -> a
*a
sinTheta forall a. Num a => a -> a -> a
+ a
ayforall a. Num a => a -> a -> a
*a
cosTheta
    bz :: a
bz =  a
az


-- | Convert quaternion to Euler angles
--
-- >>> euler321OfQuat (Quaternion 1.0 (V3 0.0 0.0 0.0))
-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 0.0}
--
-- >>> euler321OfQuat (Quaternion (sqrt(2)/2) (V3 (sqrt(2)/2) 0.0 0.0))
-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 1.5707963267948966}
--
-- >>> euler321OfQuat (Quaternion (sqrt(2)/2) (V3 0.0 (sqrt(2)/2) 0.0))
-- Euler {eYaw = 0.0, ePitch = 1.5707963267948966, eRoll = 0.0}
--
-- >>> euler321OfQuat (Quaternion (sqrt(2)/2) (V3 0.0 0.0 (sqrt(2)/2)))
-- Euler {eYaw = 1.5707963267948966, ePitch = -0.0, eRoll = 0.0}
--
euler321OfQuat :: (ArcTan2 a, Ord a) => Quaternion a -> Euler a
euler321OfQuat :: forall a. (ArcTan2 a, Ord a) => Quaternion a -> Euler a
euler321OfQuat (Quaternion a
q0 (V3 a
q1 a
q2 a
q3)) = forall a. a -> a -> a -> Euler a
Euler a
yaw a
pitch a
roll
  where
    r11 :: a
r11 = a
q0forall a. Num a => a -> a -> a
*a
q0 forall a. Num a => a -> a -> a
+ a
q1forall a. Num a => a -> a -> a
*a
q1 forall a. Num a => a -> a -> a
- a
q2forall a. Num a => a -> a -> a
*a
q2 forall a. Num a => a -> a -> a
- a
q3forall a. Num a => a -> a -> a
*a
q3
    r12 :: a
r12 = a
2.0forall a. Num a => a -> a -> a
*(a
q1forall a. Num a => a -> a -> a
*a
q2 forall a. Num a => a -> a -> a
+ a
q0forall a. Num a => a -> a -> a
*a
q3)
    mr13' :: a
mr13' = -a
2.0forall a. Num a => a -> a -> a
*(a
q1forall a. Num a => a -> a -> a
*a
q3 forall a. Num a => a -> a -> a
- a
q0forall a. Num a => a -> a -> a
*a
q2)
    mr13 :: a
mr13 -- nan protect
      | a
mr13' forall a. Ord a => a -> a -> Bool
>  a
1 =  a
1
      | a
mr13' forall a. Ord a => a -> a -> Bool
< -a
1 = -a
1
      | Bool
otherwise = a
mr13'
    r23 :: a
r23 = a
2.0forall a. Num a => a -> a -> a
*(a
q2forall a. Num a => a -> a -> a
*a
q3 forall a. Num a => a -> a -> a
+ a
q0forall a. Num a => a -> a -> a
*a
q1)
    r33 :: a
r33 = a
q0forall a. Num a => a -> a -> a
*a
q0 forall a. Num a => a -> a -> a
- a
q1forall a. Num a => a -> a -> a
*a
q1 forall a. Num a => a -> a -> a
- a
q2forall a. Num a => a -> a -> a
*a
q2 forall a. Num a => a -> a -> a
+ a
q3forall a. Num a => a -> a -> a
*a
q3

    yaw :: a
yaw   = forall a. ArcTan2 a => a -> a -> a
arctan2 a
r12 a
r11
    pitch :: a
pitch = forall a. Floating a => a -> a
asin a
mr13
    roll :: a
roll  = forall a. ArcTan2 a => a -> a -> a
arctan2 a
r23 a
r33

-- | convert a DCM to a quaternion
--
-- >>> quatOfDcm $ V3 (V3 1 0 0) (V3 0 1 0) (V3 0 0 1)
-- Quaternion 1.0 (V3 (-0.0) (-0.0) (-0.0))
--
-- >>> quatOfDcm $ V3 (V3 0 1 0) (V3 (-1) 0 0) (V3 0 0 1)
-- Quaternion 0.7071067811865477 (V3 (-0.0) (-0.0) 0.7071067811865474)
--
-- >>> let s = sqrt(2)/2 in quatOfDcm $ V3 (V3 s s 0) (V3 (-s) s 0) (V3 0 0 1)
-- Quaternion 0.9238795325112868 (V3 (-0.0) (-0.0) 0.3826834323650898)
quatOfDcm :: Floating a => M33 a -> Quaternion a
quatOfDcm :: forall a. Floating a => M33 a -> Quaternion a
quatOfDcm
  (V3
   (V3 a
r11 a
r12 a
r13)
   (V3 a
r21 a
r22 a
r23)
   (V3 a
r31 a
r32 a
r33)) = forall a. a -> V3 a -> Quaternion a
Quaternion a
q0 (forall a. a -> a -> a -> V3 a
V3 a
qi a
qj a
qk)
  where
    q0 :: a
q0 = a
0.5 forall a. Num a => a -> a -> a
* forall a. Floating a => a -> a
sqrt (a
1e-15 forall a. Num a => a -> a -> a
+ (a
1 forall a. Num a => a -> a -> a
+ a
r11 forall a. Num a => a -> a -> a
+ a
r22 forall a. Num a => a -> a -> a
+ a
r33))
    qi :: a
qi = forall a. Num a => a -> a
negate (a
r32 forall a. Num a => a -> a -> a
- a
r23) forall a. Fractional a => a -> a -> a
/ a
fourQ0
    qj :: a
qj = forall a. Num a => a -> a
negate (a
r13 forall a. Num a => a -> a -> a
- a
r31) forall a. Fractional a => a -> a -> a
/ a
fourQ0
    qk :: a
qk = forall a. Num a => a -> a
negate (a
r21 forall a. Num a => a -> a -> a
- a
r12) forall a. Fractional a => a -> a -> a
/ a
fourQ0
    fourQ0 :: a
fourQ0 = a
4 forall a. Num a => a -> a -> a
* a
q0


quatOfDcmB2A :: Floating a => M33 a -> Quaternion a
quatOfDcmB2A :: forall a. Floating a => M33 a -> Quaternion a
quatOfDcmB2A = forall a. Num a => Quaternion a -> Quaternion a
quatConjugate forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. Floating a => M33 a -> Quaternion a
quatOfDcm

-- | Convert DCM to euler angles
--
-- >>> euler321OfDcm $ V3 (V3 1 0 0) (V3 0 1 0) (V3 0 0 1)
-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 0.0}
--
-- >>> euler321OfDcm $ V3 (V3 0 1 0) (V3 (-1) 0 0) (V3 0 0 1)
-- Euler {eYaw = 1.5707963267948966, ePitch = -0.0, eRoll = 0.0}
--
-- >>> let s = sqrt(2)/2 in euler321OfDcm $ V3 (V3 s s 0) (V3 (-s) s 0) (V3 0 0 1)
-- Euler {eYaw = 0.7853981633974483, ePitch = -0.0, eRoll = 0.0}
--
euler321OfDcm :: (Ord a, ArcTan2 a) => M33 a -> Euler a
euler321OfDcm :: forall a. (Ord a, ArcTan2 a) => M33 a -> Euler a
euler321OfDcm
  (V3
   (V3 a
r11 a
r12 a
r13)
   (V3   a
_   a
_ a
r23)
   (V3   a
_   a
_ a
r33)) = forall a. a -> a -> a -> Euler a
Euler a
yaw a
pitch a
roll
  where
    mr13' :: a
mr13' = -a
r13
    mr13 :: a
mr13 -- nan protect
      | a
mr13' forall a. Ord a => a -> a -> Bool
>  a
1 =  a
1
      | a
mr13' forall a. Ord a => a -> a -> Bool
< -a
1 = -a
1
      | Bool
otherwise = a
mr13'

    yaw :: a
yaw   = forall a. ArcTan2 a => a -> a -> a
arctan2 a
r12 a
r11
    pitch :: a
pitch = forall a. Floating a => a -> a
asin a
mr13
    roll :: a
roll  = forall a. ArcTan2 a => a -> a -> a
arctan2 a
r23 a
r33

-- | Convert DCM to euler angles. Returns Nan if r[1,3] is outside (-1, 1).
--
-- >>> unsafeEuler321OfDcm $ V3 (V3 1 0 0) (V3 0 1 0) (V3 0 0 1)
-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 0.0}
--
-- >>> unsafeEuler321OfDcm $ V3 (V3 0 1 0) (V3 (-1) 0 0) (V3 0 0 1)
-- Euler {eYaw = 1.5707963267948966, ePitch = -0.0, eRoll = 0.0}
--
-- >>> let s = sqrt(2)/2 in unsafeEuler321OfDcm $ V3 (V3 s s 0) (V3 (-s) s 0) (V3 0 0 1)
-- Euler {eYaw = 0.7853981633974483, ePitch = -0.0, eRoll = 0.0}
--
-- >>> unsafeEuler321OfDcm $ V3 (V3 0 0 1.1) (V3 0 0 0) (V3 0 0 0)
-- Euler {eYaw = 0.0, ePitch = NaN, eRoll = 0.0}
--
unsafeEuler321OfDcm :: ArcTan2 a => M33 a -> Euler a
unsafeEuler321OfDcm :: forall a. ArcTan2 a => M33 a -> Euler a
unsafeEuler321OfDcm
  (V3
   (V3 a
r11 a
r12 a
r13)
   (V3   a
_   a
_ a
r23)
   (V3   a
_   a
_ a
r33)) = forall a. a -> a -> a -> Euler a
Euler a
yaw a
pitch a
roll
  where
    yaw :: a
yaw   = forall a. ArcTan2 a => a -> a -> a
arctan2 a
r12 a
r11
    pitch :: a
pitch = forall a. Floating a => a -> a
asin (-a
r13)
    roll :: a
roll  = forall a. ArcTan2 a => a -> a -> a
arctan2 a
r23 a
r33

-- | Convert Euler angles to quaternion
--
-- >>> quatOfEuler321 (Euler 0 0 0)
-- Quaternion 1.0 (V3 0.0 0.0 0.0)
--
-- >>> quatOfEuler321 (Euler (pi/2) 0 0)
-- Quaternion 0.7071067811865476 (V3 0.0 0.0 0.7071067811865475)
--
-- >>> quatOfEuler321 (Euler 0 (pi/2) 0)
-- Quaternion 0.7071067811865476 (V3 0.0 0.7071067811865475 0.0)
--
-- >>> quatOfEuler321 (Euler 0 0 (pi/2))
-- Quaternion 0.7071067811865476 (V3 0.7071067811865475 0.0 0.0)
--
quatOfEuler321 :: (Floating a, Ord a) => Euler a -> Quaternion a
quatOfEuler321 :: forall a. (Floating a, Ord a) => Euler a -> Quaternion a
quatOfEuler321 (Euler a
yaw a
pitch a
roll) = forall a. Floating a => Quaternion a -> Quaternion a
normalize' Quaternion a
q
  where
    sr2 :: a
sr2 = forall a. Floating a => a -> a
sin forall a b. (a -> b) -> a -> b
$ a
0.5forall a. Num a => a -> a -> a
*a
roll
    cr2 :: a
cr2 = forall a. Floating a => a -> a
cos forall a b. (a -> b) -> a -> b
$ a
0.5forall a. Num a => a -> a -> a
*a
roll
    sp2 :: a
sp2 = forall a. Floating a => a -> a
sin forall a b. (a -> b) -> a -> b
$ a
0.5forall a. Num a => a -> a -> a
*a
pitch
    cp2 :: a
cp2 = forall a. Floating a => a -> a
cos forall a b. (a -> b) -> a -> b
$ a
0.5forall a. Num a => a -> a -> a
*a
pitch
    sy2 :: a
sy2 = forall a. Floating a => a -> a
sin forall a b. (a -> b) -> a -> b
$ a
0.5forall a. Num a => a -> a -> a
*a
yaw
    cy2 :: a
cy2 = forall a. Floating a => a -> a
cos forall a b. (a -> b) -> a -> b
$ a
0.5forall a. Num a => a -> a -> a
*a
yaw
    q0 :: a
q0 = a
cr2forall a. Num a => a -> a -> a
*a
cp2forall a. Num a => a -> a -> a
*a
cy2 forall a. Num a => a -> a -> a
+ a
sr2forall a. Num a => a -> a -> a
*a
sp2forall a. Num a => a -> a -> a
*a
sy2
    q1 :: a
q1 = a
sr2forall a. Num a => a -> a -> a
*a
cp2forall a. Num a => a -> a -> a
*a
cy2 forall a. Num a => a -> a -> a
- a
cr2forall a. Num a => a -> a -> a
*a
sp2forall a. Num a => a -> a -> a
*a
sy2
    q2 :: a
q2 = a
cr2forall a. Num a => a -> a -> a
*a
sp2forall a. Num a => a -> a -> a
*a
cy2 forall a. Num a => a -> a -> a
+ a
sr2forall a. Num a => a -> a -> a
*a
cp2forall a. Num a => a -> a -> a
*a
sy2
    q3 :: a
q3 = a
cr2forall a. Num a => a -> a -> a
*a
cp2forall a. Num a => a -> a -> a
*a
sy2 forall a. Num a => a -> a -> a
- a
sr2forall a. Num a => a -> a -> a
*a
sp2forall a. Num a => a -> a -> a
*a
cy2

    q' :: Quaternion a
q' = forall a. a -> V3 a -> Quaternion a
Quaternion a
q0 (forall a. a -> a -> a -> V3 a
V3 a
q1 a
q2 a
q3)

    q :: Quaternion a
q
      | a
q0 forall a. Ord a => a -> a -> Bool
< a
0 = forall a. a -> V3 a -> Quaternion a
Quaternion (-a
q0) (forall a. a -> a -> a -> V3 a
V3 (-a
q1) (-a
q2) (-a
q3))
      | Bool
otherwise = Quaternion a
q'

-- | convert a quaternion to a DCM
--
-- >>> dcmOfQuat $ Quaternion 1.0 (V3 0.0 0.0 0.0)
-- V3 (V3 1.0 0.0 0.0) (V3 0.0 1.0 0.0) (V3 0.0 0.0 1.0)
--
-- >>> let s = sqrt(2)/2 in fmap trunc $ dcmOfQuat $ Quaternion s (V3 0.0 0.0 s)
-- V3 (V3 0.0 1.0 0.0) (V3 (-1.0) 0.0 0.0) (V3 0.0 0.0 1.0)
--
-- >>> dcmOfQuat $ Quaternion 0.9238795325112867 (V3 0.0 0.0 0.3826834323650898)
-- V3 (V3 0.7071067811865475 0.7071067811865476 0.0) (V3 (-0.7071067811865476) 0.7071067811865475 0.0) (V3 0.0 0.0 1.0)
--
dcmOfQuat :: Num a => Quaternion a -> M33 a
dcmOfQuat :: forall a. Num a => Quaternion a -> M33 a
dcmOfQuat Quaternion a
q = forall a. a -> a -> a -> V3 a
V3
              (forall a. a -> a -> a -> V3 a
V3 a
m11 a
m21 a
m31)
              (forall a. a -> a -> a -> V3 a
V3 a
m12 a
m22 a
m32)
              (forall a. a -> a -> a -> V3 a
V3 a
m13 a
m23 a
m33)
  where
    V3
      (V3 a
m11 a
m12 a
m13)
      (V3 a
m21 a
m22 a
m23)
      (V3 a
m31 a
m32 a
m33) = forall a. Num a => Quaternion a -> M33 a
fromQuaternion Quaternion a
q

-- | Convert DCM to euler angles
--
-- >>> fmap trunc $ dcmOfEuler321 $ Euler {eYaw = 0.0, ePitch = 0, eRoll = 0}
-- V3 (V3 1.0 0.0 0.0) (V3 0.0 1.0 0.0) (V3 0.0 0.0 1.0)
--
-- >>> fmap trunc $ dcmOfEuler321 $ Euler {eYaw = pi/2, ePitch = 0, eRoll = 0}
-- V3 (V3 0.0 1.0 0.0) (V3 (-1.0) 0.0 0.0) (V3 0.0 0.0 1.0)
--
-- >>> fmap trunc $ dcmOfEuler321 $ Euler {eYaw = pi/4, ePitch = 0, eRoll = 0}
-- V3 (V3 0.7071067811865476 0.7071067811865475 0.0) (V3 (-0.7071067811865475) 0.7071067811865476 0.0) (V3 0.0 0.0 1.0)
--
dcmOfEuler321 :: Floating a => Euler a -> M33 a
dcmOfEuler321 :: forall a. Floating a => Euler a -> M33 a
dcmOfEuler321 Euler a
euler = V3 (V3 a)
dcm
  where
    cPs :: a
cPs = forall a. Floating a => a -> a
cos (forall a. Euler a -> a
eYaw Euler a
euler)
    sPs :: a
sPs = forall a. Floating a => a -> a
sin (forall a. Euler a -> a
eYaw Euler a
euler)
    cTh :: a
cTh = forall a. Floating a => a -> a
cos (forall a. Euler a -> a
ePitch Euler a
euler)
    sTh :: a
sTh = forall a. Floating a => a -> a
sin (forall a. Euler a -> a
ePitch Euler a
euler)
    cPh :: a
cPh = forall a. Floating a => a -> a
cos (forall a. Euler a -> a
eRoll Euler a
euler)
    sPh :: a
sPh = forall a. Floating a => a -> a
sin (forall a. Euler a -> a
eRoll Euler a
euler)

    dcm :: V3 (V3 a)
dcm =
      forall a. a -> a -> a -> V3 a
V3
      (forall a. a -> a -> a -> V3 a
V3 (a
cThforall a. Num a => a -> a -> a
*a
cPs) (a
cThforall a. Num a => a -> a -> a
*a
sPs) (-a
sTh))
      (forall a. a -> a -> a -> V3 a
V3 (a
cPsforall a. Num a => a -> a -> a
*a
sThforall a. Num a => a -> a -> a
*a
sPh forall a. Num a => a -> a -> a
- a
cPhforall a. Num a => a -> a -> a
*a
sPs) ( a
cPhforall a. Num a => a -> a -> a
*a
cPs forall a. Num a => a -> a -> a
+ a
sThforall a. Num a => a -> a -> a
*a
sPhforall a. Num a => a -> a -> a
*a
sPs) (a
cThforall a. Num a => a -> a -> a
*a
sPh))
      (forall a. a -> a -> a -> V3 a
V3 (a
cPhforall a. Num a => a -> a -> a
*a
cPsforall a. Num a => a -> a -> a
*a
sTh forall a. Num a => a -> a -> a
+ a
sPhforall a. Num a => a -> a -> a
*a
sPs) (-a
cPsforall a. Num a => a -> a -> a
*a
sPh forall a. Num a => a -> a -> a
+ a
cPhforall a. Num a => a -> a -> a
*a
sThforall a. Num a => a -> a -> a
*a
sPs) (a
cThforall a. Num a => a -> a -> a
*a
cPh))

quatConjugate :: Num a => Quaternion a -> Quaternion a
quatConjugate :: forall a. Num a => Quaternion a -> Quaternion a
quatConjugate (Quaternion a
q0 V3 a
qv) = forall a. a -> V3 a -> Quaternion a
Quaternion a
q0 (forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap forall a. Num a => a -> a
negate V3 a
qv)

dcmOfQuatB2A :: Num a => Quaternion a -> M33 a
dcmOfQuatB2A :: forall a. Num a => Quaternion a -> M33 a
dcmOfQuatB2A = forall a. Num a => Quaternion a -> M33 a
dcmOfQuat forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. Num a => Quaternion a -> Quaternion a
quatConjugate

-- | vec_b = R_a2b * vec_a
rotVecByDcm :: Num a => M33 a -> V3 a -> V3 a
rotVecByDcm :: forall a. Num a => M33 a -> V3 a -> V3 a
rotVecByDcm M33 a
dcm V3 a
vec = M33 a
dcm forall (m :: * -> *) (r :: * -> *) a.
(Functor m, Foldable r, Additive r, Num a) =>
m (r a) -> r a -> m a
!* V3 a
vec

-- | vec_a = R_a2b^T * vec_b
rotVecByDcmB2A :: Num a => M33 a -> V3 a -> V3 a
rotVecByDcmB2A :: forall a. Num a => M33 a -> V3 a -> V3 a
rotVecByDcmB2A M33 a
dcm V3 a
vec = V3 a
vec forall a (t :: * -> *) (f :: * -> *).
(Num a, Foldable t, Additive f, Additive t) =>
t a -> t (f a) -> f a
*! M33 a
dcm

-- | vec_b = q_a2b * vec_a * q_a2b^(-1)
--   vec_b = R(q_a2b) * vec_a
rotVecByQuat :: Num a => Quaternion a -> V3 a -> V3 a
rotVecByQuat :: forall a. Num a => Quaternion a -> V3 a -> V3 a
rotVecByQuat Quaternion a
q = forall a. Num a => M33 a -> V3 a -> V3 a
rotVecByDcm (forall a. Num a => Quaternion a -> M33 a
dcmOfQuat Quaternion a
q)

rotVecByQuatB2A :: Num a => Quaternion a -> V3 a -> V3 a
rotVecByQuatB2A :: forall a. Num a => Quaternion a -> V3 a -> V3 a
rotVecByQuatB2A Quaternion a
q = forall a. Num a => M33 a -> V3 a -> V3 a
rotVecByDcmB2A (forall a. Num a => Quaternion a -> M33 a
dcmOfQuat Quaternion a
q)

rotVecByEuler :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a
rotVecByEuler :: forall a. (Floating a, Ord a) => Euler a -> V3 a -> V3 a
rotVecByEuler = forall a. Num a => M33 a -> V3 a -> V3 a
rotVecByDcm forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. Floating a => Euler a -> M33 a
dcmOfEuler321

rotVecByEulerB2A :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a
rotVecByEulerB2A :: forall a. (Floating a, Ord a) => Euler a -> V3 a -> V3 a
rotVecByEulerB2A = forall a. Num a => M33 a -> V3 a -> V3 a
rotVecByDcmB2A forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. Floating a => Euler a -> M33 a
dcmOfEuler321