{-# LANGUAGE BlockArguments #-}
{-# LANGUAGE DataKinds #-}
{-# LANGUAGE TypeFamilies #-}
module Geomancy.Quaternion
( Quaternion
, quaternion
, withQuaternion
, axisAngle
, rotate
, rotatePoint
, rotationBetween
, lookAtUp
, (^*)
, (^/)
, slerp
, conjugate
, norm
, quadrance
, dot
, normalize
, qNaN
) where
import Control.DeepSeq (NFData(rnf))
import Foreign (Storable(..), castPtr)
import Foreign.Ptr.Diff (peekDiffOff, pokeDiffOff)
import Graphics.Gl.Block (Block(..))
import Geomancy.Vec3 (Vec3, vec3, withVec3)
import qualified Geomancy.Vec3 as Vec3
data Quaternion = Quaternion
{-# UNPACK #-} !Float
{-# UNPACK #-} !Float
{-# UNPACK #-} !Float
{-# UNPACK #-} !Float
deriving (Quaternion -> Quaternion -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: Quaternion -> Quaternion -> Bool
$c/= :: Quaternion -> Quaternion -> Bool
== :: Quaternion -> Quaternion -> Bool
$c== :: Quaternion -> Quaternion -> Bool
Eq, Eq Quaternion
Quaternion -> Quaternion -> Bool
Quaternion -> Quaternion -> Ordering
Quaternion -> Quaternion -> Quaternion
forall a.
Eq a
-> (a -> a -> Ordering)
-> (a -> a -> Bool)
-> (a -> a -> Bool)
-> (a -> a -> Bool)
-> (a -> a -> Bool)
-> (a -> a -> a)
-> (a -> a -> a)
-> Ord a
min :: Quaternion -> Quaternion -> Quaternion
$cmin :: Quaternion -> Quaternion -> Quaternion
max :: Quaternion -> Quaternion -> Quaternion
$cmax :: Quaternion -> Quaternion -> Quaternion
>= :: Quaternion -> Quaternion -> Bool
$c>= :: Quaternion -> Quaternion -> Bool
> :: Quaternion -> Quaternion -> Bool
$c> :: Quaternion -> Quaternion -> Bool
<= :: Quaternion -> Quaternion -> Bool
$c<= :: Quaternion -> Quaternion -> Bool
< :: Quaternion -> Quaternion -> Bool
$c< :: Quaternion -> Quaternion -> Bool
compare :: Quaternion -> Quaternion -> Ordering
$ccompare :: Quaternion -> Quaternion -> Ordering
Ord, Int -> Quaternion -> ShowS
[Quaternion] -> ShowS
Quaternion -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Quaternion] -> ShowS
$cshowList :: [Quaternion] -> ShowS
show :: Quaternion -> String
$cshow :: Quaternion -> String
showsPrec :: Int -> Quaternion -> ShowS
$cshowsPrec :: Int -> Quaternion -> ShowS
Show)
{-# INLINE quaternion #-}
quaternion :: Float -> Float -> Float -> Float -> Quaternion
quaternion :: Float -> Float -> Float -> Float -> Quaternion
quaternion = Float -> Float -> Float -> Float -> Quaternion
Quaternion
{-# INLINE withQuaternion #-}
withQuaternion
:: Quaternion
-> (Float -> Float -> Float -> Float -> r)
-> r
withQuaternion :: forall r.
Quaternion -> (Float -> Float -> Float -> Float -> r) -> r
withQuaternion (Quaternion Float
a Float
b Float
c Float
d) Float -> Float -> Float -> Float -> r
f = Float -> Float -> Float -> Float -> r
f Float
a Float
b Float
c Float
d
{-# INLINE (^*) #-}
(^*) :: Quaternion -> Float -> Quaternion
Quaternion Float
a Float
b Float
c Float
d ^* :: Quaternion -> Float -> Quaternion
^* Float
x =
Float -> Float -> Float -> Float -> Quaternion
Quaternion
(Float
a forall a. Num a => a -> a -> a
* Float
x)
(Float
b forall a. Num a => a -> a -> a
* Float
x)
(Float
c forall a. Num a => a -> a -> a
* Float
x)
(Float
d forall a. Num a => a -> a -> a
* Float
x)
{-# INLINE (^/) #-}
(^/) :: Quaternion -> Float -> Quaternion
Quaternion Float
a Float
b Float
c Float
d ^/ :: Quaternion -> Float -> Quaternion
^/ Float
x =
Float -> Float -> Float -> Float -> Quaternion
Quaternion
(Float
a forall a. Fractional a => a -> a -> a
/ Float
x)
(Float
b forall a. Fractional a => a -> a -> a
/ Float
x)
(Float
c forall a. Fractional a => a -> a -> a
/ Float
x)
(Float
d forall a. Fractional a => a -> a -> a
/ Float
x)
slerp :: Quaternion -> Quaternion -> Float -> Quaternion
slerp :: Quaternion -> Quaternion -> Float -> Quaternion
slerp Quaternion
q Quaternion
p Float
t
| Float
1.0 forall a. Num a => a -> a -> a
- Float
cosphi forall a. Ord a => a -> a -> Bool
< Float
1e-8 =
Quaternion
q
| Bool
otherwise =
( (Quaternion
q Quaternion -> Float -> Quaternion
^* forall a. Floating a => a -> a
sin ((Float
1 forall a. Num a => a -> a -> a
- Float
t) forall a. Num a => a -> a -> a
* Float
phi)) forall a. Num a => a -> a -> a
+
Quaternion -> Quaternion
f Quaternion
p Quaternion -> Float -> Quaternion
^* forall a. Floating a => a -> a
sin (Float
t forall a. Num a => a -> a -> a
* Float
phi)
) Quaternion -> Float -> Quaternion
^/ forall a. Floating a => a -> a
sin Float
phi
where
phi :: Float
phi = forall a. Floating a => a -> a
acos Float
cosphi
(Float
cosphi, Quaternion -> Quaternion
f) =
if Float
dqp forall a. Ord a => a -> a -> Bool
< Float
0 then
(-Float
dqp, forall a. Num a => a -> a
negate)
else
(Float
dqp, forall a. a -> a
id)
dqp :: Float
dqp = Quaternion -> Quaternion -> Float
dot Quaternion
q Quaternion
p
{-# INLINE conjugate #-}
conjugate :: Quaternion -> Quaternion
conjugate :: Quaternion -> Quaternion
conjugate (Quaternion Float
e Float
x Float
y Float
z) = Float -> Float -> Float -> Float -> Quaternion
Quaternion Float
e (-Float
x) (-Float
y) (-Float
z)
{-# INLINE norm #-}
norm :: Quaternion -> Float
norm :: Quaternion -> Float
norm = forall a. Floating a => a -> a
sqrt forall b c a. (b -> c) -> (a -> b) -> a -> c
. Quaternion -> Float
quadrance
{-# INLINE quadrance #-}
quadrance :: Quaternion -> Float
quadrance :: Quaternion -> Float
quadrance Quaternion
q = Quaternion -> Quaternion -> Float
dot Quaternion
q Quaternion
q
{-# INLINE dot #-}
dot :: Quaternion -> Quaternion -> Float
dot :: Quaternion -> Quaternion -> Float
dot (Quaternion Float
a Float
b Float
c Float
d) (Quaternion Float
e Float
f Float
g Float
h) =
Float
a forall a. Num a => a -> a -> a
* Float
e forall a. Num a => a -> a -> a
+
Float
b forall a. Num a => a -> a -> a
* Float
f forall a. Num a => a -> a -> a
+
Float
c forall a. Num a => a -> a -> a
* Float
g forall a. Num a => a -> a -> a
+
Float
d forall a. Num a => a -> a -> a
* Float
h
{-# INLINE normalize #-}
normalize :: Quaternion -> Quaternion
normalize :: Quaternion -> Quaternion
normalize Quaternion
v =
if forall {a}. (Ord a, Fractional a) => a -> Bool
nearZero Float
q Bool -> Bool -> Bool
|| forall {a}. (Ord a, Fractional a) => a -> Bool
nearZero (Float
1forall a. Num a => a -> a -> a
-Float
q) then
Quaternion
v
else
let
Quaternion Float
e Float
i Float
j Float
k = Quaternion
v
in
Float -> Float -> Float -> Float -> Quaternion
Quaternion (Float
e forall a. Fractional a => a -> a -> a
/ Float
l) (Float
i forall a. Fractional a => a -> a -> a
/ Float
l) (Float
j forall a. Fractional a => a -> a -> a
/ Float
l) (Float
k forall a. Fractional a => a -> a -> a
/ Float
l)
where
q :: Float
q = Quaternion -> Quaternion -> Float
dot Quaternion
v Quaternion
v
l :: Float
l = forall a. Floating a => a -> a
sqrt Float
q
nearZero :: a -> Bool
nearZero a
a = forall a. Num a => a -> a
abs a
a forall a. Ord a => a -> a -> Bool
<= a
1e-6
instance NFData Quaternion where
rnf :: Quaternion -> ()
rnf Quaternion{} = ()
instance Num Quaternion where
{-# INLINE (+) #-}
Quaternion Float
a Float
b Float
c Float
d + :: Quaternion -> Quaternion -> Quaternion
+ Quaternion Float
e Float
f Float
g Float
h =
Float -> Float -> Float -> Float -> Quaternion
Quaternion
(Float
a forall a. Num a => a -> a -> a
+ Float
e)
(Float
b forall a. Num a => a -> a -> a
+ Float
f)
(Float
c forall a. Num a => a -> a -> a
+ Float
g)
(Float
d forall a. Num a => a -> a -> a
+ Float
h)
{-# INLINE (-) #-}
Quaternion Float
a Float
b Float
c Float
d - :: Quaternion -> Quaternion -> Quaternion
- Quaternion Float
e Float
f Float
g Float
h =
Float -> Float -> Float -> Float -> Quaternion
Quaternion
(Float
a forall a. Num a => a -> a -> a
- Float
e)
(Float
b forall a. Num a => a -> a -> a
- Float
f)
(Float
c forall a. Num a => a -> a -> a
- Float
g)
(Float
d forall a. Num a => a -> a -> a
- Float
h)
{-# INLINE (*) #-}
Quaternion Float
a Float
b Float
c Float
d * :: Quaternion -> Quaternion -> Quaternion
* Quaternion Float
e Float
f Float
g Float
h =
forall r. Vec3 -> (Float -> Float -> Float -> r) -> r
withVec3 Vec3
v \Float
y Float
z Float
w ->
Float -> Float -> Float -> Float -> Quaternion
Quaternion Float
x Float
y Float
z Float
w
where
x :: Float
x = Float
a forall a. Num a => a -> a -> a
* Float
e forall a. Num a => a -> a -> a
- Vec3 -> Vec3 -> Float
Vec3.dot Vec3
v1 Vec3
v2
v :: Vec3
v = Vec3 -> Vec3 -> Vec3
Vec3.cross Vec3
v1 Vec3
v2 forall a. Num a => a -> a -> a
+ Vec3
v2 Vec3 -> Float -> Vec3
Vec3.^* Float
a forall a. Num a => a -> a -> a
+ Vec3
v1 Vec3 -> Float -> Vec3
Vec3.^* Float
e
v1 :: Vec3
v1 = Float -> Float -> Float -> Vec3
vec3 Float
b Float
c Float
d
v2 :: Vec3
v2 = Float -> Float -> Float -> Vec3
vec3 Float
f Float
g Float
h
{-# INLINE fromInteger #-}
fromInteger :: Integer -> Quaternion
fromInteger Integer
x = Float -> Float -> Float -> Float -> Quaternion
Quaternion (forall a. Num a => Integer -> a
fromInteger Integer
x) Float
0 Float
0 Float
0
{-# INLINE abs #-}
abs :: Quaternion -> Quaternion
abs Quaternion
z = Float -> Float -> Float -> Float -> Quaternion
Quaternion (Quaternion -> Float
norm Quaternion
z) Float
0 Float
0 Float
0
{-# INLINE signum #-}
signum :: Quaternion -> Quaternion
signum q :: Quaternion
q@(Quaternion Float
e Float
i Float
j Float
k)
| Float
m forall a. Eq a => a -> a -> Bool
== Float
0 =
Quaternion
q
| Bool -> Bool
not (forall a. RealFloat a => a -> Bool
isInfinite Float
m Bool -> Bool -> Bool
|| forall a. RealFloat a => a -> Bool
isNaN Float
m) =
Float -> Float -> Float -> Float -> Quaternion
Quaternion (Float
e forall a. Num a => a -> a -> a
* Float
misqrt) (Float
i forall a. Num a => a -> a -> a
* Float
misqrt) (Float
j forall a. Num a => a -> a -> a
* Float
misqrt) (Float
k forall a. Num a => a -> a -> a
* Float
misqrt)
| forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
any forall a. RealFloat a => a -> Bool
isNaN [Float
e, Float
i, Float
j, Float
k] = Quaternion
qNaN
| Bool -> Bool
not (Bool
ii Bool -> Bool -> Bool
|| Bool
ij Bool -> Bool -> Bool
|| Bool
ik) = Float -> Float -> Float -> Float -> Quaternion
Quaternion Float
1 Float
0 Float
0 Float
0
| Bool -> Bool
not (Bool
ie Bool -> Bool -> Bool
|| Bool
ij Bool -> Bool -> Bool
|| Bool
ik) = Float -> Float -> Float -> Float -> Quaternion
Quaternion Float
0 Float
1 Float
0 Float
0
| Bool -> Bool
not (Bool
ie Bool -> Bool -> Bool
|| Bool
ii Bool -> Bool -> Bool
|| Bool
ik) = Float -> Float -> Float -> Float -> Quaternion
Quaternion Float
0 Float
0 Float
1 Float
0
| Bool -> Bool
not (Bool
ie Bool -> Bool -> Bool
|| Bool
ii Bool -> Bool -> Bool
|| Bool
ij) = Float -> Float -> Float -> Float -> Quaternion
Quaternion Float
0 Float
0 Float
0 Float
1
| Bool
otherwise = Quaternion
qNaN
where
m :: Float
m = Quaternion -> Float
quadrance Quaternion
q
misqrt :: Float
misqrt = forall a. Fractional a => a -> a
recip (forall a. Floating a => a -> a
sqrt Float
m)
ie :: Bool
ie = forall a. RealFloat a => a -> Bool
isInfinite Float
e
ii :: Bool
ii = forall a. RealFloat a => a -> Bool
isInfinite Float
i
ij :: Bool
ij = forall a. RealFloat a => a -> Bool
isInfinite Float
j
ik :: Bool
ik = forall a. RealFloat a => a -> Bool
isInfinite Float
k
{-# INLINE qNaN #-}
qNaN :: Quaternion
qNaN :: Quaternion
qNaN = Float -> Float -> Float -> Float -> Quaternion
Quaternion Float
fNaN Float
fNaN Float
fNaN Float
fNaN
where
fNaN :: Float
fNaN = Float
0forall a. Fractional a => a -> a -> a
/Float
0
instance Storable Quaternion where
{-# INLINE sizeOf #-}
sizeOf :: Quaternion -> Int
sizeOf Quaternion
_ = Int
16
{-# INLINE alignment #-}
alignment :: Quaternion -> Int
alignment Quaternion
_ = Int
16
{-# INLINE poke #-}
poke :: Ptr Quaternion -> Quaternion -> IO ()
poke Ptr Quaternion
ptr (Quaternion Float
a Float
b Float
c Float
d) = do
forall a. Storable a => Ptr a -> a -> IO ()
poke Ptr Float
ptr' Float
a
forall a. Storable a => Ptr a -> Int -> a -> IO ()
pokeElemOff Ptr Float
ptr' Int
1 Float
b
forall a. Storable a => Ptr a -> Int -> a -> IO ()
pokeElemOff Ptr Float
ptr' Int
2 Float
c
forall a. Storable a => Ptr a -> Int -> a -> IO ()
pokeElemOff Ptr Float
ptr' Int
3 Float
d
where
ptr' :: Ptr Float
ptr' = forall a b. Ptr a -> Ptr b
castPtr Ptr Quaternion
ptr
{-# INLINE peek #-}
peek :: Ptr Quaternion -> IO Quaternion
peek Ptr Quaternion
ptr = Float -> Float -> Float -> Float -> Quaternion
Quaternion
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall a. Storable a => Ptr a -> IO a
peek Ptr Float
ptr'
forall (f :: * -> *) a b. Applicative f => f (a -> b) -> f a -> f b
<*> forall a. Storable a => Ptr a -> Int -> IO a
peekElemOff Ptr Float
ptr' Int
1
forall (f :: * -> *) a b. Applicative f => f (a -> b) -> f a -> f b
<*> forall a. Storable a => Ptr a -> Int -> IO a
peekElemOff Ptr Float
ptr' Int
2
forall (f :: * -> *) a b. Applicative f => f (a -> b) -> f a -> f b
<*> forall a. Storable a => Ptr a -> Int -> IO a
peekElemOff Ptr Float
ptr' Int
3
where
ptr' :: Ptr Float
ptr' = forall a b. Ptr a -> Ptr b
castPtr Ptr Quaternion
ptr
{-# INLINE axisAngle #-}
axisAngle :: Vec3 -> Float -> Quaternion
axisAngle :: Vec3 -> Float -> Quaternion
axisAngle Vec3
axis Float
rads =
forall r. Vec3 -> (Float -> Float -> Float -> r) -> r
withVec3 (Vec3 -> Vec3
Vec3.normalize Vec3
axis Vec3 -> Float -> Vec3
Vec3.^* forall a. Floating a => a -> a
sin Float
half) forall a b. (a -> b) -> a -> b
$
Float -> Float -> Float -> Float -> Quaternion
quaternion (forall a. Floating a => a -> a
cos Float
half)
where
half :: Float
half = Float
rads forall a. Fractional a => a -> a -> a
/ Float
2
{-# INLINE rotate #-}
rotate :: Quaternion -> Vec3 -> Vec3
rotate :: Quaternion -> Vec3 -> Vec3
rotate Quaternion
q Vec3
v = forall r.
Quaternion -> (Float -> Float -> Float -> Float -> r) -> r
withQuaternion Quaternion
q' \Float
_a Float
b Float
c Float
d -> Float -> Float -> Float -> Vec3
vec3 Float
b Float
c Float
d
where
q' :: Quaternion
q' = forall r. Vec3 -> (Float -> Float -> Float -> r) -> r
withVec3 Vec3
v \Float
x Float
y Float
z ->
Quaternion
q forall a. Num a => a -> a -> a
* Float -> Float -> Float -> Float -> Quaternion
quaternion Float
0 Float
x Float
y Float
z forall a. Num a => a -> a -> a
* Quaternion -> Quaternion
conjugate Quaternion
q
{-# INLINE rotatePoint #-}
rotatePoint :: Quaternion -> Vec3 -> Vec3 -> Vec3
rotatePoint :: Quaternion -> Vec3 -> Vec3 -> Vec3
rotatePoint Quaternion
q Vec3
origin Vec3
point =
Vec3
origin forall a. Num a => a -> a -> a
+ Quaternion -> Vec3 -> Vec3
rotate Quaternion
q (Vec3
point forall a. Num a => a -> a -> a
- Vec3
origin)
rotationBetween :: Vec3 -> Vec3 -> Quaternion
rotationBetween :: Vec3 -> Vec3 -> Quaternion
rotationBetween Vec3
v1 Vec3
v2 = Vec3 -> Float -> Quaternion
axisAngle Vec3
axis Float
angle
where
axis :: Vec3
axis = Vec3 -> Vec3 -> Vec3
Vec3.cross Vec3
v1 Vec3
v2
angle :: Float
angle = forall a. Floating a => a -> a
acos Float
cosAngle
cosAngle :: Float
cosAngle =
forall a. Ord a => a -> a -> a
max (-Float
1) forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. Ord a => a -> a -> a
min Float
1 forall a b. (a -> b) -> a -> b
$
Vec3 -> Vec3 -> Float
Vec3.dot (Vec3 -> Vec3
Vec3.normalize Vec3
v1) (Vec3 -> Vec3
Vec3.normalize Vec3
v2)
lookAtUp :: Vec3 -> Vec3 -> Vec3 -> Quaternion
lookAtUp :: Vec3 -> Vec3 -> Vec3 -> Quaternion
lookAtUp Vec3
src Vec3
dst Vec3
up = Quaternion
rot2 forall a. Num a => a -> a -> a
* Quaternion
rot1
where
dir3 :: Vec3
dir3 = Vec3
dst forall a. Num a => a -> a -> a
- Vec3
src
rot1 :: Quaternion
rot1 = Vec3 -> Vec3 -> Quaternion
rotationBetween (Float -> Float -> Float -> Vec3
vec3 Float
0 Float
0 Float
1) Vec3
dir3
rot2 :: Quaternion
rot2 = Vec3 -> Vec3 -> Quaternion
rotationBetween Vec3
newUp Vec3
fixedUp
newUp :: Vec3
newUp = Quaternion -> Vec3 -> Vec3
rotate Quaternion
rot1 Vec3
up
fixedUp :: Vec3
fixedUp = Vec3 -> Vec3 -> Vec3
Vec3.cross (Vec3 -> Vec3 -> Vec3
Vec3.cross Vec3
dir3 Vec3
up) Vec3
dir3
instance Block Quaternion where
type PackedSize Quaternion = 16
alignment140 :: forall (proxy :: * -> *). proxy Quaternion -> Int
alignment140 proxy Quaternion
_ = Int
16
sizeOf140 :: forall (proxy :: * -> *). proxy Quaternion -> Int
sizeOf140 = forall b (proxy :: * -> *). Block b => proxy b -> Int
sizeOfPacked
alignment430 :: forall (proxy :: * -> *). proxy Quaternion -> Int
alignment430 = forall b (proxy :: * -> *). Block b => proxy b -> Int
alignment140
sizeOf430 :: forall (proxy :: * -> *). proxy Quaternion -> Int
sizeOf430 = forall b (proxy :: * -> *). Block b => proxy b -> Int
sizeOf140
isStruct :: forall (proxy :: * -> *). proxy Quaternion -> Bool
isStruct proxy Quaternion
_ = Bool
True
read140 :: forall (m :: * -> *) a.
MonadIO m =>
Ptr a -> Diff a Quaternion -> m Quaternion
read140 = forall (m :: * -> *) b a.
(MonadIO m, Storable b) =>
Ptr a -> Diff a b -> m b
peekDiffOff
write140 :: forall (m :: * -> *) a.
MonadIO m =>
Ptr a -> Diff a Quaternion -> Quaternion -> m ()
write140 = forall (m :: * -> *) b a.
(MonadIO m, Storable b) =>
Ptr a -> Diff a b -> b -> m ()
pokeDiffOff
read430 :: forall (m :: * -> *) a.
MonadIO m =>
Ptr a -> Diff a Quaternion -> m Quaternion
read430 = forall b (m :: * -> *) a.
(Block b, MonadIO m) =>
Ptr a -> Diff a b -> m b
read140
write430 :: forall (m :: * -> *) a.
MonadIO m =>
Ptr a -> Diff a Quaternion -> Quaternion -> m ()
write430 = forall b (m :: * -> *) a.
(Block b, MonadIO m) =>
Ptr a -> Diff a b -> b -> m ()
write140
readPacked :: forall (m :: * -> *) a.
MonadIO m =>
Ptr a -> Diff a Quaternion -> m Quaternion
readPacked = forall b (m :: * -> *) a.
(Block b, MonadIO m) =>
Ptr a -> Diff a b -> m b
read140
writePacked :: forall (m :: * -> *) a.
MonadIO m =>
Ptr a -> Diff a Quaternion -> Quaternion -> m ()
writePacked = forall b (m :: * -> *) a.
(Block b, MonadIO m) =>
Ptr a -> Diff a b -> b -> m ()
write140
{-# INLINE alignment140 #-}
{-# INLINE sizeOf140 #-}
{-# INLINE alignment430 #-}
{-# INLINE sizeOf430 #-}
{-# INLINE isStruct #-}
{-# INLINE read140 #-}
{-# INLINE write140 #-}
{-# INLINE read430 #-}
{-# INLINE write430 #-}
{-# INLINE readPacked #-}
{-# INLINE writePacked #-}