spatial-math 0.2.0.1 → 0.2.1.0
raw patch · 6 files changed
+474/−305 lines, 6 filesdep +cereal
Dependencies added: cereal
Files
- SpatialMath.hs +0/−303
- spatial-math.cabal +5/−1
- src/SpatialMath.hs +276/−0
- src/SpatialMathT.hs +143/−0
- src/Types.hs +49/−0
- tests/doctests.hs +1/−1
− SpatialMath.hs
@@ -1,303 +0,0 @@-{-# OPTIONS_GHC -Wall #-}-{-# Language StandaloneDeriving #-}-{-# Language DeriveDataTypeable #-}-{-# LANGUAGE CPP #-}-#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702-{-# Language DeriveGeneric #-}-#endif-{-# Language DeriveFunctor #-}-{-# Language DeriveFoldable #-}-{-# Language DeriveTraversable #-}--module SpatialMath ( Euler(..)- , rotateXyzAboutX- , rotateXyzAboutY- , rotateXyzAboutZ- , euler321OfQuat- , euler321OfDcm- , quatOfEuler321- , dcmOfQuat- , dcmOfQuatB2A- , dcmOfEuler321- , quatOfDcm- , quatOfDcmB2A- , rotVecByDcm- , rotVecByDcmB2A- , rotVecByQuat- , rotVecByQuatB2A- , rotVecByEuler- , rotVecByEulerB2A- -- * re-exported from linear- , M33- , V3(..)- , Quaternion(..)- ) where--import Data.Data ( Data )-import Data.Foldable ( Foldable )-import Data.Traversable ( Traversable )-import Data.Typeable ( Typeable1 )-#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702-import GHC.Generics (Generic)-#endif-#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706-import GHC.Generics (Generic1)-#endif-import Linear--normalize' :: Floating a => Quaternion a -> Quaternion a-normalize' q = fmap (* normInv) q- where- normInv = 1/(norm q)----normalize' :: (Floating a, Epsilon a) => Quaternion a -> Quaternion a---normalize' = normalize---- | 3-2-1 Euler angle rotation sequence-data Euler a = Euler { eYaw :: a- , ePitch :: a- , eRoll :: a- } deriving (Eq, Show, Functor, Foldable, Traversable, Ord-#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702- , Generic-#endif-#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706- , Generic1-#endif- )--deriving instance Typeable1 Euler-deriving instance Data a => Data (Euler a)---- | Rotate a vector about the X axis------ >>> rotateXyzAboutX (V3 0 1 0) (pi/2)--- V3 0.0 6.123233995736766e-17 1.0------ >>> rotateXyzAboutX (V3 0 0 1) (pi/2)--- V3 0.0 (-1.0) 6.123233995736766e-17-rotateXyzAboutX :: Floating a => V3 a -> a -> V3 a-rotateXyzAboutX (V3 ax ay az) rotAngle = V3 bx by bz- where- cosTheta = cos rotAngle- sinTheta = sin rotAngle-- bx = ax- by = ay*cosTheta - az*sinTheta- bz = ay*sinTheta + az*cosTheta---- | Rotate a vector about the Y axis------ >>> rotateXyzAboutY (V3 0 0 1) (pi/2)--- V3 1.0 0.0 6.123233995736766e-17------ >>> rotateXyzAboutY (V3 1 0 0) (pi/2)--- V3 6.123233995736766e-17 0.0 (-1.0)-rotateXyzAboutY :: Floating a => V3 a -> a -> V3 a-rotateXyzAboutY (V3 ax ay az) rotAngle = V3 bx by bz- where- cosTheta = cos rotAngle- sinTheta = sin rotAngle-- bx = ax*cosTheta + az*sinTheta- by = ay- bz = -ax*sinTheta + az*cosTheta---- | Rotate a vector about the Z axis------ >>> rotateXyzAboutZ (V3 1 0 0) (pi/2)--- V3 6.123233995736766e-17 1.0 0.0------ >>> rotateXyzAboutZ (V3 0 1 0) (pi/2)--- V3 (-1.0) 6.123233995736766e-17 0.0----rotateXyzAboutZ :: Floating a => V3 a -> a -> V3 a-rotateXyzAboutZ (V3 ax ay az) rotAngle = V3 bx by bz- where- cosTheta = cos rotAngle- sinTheta = sin rotAngle-- bx = ax*cosTheta - ay*sinTheta- by = ax*sinTheta + ay*cosTheta- bz = 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 :: RealFloat a => Quaternion a -> Euler a-euler321OfQuat (Quaternion q0 (V3 q1 q2 q3)) = Euler yaw pitch roll- where- r11 = q0*q0 + q1*q1 - q2*q2 - q3*q3- r12 = 2.0*(q1*q2 + q0*q3)- mr13' = -2.0*(q1*q3 - q0*q2)- mr13 -- nan protect- | mr13' > 1 = 1- | mr13' < -1 = -1- | otherwise = mr13'- r23 = 2.0*(q2*q3 + q0*q1)- r33 = q0*q0 - q1*q1 - q2*q2 + q3*q3-- yaw = atan2 r12 r11- pitch = asin mr13- roll = atan2 r23 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.7071067811865476 (V3 0.0 0.0 0.7071067811865475)------ >>> let s = sqrt(2)/2 in quatOfDcm $ V3 (V3 s s 0) (V3 (-s) s 0) (V3 0 0 1)--- Quaternion 0.9238795325112867 (V3 0.0 0.0 0.3826834323650898)----quatOfDcm :: RealFloat a => M33 a -> Quaternion a-quatOfDcm = quatOfEuler321 . euler321OfDcm--quatOfDcmB2A :: (Conjugate a, RealFloat a) => M33 a -> Quaternion a-quatOfDcmB2A = conjugate . 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 :: RealFloat a => M33 a -> Euler a-euler321OfDcm- (V3- (V3 r11 r12 r13)- (V3 _ _ r23)- (V3 _ _ r33)) = Euler yaw pitch roll- where- mr13' = -r13- mr13 -- nan protect- | mr13' > 1 = 1- | mr13' < -1 = -1- | otherwise = mr13'-- yaw = atan2 r12 r11- pitch = asin mr13- roll = atan2 r23 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 (Euler yaw pitch roll) = normalize' q- where- sr2 = sin $ 0.5*roll- cr2 = cos $ 0.5*roll- sp2 = sin $ 0.5*pitch- cp2 = cos $ 0.5*pitch- sy2 = sin $ 0.5*yaw- cy2 = cos $ 0.5*yaw- q0 = cr2*cp2*cy2 + sr2*sp2*sy2- q1 = sr2*cp2*cy2 - cr2*sp2*sy2- q2 = cr2*sp2*cy2 + sr2*cp2*sy2- q3 = cr2*cp2*sy2 - sr2*sp2*cy2-- q' = Quaternion q0 (V3 q1 q2 q3)-- q- | q0 < 0 = Quaternion (-q0) (V3 (-q1) (-q2) (-q3))- | otherwise = 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 dcmOfQuat $ Quaternion s (V3 0.0 0.0 s)--- V3 (V3 0.0 1.0000000000000002 0.0) (V3 (-1.0000000000000002) 0.0 0.0) (V3 0.0 0.0 1.0000000000000002)------ >>> 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 (Quaternion q0 (V3 q1 q2 q3)) = V3 (V3 r0 r1 r2)- (V3 r3 r4 r5)- (V3 r6 r7 r8)- where- -- 1st column- r0 = q0*q0 + q1*q1 - q2*q2 - q3*q3- r3 = 2*(q1*q2 - q0*q3)- r6 = 2*(q1*q3 + q0*q2)-- -- 2nd column- r1 = 2*(q1*q2 + q0*q3)- r4 = q0*q0 - q1*q1 + q2*q2 - q3*q3- r7 = 2*(q2*q3 - q0*q1)-- -- 3rd column- r2 = 2*(q1*q3 - q0*q2)- r5 = 2*(q2*q3 + q0*q1)- r8 = q0*q0 - q1*q1 - q2*q2 + q3*q3---- | Convert DCM to euler angles------ >>> 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)------ >>> dcmOfEuler321 $ Euler {eYaw = pi/2, ePitch = 0, eRoll = 0}--- V3 (V3 2.220446049250313e-16 1.0 0.0) (V3 (-1.0) 2.220446049250313e-16 0.0) (V3 0.0 0.0 1.0)------ >>> dcmOfEuler321 $ Euler {eYaw = pi/4, ePitch = 0, eRoll = 0}--- V3 (V3 0.7071067811865475 0.7071067811865476 0.0) (V3 (-0.7071067811865476) 0.7071067811865475 0.0) (V3 0.0 0.0 1.0)----dcmOfEuler321 :: (Floating a, Ord a) => Euler a -> M33 a-dcmOfEuler321 = dcmOfQuat . quatOfEuler321--dcmOfQuatB2A :: (Conjugate a, RealFloat a) => Quaternion a -> M33 a-dcmOfQuatB2A = dcmOfQuat . conjugate---- | vec_b = R_a2b * vec_a-rotVecByDcm :: Num a => M33 a -> V3 a -> V3 a-rotVecByDcm dcm vec = dcm !* vec---- | vec_a = R_a2b^T * vec_b-rotVecByDcmB2A :: Num a => M33 a -> V3 a -> V3 a-rotVecByDcmB2A dcm vec = vec *! 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 q = rotVecByDcm (dcmOfQuat q)--rotVecByQuatB2A :: Num a => Quaternion a -> V3 a -> V3 a-rotVecByQuatB2A q = rotVecByDcmB2A (dcmOfQuat q)--rotVecByEuler :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a-rotVecByEuler = rotVecByDcm . dcmOfEuler321--rotVecByEulerB2A :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a-rotVecByEulerB2A = rotVecByDcmB2A . dcmOfEuler321
spatial-math.cabal view
@@ -1,5 +1,5 @@ name: spatial-math-version: 0.2.0.1+version: 0.2.1.0 synopsis: 3d math including quaternions/euler angles/dcms and utility functions description: This is a port of my 'mathlib' C library: `https://github.com/ghorn/mathlib` license: BSD3@@ -15,9 +15,13 @@ changelog.txt library+ hs-source-dirs: src exposed-modules: SpatialMath+ SpatialMathT+ other-modules: Types build-depends: base >= 4 && < 5, ghc-prim,+ cereal, linear >= 1.3.1 default-language: Haskell2010
+ src/SpatialMath.hs view
@@ -0,0 +1,276 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ScopedTypeVariables #-}++module SpatialMath+ ( Euler(..)+ , rotateXyzAboutX+ , rotateXyzAboutY+ , rotateXyzAboutZ+ , euler321OfQuat+ , euler321OfDcm+ , quatOfEuler321+ , dcmOfQuat+ , dcmOfQuatB2A+ , dcmOfEuler321+ , quatOfDcm+ , quatOfDcmB2A+ , rotVecByDcm+ , rotVecByDcmB2A+ , rotVecByQuat+ , rotVecByQuatB2A+ , rotVecByEuler+ , rotVecByEulerB2A+ -- * re-exported from linear+ , M33+ , V3(..)+ , Quaternion(..)+ ) where++import Linear++import Types++-- $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' q = fmap (* normInv) q+ where+ normInv = 1/(norm 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 (V3 ax ay az) rotAngle = V3 bx by bz+ where+ cosTheta = cos rotAngle+ sinTheta = sin rotAngle++ bx = ax+ by = ay*cosTheta - az*sinTheta+ bz = ay*sinTheta + az*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 (V3 ax ay az) rotAngle = V3 bx by bz+ where+ cosTheta = cos rotAngle+ sinTheta = sin rotAngle++ bx = ax*cosTheta + az*sinTheta+ by = ay+ bz = -ax*sinTheta + az*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 (V3 ax ay az) rotAngle = V3 bx by bz+ where+ cosTheta = cos rotAngle+ sinTheta = sin rotAngle++ bx = ax*cosTheta - ay*sinTheta+ by = ax*sinTheta + ay*cosTheta+ bz = 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 :: RealFloat a => Quaternion a -> Euler a+euler321OfQuat (Quaternion q0 (V3 q1 q2 q3)) = Euler yaw pitch roll+ where+ r11 = q0*q0 + q1*q1 - q2*q2 - q3*q3+ r12 = 2.0*(q1*q2 + q0*q3)+ mr13' = -2.0*(q1*q3 - q0*q2)+ mr13 -- nan protect+ | mr13' > 1 = 1+ | mr13' < -1 = -1+ | otherwise = mr13'+ r23 = 2.0*(q2*q3 + q0*q1)+ r33 = q0*q0 - q1*q1 - q2*q2 + q3*q3++ yaw = atan2 r12 r11+ pitch = asin mr13+ roll = atan2 r23 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.7071067811865476 (V3 0.0 0.0 0.7071067811865475)+--+-- >>> let s = sqrt(2)/2 in quatOfDcm $ V3 (V3 s s 0) (V3 (-s) s 0) (V3 0 0 1)+-- Quaternion 0.9238795325112867 (V3 0.0 0.0 0.3826834323650898)+--+quatOfDcm :: RealFloat a => M33 a -> Quaternion a+quatOfDcm = quatOfEuler321 . euler321OfDcm++quatOfDcmB2A :: (Conjugate a, RealFloat a) => M33 a -> Quaternion a+quatOfDcmB2A = conjugate . 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 :: RealFloat a => M33 a -> Euler a+euler321OfDcm+ (V3+ (V3 r11 r12 r13)+ (V3 _ _ r23)+ (V3 _ _ r33)) = Euler yaw pitch roll+ where+ mr13' = -r13+ mr13 -- nan protect+ | mr13' > 1 = 1+ | mr13' < -1 = -1+ | otherwise = mr13'++ yaw = atan2 r12 r11+ pitch = asin mr13+ roll = atan2 r23 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 (Euler yaw pitch roll) = normalize' q+ where+ sr2 = sin $ 0.5*roll+ cr2 = cos $ 0.5*roll+ sp2 = sin $ 0.5*pitch+ cp2 = cos $ 0.5*pitch+ sy2 = sin $ 0.5*yaw+ cy2 = cos $ 0.5*yaw+ q0 = cr2*cp2*cy2 + sr2*sp2*sy2+ q1 = sr2*cp2*cy2 - cr2*sp2*sy2+ q2 = cr2*sp2*cy2 + sr2*cp2*sy2+ q3 = cr2*cp2*sy2 - sr2*sp2*cy2++ q' = Quaternion q0 (V3 q1 q2 q3)++ q+ | q0 < 0 = Quaternion (-q0) (V3 (-q1) (-q2) (-q3))+ | otherwise = 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 q = V3+ (V3 m11 m21 m31)+ (V3 m12 m22 m32)+ (V3 m13 m23 m33)+ where+ V3+ (V3 m11 m12 m13)+ (V3 m21 m22 m23)+ (V3 m31 m32 m33) = fromQuaternion q++-- | Convert DCM to euler angles+--+-- >>> 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)+--+-- >>> dcmOfEuler321 $ Euler {eYaw = pi/4, ePitch = 0, eRoll = 0}+-- V3 (V3 0.7071067811865475 0.7071067811865476 0.0) (V3 (-0.7071067811865476) 0.7071067811865475 0.0) (V3 0.0 0.0 1.0)+--+dcmOfEuler321 :: (Floating a, Ord a) => Euler a -> M33 a+dcmOfEuler321 = dcmOfQuat . quatOfEuler321++dcmOfQuatB2A :: (Conjugate a, RealFloat a) => Quaternion a -> M33 a+dcmOfQuatB2A = dcmOfQuat . conjugate++-- | vec_b = R_a2b * vec_a+rotVecByDcm :: Num a => M33 a -> V3 a -> V3 a+rotVecByDcm dcm vec = dcm !* vec++-- | vec_a = R_a2b^T * vec_b+rotVecByDcmB2A :: Num a => M33 a -> V3 a -> V3 a+rotVecByDcmB2A dcm vec = vec *! 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 q = rotVecByDcm (dcmOfQuat q)++rotVecByQuatB2A :: Num a => Quaternion a -> V3 a -> V3 a+rotVecByQuatB2A q = rotVecByDcmB2A (dcmOfQuat q)++rotVecByEuler :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a+rotVecByEuler = rotVecByDcm . dcmOfEuler321++rotVecByEulerB2A :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a+rotVecByEulerB2A = rotVecByDcmB2A . dcmOfEuler321
+ src/SpatialMathT.hs view
@@ -0,0 +1,143 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language MultiParamTypeClasses #-}+{-# Language FunctionalDependencies #-}+{-# Language FlexibleInstances #-}+{-# Language GeneralizedNewtypeDeriving #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveFoldable #-}+{-# Language DeriveTraversable #-}+{-# Language DeriveGeneric #-}++module SpatialMathT+ ( Rotation(..)+ , Rot(..)+ , V3T(..)+ , M33T+ , cross+ , orthonormalize+ ) where++import Control.Applicative ( Applicative )+import Data.Foldable ( Foldable )+import Data.Serialize ( Serialize(..) )+import Data.Traversable ( Traversable )+import Foreign.Storable ( Storable )+import GHC.Generics ( Generic, Generic1 )++import Linear hiding ( cross )+import qualified Linear as L++import SpatialMath++newtype V3T f a = V3T {unV :: V3 a}+ deriving ( Functor, Foldable, Traversable+ , Applicative+ , Additive, Storable+ , Num, Fractional, Eq, Show+ , Generic1, Generic+ )++instance Serialize a => Serialize (V3T f a) where+ get = do+ x <- get+ y <- get+ z <- get+ return (V3T (V3 x y z))+ put (V3T (V3 x y z)) = do+ put x+ put y+ put z++cross :: Num a => V3T f a -> V3T f a -> V3T f a+cross (V3T vx) (V3T vy) = V3T (vx `L.cross` vy)++newtype Rot f1 f2 r =+ Rot { unR :: r }+ deriving ( Functor, Foldable, Traversable+ , Storable+ , Num, Fractional, Eq, Show, Serialize+ , Generic1, Generic+ )++type M33T f1 f2 a = V3T f1 (V3T f2 a)++class Rotation p a | p -> a where+ compose :: Rot f1 f2 p -> Rot f2 f3 p -> Rot f1 f3 p+ rot :: Rot f1 f2 p -> V3T f1 a -> V3T f2 a+ rot' :: Rot f1 f2 p -> V3T f2 a -> V3T f1 a+ toDcm :: Rot f1 f2 p -> Rot f1 f2 (M33 a)+-- fromDcm :: Rot f1 f2 (M33 a) -> Rot f1 f2 (p a)+ transpose :: Rot f1 f2 p -> Rot f2 f1 p++instance Num a => Rotation (Quaternion a) a where+ compose (Rot q_a2b) (Rot q_b2c) = Rot (q_a2b `quatMult` q_b2c)+ rot (Rot q_a2b) (V3T va) = V3T (rotVecByQuat q_a2b va)+ rot' (Rot q_a2b) (V3T vb) = V3T (rotVecByQuatB2A q_a2b vb)+ toDcm (Rot q_a2b) = Rot (dcmOfQuat q_a2b)+-- fromDcm (Rot dcm_a2b) = Rot (quatOfDcm dcm_a2b)+ transpose (Rot (Quaternion q0 qxyz)) = Rot (Quaternion q0 (fmap negate qxyz))++-- quaternion multiplication which doesn't require RealFrac+quatMult :: Num a => Quaternion a -> Quaternion a -> Quaternion a+quatMult (Quaternion s1 v1) (Quaternion s2 v2) =+ Quaternion (s1*s2 - (v1 `dot` v2)) $+ (v1 `L.cross` v2) + s1*^v2 + s2*^v1++instance Num a => Rotation (M33 a) a where+ compose (Rot dcm_a2b) (Rot dcm_b2c) = Rot (dcm_b2c !*! dcm_a2b)+ rot (Rot dcm_a2b) (V3T va) = V3T (rotVecByDcm dcm_a2b va)+ rot' (Rot dcm_a2b) (V3T vb) = V3T (rotVecByDcmB2A dcm_a2b vb)+ toDcm = id+ transpose (Rot (V3+ (V3 e11 e12 e13)+ (V3 e21 e22 e23)+ (V3 e31 e32 e33))) =+ Rot (V3+ (V3 e11 e21 e31)+ (V3 e12 e22 e32)+ (V3 e13 e23 e33))++orthonormalize :: Floating a => Rot f1 f2 (M33 a) -> Rot f1 f2 (M33 a)+orthonormalize (Rot (V3+ (V3 m00 m01 m02)+ (V3 m10 m11 m12)+ (V3 m20 m21 m22))) = Rot ret+ where+ -- compute q0+ fInvLength0 = 1.0/sqrt(m00*m00 + m10*m10 + m20*m20)++ m00' = m00*fInvLength0+ m10' = m10*fInvLength0+ m20' = m20*fInvLength0++ -- compute q1+ fDot0' = m00'*m01 + m10'*m11 + m20'*m21++ m01' = m01 - fDot0'*m00'+ m11' = m11 - fDot0'*m10'+ m21' = m21 - fDot0'*m20'++ fInvLength1 = 1.0/sqrt(m01'*m01' + m11'*m11' + m21'*m21')++ m01'' = m01' * fInvLength1+ m11'' = m11' * fInvLength1+ m21'' = m21' * fInvLength1++ -- compute q2+ fDot1 = m01''*m02 + m11''*m12 + m21''*m22+ fDot0 = m00'*m02 + m10'*m12 + m20'*m22++ m02' = m02 - (fDot0*m00' + fDot1*m01'')+ m12' = m12 - (fDot0*m10' + fDot1*m11'')+ m22' = m22 - (fDot0*m20' + fDot1*m21'')++ fInvLength2 = 1.0/sqrt(m02'*m02' + m12'*m12' + m22'*m22')++ m02'' = m02' * fInvLength2+ m12'' = m12' * fInvLength2+ m22'' = m22' * fInvLength2++ ret = (V3+ (V3 m00' m01'' m02'')+ (V3 m10' m11'' m12'')+ (V3 m20' m21'' m22''))
+ src/Types.hs view
@@ -0,0 +1,49 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language StandaloneDeriving #-}+{-# Language DeriveDataTypeable #-}+{-# LANGUAGE CPP #-}+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702+{-# Language DeriveGeneric #-}+#endif+{-# Language DeriveFunctor #-}+{-# Language DeriveFoldable #-}+{-# Language DeriveTraversable #-}++module Types ( Euler(..) ) where++import Data.Data ( Data )+import Data.Foldable ( Foldable )+import Data.Traversable ( Traversable )+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702+import GHC.Generics (Generic)+#endif+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706+import GHC.Generics (Generic1)+#endif+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 708+import Data.Typeable ( Typeable )+#else+import Data.Typeable ( Typeable1 )+#endif++-- | 3-2-1 Euler angle rotation sequence+data Euler a = Euler { eYaw :: a+ , ePitch :: a+ , eRoll :: a+ } deriving (Eq, Show, Functor, Foldable, Traversable, Ord+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702+ , Generic+#endif+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706+ , Generic1+#endif+ )++deriving instance Data a => Data (Euler a)++#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 708+deriving instance Typeable Euler+#else+deriving instance Typeable1 Euler+#endif+
tests/doctests.hs view
@@ -5,4 +5,4 @@ import Test.DocTest main :: IO ()-main = doctest ["SpatialMath.hs"]+main = doctest ["src/Types.hs", "src/SpatialMath.hs"]