spatial-math (empty) → 0.1.0.0
raw patch · 6 files changed
+414/−0 lines, 6 filesdep +basedep +hmatrixsetup-changed
Dependencies added: base, hmatrix
Files
- LICENSE +30/−0
- Setup.hs +2/−0
- SpatialMath.hs +198/−0
- SpatialMath/Quat.hs +65/−0
- SpatialMath/Xyz.hs +95/−0
- spatial-math.cabal +24/−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.
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ SpatialMath.hs view
@@ -0,0 +1,198 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language StandaloneDeriving #-}+{-# Language DeriveDataTypeable #-}++module SpatialMath ( module Xyz+ , module Quat+ , Xyz(..)+ , Quat(..)+ , Euler(..)+ , rotateXyzAboutX+ , euler321OfQuat+ , euler321OfDcm+ , quatOfEuler321+ , dcmOfQuat+ , dcmOfQuatB2A+ , quatOfDcm+ , quatOfDcmB2A+ , rotVecByDcm+ , rotVecByDcmB2A+ , rotVecByQuat+ , rotVecByQuatB2A+ ) where++import qualified SpatialMath.Xyz as Xyz+import qualified SpatialMath.Quat as Quat+import SpatialMath.Xyz ( Xyz(..) )+import SpatialMath.Quat ( Quat(..) )++import Numeric.LinearAlgebra+import Foreign.Storable ( Storable )+import Data.Data ( Data )+import Data.Typeable ( Typeable1 )++data Euler a = Euler a a a deriving (Eq, Show)++deriving instance Typeable1 Euler+deriving instance Data a => Data (Euler a)++rotateXyzAboutX :: Floating a => Xyz a -> a -> Xyz a+rotateXyzAboutX (Xyz ax ay az) rotAngle = Xyz bx by bz+ where+ cosTheta = cos rotAngle+ sinTheta = sin rotAngle++ bx = ax+ by = ay*cosTheta + az*sinTheta+ bz = -ay*sinTheta + az*cosTheta++euler321OfQuat :: RealFloat a => Quat a -> Euler a+euler321OfQuat (Quat q0 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++quatOfDcm :: (Storable a, RealFloat a) => Matrix a -> Quat a+quatOfDcm = quatOfEuler321 . euler321OfDcm++quatOfDcmB2A :: (Storable a, RealFloat a) => Matrix a -> Quat a+quatOfDcmB2A = Quat.inv . quatOfDcm++euler321OfDcm :: (RealFloat a, Storable a) => Matrix a -> Euler a+euler321OfDcm r = Euler yaw pitch roll+ where+ r11 = r @@> (0,0)+ r12 = r @@> (0,1)+ mr13' = -(r @@> (0,2))+ mr13 -- nan protect+ | mr13' > 1 = 1+ | mr13' < -1 = -1+ | otherwise = mr13'+ r23 = r @@> (1,2)+ r33 = r @@> (2,2)+ + yaw = atan2 r12 r11+ pitch = asin mr13+ roll = atan2 r23 r33++quatOfEuler321 :: (Floating a, Ord a) => Euler a -> Quat a+quatOfEuler321 (Euler yaw pitch roll) = Quat.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' = Quat q0 q1 q2 q3+ + q+ | q0 < 0 = negate q'+ | otherwise = q'++dcmOfQuat :: (Num a, Element a) => Quat a -> Matrix a+dcmOfQuat (Quat q0 q1 q2 q3) = fromLists [ [r0, r1, r2]+ , [r3, r4, r5]+ , [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++dcmOfQuatB2A :: (Num a, Element a) => Quat a -> Matrix a+dcmOfQuatB2A = dcmOfQuat . Quat.inv++-- | vec_b = R_a2b * vec_a+rotVecByDcm :: (Num a, Storable a) => Matrix a -> Xyz a -> Xyz a+rotVecByDcm dcm vec = Xyz.mult3x3ByXyz dcm vec++-- | vec_a = R_a2b^T * vec_b+rotVecByDcmB2A :: (Num a, Storable a) => Matrix a -> Xyz a -> Xyz a+rotVecByDcmB2A dcm vec = Xyz.mult3x3TransposeByXyz dcm vec++-- | vec_b = q_a2b * vec_a * q_a2b^(-1)+-- vec_b = R(q_a2b) * vec_a+rotVecByQuat :: (Num a, Element a) => Quat a -> Xyz a -> Xyz a+rotVecByQuat q = rotVecByDcm (dcmOfQuat q)++rotVecByQuatB2A :: (Num a, Element a) => Quat a -> Xyz a -> Xyz a+rotVecByQuatB2A q = rotVecByDcmB2A (dcmOfQuat q)++-- void+-- get_wind_angles_from_v_bw_b(double * alpha, double * beta, double * airspeed, const xyz_t * const v_bw_b)+-- {+-- double airspeed_internal_memory;+-- double * airspeed_internal = &airspeed_internal_memory;+-- +-- if (airspeed != NULL)+-- *airspeed = xyz_norm(v_bw_b) + 1e-12;+-- +-- if (beta != NULL)+-- {+-- if (airspeed != NULL)+-- airspeed_internal = airspeed;+-- else+-- *airspeed_internal = xyz_norm(v_bw_b) + 1e-12;+-- +-- *beta = asin ( v_bw_b->y / *airspeed_internal );+-- }+-- +-- if (alpha != NULL)+-- *alpha = atan2( v_bw_b->z, v_bw_b->x );+-- }+-- +-- void+-- get_wind_angles( double * alpha,+-- double * beta,+-- double * airspeed,+-- xyz_t * v_bw_b_out,+-- const quat_t * const q_n2b,+-- const xyz_t * const v_bn_b,+-- const xyz_t * const v_wn_n)+-- {+-- xyz_t v_wn_b;+-- rot_vec_by_quat_a2b( &v_wn_b, q_n2b, v_wn_n);+-- xyz_t v_bw_b;+-- xyz_diff( &v_bw_b, v_bn_b, &v_wn_b);+-- +-- get_wind_angles_from_v_bw_b( alpha, beta, airspeed, &v_bw_b );+-- +-- if (v_bw_b_out != NULL)+-- xyz_memcpy( v_bw_b_out, &v_bw_b);+-- }+-- +-- void+-- v_bw_b_from_wind_angles( xyz_t * v_bw_b, const double alpha, const double beta, const double airspeed)+-- {+-- v_bw_b->x = airspeed*cos(alpha)*cos(beta);+-- v_bw_b->y = airspeed*sin(beta);+-- v_bw_b->z = airspeed*cos(beta)*sin(alpha);+-- }
+ SpatialMath/Quat.hs view
@@ -0,0 +1,65 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language StandaloneDeriving #-}+{-# Language DeriveDataTypeable #-}++module SpatialMath.Quat ( Quat(..)+ , zipWithQuat+ , inv+ , norm+ , normalize+ , qmult+ , qmult'+ ) where++import Data.Data ( Data )+import Data.Typeable ( Typeable1 )++data Quat a = Quat a a a a deriving (Show, Eq)++deriving instance Typeable1 Quat+deriving instance Data a => Data (Quat a)++instance Functor Quat where+ fmap f (Quat q0 q1 q2 q3) = Quat (f q0) (f q1) (f q2) (f q3)++zipWithQuat :: (a -> b -> c) -> Quat a -> Quat b -> Quat c+zipWithQuat f (Quat p0 p1 p2 p3) (Quat q0 q1 q2 q3) = Quat (f p0 q0) (f p1 q1) (f p2 q2) (f p3 q3)++instance (Num a, Ord a) => Num (Quat a) where+ (+) = zipWithQuat (+)+ (-) = zipWithQuat (-)+ negate = fmap negate+ (*) = qmult+ abs = fmap abs+ signum = error "signum undefined for Quat"+ fromInteger = error "fromInteger undefined for Quat"++-- | q_out = q_in^-1+inv :: Num a => Quat a -> Quat a+inv (Quat q0 q1 q2 q3) = Quat q0 (-q1) (-q2) (-q3)++-- | return ||q||+norm :: Floating a => Quat a -> a+norm (Quat q0 q1 q2 q3) = sqrt $ q0*q0 + q1*q1 + q2*q2 + q3*q3++-- | q /= ||q||+normalize :: Floating a => Quat a -> Quat a+normalize q = fmap (* normInv) q+ where+ normInv = 1/(norm q)++-- | quaternion multiply: qa * qb+qmult :: (Num a, Ord a) => Quat a -> Quat a -> Quat a+qmult (Quat p0 p1 p2 p3) (Quat q0 q1 q2 q3)+ | r0 < 0 = negate qOut+ | otherwise = qOut+ where+ qOut = Quat r0 r1 r2 r3+ r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3+ r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2+ r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1+ r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0++-- | quaternion multiply then normalize+qmult' :: (Floating a, Ord a) => Quat a -> Quat a -> Quat a+qmult' p q = normalize (qmult q p)
+ SpatialMath/Xyz.hs view
@@ -0,0 +1,95 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language StandaloneDeriving #-}+{-# Language DeriveDataTypeable #-}++module SpatialMath.Xyz ( Xyz(..)+ , zipWithXyz+ , cross+ , dot+ , normSquared+ , norm+ , distance+ , scale+ , normalizeTo+ , normalize+ , mult3x3ByXyz+ , mult3x3TransposeByXyz+ ) where+++import Numeric.LinearAlgebra ( (@@>), Matrix )+import Foreign.Storable ( Storable )+import Data.Data ( Data )+import Data.Typeable ( Typeable1 )++data Xyz a = Xyz a a a deriving (Show, Eq)++deriving instance Typeable1 Xyz+deriving instance Data a => Data (Xyz a)++instance Functor Xyz where+ fmap f (Xyz x y z) = Xyz (f x) (f y) (f z)++zipWithXyz :: (a -> b -> c) -> Xyz a -> Xyz b -> Xyz c+zipWithXyz f (Xyz x0 y0 z0) (Xyz x1 y1 z1) = Xyz (f x0 x1) (f y0 y1) (f z0 z1)++instance (Num a) => Num (Xyz a) where+ (+) = zipWithXyz (+)+ (-) = zipWithXyz (-)+ negate = fmap negate+ (*) = error "(*) undefined for Xyz"+ abs = error "abs undefined for Xyz"+ signum = error "signum undefined for Xyz"+ fromInteger = error "fromInteger undefined for Xyz"++-- | c = a (cross) b+cross :: Num a => Xyz a -> Xyz a -> Xyz a+cross (Xyz ax ay az) (Xyz bx by bz) = Xyz cx cy cz+ where+ cx = ay*bz - az*by+ cy = - ax*bz + az*bx+ cz = ax*by - ay*bx++-- | c = a (dot) b+dot :: Num a => Xyz a -> Xyz a -> a+dot (Xyz ax ay az) (Xyz bx by bz) = ax*bx + ay*by + az*bz;++-- | c = vec (dot) vec+normSquared :: Num a => Xyz a -> a+normSquared x = dot x x++-- | norm(x)+norm :: Floating a => Xyz a -> a+norm x = sqrt $ dot x x++-- | norm(a - b)+distance :: Floating a => Xyz a -> Xyz a -> a+distance a b = norm $ a - b++-- | vec_out = vec_in*scale_factor+scale :: Num a => a -> Xyz a -> Xyz a+scale k = fmap (k *)++-- | vec_out = scale (new_norm/norm(vec_in)) vec_in+normalizeTo :: Floating a => a -> Xyz a -> Xyz a -> Xyz a+normalizeTo newNorm vec = scale (newNorm/(norm(vec) + 1e-12))++-- | vec_out = vec_in/norm(vec_in)+normalize :: Floating a => Xyz a -> Xyz a -> Xyz a+normalize = normalizeTo 1++-- | v_out = M*v+mult3x3ByXyz :: (Num a, Storable a) => Matrix a -> Xyz a -> Xyz a+mult3x3ByXyz mat (Xyz x y z) = Xyz x' y' z'+ where+ x' = (mat @@> (0,0))*x + (mat @@> (0,1))*y + (mat @@> (0,2))*z+ y' = (mat @@> (1,0))*x + (mat @@> (1,1))*y + (mat @@> (1,2))*z+ z' = (mat @@> (2,0))*x + (mat @@> (2,1))*y + (mat @@> (2,2))*z++-- // v_out = M^T*v+mult3x3TransposeByXyz :: (Num a, Storable a) => Matrix a -> Xyz a -> Xyz a+mult3x3TransposeByXyz mat (Xyz x y z) = Xyz x' y' z'+ where+ x' = (mat @@> (0,0))*x + (mat @@> (1,0))*y + (mat @@> (2,0))*z+ y' = (mat @@> (0,1))*x + (mat @@> (1,1))*y + (mat @@> (2,1))*z+ z' = (mat @@> (0,2))*x + (mat @@> (1,2))*y + (mat @@> (2,2))*z
+ spatial-math.cabal view
@@ -0,0 +1,24 @@+name: spatial-math+version: 0.1.0.0+synopsis: 3d math including quaternions/euler angles/dcms and utility functions+description: This is a port of my "mathlib" C library+license: BSD3+license-file: LICENSE+author: Greg Horn+maintainer: gregmainland@gmail.com+-- copyright: +category: Math+build-type: Simple+cabal-version: >=1.8++library+ exposed-modules: SpatialMath,+ SpatialMath.Quat,+ SpatialMath.Xyz+ -- other-modules: + build-depends: base >= 4 && < 5,+ hmatrix >= 0.14 && < 0.15++source-repository head+ type: git+ location: git://github.com/ghorn/spatial-math.git