spatial-math 0.2.7.0 → 0.3.0.0
raw patch · 5 files changed
+152/−56 lines, 5 filesdep +TypeCompose
Dependencies added: TypeCompose
Files
- LICENSE +1/−1
- spatial-math.cabal +3/−2
- src/SpatialMath.hs +32/−7
- src/SpatialMathT.hs +108/−43
- tests/Tests.hs +8/−3
LICENSE view
@@ -1,4 +1,4 @@-Copyright (c) 2012, Greg Horn+Copyright (c) 2012-2016, Greg Horn All rights reserved.
spatial-math.cabal view
@@ -1,5 +1,5 @@ name: spatial-math-version: 0.2.7.0+version: 0.3.0.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@@ -24,7 +24,8 @@ cereal, binary, linear >= 1.17.1,- lens+ lens,+ TypeCompose >= 0.9.11 default-language: Haskell2010 source-repository head
src/SpatialMath.hs view
@@ -8,6 +8,7 @@ , rotateXyzAboutY , rotateXyzAboutZ , euler321OfQuat+ , unsafeEuler321OfQuat , euler321OfDcm , unsafeEuler321OfDcm , quatOfEuler321@@ -143,6 +144,33 @@ pitch = asin mr13 roll = arctan2 r23 r33 +-- | Convert quaternion to Euler angles. Returns Nan if 2.0*(q1*q3 - q0*q2) is outside [-1, 1].+--+-- >>> unsafeEuler321OfQuat (Quaternion 1.0 (V3 0.0 0.0 0.0))+-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 0.0}+--+-- >>> unsafeEuler321OfQuat (Quaternion (sqrt(2)/2) (V3 (sqrt(2)/2) 0.0 0.0))+-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 1.5707963267948966}+--+-- >>> unsafeEuler321OfQuat (Quaternion (sqrt(2)/2) (V3 0.0 (sqrt(2)/2) 0.0))+-- Euler {eYaw = 0.0, ePitch = NaN, eRoll = 0.0}+--+-- >>> unsafeEuler321OfQuat (Quaternion (sqrt(2)/2) (V3 0.0 0.0 (sqrt(2)/2)))+-- Euler {eYaw = 1.5707963267948966, ePitch = -0.0, eRoll = 0.0}+--+unsafeEuler321OfQuat :: ArcTan2 a => Quaternion a -> Euler a+unsafeEuler321OfQuat (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)+ r23 = 2.0*(q2*q3 + q0*q1)+ r33 = q0*q0 - q1*q1 - q2*q2 + q3*q3++ yaw = arctan2 r12 r11+ pitch = asin mr13+ roll = arctan2 r23 r33+ -- | convert a DCM to a quaternion -- -- >>> quatOfDcm $ V3 (V3 1 0 0) (V3 0 1 0) (V3 0 0 1)@@ -198,7 +226,7 @@ pitch = asin mr13 roll = arctan2 r23 r33 --- | Convert DCM to euler angles. Returns Nan if r[1,3] is outside (-1, 1).+-- | 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}@@ -223,7 +251,7 @@ pitch = asin (-r13) roll = arctan2 r23 r33 --- | Convert Euler angles to quaternion+-- | Convert Euler angles to quaternion. The scalar part of the result may be positive or negative. -- -- >>> quatOfEuler321 (Euler 0 0 0) -- Quaternion 1.0 (V3 0.0 0.0 0.0)@@ -237,7 +265,7 @@ -- >>> 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 :: Floating a => Euler a -> Quaternion a quatOfEuler321 (Euler yaw pitch roll) = normalize' q where sr2 = sin $ 0.5*roll@@ -251,11 +279,8 @@ q2 = cr2*sp2*cy2 + sr2*cp2*sy2 q3 = cr2*cp2*sy2 - sr2*sp2*cy2 - q' = Quaternion q0 (V3 q1 q2 q3)+ q = Quaternion q0 (V3 q1 q2 q3) - q- | q0 < 0 = Quaternion (-q0) (V3 (-q1) (-q2) (-q3))- | otherwise = q' -- | convert a quaternion to a DCM --
src/SpatialMathT.hs view
@@ -7,18 +7,32 @@ {-# Language DeriveFoldable #-} {-# Language DeriveTraversable #-} {-# Language DeriveGeneric #-}+{-# Language TypeOperators #-} module SpatialMathT- ( Rotation(..)+ ( ArcTan2(..)+ , Euler(..)+ , Quaternion(..), V3(..)+ , Rotation(..) , Rot(..) , V3T(..) , R1(..), R2(..), R3(..)- , M33T , cross , orthonormalize+ , dcmOfQuat+ , dcmOfEuler321+ , quatOfDcm+ , quatOfEuler321+ , euler321OfDcm+ , unsafeEuler321OfDcm+ , euler321OfQuat+ , unsafeEuler321OfQuat+ -- * re-export for convenience+ , (:.)(..), unO ) where import Control.Applicative ( Applicative )+import Control.Compose ( (:.)(..), unO ) import Data.Foldable ( Foldable ) import Data.Binary ( Binary(..) ) import Data.Serialize ( Serialize(..) )@@ -26,10 +40,11 @@ import Foreign.Storable ( Storable ) import GHC.Generics ( Generic, Generic1 ) -import Linear hiding ( cross )+import Linear hiding ( cross, normalize, transpose ) import qualified Linear as L -import SpatialMath+import SpatialMath ( ArcTan2(..), Euler(..) )+import qualified SpatialMath as SM newtype V3T f a = V3T {unV :: V3 a} deriving ( Functor, Foldable, Traversable@@ -57,8 +72,8 @@ 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 }+newtype Rot f1 f2 r a =+ Rot { unRot :: r a } deriving ( Functor, Foldable, Traversable , Storable , Num, Fractional, Eq, Show, Ord@@ -66,49 +81,99 @@ , Serialize, Binary ) -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+class Rotation g a where+ compose :: Rot f1 f2 g a -> Rot f2 f3 g a -> Rot f1 f3 g a+ rot :: Rot f1 f2 g a -> V3T f1 a -> V3T f2 a+ rot' :: Rot f1 f2 g a -> V3T f2 a -> V3T f1 a+ transpose :: Rot f1 f2 g a -> Rot f2 f1 g a+ identity :: Rot f1 f2 g a -instance Num a => Rotation (Quaternion a) a where+instance Num a => Rotation Quaternion 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)+ where+ -- 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++ rot (Rot q_a2b) (V3T va) = V3T (SM.rotVecByQuat q_a2b va)+ rot' (Rot q_a2b) (V3T vb) = V3T (SM.rotVecByQuatB2A q_a2b vb) transpose (Rot (Quaternion q0 qxyz)) = Rot (Quaternion q0 (fmap negate qxyz))+ identity = Rot (Quaternion 1 (pure 0)) --- 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 (V3 :. V3) a where+ compose (Rot (O dcm_a2b)) (Rot (O dcm_b2c)) = Rot $ O (dcm_b2c !*! dcm_a2b)+ rot (Rot (O dcm_a2b)) (V3T va) = V3T (SM.rotVecByDcm dcm_a2b va)+ rot' (Rot (O dcm_a2b)) (V3T vb) = V3T (SM.rotVecByDcmB2A dcm_a2b vb)+ transpose+ (Rot+ (O+ (V3+ (V3 e11 e12 e13)+ (V3 e21 e22 e23)+ (V3 e31 e32 e33)))) =+ Rot $ O $+ V3+ (V3 e11 e21 e31)+ (V3 e12 e22 e32)+ (V3 e13 e23 e33)+ identity =+ Rot $ O $+ V3+ (V3 1 0 0)+ (V3 0 1 0)+ (V3 0 0 1) -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+dcmOfQuat :: Num a => Rot f g Quaternion a -> Rot f g (V3 :. V3) a+dcmOfQuat = Rot . O . SM.dcmOfQuat . unRot++dcmOfEuler321 :: Floating a => Rot f g Euler a -> Rot f g (V3 :. V3) a+dcmOfEuler321 = Rot . O . SM.dcmOfEuler321 . unRot+++quatOfDcm :: Floating a => Rot f g (V3 :. V3) a -> Rot f g Quaternion a+quatOfDcm = Rot . SM.quatOfDcm . unO . unRot++quatOfEuler321 :: Floating a => Rot f g Euler a -> Rot f g Quaternion a+quatOfEuler321 = Rot . SM.quatOfEuler321 . unRot+++unsafeEuler321OfDcm :: ArcTan2 a => Rot f g (V3 :. V3) a -> Rot f g Euler a+unsafeEuler321OfDcm = Rot . SM.unsafeEuler321OfDcm . unO . unRot++euler321OfDcm :: (ArcTan2 a, Ord a) => Rot f g (V3 :. V3) a -> Rot f g Euler a+euler321OfDcm = Rot . SM.euler321OfDcm . unO . unRot++euler321OfQuat :: (ArcTan2 a, Ord a) => Rot f g Quaternion a -> Rot f g Euler a+euler321OfQuat = Rot . SM.euler321OfQuat . unRot++unsafeEuler321OfQuat :: ArcTan2 a => Rot f g Quaternion a -> Rot f g Euler a+unsafeEuler321OfQuat = Rot . SM.unsafeEuler321OfQuat . unRot++instance (ArcTan2 a, Floating a, Ord a) => Rotation Euler a where+ -- defined in terms of quaternion composition+ compose e_a2b e_b2c = euler321OfQuat q_a2c+ where+ q_a2b = quatOfEuler321 e_a2b+ q_b2c = quatOfEuler321 e_b2c+ q_a2c = compose q_a2b q_b2c++ rot (Rot e_a2b) (V3T va) = V3T (SM.rotVecByEuler e_a2b va)+ rot' (Rot e_a2b) (V3T vb) = V3T (SM.rotVecByEulerB2A e_a2b vb)+ transpose = euler321OfQuat . transpose . quatOfEuler321+ identity = Rot (Euler 0 0 0)+++orthonormalize :: Floating a => Rot f1 f2 (V3 :. V3) a -> Rot f1 f2 (V3 :. V3) a+orthonormalize+ (Rot+ (O+ (V3+ (V3 m00 m01 m02)+ (V3 m10 m11 m12)+ (V3 m20 m21 m22)))) = Rot (O ret) where -- compute q0 fInvLength0 = 1.0/sqrt(m00*m00 + m10*m10 + m20*m20)
tests/Tests.hs view
@@ -121,7 +121,7 @@ prop_e2q_e2d2q :: Euler Double -> Property prop_e2q_e2d2q euler = testDoubleConversion euler quat0 quat1 (close 1e-9 quat0 quat1) where- quat0 = quatOfEuler321 euler+ quat0 = makeScalarPositive (quatOfEuler321 euler) quat1 = quatOfDcm (dcmOfEuler321 euler) prop_q2e_q2d2e :: Quaternion Double -> Property@@ -143,10 +143,15 @@ euler1 = euler321OfQuat (quatOfDcm dcm) prop_d2q_d2e2q :: M33 Double -> Property-prop_d2q_d2e2q dcm = testDoubleConversion dcm quat0 quat1 (close 1e-6 quat0 quat1)+prop_d2q_d2e2q dcm = testDoubleConversion dcm quat0 quat1 (close 1e-5 quat0 quat1) where quat0 = quatOfDcm dcm- quat1 = quatOfEuler321 (euler321OfDcm dcm)+ quat1 = makeScalarPositive (quatOfEuler321 (euler321OfDcm dcm))++makeScalarPositive :: Quaternion Double -> Quaternion Double+makeScalarPositive quat0'@(Quaternion q0 _)+ | q0 < 0 = fmap negate quat0'+ | otherwise = quat0' tests :: [Test] tests =