spatial-math 0.3.1.0 → 0.4.0.0

raw patch · 5 files changed

+130/−71 lines, 5 filesPVP ok

version bump matches the API change (PVP)

API changes (from Hackage documentation)

Files

• changelog.txt +3/−0
• spatial-math.cabal +1/−1
• src/SpatialMath.hs +61/−14
• src/SpatialMathT.hs +1/−1
• tests/Tests.hs +64/−55

@@ -1,3 +1,6 @@+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

@@ -1,5 +1,5 @@ name:                spatial-math-version:             0.3.1.0+version:             0.4.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

@@ -174,28 +174,75 @@ -- | 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

@@ -134,7 +134,7 @@ dcmOfEuler321 = Rot . O . SM.dcmOfEuler321 . unRot  -quatOfDcm :: Floating a => Rot f g (V3 :. V3) a -> Rot f g Quaternion a+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

@@ -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,73 +96,61 @@ 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 = makeScalarPositive (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-5 quat0 quat1)-  where-    quat0 = quatOfDcm dcm-    quat1 = makeScalarPositive (quatOfEuler321 (euler321OfDcm dcm))+prop_d2q_d2e2q = testConversion (closeQuat 1e-5) quatOfDcm (makeScalarPositive . quatOfEuler321 . euler321OfDcm)  makeScalarPositive :: Quaternion Double -> Quaternion Double makeScalarPositive quat0'@(Quaternion q0 _)@@ -156,16 +160,20 @@ 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     ]   ] @@ -181,4 +189,5 @@ my_test_opts =   Mo.mempty   { topt_timeout = Just (Just 15000000)+  , topt_maximum_generated_tests = Just 1000   }