diff --git a/LICENSE b/LICENSE --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright (c) 2012, Greg Horn +Copyright (c) 2012-2016, Greg Horn All rights reserved. diff --git a/changelog.txt b/changelog.txt --- a/changelog.txt +++ b/changelog.txt @@ -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 diff --git a/spatial-math.cabal b/spatial-math.cabal --- a/spatial-math.cabal +++ b/spatial-math.cabal @@ -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 diff --git a/src/SpatialMath.hs b/src/SpatialMath.hs --- a/src/SpatialMath.hs +++ b/src/SpatialMath.hs @@ -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 -- diff --git a/src/SpatialMath/Internal.hs b/src/SpatialMath/Internal.hs new file mode 100644 --- /dev/null +++ b/src/SpatialMath/Internal.hs @@ -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) diff --git a/src/SpatialMathT.hs b/src/SpatialMathT.hs --- a/src/SpatialMathT.hs +++ b/src/SpatialMathT.hs @@ -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) diff --git a/tests/Tests.hs b/tests/Tests.hs --- a/tests/Tests.hs +++ b/tests/Tests.hs @@ -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 } diff --git a/tests/doctests.hs b/tests/doctests.hs --- a/tests/doctests.hs +++ b/tests/doctests.hs @@ -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"]