SpatialMath (empty) → 0.2.7.0
raw patch · 10 files changed
+824/−0 lines, 10 filesdep +QuickCheckdep +SpatialMathdep +basesetup-changed
Dependencies added: QuickCheck, SpatialMath, base, binary, cereal, doctest, ghc-prim, lens, linear, test-framework, test-framework-quickcheck2
Files
- LICENSE +30/−0
- README.md +3/−0
- Setup.hs +2/−0
- SpatialMath.cabal +54/−0
- changelog.txt +2/−0
- src/SpatialMath.hs +335/−0
- src/SpatialMathT.hs +150/−0
- src/Types.hs +61/−0
- tests/Tests.hs +179/−0
- tests/doctests.hs +8/−0
+ LICENSE view
@@ -0,0 +1,30 @@+Copyright (c) 2012, Greg Horn++All rights reserved.++Redistribution and use in source and binary forms, with or without+modification, are permitted provided that the following conditions are met:++ * Redistributions of source code must retain the above copyright+ notice, this list of conditions and the following disclaimer.++ * Redistributions in binary form must reproduce the above+ copyright notice, this list of conditions and the following+ disclaimer in the documentation and/or other materials provided+ with the distribution.++ * Neither the name of Greg Horn nor the names of other+ contributors may be used to endorse or promote products derived+ from this software without specific prior written permission.++THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ README.md view
@@ -0,0 +1,3 @@+SpatialMath+===+3d math including quaternions/euler angles/dcms and utility functions.
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ SpatialMath.cabal view
@@ -0,0 +1,54 @@+name: SpatialMath+version: 0.2.7.0+synopsis: 3d math including quaternions/euler angles/dcms and utility functions+description: This is a fork of Greg Horn's spatial-math package. It's reason for+ being is that its most recent version does not require deprecated+ packages.+license: BSD3+license-file: LICENSE+author: Greg Horn+maintainer: Scott N. Walck <walck@lvc.edu>+copyright: Copyright (c) 2012, Greg Horn+category: Math+build-type: Simple+cabal-version: >=1.10++extra-source-files: README.md+ changelog.txt++library+ hs-source-dirs: src+ exposed-modules: SpatialMath+ SpatialMathT+ other-modules: Types+ build-depends: base >= 4 && < 5 ,+ ghc-prim >= 0.8.0 && < 0.9,+ binary >= 0.8.9 && < 0.9,+ cereal >= 0.5.8 && < 0.6,+ lens >= 5.2.3 && < 5.3,+ linear >= 1.22 && < 1.23+ default-language: Haskell2010++source-repository head+ type: git+ location: git://github.com/ghorn/spatial-math.git++test-suite doctests+ type: exitcode-stdio-1.0+ main-is: doctests.hs+ build-depends: base >= 4 && < 5,+ doctest >= 0.22.1 && < 1+ default-language: Haskell2010+ ghc-options: -threaded+ hs-source-dirs: tests++test-suite unit-tests+ type: exitcode-stdio-1.0+ hs-source-dirs: tests+ main-is: Tests.hs+ default-language: Haskell2010+ build-depends: base >=4.6 && < 5,+ SpatialMath,+ QuickCheck >= 2.14.3 && < 3,+ test-framework >= 0.8.2.0 && < 1,+ test-framework-quickcheck2 >= 0.3.0.5 && < 1
+ changelog.txt view
@@ -0,0 +1,2 @@+0.2.7.0+initial fork of spatial-math-0.2.7.0
+ src/SpatialMath.hs view
@@ -0,0 +1,335 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language ScopedTypeVariables #-}++module SpatialMath+ ( Euler(..)+ , ArcTan2(..)+ , rotateXyzAboutX+ , rotateXyzAboutY+ , rotateXyzAboutZ+ , euler321OfQuat+ , euler321OfDcm+ , unsafeEuler321OfDcm+ , quatOfEuler321+ , dcmOfQuat+ , dcmOfQuatB2A+ , dcmOfEuler321+ , quatOfDcm+ , quatOfDcmB2A+ , rotVecByDcm+ , rotVecByDcmB2A+ , rotVecByQuat+ , rotVecByQuatB2A+ , rotVecByEuler+ , rotVecByEulerB2A+ -- * re-exported from linear+ , M33+ , V3(..)+ , Quaternion(..)+ ) where++import Linear++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+-- |+-- >>> :{+-- let trunc :: Functor f => f Double -> f Double+-- trunc = fmap trunc'+-- where+-- trunc' x+-- | nearZero x = 0+-- | nearZero (x - 1) = 1+-- | nearZero (x + 1) = -1+-- | otherwise = x+-- :}++normalize' :: Floating a => Quaternion a -> Quaternion a+normalize' q = fmap (* normInv) q+ where+ normInv = 1/(norm q)++--normalize' :: (Floating a, Epsilon a) => Quaternion a -> Quaternion a+--normalize' = normalize++-- | Rotate a vector about the X axis+--+-- >>> trunc $ rotateXyzAboutX (V3 0 1 0) (pi/2)+-- V3 0.0 0.0 1.0+--+-- >>> trunc $ rotateXyzAboutX (V3 0 0 1) (pi/2)+-- V3 0.0 (-1.0) 0.0+rotateXyzAboutX :: Floating a => V3 a -> a -> V3 a+rotateXyzAboutX (V3 ax ay az) rotAngle = V3 bx by bz+ where+ cosTheta = cos rotAngle+ sinTheta = sin rotAngle++ bx = ax+ by = ay*cosTheta - az*sinTheta+ bz = ay*sinTheta + az*cosTheta++-- | Rotate a vector about the Y axis+--+-- >>> trunc $ rotateXyzAboutY (V3 0 0 1) (pi/2)+-- V3 1.0 0.0 0.0+--+-- >>> trunc $ rotateXyzAboutY (V3 1 0 0) (pi/2)+-- V3 0.0 0.0 (-1.0)+rotateXyzAboutY :: Floating a => V3 a -> a -> V3 a+rotateXyzAboutY (V3 ax ay az) rotAngle = V3 bx by bz+ where+ cosTheta = cos rotAngle+ sinTheta = sin rotAngle++ bx = ax*cosTheta + az*sinTheta+ by = ay+ bz = -ax*sinTheta + az*cosTheta++-- | Rotate a vector about the Z axis+--+-- >>> trunc $ rotateXyzAboutZ (V3 1 0 0) (pi/2)+-- V3 0.0 1.0 0.0+--+-- >>> trunc $ rotateXyzAboutZ (V3 0 1 0) (pi/2)+-- V3 (-1.0) 0.0 0.0+--+rotateXyzAboutZ :: Floating a => V3 a -> a -> V3 a+rotateXyzAboutZ (V3 ax ay az) rotAngle = V3 bx by bz+ where+ cosTheta = cos rotAngle+ sinTheta = sin rotAngle++ bx = ax*cosTheta - ay*sinTheta+ by = ax*sinTheta + ay*cosTheta+ bz = az+++-- | Convert quaternion to Euler angles+--+-- >>> euler321OfQuat (Quaternion 1.0 (V3 0.0 0.0 0.0))+-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 0.0}+--+-- >>> euler321OfQuat (Quaternion (sqrt(2)/2) (V3 (sqrt(2)/2) 0.0 0.0))+-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 1.5707963267948966}+--+-- >>> euler321OfQuat (Quaternion (sqrt(2)/2) (V3 0.0 (sqrt(2)/2) 0.0))+-- Euler {eYaw = 0.0, ePitch = 1.5707963267948966, eRoll = 0.0}+--+-- >>> euler321OfQuat (Quaternion (sqrt(2)/2) (V3 0.0 0.0 (sqrt(2)/2)))+-- Euler {eYaw = 1.5707963267948966, ePitch = -0.0, eRoll = 0.0}+--+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+ r12 = 2.0*(q1*q2 + q0*q3)+ mr13' = -2.0*(q1*q3 - q0*q2)+ mr13 -- nan protect+ | mr13' > 1 = 1+ | mr13' < -1 = -1+ | otherwise = mr13'+ 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))+--+-- >>> quatOfDcm $ V3 (V3 0 1 0) (V3 (-1) 0 0) (V3 0 0 1)+-- 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.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 :: Floating a => M33 a -> Quaternion a+quatOfDcmB2A = quatConjugate . quatOfDcm++-- | Convert DCM to euler angles+--+-- >>> euler321OfDcm $ V3 (V3 1 0 0) (V3 0 1 0) (V3 0 0 1)+-- Euler {eYaw = 0.0, ePitch = -0.0, eRoll = 0.0}+--+-- >>> euler321OfDcm $ 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 euler321OfDcm $ V3 (V3 s s 0) (V3 (-s) s 0) (V3 0 0 1)+-- Euler {eYaw = 0.7853981633974483, ePitch = -0.0, eRoll = 0.0}+--+euler321OfDcm :: (Ord a, ArcTan2 a) => M33 a -> Euler a+euler321OfDcm+ (V3+ (V3 r11 r12 r13)+ (V3 _ _ r23)+ (V3 _ _ r33)) = Euler yaw pitch roll+ where+ mr13' = -r13+ mr13 -- nan protect+ | mr13' > 1 = 1+ | mr13' < -1 = -1+ | otherwise = mr13'++ yaw = arctan2 r12 r11+ pitch = asin mr13+ roll = arctan2 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 :: ArcTan2 a => M33 a -> Euler a+unsafeEuler321OfDcm+ (V3+ (V3 r11 r12 r13)+ (V3 _ _ r23)+ (V3 _ _ r33)) = Euler yaw pitch roll+ where+ yaw = arctan2 r12 r11+ pitch = asin (-r13)+ roll = arctan2 r23 r33++-- | Convert Euler angles to quaternion+--+-- >>> quatOfEuler321 (Euler 0 0 0)+-- Quaternion 1.0 (V3 0.0 0.0 0.0)+--+-- >>> quatOfEuler321 (Euler (pi/2) 0 0)+-- Quaternion 0.7071067811865476 (V3 0.0 0.0 0.7071067811865475)+--+-- >>> quatOfEuler321 (Euler 0 (pi/2) 0)+-- Quaternion 0.7071067811865476 (V3 0.0 0.7071067811865475 0.0)+--+-- >>> 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 (Euler yaw pitch roll) = normalize' q+ where+ sr2 = sin $ 0.5*roll+ cr2 = cos $ 0.5*roll+ sp2 = sin $ 0.5*pitch+ cp2 = cos $ 0.5*pitch+ sy2 = sin $ 0.5*yaw+ cy2 = cos $ 0.5*yaw+ q0 = cr2*cp2*cy2 + sr2*sp2*sy2+ q1 = sr2*cp2*cy2 - cr2*sp2*sy2+ q2 = cr2*sp2*cy2 + sr2*cp2*sy2+ q3 = cr2*cp2*sy2 - sr2*sp2*cy2++ q' = Quaternion q0 (V3 q1 q2 q3)++ q+ | q0 < 0 = Quaternion (-q0) (V3 (-q1) (-q2) (-q3))+ | otherwise = q'++-- | convert a quaternion to a DCM+--+-- >>> dcmOfQuat $ Quaternion 1.0 (V3 0.0 0.0 0.0)+-- V3 (V3 1.0 0.0 0.0) (V3 0.0 1.0 0.0) (V3 0.0 0.0 1.0)+--+-- >>> let s = sqrt(2)/2 in fmap trunc $ dcmOfQuat $ Quaternion s (V3 0.0 0.0 s)+-- V3 (V3 0.0 1.0 0.0) (V3 (-1.0) 0.0 0.0) (V3 0.0 0.0 1.0)+--+-- >>> dcmOfQuat $ Quaternion 0.9238795325112867 (V3 0.0 0.0 0.3826834323650898)+-- V3 (V3 0.7071067811865475 0.7071067811865476 0.0) (V3 (-0.7071067811865476) 0.7071067811865475 0.0) (V3 0.0 0.0 1.0)+--+dcmOfQuat :: Num a => Quaternion a -> M33 a+dcmOfQuat q = V3+ (V3 m11 m21 m31)+ (V3 m12 m22 m32)+ (V3 m13 m23 m33)+ where+ V3+ (V3 m11 m12 m13)+ (V3 m21 m22 m23)+ (V3 m31 m32 m33) = fromQuaternion q++-- | Convert DCM to euler angles+--+-- >>> fmap trunc $ dcmOfEuler321 $ Euler {eYaw = 0.0, ePitch = 0, eRoll = 0}+-- V3 (V3 1.0 0.0 0.0) (V3 0.0 1.0 0.0) (V3 0.0 0.0 1.0)+--+-- >>> fmap trunc $ dcmOfEuler321 $ Euler {eYaw = pi/2, ePitch = 0, eRoll = 0}+-- V3 (V3 0.0 1.0 0.0) (V3 (-1.0) 0.0 0.0) (V3 0.0 0.0 1.0)+--+-- >>> fmap trunc $ dcmOfEuler321 $ Euler {eYaw = pi/4, ePitch = 0, eRoll = 0}+-- V3 (V3 0.7071067811865476 0.7071067811865475 0.0) (V3 (-0.7071067811865475) 0.7071067811865476 0.0) (V3 0.0 0.0 1.0)+--+dcmOfEuler321 :: Floating a => Euler a -> M33 a+dcmOfEuler321 euler = dcm+ where+ cPs = cos (eYaw euler)+ sPs = sin (eYaw euler)+ cTh = cos (ePitch euler)+ sTh = sin (ePitch euler)+ cPh = cos (eRoll euler)+ sPh = sin (eRoll euler)++ dcm =+ V3+ (V3 (cTh*cPs) (cTh*sPs) (-sTh))+ (V3 (cPs*sTh*sPh - cPh*sPs) ( cPh*cPs + sTh*sPh*sPs) (cTh*sPh))+ (V3 (cPh*cPs*sTh + sPh*sPs) (-cPs*sPh + cPh*sTh*sPs) (cTh*cPh))++quatConjugate :: Num a => Quaternion a -> Quaternion a+quatConjugate (Quaternion q0 qv) = Quaternion q0 (fmap negate qv)++dcmOfQuatB2A :: Num a => Quaternion a -> M33 a+dcmOfQuatB2A = dcmOfQuat . quatConjugate++-- | vec_b = R_a2b * vec_a+rotVecByDcm :: Num a => M33 a -> V3 a -> V3 a+rotVecByDcm dcm vec = dcm !* vec++-- | vec_a = R_a2b^T * vec_b+rotVecByDcmB2A :: Num a => M33 a -> V3 a -> V3 a+rotVecByDcmB2A dcm vec = vec *! dcm++-- | vec_b = q_a2b * vec_a * q_a2b^(-1)+-- vec_b = R(q_a2b) * vec_a+rotVecByQuat :: Num a => Quaternion a -> V3 a -> V3 a+rotVecByQuat q = rotVecByDcm (dcmOfQuat q)++rotVecByQuatB2A :: Num a => Quaternion a -> V3 a -> V3 a+rotVecByQuatB2A q = rotVecByDcmB2A (dcmOfQuat q)++rotVecByEuler :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a+rotVecByEuler = rotVecByDcm . dcmOfEuler321++rotVecByEulerB2A :: (Floating a, Ord a) => Euler a -> V3 a -> V3 a+rotVecByEulerB2A = rotVecByDcmB2A . dcmOfEuler321
+ src/SpatialMathT.hs view
@@ -0,0 +1,150 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language MultiParamTypeClasses #-}+{-# Language FunctionalDependencies #-}+{-# Language FlexibleInstances #-}+{-# Language GeneralizedNewtypeDeriving #-}+{-# Language DeriveFunctor #-}+{-# Language DeriveFoldable #-}+{-# Language DeriveTraversable #-}+{-# Language DeriveGeneric #-}++module SpatialMathT+ ( Rotation(..)+ , Rot(..)+ , V3T(..)+ , R1(..), R2(..), R3(..)+ , M33T+ , cross+ , orthonormalize+ ) where++import Control.Applicative ( Applicative )+import Data.Foldable ( Foldable )+import Data.Binary ( Binary(..) )+import Data.Serialize ( Serialize(..) )+import Data.Traversable ( Traversable )+import Foreign.Storable ( Storable )+import GHC.Generics ( Generic, Generic1 )++import Linear hiding ( cross )+import qualified Linear as L++import SpatialMath++newtype V3T f a = V3T {unV :: V3 a}+ deriving ( Functor, Foldable, Traversable+ , Applicative+ , Additive, Metric, Storable+ , Num, Fractional, Eq, Show, Ord+ , Generic1, Generic+ , Serialize, Binary+ )++instance R1 (V3T f) where+ _x f (V3T v) = fmap V3T $ _x f v+ {-# INLINE _x #-}+instance R2 (V3T f) where+ _y f (V3T v) = fmap V3T $ _y f v+ {-# INLINE _y #-}+ _xy f (V3T v) = fmap V3T $ _xy f v+ {-# INLINE _xy #-}+instance R3 (V3T f) where+ _z f (V3T v) = fmap V3T $ _z f v+ {-# INLINE _z #-}+ _xyz f (V3T v) = fmap V3T $ _xyz f v+ {-# INLINE _xyz #-}++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 }+ deriving ( Functor, Foldable, Traversable+ , 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++instance Num a => Rotation (Quaternion a) 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)+ transpose (Rot (Quaternion q0 qxyz)) = Rot (Quaternion q0 (fmap negate qxyz))++-- 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 (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+ where+ -- compute q0+ fInvLength0 = 1.0/sqrt(m00*m00 + m10*m10 + m20*m20)++ m00' = m00*fInvLength0+ m10' = m10*fInvLength0+ m20' = m20*fInvLength0++ -- compute q1+ fDot0' = m00'*m01 + m10'*m11 + m20'*m21++ m01' = m01 - fDot0'*m00'+ m11' = m11 - fDot0'*m10'+ m21' = m21 - fDot0'*m20'++ fInvLength1 = 1.0/sqrt(m01'*m01' + m11'*m11' + m21'*m21')++ m01'' = m01' * fInvLength1+ m11'' = m11' * fInvLength1+ m21'' = m21' * fInvLength1++ -- compute q2+ fDot1 = m01''*m02 + m11''*m12 + m21''*m22+ fDot0 = m00'*m02 + m10'*m12 + m20'*m22++ m02' = m02 - (fDot0*m00' + fDot1*m01'')+ m12' = m12 - (fDot0*m10' + fDot1*m11'')+ m22' = m22 - (fDot0*m20' + fDot1*m21'')++ fInvLength2 = 1.0/sqrt(m02'*m02' + m12'*m12' + m22'*m22')++ m02'' = m02' * fInvLength2+ m12'' = m12' * fInvLength2+ m22'' = m22' * fInvLength2++ ret = (V3+ (V3 m00' m01'' m02'')+ (V3 m10' m11'' m12'')+ (V3 m20' m21'' m22''))
+ src/Types.hs view
@@ -0,0 +1,61 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language StandaloneDeriving #-}+{-# Language DeriveDataTypeable #-}+{-# LANGUAGE CPP #-}+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702+{-# Language DeriveGeneric #-}+#endif+{-# Language DeriveFunctor #-}+{-# Language DeriveFoldable #-}+{-# Language DeriveTraversable #-}++module Types ( Euler(..) ) where++import Control.Applicative ( Applicative(..) )+import Data.Data ( Data )+import Data.Foldable ( Foldable )+import Data.Traversable ( Traversable )+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702+import GHC.Generics ( Generic )+import Data.Serialize ( Serialize )+import Data.Binary ( Binary )+#endif+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706+import GHC.Generics ( Generic1 )+#endif+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 708+import Data.Typeable ( Typeable )+#else+import Data.Typeable ( Typeable1 )+#endif++-- | 3-2-1 Euler angle rotation sequence+data Euler a = Euler { eYaw :: a+ , ePitch :: a+ , eRoll :: a+ } deriving (Eq, Show, Functor, Foldable, Traversable, Ord+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702+ , Generic+#endif+#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706+ , Generic1+#endif+ )++deriving instance Data a => Data (Euler a)++#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 708+deriving instance Typeable Euler+#else+deriving instance Typeable1 Euler+#endif+++#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702+instance Serialize a => Serialize (Euler a)+instance Binary a => Binary (Euler a)+#endif++instance Applicative Euler where+ pure x = Euler x x x+ Euler f0 f1 f2 <*> Euler x0 x1 x2 = Euler (f0 x0) (f1 x1) (f2 x2)
+ tests/Tests.hs view
@@ -0,0 +1,179 @@+{-# OPTIONS_GHC -Wall #-}+{-# OPTIONS_GHC -fno-warn-orphans #-}+{-# LANGUAGE ScopedTypeVariables #-}+{-# LANGUAGE FlexibleInstances #-}++module Main ( main ) where++import Control.Applicative+import qualified Data.Foldable as F+import qualified Data.Monoid as Mo+import Test.Framework+ ( Test, ColorMode(..), RunnerOptions'(..), TestOptions'(..)+ , defaultMainWithOpts, testGroup )+import Test.Framework.Providers.QuickCheck2 ( testProperty )+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 -> 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 -> 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++ deltas = concatMap F.toList (F.toList delta)++instance Arbitrary (Euler Double) where+ arbitrary = do+ yaw <- choose (-0.99*pi, 0.99*pi)+ pitch <- choose (-0.9*pi/2, 0.9*pi/2)+ roll <- choose (-0.99*pi, 0.99*pi)+ return+ Euler+ { eYaw = yaw+ , ePitch = pitch+ , eRoll = roll+ }++instance Arbitrary (Quaternion Double) where+-- arbitrary = quatOfEuler321 <$> arbitrary+ arbitrary = do+ w <- arbitrary+ x <- arbitrary+ y <- arbitrary+ z <- arbitrary+ let norm = sqrt (w*w + x*x + y*y + z*z)+ ret+ | norm == 0 =+ elements+ [ Quaternion 1 (V3 0 0 0)+ , Quaternion 0 (V3 1 0 0)+ , Quaternion 0 (V3 0 1 0)+ , Quaternion 0 (V3 0 0 1)+ , Quaternion (-1) (V3 0 0 0)+ , Quaternion 0 (V3 (-1) 0 0)+ , Quaternion 0 (V3 0 (-1) 0)+ , Quaternion 0 (V3 0 0 (-1))+ ]+ | otherwise = return $ Quaternion (w/norm) (V3 (x/norm) (y/norm) (z/norm))+ ret++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+ -> Property+testConversion eps f x0 = counterexample msg ret+ where+ (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+prop_e2q2e = testConversion 1e-9 (euler321OfQuat . quatOfEuler321)++prop_e2d2e :: Euler Double -> Property+prop_e2d2e = testConversion 1e-9 (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_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_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_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_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_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_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)++tests :: [Test]+tests =+ [ testGroup "inverses"+ [ testProperty "(euler -> quat -> euler) == euler" prop_e2q2e+ , testProperty "(euler -> dcm -> euler) == euler" prop_e2d2e+ ]+ , 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+ ]+ ]++opts :: RunnerOptions' Maybe+opts =+ Mo.mempty+ { ropt_color_mode = Just ColorAlways+ , ropt_threads = Just 1+ , ropt_test_options = Just my_test_opts+ }++my_test_opts :: TestOptions' Maybe+my_test_opts =+ Mo.mempty+ { topt_timeout = Just (Just 15000000)+ }
+ tests/doctests.hs view
@@ -0,0 +1,8 @@+{-# OPTIONS_GHC -Wall #-}++module Main where++import Test.DocTest++main :: IO ()+main = doctest ["src/Types.hs", "src/SpatialMath.hs"]