diff --git a/SpatialMath.hs b/SpatialMath.hs deleted file mode 100644 --- a/SpatialMath.hs +++ /dev/null @@ -1,303 +0,0 @@ -{-# 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 SpatialMath ( Euler(..) - , rotateXyzAboutX - , rotateXyzAboutY - , rotateXyzAboutZ - , euler321OfQuat - , euler321OfDcm - , quatOfEuler321 - , dcmOfQuat - , dcmOfQuatB2A - , dcmOfEuler321 - , quatOfDcm - , quatOfDcmB2A - , rotVecByDcm - , rotVecByDcmB2A - , rotVecByQuat - , rotVecByQuatB2A - , rotVecByEuler - , rotVecByEulerB2A - -- * re-exported from linear - , M33 - , V3(..) - , Quaternion(..) - ) where - -import Data.Data ( Data ) -import Data.Foldable ( Foldable ) -import Data.Traversable ( Traversable ) -import Data.Typeable ( Typeable1 ) -#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702 -import GHC.Generics (Generic) -#endif -#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706 -import GHC.Generics (Generic1) -#endif -import Linear - -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 - --- | 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 Typeable1 Euler -deriving instance Data a => Data (Euler a) - --- | Rotate a vector about the X axis --- --- >>> rotateXyzAboutX (V3 0 1 0) (pi/2) --- V3 0.0 6.123233995736766e-17 1.0 --- --- >>> rotateXyzAboutX (V3 0 0 1) (pi/2) --- V3 0.0 (-1.0) 6.123233995736766e-17 -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 --- --- >>> rotateXyzAboutY (V3 0 0 1) (pi/2) --- V3 1.0 0.0 6.123233995736766e-17 --- --- >>> rotateXyzAboutY (V3 1 0 0) (pi/2) --- V3 6.123233995736766e-17 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 --- --- >>> rotateXyzAboutZ (V3 1 0 0) (pi/2) --- V3 6.123233995736766e-17 1.0 0.0 --- --- >>> rotateXyzAboutZ (V3 0 1 0) (pi/2) --- V3 (-1.0) 6.123233995736766e-17 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 :: RealFloat 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 = atan2 r12 r11 - pitch = asin mr13 - roll = atan2 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.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.9238795325112867 (V3 0.0 0.0 0.3826834323650898) --- -quatOfDcm :: RealFloat a => M33 a -> Quaternion a -quatOfDcm = quatOfEuler321 . euler321OfDcm - -quatOfDcmB2A :: (Conjugate a, RealFloat a) => M33 a -> Quaternion a -quatOfDcmB2A = conjugate . 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 :: RealFloat 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 = atan2 r12 r11 - pitch = asin mr13 - roll = atan2 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 dcmOfQuat $ Quaternion s (V3 0.0 0.0 s) --- V3 (V3 0.0 1.0000000000000002 0.0) (V3 (-1.0000000000000002) 0.0 0.0) (V3 0.0 0.0 1.0000000000000002) --- --- >>> 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 (Quaternion q0 (V3 q1 q2 q3)) = V3 (V3 r0 r1 r2) - (V3 r3 r4 r5) - (V3 r6 r7 r8) - where - -- 1st column - r0 = q0*q0 + q1*q1 - q2*q2 - q3*q3 - r3 = 2*(q1*q2 - q0*q3) - r6 = 2*(q1*q3 + q0*q2) - - -- 2nd column - r1 = 2*(q1*q2 + q0*q3) - r4 = q0*q0 - q1*q1 + q2*q2 - q3*q3 - r7 = 2*(q2*q3 - q0*q1) - - -- 3rd column - r2 = 2*(q1*q3 - q0*q2) - r5 = 2*(q2*q3 + q0*q1) - r8 = q0*q0 - q1*q1 - q2*q2 + q3*q3 - --- | Convert DCM to euler angles --- --- >>> 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) --- --- >>> dcmOfEuler321 $ Euler {eYaw = pi/2, ePitch = 0, eRoll = 0} --- V3 (V3 2.220446049250313e-16 1.0 0.0) (V3 (-1.0) 2.220446049250313e-16 0.0) (V3 0.0 0.0 1.0) --- --- >>> dcmOfEuler321 $ Euler {eYaw = pi/4, ePitch = 0, eRoll = 0} --- V3 (V3 0.7071067811865475 0.7071067811865476 0.0) (V3 (-0.7071067811865476) 0.7071067811865475 0.0) (V3 0.0 0.0 1.0) --- -dcmOfEuler321 :: (Floating a, Ord a) => Euler a -> M33 a -dcmOfEuler321 = dcmOfQuat . quatOfEuler321 - -dcmOfQuatB2A :: (Conjugate a, RealFloat a) => Quaternion a -> M33 a -dcmOfQuatB2A = dcmOfQuat . conjugate - --- | 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/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.0.1 +version: 0.2.1.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 @@ -15,9 +15,13 @@ changelog.txt library + hs-source-dirs: src exposed-modules: SpatialMath + SpatialMathT + other-modules: Types build-depends: base >= 4 && < 5, ghc-prim, + cereal, linear >= 1.3.1 default-language: Haskell2010 diff --git a/src/SpatialMath.hs b/src/SpatialMath.hs new file mode 100644 --- /dev/null +++ b/src/SpatialMath.hs @@ -0,0 +1,276 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language ScopedTypeVariables #-} + +module SpatialMath + ( Euler(..) + , rotateXyzAboutX + , rotateXyzAboutY + , rotateXyzAboutZ + , euler321OfQuat + , euler321OfDcm + , quatOfEuler321 + , dcmOfQuat + , dcmOfQuatB2A + , dcmOfEuler321 + , quatOfDcm + , quatOfDcmB2A + , rotVecByDcm + , rotVecByDcmB2A + , rotVecByQuat + , rotVecByQuatB2A + , rotVecByEuler + , rotVecByEulerB2A + -- * re-exported from linear + , M33 + , V3(..) + , Quaternion(..) + ) where + +import Linear + +import Types + +-- $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 :: RealFloat 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 = atan2 r12 r11 + pitch = asin mr13 + roll = atan2 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.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.9238795325112867 (V3 0.0 0.0 0.3826834323650898) +-- +quatOfDcm :: RealFloat a => M33 a -> Quaternion a +quatOfDcm = quatOfEuler321 . euler321OfDcm + +quatOfDcmB2A :: (Conjugate a, RealFloat a) => M33 a -> Quaternion a +quatOfDcmB2A = conjugate . 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 :: RealFloat 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 = atan2 r12 r11 + pitch = asin mr13 + roll = atan2 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 +-- +-- >>> 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) +-- +-- >>> dcmOfEuler321 $ Euler {eYaw = pi/4, ePitch = 0, eRoll = 0} +-- V3 (V3 0.7071067811865475 0.7071067811865476 0.0) (V3 (-0.7071067811865476) 0.7071067811865475 0.0) (V3 0.0 0.0 1.0) +-- +dcmOfEuler321 :: (Floating a, Ord a) => Euler a -> M33 a +dcmOfEuler321 = dcmOfQuat . quatOfEuler321 + +dcmOfQuatB2A :: (Conjugate a, RealFloat a) => Quaternion a -> M33 a +dcmOfQuatB2A = dcmOfQuat . conjugate + +-- | 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,143 @@ +{-# OPTIONS_GHC -Wall #-} +{-# Language MultiParamTypeClasses #-} +{-# Language FunctionalDependencies #-} +{-# Language FlexibleInstances #-} +{-# Language GeneralizedNewtypeDeriving #-} +{-# Language DeriveFunctor #-} +{-# Language DeriveFoldable #-} +{-# Language DeriveTraversable #-} +{-# Language DeriveGeneric #-} + +module SpatialMathT + ( Rotation(..) + , Rot(..) + , V3T(..) + , M33T + , cross + , orthonormalize + ) where + +import Control.Applicative ( Applicative ) +import Data.Foldable ( Foldable ) +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, Storable + , Num, Fractional, Eq, Show + , Generic1, Generic + ) + +instance Serialize a => Serialize (V3T f a) where + get = do + x <- get + y <- get + z <- get + return (V3T (V3 x y z)) + put (V3T (V3 x y z)) = do + put x + put y + put z + +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, Serialize + , Generic1, Generic + ) + +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,49 @@ +{-# 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 Data.Data ( Data ) +import Data.Foldable ( Foldable ) +import Data.Traversable ( Traversable ) +#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702 +import GHC.Generics (Generic) +#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 + 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 ["SpatialMath.hs"] +main = doctest ["src/Types.hs", "src/SpatialMath.hs"]