spatial-math 0.2.7.0 → 0.5.0.1
raw patch · 8 files changed
Files
- LICENSE +1/−1
- changelog.txt +10/−0
- spatial-math.cabal +4/−2
- src/SpatialMath.hs +143/−26
- src/SpatialMath/Internal.hs +25/−0
- src/SpatialMathT.hs +110/−44
- tests/Tests.hs +69/−55
- tests/doctests.hs +1/−1
LICENSE view
@@ -1,4 +1,4 @@-Copyright (c) 2012, Greg Horn+Copyright (c) 2012-2016, Greg Horn All rights reserved.
changelog.txt view
@@ -1,3 +1,13 @@+0.5.0.1+- Fix doctest include path.++0.5.0+- Replace calls to Prelude's `atan2` function with calls to the C math+ library in `ArcTan2` instances for `Float` and `Double`++0.4.0+- Switch quat2dcm mode to avoid divide by 0, add Ord constraint+ 0.2.0 - convert to using `linear` V3, M33, Quaternion types - doctests
spatial-math.cabal view
@@ -1,5 +1,5 @@ name: spatial-math-version: 0.2.7.0+version: 0.5.0.1 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@@ -18,13 +18,15 @@ hs-source-dirs: src exposed-modules: SpatialMath SpatialMathT+ SpatialMath.Internal other-modules: Types build-depends: base >= 4 && < 5, ghc-prim, 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@@ -32,11 +33,7 @@ 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 = atan2-instance ArcTan2 Float where arctan2 = atan2+import SpatialMath.Internal ( libm_atan2, libm_atan2f ) -- $setup -- |@@ -49,8 +46,57 @@ -- | nearZero (x - 1) = 1 -- | nearZero (x + 1) = -1 -- | otherwise = x+-- inf = 1/0+-- neginf = negate inf -- :} ++-- | doesn't require RealFloat, used for overloading symbolics+class Floating a => ArcTan2 a where+ -- | @arctan2 y x@ computes the arctangent from two arguments. The+ -- 'Double' and 'Float' instances call out to a sufficiently recent+ -- version of @libm@ to compute this.+ --+ -- The following test cases are the /full/ set of recommended+ -- function properties specified for function @atan2Pi()@ on page 45+ -- of the IEEE Std 754-2008 document.+ --+ -- >>> arctan2 0 (-0) :: Double+ -- 3.141592653589793+ -- >>> arctan2 (-0) (-0) :: Double+ -- -3.141592653589793+ -- >>> arctan2 0 0 :: Double+ -- 0.0+ -- >>> arctan2 (-0) 0 :: Double+ -- -0.0+ --+ -- prop> \x -> x < 0 ==> arctan2 (-0) x == (-pi :: Double)+ -- prop> \x -> x < 0 ==> arctan2 0 x == (pi :: Double)+ -- prop> \x -> x > 0 ==> arctan2 (-0) x == (-0 :: Double)+ -- prop> \x -> x > 0 ==> arctan2 0 x == (0 :: Double)+ -- prop> \y -> y < 0 ==> arctan2 y (-0) == (-pi / 2 :: Double)+ -- prop> \y -> y > 0 ==> arctan2 y 0 == (pi / 2 :: Double)+ -- prop> \y -> y > 0 && not (isNaN y || isInfinite y) ==> arctan2 y (negate $ 1/0) == (pi :: Double)+ -- prop> \y -> y < 0 && not (isNaN y || isInfinite y) ==> arctan2 y (negate $ 1/0) == (-pi :: Double)+ -- prop> \y -> y > 0 && not (isNaN y || isInfinite y) ==> arctan2 y (1/0) == (0 :: Double)+ -- prop> \y -> y < 0 && not (isNaN y || isInfinite y) ==> arctan2 y (1/0) == (-0 :: Double)+ -- prop> \x -> not (isNaN x || isInfinite x) ==> arctan2 (negate $ 1/0) x == (-pi/2 :: Double)+ -- prop> \x -> not (isNaN x || isInfinite x) ==> arctan2 (1/0) x == (pi/2 :: Double)+ --+ -- >>> arctan2 neginf neginf :: Double+ -- -2.356194490192345+ -- >>> arctan2 inf neginf :: Double+ -- 2.356194490192345+ -- >>> arctan2 neginf inf :: Double+ -- -0.7853981633974483+ -- >>> arctan2 inf inf :: Double+ -- 0.7853981633974483+ arctan2 :: a -> a -> a+instance ArcTan2 Double where+ arctan2 = libm_atan2+instance ArcTan2 Float where+ arctan2 = libm_atan2f+ normalize' :: Floating a => Quaternion a -> Quaternion a normalize' q = fmap (* normInv) q where@@ -143,31 +189,105 @@ 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)--- Quaternion 1.0 (V3 (-0.0) (-0.0) (-0.0))+-- 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)+-- 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.9238795325112868 (V3 (-0.0) (-0.0) 0.3826834323650898)-quatOfDcm :: Floating a => M33 a -> Quaternion a+-- Quaternion 0.9238795325112867 (V3 0.0 0.0 0.3826834323650898)+quatOfDcm :: (Floating a, Ord a) => M33 a -> Quaternion a quatOfDcm (V3- (V3 r11 r12 r13)- (V3 r21 r22 r23)- (V3 r31 r32 r33)) = Quaternion q0 (V3 qi qj qk)- where- q0 = 0.5 * sqrt (1e-15 + (1 + r11 + r22 + r33))- qi = negate (r32 - r23) / fourQ0- qj = negate (r13 - r31) / fourQ0- qk = negate (r21 - r12) / fourQ0- fourQ0 = 4 * q0+ (V3 r11 r12 r13)+ (V3 r21 r22 r23)+ (V3 r31 r32 r33)+ )+ | r11 + r22 + r33 > 0 =+ let sqtrp1 = sqrt (r11 + r22 + r33 + 1)+ q0 = 0.5*sqtrp1+ qx = (r23 - r32)/(2.0*sqtrp1)+ qy = (r31 - r13)/(2.0*sqtrp1)+ qz = (r12 - r21)/(2.0*sqtrp1)+ in Quaternion q0 (V3 qx qy qz)+ | (r22 > r11) && (r22 > r33) =+ let -- max value at r22+ sqdip1' = sqrt (r22 - r11 - r33 + 1) + qy = 0.5*sqdip1' -quatOfDcmB2A :: Floating a => M33 a -> Quaternion a+ sqdip1+ | sqdip1' == 0 = 0+ | otherwise = 0.5/sqdip1'++ q0 = (r31 - r13)*sqdip1+ qx = (r12 + r21)*sqdip1+ qz = (r23 + r32)*sqdip1++ in Quaternion q0 (V3 qx qy qz)+ | r33 > r11 =+ let -- max value at r33+ sqdip1' = sqrt (r33 - r11 - r22 + 1)++ qz = 0.5*sqdip1'++ sqdip1+ | sqdip1' == 0 = 0+ | otherwise = 0.5/sqdip1'++ q0 = (r12 - r21)*sqdip1+ qx = (r31 + r13)*sqdip1+ qy = (r23 + r32)*sqdip1++ in Quaternion q0 (V3 qx qy qz)+ | otherwise =+ let -- max value at r11+ sqdip1' = sqrt (r11 - r22 - r33 + 1)++ qx = 0.5*sqdip1'++ sqdip1+ | sqdip1' == 0 = 0+ | otherwise = 0.5/sqdip1'++ q0 = (r23 - r32)*sqdip1+ qy = (r12 + r21)*sqdip1+ qz = (r31 + r13)*sqdip1++ in Quaternion q0 (V3 qx qy qz)+++quatOfDcmB2A :: (Floating a, Ord a) => M33 a -> Quaternion a quatOfDcmB2A = quatConjugate . quatOfDcm -- | Convert DCM to euler angles@@ -198,7 +318,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 +343,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 +357,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 +371,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/SpatialMath/Internal.hs view
@@ -0,0 +1,25 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ForeignFunctionInterface #-}++module SpatialMath.Internal (+ libm_atan2+ , libm_atan2f+ ) where++import Foreign.C.Types ( CDouble(..), CFloat(..) )++foreign import ccall unsafe "math.h atan2" c_atan2+ :: CDouble -> CDouble -> CDouble++foreign import ccall unsafe "math.h atan2f" c_atan2f+ :: CFloat -> CFloat -> CFloat++libm_atan2 :: Double -> Double -> Double+libm_atan2 y x = ret+ where+ CDouble ret = c_atan2 (CDouble y) (CDouble x)++libm_atan2f :: Float -> Float -> Float+libm_atan2f y x = ret+ where+ CFloat ret = c_atan2f (CFloat y) (CFloat x)
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.Applicative ( Applicative, pure)+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,58 +72,109 @@ 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+ , Applicative , Storable , Num, Fractional, Eq, Show, Ord , Generic1, Generic , 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, Ord 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
@@ -22,16 +22,32 @@ main :: IO () main = defaultMainWithOpts tests opts -close :: forall f . (F.Foldable f, Applicative f) => Double -> f Double -> f Double -> Maybe Double-close eps f0 f1+closeEuler :: Double -> Euler Double -> Euler Double -> Maybe Double+closeEuler eps f0 f1 | all (\x -> abs x <= eps) deltas = Nothing | otherwise = Just $ maximum $ map abs deltas where- delta :: f Double+ delta :: Euler Double delta = (-) <$> f0 <*> f1 deltas = F.toList delta +closeQuat :: Double -> Quaternion Double -> Quaternion Double -> Maybe Double+closeQuat eps f0 f1+ | worstDelta <= eps = Nothing+ | otherwise = Just worstDelta+ where+ deltas0 :: Quaternion Double+ deltas0 = (-) <$> f0 <*> f1++ deltas1 :: Quaternion Double+ deltas1 = (-) <$> f0 <*> (negate <$> f1)++ worstDelta =+ min+ (maximum (map abs (F.toList deltas0)))+ (maximum (map abs (F.toList deltas1)))+ closeDcm :: Double -> M33 Double -> M33 Double -> Maybe Double closeDcm eps f0 f1 | all (\x -> abs x <= eps) deltas = Nothing@@ -80,87 +96,84 @@ instance Arbitrary (V3 (V3 Double)) where arbitrary = dcmOfEuler321 <$> arbitrary -testConversion :: (F.Foldable f, Applicative f, Show (f Double))- => Double -> (f Double -> f Double) -> f Double+testConversion :: (Show a, Show b)+ => (b -> b -> Maybe Double)+ -> (a -> b) -> (a -> b) -> a -> Property-testConversion eps f x0 = counterexample msg ret+testConversion toErr f0 f1 x = counterexample msg ret where- (ret, errmsg) = case close eps x0 x1 of+ y0 = f0 x+ y1 = f1 x+ (ret, errmsg) = case toErr y0 y1 of Nothing -> (True, []) Just worstErr -> (False, [printf "worst error: %.3g" worstErr]) msg = init $ unlines $- [ "original: " ++ show x0- , "converted: " ++ show x1+ [ "original: " ++ show x+ , "first route: " ++ show y0+ , "second route: " ++ show y1 ] ++ errmsg- x1 = f x0 +-- inverses prop_e2q2e :: Euler Double -> Property-prop_e2q2e = testConversion 1e-9 (euler321OfQuat . quatOfEuler321)+prop_e2q2e = testConversion (closeEuler 1e-9) id (euler321OfQuat . quatOfEuler321) prop_e2d2e :: Euler Double -> Property-prop_e2d2e = testConversion 1e-9 (euler321OfDcm . dcmOfEuler321)+prop_e2d2e = testConversion (closeEuler 1e-9) id (euler321OfDcm . dcmOfEuler321) -testDoubleConversion :: (Show f, Show g) => f -> g -> g -> Maybe Double -> Property-testDoubleConversion orig res0 res1 err = counterexample msg ret- where- (ret, errmsg) = case err of- Nothing -> (True, [])- Just worstErr -> (False, [printf "worst error: %.3g" worstErr])- msg = init $ unlines $- [ "original: " ++ show orig- , "first route: " ++ show res0- , "second route: " ++ show res1- ] ++ errmsg+prop_d2e2d :: M33 Double -> Property+prop_d2e2d = testConversion (closeDcm 1e-9) id (dcmOfEuler321 . euler321OfDcm) +prop_d2q2d :: M33 Double -> Property+prop_d2q2d = testConversion (closeDcm 1e-9) id (dcmOfQuat . quatOfDcm)++prop_q2e2q :: Quaternion Double -> Property+prop_q2e2q = testConversion (closeQuat 1e-9) id (quatOfEuler321 . euler321OfQuat)++prop_q2d2q :: Quaternion Double -> Property+prop_q2d2q = testConversion (closeQuat 1e-9) id (quatOfDcm . dcmOfQuat)++-- two routes prop_e2d_e2q2d :: Euler Double -> Property-prop_e2d_e2q2d euler = testDoubleConversion euler dcm0 dcm1 (closeDcm 1e-9 dcm0 dcm1)- where- dcm0 = dcmOfEuler321 euler- dcm1 = dcmOfQuat (quatOfEuler321 euler)+prop_e2d_e2q2d = testConversion (closeDcm 1e-9) dcmOfEuler321 (dcmOfQuat . quatOfEuler321) prop_e2q_e2d2q :: Euler Double -> Property-prop_e2q_e2d2q euler = testDoubleConversion euler quat0 quat1 (close 1e-9 quat0 quat1)- where- quat0 = quatOfEuler321 euler- quat1 = quatOfDcm (dcmOfEuler321 euler)+prop_e2q_e2d2q =+ testConversion (closeQuat 1e-9) (makeScalarPositive . quatOfEuler321) (quatOfDcm . dcmOfEuler321) prop_q2e_q2d2e :: Quaternion Double -> Property-prop_q2e_q2d2e quat = testDoubleConversion quat euler0 euler1 (close 1e-9 euler0 euler1)- where- euler0 = euler321OfQuat quat- euler1 = euler321OfDcm (dcmOfQuat quat)+prop_q2e_q2d2e = testConversion (closeEuler 1e-9) euler321OfQuat (euler321OfDcm . dcmOfQuat) prop_q2d_q2e2d :: Quaternion Double -> Property-prop_q2d_q2e2d quat = testDoubleConversion quat dcm0 dcm1 (closeDcm 1e-9 dcm0 dcm1)- where- dcm0 = dcmOfQuat quat- dcm1 = dcmOfEuler321 (euler321OfQuat quat)+prop_q2d_q2e2d = testConversion (closeDcm 1e-9) dcmOfQuat (dcmOfEuler321 . euler321OfQuat) prop_d2e_d2q2e :: M33 Double -> Property-prop_d2e_d2q2e dcm = testDoubleConversion dcm euler0 euler1 (close 1e-7 euler0 euler1)- where- euler0 = euler321OfDcm dcm- euler1 = euler321OfQuat (quatOfDcm dcm)+prop_d2e_d2q2e = testConversion (closeEuler 1e-7) euler321OfDcm (euler321OfQuat . quatOfDcm) prop_d2q_d2e2q :: M33 Double -> Property-prop_d2q_d2e2q dcm = testDoubleConversion dcm quat0 quat1 (close 1e-6 quat0 quat1)- where- quat0 = quatOfDcm dcm- quat1 = quatOfEuler321 (euler321OfDcm dcm)+prop_d2q_d2e2q = testConversion (closeQuat 1e-5) quatOfDcm (makeScalarPositive . quatOfEuler321 . euler321OfDcm) +makeScalarPositive :: Quaternion Double -> Quaternion Double+makeScalarPositive quat0'@(Quaternion q0 _)+ | q0 < 0 = fmap negate quat0'+ | otherwise = quat0'+ tests :: [Test] tests = [ testGroup "inverses"- [ testProperty "(euler -> quat -> euler) == euler" prop_e2q2e- , testProperty "(euler -> dcm -> euler) == euler" prop_e2d2e+ [ testProperty "euler == (euler -> quat -> euler)" prop_e2q2e+ , testProperty "euler == (euler -> dcm -> euler)" prop_e2d2e+ , testProperty "dcm == (dcm -> euler -> dcm )" prop_d2e2d+ , testProperty "dcm == (dcm -> quat -> dcm )" prop_d2q2d+ , testProperty "quat == (quat -> euler -> quat )" prop_q2e2q+ , testProperty "quat == (quat -> dcm -> quat )" prop_q2d2q ] , testGroup "two routes"- [ testProperty "(euler -> dcm) == (euler -> quat -> dcm)" prop_e2d_e2q2d- , testProperty "(euler -> quat) == (euler -> dcm -> quat)" prop_e2q_e2d2q- , testProperty "(quat -> euler) == (quat -> dcm -> euler)" prop_q2e_q2d2e- , testProperty "(quat -> dcm) == (quat -> euler -> dcm)" prop_q2d_q2e2d- , testProperty "(dcm -> euler) == (dcm -> quat -> euler)" prop_d2e_d2q2e- , testProperty "(dcm -> quat) == (dcm -> euler -> quat)" prop_d2q_d2e2q+ [ testProperty "(euler -> dcm ) == (euler -> quat -> dcm )" prop_e2d_e2q2d+ , testProperty "(euler -> quat ) == (euler -> dcm -> quat )" prop_e2q_e2d2q+ , testProperty "(quat -> euler) == (quat -> dcm -> euler)" prop_q2e_q2d2e+ , testProperty "(quat -> dcm ) == (quat -> euler -> dcm )" prop_q2d_q2e2d+ , testProperty "(dcm -> euler) == (dcm -> quat -> euler)" prop_d2e_d2q2e+ , testProperty "(dcm -> quat ) == (dcm -> euler -> quat )" prop_d2q_d2e2q ] ] @@ -176,4 +189,5 @@ my_test_opts = Mo.mempty { topt_timeout = Just (Just 15000000)+ , topt_maximum_generated_tests = Just 1000 }
tests/doctests.hs view
@@ -5,4 +5,4 @@ import Test.DocTest main :: IO ()-main = doctest ["src/Types.hs", "src/SpatialMath.hs"]+main = doctest ["-isrc", "src/Types.hs", "src/SpatialMath.hs"]