#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702
#endif
#ifndef MIN_VERSION_vector
#define MIN_VERSION_vector(x,y,z) 1
#endif
module Linear.Quaternion
( Quaternion(..)
, Complicated(..)
, Hamiltonian(..)
, ee, ei, ej, ek
, slerp
, asinq
, acosq
, atanq
, asinhq
, acoshq
, atanhq
, absi
, pow
, rotate
, axisAngle
) where
import Control.Applicative
import Control.DeepSeq (NFData(rnf))
import Control.Monad (liftM)
import Control.Monad.Fix
import Control.Monad.Zip
import Control.Lens hiding ((<.>))
import Data.Binary as Binary
import Data.Bytes.Serial
import Data.Complex (Complex((:+)))
import Data.Data
import Data.Distributive
import Data.Foldable
import Data.Functor.Bind
import Data.Functor.Classes
import Data.Functor.Rep
import Data.Hashable
import Data.Serialize as Cereal
import GHC.Arr (Ix(..))
import qualified Data.Foldable as F
import Data.Monoid
import Foreign.Ptr (castPtr, plusPtr)
import Foreign.Storable (Storable(..))
#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 qualified Data.Vector.Generic.Mutable as M
import qualified Data.Vector.Generic as G
import qualified Data.Vector.Unboxed.Base as U
import Linear.Epsilon
import Linear.Conjugate
import Linear.Metric
import Linear.V3
import Linear.Vector
import Prelude hiding (any)
data Quaternion a = Quaternion !a !(V3 a)
deriving (Eq,Ord,Read,Show,Data,Typeable
#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 702
,Generic
#endif
#if defined(__GLASGOW_HASKELL__) && __GLASGOW_HASKELL__ >= 706
,Generic1
#endif
)
instance Functor Quaternion where
fmap f (Quaternion e v) = Quaternion (f e) (fmap f v)
a <$ _ = Quaternion a (V3 a a a)
instance Apply Quaternion where
Quaternion f fv <.> Quaternion a v = Quaternion (f a) (fv <.> v)
instance Applicative Quaternion where
pure a = Quaternion a (pure a)
Quaternion f fv <*> Quaternion a v = Quaternion (f a) (fv <*> v)
instance Additive Quaternion where
zero = pure 0
liftU2 = liftA2
liftI2 = liftA2
instance Bind Quaternion where
Quaternion a (V3 b c d) >>- f = Quaternion a' (V3 b' c' d') where
Quaternion a' _ = f a
Quaternion _ (V3 b' _ _) = f b
Quaternion _ (V3 _ c' _) = f c
Quaternion _ (V3 _ _ d') = f d
instance Monad Quaternion where
return = pure
Quaternion a (V3 b c d) >>= f = Quaternion a' (V3 b' c' d') where
Quaternion a' _ = f a
Quaternion _ (V3 b' _ _) = f b
Quaternion _ (V3 _ c' _) = f c
Quaternion _ (V3 _ _ d') = f d
instance Ix a => Ix (Quaternion a) where
range (Quaternion l1 l2, Quaternion u1 u2) =
[ Quaternion i1 i2 | i1 <- range (l1,u1), i2 <- range (l2,u2) ]
unsafeIndex (Quaternion l1 l2, Quaternion u1 u2) (Quaternion i1 i2) =
unsafeIndex (l1,u1) i1 * unsafeRangeSize (l2,u2) + unsafeIndex (l2,u2) i2
inRange (Quaternion l1 l2, Quaternion u1 u2) (Quaternion i1 i2) =
inRange (l1,u1) i1 && inRange (l2,u2) i2
instance Representable Quaternion where
type Rep Quaternion = E Quaternion
tabulate f = Quaternion (f ee) (V3 (f ei) (f ej) (f ek))
index xs (E l) = view l xs
instance FunctorWithIndex (E Quaternion) Quaternion where
imap f (Quaternion a (V3 b c d)) = Quaternion (f ee a) $ V3 (f ei b) (f ej c) (f ek d)
instance FoldableWithIndex (E Quaternion) Quaternion where
ifoldMap f (Quaternion a (V3 b c d)) = f ee a `mappend` f ei b `mappend` f ej c `mappend` f ek d
instance TraversableWithIndex (E Quaternion) Quaternion where
itraverse f (Quaternion a (V3 b c d)) = Quaternion <$> f ee a <*> (V3 <$> f ei b <*> f ej c <*> f ek d)
type instance Index (Quaternion a) = E Quaternion
type instance IxValue (Quaternion a) = a
instance Ixed (Quaternion a) where
ix = el
instance Each (Quaternion a) (Quaternion b) a b where
each = traverse
instance Foldable Quaternion where
foldMap f (Quaternion e v) = f e `mappend` foldMap f v
foldr f z (Quaternion e v) = f e (F.foldr f z v)
instance Traversable Quaternion where
traverse f (Quaternion e v) = Quaternion <$> f e <*> traverse f v
instance Storable a => Storable (Quaternion a) where
sizeOf _ = 4 * sizeOf (undefined::a)
alignment _ = alignment (undefined::a)
poke ptr (Quaternion e v) = poke (castPtr ptr) e >>
poke (castPtr (ptr `plusPtr` sz)) v
where sz = sizeOf (undefined::a)
peek ptr = Quaternion <$> peek (castPtr ptr)
<*> peek (castPtr (ptr `plusPtr` sz))
where sz = sizeOf (undefined::a)
instance RealFloat a => Num (Quaternion a) where
(+) = liftA2 (+)
() = liftA2 ()
negate = fmap negate
Quaternion s1 v1 * Quaternion s2 v2 = Quaternion (s1*s2 (v1 `dot` v2)) $
(v1 `cross` v2) + s1*^v2 + s2*^v1
fromInteger x = Quaternion (fromInteger x) 0
abs z = Quaternion (norm z) 0
signum q@(Quaternion e (V3 i j k))
| m == 0.0 = q
| not (isInfinite m || isNaN m) = q ^/ sqrt m
| any isNaN q = qNaN
| not (ii || ij || ik) = Quaternion 1 (V3 0 0 0)
| not (ie || ij || ik) = Quaternion 0 (V3 1 0 0)
| not (ie || ii || ik) = Quaternion 0 (V3 0 1 0)
| not (ie || ii || ij) = Quaternion 0 (V3 0 0 1)
| otherwise = qNaN
where
m = quadrance q
ie = isInfinite e
ii = isInfinite i
ij = isInfinite j
ik = isInfinite k
instance Hashable a => Hashable (Quaternion a) where
hashWithSalt s (Quaternion a b) = s `hashWithSalt` a `hashWithSalt` b
qNaN :: RealFloat a => Quaternion a
qNaN = Quaternion fNaN (V3 fNaN fNaN fNaN) where fNaN = 0/0
instance RealFloat a => Fractional (Quaternion a) where
Quaternion q0 (V3 q1 q2 q3) / Quaternion r0 (V3 r1 r2 r3) =
Quaternion (r0*q0+r1*q1+r2*q2+r3*q3)
(V3 (r0*q1r1*q0r2*q3+r3*q2)
(r0*q2+r1*q3r2*q0r3*q1)
(r0*q3r1*q2+r2*q1r3*q0))
^/ (r0*r0 + r1*r1 + r2*r2 + r3*r3)
recip q = q ^/ quadrance q
fromRational x = Quaternion (fromRational x) 0
instance Metric Quaternion where
Quaternion e v `dot` Quaternion e' v' = e*e' + (v `dot` v')
class Complicated t where
_e, _i :: Lens' (t a) a
ee, ei :: Complicated t => E t
ee = E _e
ei = E _i
instance Complicated Complex where
_e f (a :+ b) = (:+ b) <$> f a
_i f (a :+ b) = (a :+) <$> f b
instance Complicated Quaternion where
_e f (Quaternion a v) = (`Quaternion` v) <$> f a
_i f (Quaternion a v) = Quaternion a <$> _x f v
class Complicated t => Hamiltonian t where
_j, _k :: Lens' (t a) a
_ijk :: Lens' (t a) (V3 a)
ej, ek :: Hamiltonian t => E t
ej = E _j
ek = E _k
instance Hamiltonian Quaternion where
_j f (Quaternion a v) = Quaternion a <$> _y f v
_k f (Quaternion a v) = Quaternion a <$> _z f v
_ijk f (Quaternion a v) = Quaternion a <$> f v
instance Distributive Quaternion where
distribute f = Quaternion (fmap (\(Quaternion x _) -> x) f) $ V3
(fmap (\(Quaternion _ (V3 y _ _)) -> y) f)
(fmap (\(Quaternion _ (V3 _ z _)) -> z) f)
(fmap (\(Quaternion _ (V3 _ _ w)) -> w) f)
instance (Conjugate a, RealFloat a) => Conjugate (Quaternion a) where
conjugate (Quaternion e v) = Quaternion (conjugate e) (negate v)
reimagine :: RealFloat a => a -> a -> Quaternion a -> Quaternion a
reimagine r s (Quaternion _ v)
| isNaN s || isInfinite s = let aux 0 = 0
aux x = s * x
in Quaternion r (aux <$> v)
| otherwise = Quaternion r (v^*s)
qi :: Num a => Quaternion a -> a
qi (Quaternion _ v) = quadrance v
absi :: Floating a => Quaternion a -> a
absi = sqrt . qi
pow :: RealFloat a => Quaternion a -> a -> Quaternion a
pow q t = exp (t *^ log q)
instance RealFloat a => Floating (Quaternion a) where
pi = Quaternion pi 0
exp q@(Quaternion e v)
| qiq == 0 = Quaternion (exp e) v
| ai <- sqrt qiq, exe <- exp e = reimagine (exe * cos ai) (exe * (sin ai / ai)) q
where qiq = qi q
log q@(Quaternion e v@(V3 _i j k))
| qiq == 0 = if e >= 0
then Quaternion (log e) v
else Quaternion (log (negate e)) (V3 pi j k)
| ai <- sqrt qiq, m <- sqrt (e*e + qiq) = reimagine (log m) (atan2 m e / ai) q
where qiq = qi q
x ** y = exp (y * log x)
sqrt q@(Quaternion e v)
| m == 0 = q
| qiq == 0 = if e > 0
then Quaternion (sqrt e) 0
else Quaternion 0 (V3 (sqrt (negate e)) 0 0)
| im <- sqrt (0.5*(me)) / sqrt qiq = Quaternion (0.5*(m+e)) (v^*im)
where qiq = qi q
m = sqrt (e*e + qiq)
cos q@(Quaternion e v)
| qiq == 0 = Quaternion (cos e) v
| ai <- sqrt qiq = reimagine (cos e * cosh ai) ( sin e * (sinh ai / ai)) q
where qiq = qi q
sin q@(Quaternion e v)
| qiq == 0 = Quaternion (sin e) v
| ai <- sqrt qiq = reimagine (sin e * cosh ai) (cos e * (sinh ai / ai)) q
where qiq = qi q
tan q@(Quaternion e v)
| qiq == 0 = Quaternion (tan e) v
| ai <- sqrt qiq, ce <- cos e, sai <- sinh ai, d <- ce*ce + sai*sai =
reimagine (ce * sin e / d) (cosh ai * (sai / ai) / d) q
where qiq = qi q
sinh q@(Quaternion e v)
| qiq == 0 = Quaternion (sinh e) v
| ai <- sqrt qiq = reimagine (sinh e * cos ai) (cosh e * (sin ai / ai)) q
where qiq = qi q
cosh q@(Quaternion e v)
| qiq == 0 = Quaternion (cosh e) v
| ai <- sqrt qiq = reimagine (cosh e * cos ai) ((sinh e * sin ai) / ai) q
where qiq = qi q
tanh q@(Quaternion e v)
| qiq == 0 = Quaternion (tanh e) v
| ai <- sqrt qiq, se <- sinh e, cai <- cos ai, d <- se*se + cai*cai =
reimagine ((cosh e * se) / d) ((cai * (sin ai / ai)) / d) q
where qiq = qi q
asin = cut asin
acos = cut acos
atan = cut atan
asinh = cut asinh
acosh = cut acosh
atanh = cut atanh
cut :: RealFloat a => (Complex a -> Complex a) -> Quaternion a -> Quaternion a
cut f q@(Quaternion e (V3 _ y z))
| qiq == 0 = Quaternion a (V3 b y z)
| otherwise = reimagine a (b / ai) q
where qiq = qi q
ai = sqrt qiq
a :+ b = f (e :+ ai)
cutWith :: RealFloat a => Complex a -> Quaternion a -> Quaternion a
cutWith (r :+ im) q@(Quaternion e v)
| e /= 0 || qiq == 0 || isNaN qiq || isInfinite qiq = error "bad cut"
| s <- im / sqrt qiq = Quaternion r (v^*s)
where qiq = qi q
asinq :: RealFloat a => Quaternion a -> Quaternion a -> Quaternion a
asinq q@(Quaternion e _) u
| qiq /= 0.0 || e >= 1 && e <= 1 = asin q
| otherwise = cutWith (asin (e :+ sqrt qiq)) u
where qiq = qi q
acosq :: RealFloat a => Quaternion a -> Quaternion a -> Quaternion a
acosq q@(Quaternion e _) u
| qiq /= 0.0 || e >= 1 && e <= 1 = acos q
| otherwise = cutWith (acos (e :+ sqrt qiq)) u
where qiq = qi q
atanq :: RealFloat a => Quaternion a -> Quaternion a -> Quaternion a
atanq q@(Quaternion e _) u
| e /= 0.0 || qiq >= 1 && qiq <= 1 = atan q
| otherwise = cutWith (atan (e :+ sqrt qiq)) u
where qiq = qi q
asinhq :: RealFloat a => Quaternion a -> Quaternion a -> Quaternion a
asinhq q@(Quaternion e _) u
| e /= 0.0 || qiq >= 1 && qiq <= 1 = asinh q
| otherwise = cutWith (asinh (e :+ sqrt qiq)) u
where qiq = qi q
acoshq :: RealFloat a => Quaternion a -> Quaternion a -> Quaternion a
acoshq q@(Quaternion e _) u
| qiq /= 0.0 || e >= 1 = asinh q
| otherwise = cutWith (acosh (e :+ sqrt qiq)) u
where qiq = qi q
atanhq :: RealFloat a => Quaternion a -> Quaternion a -> Quaternion a
atanhq q@(Quaternion e _) u
| qiq /= 0.0 || e > 1 && e < 1 = atanh q
| otherwise = cutWith (atanh (e :+ sqrt qiq)) u
where qiq = qi q
slerp :: RealFloat a => Quaternion a -> Quaternion a -> a -> Quaternion a
slerp q p t
| 1.0 cosphi < 1e-8 = q
| otherwise = ((sin ((1t)*phi) *^ q) + sin (t*phi) *^ f p) ^/ sin phi
where
dqp = dot q p
(cosphi, f) = if dqp < 0 then (dqp, negate) else (dqp, id)
phi = acos cosphi
rotate :: (Conjugate a, RealFloat a) => Quaternion a -> V3 a -> V3 a
rotate q v = ijk where
Quaternion _ ijk = q * Quaternion 0 v * conjugate q
instance (RealFloat a, Epsilon a) => Epsilon (Quaternion a) where
nearZero = nearZero . quadrance
axisAngle :: (Epsilon a, Floating a) => V3 a -> a -> Quaternion a
axisAngle axis theta = Quaternion (cos half) (sin half *^ normalize axis)
where half = theta / 2
data instance U.Vector (Quaternion a) = V_Quaternion !Int (U.Vector a)
data instance U.MVector s (Quaternion a) = MV_Quaternion !Int (U.MVector s a)
instance U.Unbox a => U.Unbox (Quaternion a)
instance U.Unbox a => M.MVector U.MVector (Quaternion a) where
basicLength (MV_Quaternion n _) = n
basicUnsafeSlice m n (MV_Quaternion _ v) = MV_Quaternion n (M.basicUnsafeSlice (4*m) (4*n) v)
basicOverlaps (MV_Quaternion _ v) (MV_Quaternion _ u) = M.basicOverlaps v u
basicUnsafeNew n = liftM (MV_Quaternion n) (M.basicUnsafeNew (4*n))
basicUnsafeRead (MV_Quaternion _ v) i =
do let o = 4*i
x <- M.basicUnsafeRead v o
y <- M.basicUnsafeRead v (o+1)
z <- M.basicUnsafeRead v (o+2)
w <- M.basicUnsafeRead v (o+3)
return (Quaternion x (V3 y z w))
basicUnsafeWrite (MV_Quaternion _ v) i (Quaternion x (V3 y z w)) =
do let o = 4*i
M.basicUnsafeWrite v o x
M.basicUnsafeWrite v (o+1) y
M.basicUnsafeWrite v (o+2) z
M.basicUnsafeWrite v (o+3) w
#if MIN_VERSION_vector(0,11,0)
basicInitialize (MV_Quaternion _ v) = M.basicInitialize v
#endif
instance U.Unbox a => G.Vector U.Vector (Quaternion a) where
basicUnsafeFreeze (MV_Quaternion n v) = liftM ( V_Quaternion n) (G.basicUnsafeFreeze v)
basicUnsafeThaw ( V_Quaternion n v) = liftM (MV_Quaternion n) (G.basicUnsafeThaw v)
basicLength ( V_Quaternion n _) = n
basicUnsafeSlice m n (V_Quaternion _ v) = V_Quaternion n (G.basicUnsafeSlice (4*m) (4*n) v)
basicUnsafeIndexM (V_Quaternion _ v) i =
do let o = 4*i
x <- G.basicUnsafeIndexM v o
y <- G.basicUnsafeIndexM v (o+1)
z <- G.basicUnsafeIndexM v (o+2)
w <- G.basicUnsafeIndexM v (o+3)
return (Quaternion x (V3 y z w))
instance MonadZip Quaternion where
mzipWith = liftA2
instance MonadFix Quaternion where
mfix f = Quaternion (let Quaternion a _ = f a in a)
(V3 (let Quaternion _ (V3 a _ _) = f a in a)
(let Quaternion _ (V3 _ a _) = f a in a)
(let Quaternion _ (V3 _ _ a) = f a in a))
instance NFData a => NFData (Quaternion a) where
rnf (Quaternion a b) = rnf a `seq` rnf b
instance Serial1 Quaternion where
serializeWith f (Quaternion a b) = f a >> serializeWith f b
deserializeWith f = Quaternion <$> f <*> deserializeWith f
instance Serial a => Serial (Quaternion a) where
serialize = serializeWith serialize
deserialize = deserializeWith deserialize
instance Binary a => Binary (Quaternion a) where
put = serializeWith Binary.put
get = deserializeWith Binary.get
instance Serialize a => Serialize (Quaternion a) where
put = serializeWith Cereal.put
get = deserializeWith Cereal.get
instance Eq1 Quaternion where eq1 = (==)
instance Ord1 Quaternion where compare1 = compare
instance Show1 Quaternion where showsPrec1 = showsPrec
instance Read1 Quaternion where readsPrec1 = readsPrec