spatial-math 0.2.5.0 → 0.2.6.0
raw patch · 4 files changed
+82/−23 lines, 4 files
Files
- changelog.txt +5/−0
- spatial-math.cabal +1/−1
- src/SpatialMath.hs +52/−9
- tests/Tests.hs +24/−13
changelog.txt view
@@ -15,3 +15,8 @@ 0.2.5 - instance Applicative Euler - specialized dcmOfEuler321 without Ord constraint++0.2.6+- use ArcTan2 instead of RealFloat+- unsafe specialized euler321OrDcm without Ord constraint+- primitive implementation of quatOfDcm
spatial-math.cabal view
@@ -1,5 +1,5 @@ name: spatial-math-version: 0.2.5.0+version: 0.2.6.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
src/SpatialMath.hs view
@@ -3,11 +3,13 @@ module SpatialMath ( Euler(..)+ , ArcTan2(..) , rotateXyzAboutX , rotateXyzAboutY , rotateXyzAboutZ , euler321OfQuat , euler321OfDcm+ , unsafeEuler321OfDcm , quatOfEuler321 , dcmOfQuat , dcmOfQuatB2A@@ -30,6 +32,12 @@ 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+ -- $setup -- | -- >>> :{@@ -118,7 +126,7 @@ -- >>> 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 :: (ArcTan2 a, Ord 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@@ -131,24 +139,34 @@ r23 = 2.0*(q2*q3 + q0*q1) r33 = q0*q0 - q1*q1 - q2*q2 + q3*q3 - yaw = atan2 r12 r11+ yaw = arctan2 r12 r11 pitch = asin mr13- roll = atan2 r23 r33+ 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.7071067811865476 (V3 0.0 0.0 0.7071067811865475)+-- Quaternion 0.7071067811865477 (V3 (-0.0) (-0.0) 0.7071067811865474) -- -- >>> 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+-- Quaternion 0.9238795325112868 (V3 (-0.0) (-0.0) 0.3826834323650898)+quatOfDcm :: Floating 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 + quatOfDcmB2A :: (Conjugate a, RealFloat a) => M33 a -> Quaternion a quatOfDcmB2A = conjugate . quatOfDcm @@ -178,6 +196,31 @@ yaw = atan2 r12 r11 pitch = asin mr13+ roll = atan2 r23 r33++-- | 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}+--+-- >>> unsafeEuler321OfDcm $ 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 unsafeEuler321OfDcm $ V3 (V3 s s 0) (V3 (-s) s 0) (V3 0 0 1)+-- Euler {eYaw = 0.7853981633974483, ePitch = -0.0, eRoll = 0.0}+--+-- >>> unsafeEuler321OfDcm $ V3 (V3 0 0 1.1) (V3 0 0 0) (V3 0 0 0)+-- Euler {eYaw = 0.0, ePitch = NaN, eRoll = 0.0}+--+unsafeEuler321OfDcm :: RealFloat a => M33 a -> Euler a+unsafeEuler321OfDcm+ (V3+ (V3 r11 r12 r13)+ (V3 _ _ r23)+ (V3 _ _ r33)) = Euler yaw pitch roll+ where+ yaw = atan2 r12 r11+ pitch = asin (-r13) roll = atan2 r23 r33 -- | Convert Euler angles to quaternion
tests/Tests.hs view
@@ -15,22 +15,27 @@ import Test.QuickCheck import Test.QuickCheck.Arbitrary -- import Test.QuickCheck.Gen+import Text.Printf ( printf ) import SpatialMath main :: IO () main = defaultMainWithOpts tests opts -close :: forall f . (F.Foldable f, Applicative f) => Double -> f Double -> f Double -> Bool-close eps f0 f1 = all (\x -> abs x <= eps) deltas+close :: forall f . (F.Foldable f, Applicative f) => Double -> f Double -> f Double -> Maybe Double+close eps f0 f1+ | all (\x -> abs x <= eps) deltas = Nothing+ | otherwise = Just $ maximum $ map abs deltas where delta :: f Double delta = (-) <$> f0 <*> f1 deltas = F.toList delta -closeDcm :: Double -> M33 Double -> M33 Double -> Bool-closeDcm eps f0 f1 = all (\x -> abs x <= eps) deltas+closeDcm :: Double -> M33 Double -> M33 Double -> Maybe Double+closeDcm eps f0 f1+ | all (\x -> abs x <= eps) deltas = Nothing+ | otherwise = Just $ maximum $ map abs deltas where delta :: V3 (V3 Double) delta = (-) <$> f0 <*> f1@@ -78,12 +83,15 @@ testConversion :: (F.Foldable f, Applicative f, Show (f Double)) => Double -> (f Double -> f Double) -> f Double -> Property-testConversion eps f x0 = counterexample msg (close eps x0 x1)+testConversion eps f x0 = counterexample msg ret where- msg = init $ unlines+ (ret, errmsg) = case close eps x0 x1 of+ Nothing -> (True, [])+ Just worstErr -> (False, [printf "worst error: %.3g" worstErr])+ msg = init $ unlines $ [ "original: " ++ show x0 , "converted: " ++ show x1- ]+ ] ++ errmsg x1 = f x0 prop_e2q2e :: Euler Double -> Property@@ -92,14 +100,17 @@ prop_e2d2e :: Euler Double -> Property prop_e2d2e = testConversion 1e-9 (euler321OfDcm . dcmOfEuler321) -testDoubleConversion :: (Show f, Show g) => f -> g -> g -> Bool -> Property-testDoubleConversion orig res0 res1 ret = counterexample msg ret+testDoubleConversion :: (Show f, Show g) => f -> g -> g -> Maybe Double -> Property+testDoubleConversion orig res0 res1 err = counterexample msg ret where- msg = init $ unlines+ (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_e2d_e2q2d :: Euler Double -> Property prop_e2d_e2q2d euler = testDoubleConversion euler dcm0 dcm1 (closeDcm 1e-9 dcm0 dcm1)@@ -126,13 +137,13 @@ dcm1 = dcmOfEuler321 (euler321OfQuat quat) prop_d2e_d2q2e :: M33 Double -> Property-prop_d2e_d2q2e dcm = testDoubleConversion dcm euler0 euler1 (close 1e-9 euler0 euler1)+prop_d2e_d2q2e dcm = testDoubleConversion dcm euler0 euler1 (close 1e-7 euler0 euler1) where euler0 = euler321OfDcm dcm euler1 = euler321OfQuat (quatOfDcm dcm) prop_d2q_d2e2q :: M33 Double -> Property-prop_d2q_d2e2q dcm = testDoubleConversion dcm quat0 quat1 (close 1e-9 quat0 quat1)+prop_d2q_d2e2q dcm = testDoubleConversion dcm quat0 quat1 (close 1e-6 quat0 quat1) where quat0 = quatOfDcm dcm quat1 = quatOfEuler321 (euler321OfDcm dcm)