diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -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. diff --git a/README.md b/README.md new file mode 100644 --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ +SpatialMath +=== +3d math including quaternions/euler angles/dcms and utility functions. diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/SpatialMath.cabal b/SpatialMath.cabal new file mode 100644 --- /dev/null +++ b/SpatialMath.cabal @@ -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 +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 diff --git a/changelog.txt b/changelog.txt new file mode 100644 --- /dev/null +++ b/changelog.txt @@ -0,0 +1,2 @@ +0.2.7.0 +initial fork of spatial-math-0.2.7.0 diff --git a/src/SpatialMath.hs b/src/SpatialMath.hs new file mode 100644 --- /dev/null +++ b/src/SpatialMath.hs @@ -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 diff --git a/src/SpatialMathT.hs b/src/SpatialMathT.hs new file mode 100644 --- /dev/null +++ b/src/SpatialMathT.hs @@ -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'')) diff --git a/src/Types.hs b/src/Types.hs new file mode 100644 --- /dev/null +++ b/src/Types.hs @@ -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) diff --git a/tests/Tests.hs b/tests/Tests.hs new file mode 100644 --- /dev/null +++ b/tests/Tests.hs @@ -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) + } diff --git a/tests/doctests.hs b/tests/doctests.hs new file mode 100644 --- /dev/null +++ b/tests/doctests.hs @@ -0,0 +1,8 @@ +{-# OPTIONS_GHC -Wall #-} + +module Main where + +import Test.DocTest + +main :: IO () +main = doctest ["src/Types.hs", "src/SpatialMath.hs"]