spatial-math 0.1.0.0 → 0.1.1
raw patch · 6 files changed
+169/−168 lines, 6 files
Files
- Quat.hs +65/−0
- SpatialMath.hs +5/−4
- SpatialMath/Quat.hs +0/−65
- SpatialMath/Xyz.hs +0/−95
- Xyz.hs +95/−0
- spatial-math.cabal +4/−4
+ Quat.hs view
@@ -0,0 +1,65 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language StandaloneDeriving #-}+{-# Language DeriveDataTypeable #-}++module 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.hs view
@@ -21,15 +21,16 @@ , rotVecByQuatB2A ) where -import qualified SpatialMath.Xyz as Xyz-import qualified SpatialMath.Quat as Quat-import SpatialMath.Xyz ( Xyz(..) )-import SpatialMath.Quat ( Quat(..) )+import qualified Xyz+import qualified Quat+import Xyz ( Xyz(..) )+import 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)
− SpatialMath/Quat.hs
@@ -1,65 +0,0 @@-{-# 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
@@ -1,95 +0,0 @@-{-# 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
+ Xyz.hs view
@@ -0,0 +1,95 @@+{-# OPTIONS_GHC -Wall #-}+{-# Language StandaloneDeriving #-}+{-# Language DeriveDataTypeable #-}++module 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
@@ -1,7 +1,7 @@ name: spatial-math-version: 0.1.0.0+version: 0.1.1 synopsis: 3d math including quaternions/euler angles/dcms and utility functions-description: This is a port of my "mathlib" C library+description: This is a port of my 'mathlib' C library: https://github.com/ghorn/mathlib license: BSD3 license-file: LICENSE author: Greg Horn@@ -13,8 +13,8 @@ library exposed-modules: SpatialMath,- SpatialMath.Quat,- SpatialMath.Xyz+ Quat,+ Xyz -- other-modules: build-depends: base >= 4 && < 5, hmatrix >= 0.14 && < 0.15