module Data.Vect.Floating.Util.Quaternion where
import Data.Typeable
import Data.Vect.Floating.Base hiding (angle)
import Data.Vect.Floating.Interpolate
import Foreign.Storable
import System.Random
newtype Quaternion a = Q (Vec4 a)
deriving (Read,Show,Storable,AbelianGroup,Random,Interpolate a, Typeable)
deriving instance Floating a => Vector a Quaternion
deriving instance Floating a => DotProd a Quaternion
newtype UnitQuaternion a = U (Vec4 a)
deriving (Read,Show,Storable,Typeable)
deriving instance Floating a => DotProd a UnitQuaternion
type Q = Quaternion
type U = UnitQuaternion
instance Floating a => UnitVector a Quaternion UnitQuaternion where
mkNormal (Q v) = U (normalize v)
toNormalUnsafe (Q v) = U v
fromNormal (U v) = Q v
fromNormalRadius r (U v) = Q (v &* r)
unitQ :: Num a => Q a
unitQ = Q (Vec4 1 0 0 0)
zeroQ :: Num a => Q a
zeroQ = Q (Vec4 0 0 0 0)
multQ :: Num a => Q a -> Q a -> Q a
multQ (Q (Vec4 a1 b1 c1 d1)) (Q (Vec4 a2 b2 c2 d2)) = Q $ Vec4
(a1*a2 b1*b2 c1*c2 d1*d2)
(a1*b2 + b1*a2 + c1*d2 d1*c2)
(a1*c2 b1*d2 + c1*a2 + d1*b2)
(a1*d2 + b1*c2 c1*b2 + d1*a2)
negQ :: Floating a => Q a -> Q a
negQ (Q v) = Q (neg v)
normalizeQ :: Floating a => Q a -> Q a
normalizeQ (Q v) = Q (normalize v)
invQ :: Floating a => Q a -> Q a
invQ (Q (Vec4 a b c d)) = Q (v &* (1 / normsqr v)) where
v = Vec4 a (b) (c) (d)
fromQ :: Q a -> Vec4 a
fromQ (Q v) = v
toQ :: Vec4 a -> Q a
toQ = Q
instance Num a => MultSemiGroup (Quaternion a) where
one = unitQ
(.*.) = multQ
unitU :: Num a => U a
unitU = U (Vec4 1 0 0 0)
multU :: Num a => U a -> U a -> U a
multU (U (Vec4 a1 b1 c1 d1)) (U (Vec4 a2 b2 c2 d2)) = U $ Vec4
(a1*a2 b1*b2 c1*c2 d1*d2)
(a1*b2 + b1*a2 + c1*d2 d1*c2)
(a1*c2 b1*d2 + c1*a2 + d1*b2)
(a1*d2 + b1*c2 c1*b2 + d1*a2)
negU :: Floating a => U a -> U a
negU (U v) = U (neg v)
normalizeU :: Floating a => U a -> U a
normalizeU (U v) = U (normalize v)
invU :: Num a => U a -> U a
invU (U (Vec4 a b c d)) = U $ Vec4 a (b) (c) (d)
fromU :: Num a => U a -> Vec4 a
fromU (U v) = v
fromU' :: Floating a => U a -> Normal4 a
fromU' (U v) = toNormalUnsafe v
mkU :: Floating a => Vec4 a -> U a
mkU = U . normalize
toU :: Floating a => Normal4 a -> U a
toU = U . fromNormal
unsafeToU :: Num a => Vec4 a -> U a
unsafeToU = U
instance Floating a => MultSemiGroup (UnitQuaternion a) where
one = unitU
(.*.) = multU
instance Num a => LeftModule (UnitQuaternion a) (Vec3 a) where
lmul u v = actU u v
instance (Floating a, Ord a, Random a) => Random (UnitQuaternion a) where
random g = let (n, h) = random g
v = fromNormal n
in (U v, h)
randomR _ = random
actU :: Num a => U a -> Vec3 a -> Vec3 a
actU (U (Vec4 a b c d)) (Vec3 x y z) = Vec3 x' y' z' where
x' = x*(aa + bb cc dd) + y*( 2 * (bc ad) ) + z*( 2 * (bd + ac) )
y' = x*( 2 * (bc + ad) ) + y*(aa bb + cc dd) + z*( 2 * (cd ab) )
z' = x*( 2 * (bd ac) ) + y*( 2 * (cd + ab) ) + z*(aa bb cc + dd)
aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d
ab = a*b ; ac = a*c ; ad = a*d
bc = b*c ; bd = b*d ; cd = c*d
actU' :: Floating a => U a -> Normal3 a -> Normal3 a
actU' u n = toNormalUnsafe $ actU u (fromNormal n)
rotU :: Floating a => Vec3 a -> a -> U a
rotU axis angle = rotU' (mkNormal axis) angle
rotU' :: Floating a => Normal3 a -> a -> U a
rotU' axis angle = U (Vec4 c (x*s) (y*s) (z*s)) where
Vec3 x y z = fromNormal axis
half = 0.5 * angle
c = cos half
s = sin half
longSlerpU :: Floating a => a -> U a -> U a -> U a
longSlerpU t (U p0) (U p1) = U v where
v = (p0 &* y0) &+ (p1 &* y1)
omega = acos (p0 &. p1)
s = sin omega
y0 = sin (omega*(1t)) / s
y1 = sin (omega* t ) / s
slerpU :: (Floating a, Ord a) => a -> U a -> U a -> U a
slerpU t (U p0') (U p1) = U v where
v = (p0 &* y0) &+ (p1 &* y1)
d' = p0' &. p1
(d,p0) = if d' >= 0
then ( d', p0')
else (d', neg p0')
omega = acos d
s = sin omega
y0 = sin (omega*(1t)) / s
y1 = sin (omega* t ) / s
rightOrthoU :: Floating a => U a -> Ortho3 a
rightOrthoU = toOrthoUnsafe . transpose . fromOrtho . leftOrthoU
leftOrthoU :: Floating a => U a -> Ortho3 a
leftOrthoU (U (Vec4 a b c d)) = toOrthoUnsafe $ Mat3 row1 row2 row3 where
row1 = Vec3 (aa + bb cc dd) ( 2 * (bc ad) ) ( 2 * (bd + ac) )
row2 = Vec3 ( 2 * (bc + ad) ) (aa bb + cc dd) ( 2 * (cd ab) )
row3 = Vec3 ( 2 * (bd ac) ) ( 2 * (cd + ab) ) (aa bb cc + dd)
aa = a*a ; bb = b*b ; cc = c*c ; dd = d*d
ab = a*b ; ac = a*c ; ad = a*d
bc = b*c ; bd = b*d ; cd = c*d