{- ORMOLU_DISABLE -}
-- Implicit CAD. Copyright (C) 2011, Christopher Olah (chris@colah.ca)
-- Copyright (C) 2014 2015 2016, Julia Longtin (julial@turinglace.com)
-- Released under the GNU AGPLV3+, see LICENSE

{-# LANGUAGE FlexibleContexts #-}
{-# LANGUAGE ViewPatterns #-}

-- A module of math utilities.
module Graphics.Implicit.MathUtil (rmax, rmaximum, rminimum, distFromLineSeg, pack, box3sWithin, reflect, alaV3, packV3, unpackV3, quaternionToEuler, infty) where

-- Explicitly include what we need from Prelude.
import Prelude (Num, Fractional, Bool, RealFloat, Ordering, (.), (>), (<), (+), ($), (/), otherwise, not, (||), (&&), abs, (-), (*), sin, asin, pi, max, sqrt, min, compare, (<=), fst, snd, (<>), flip, (>=), signum, atan2, error, (/=))

import Graphics.Implicit.Definitions (ℝ, ℝ2, ℝ3, Box2)

import Data.List (sort, sortBy)
import Linear (Metric, (*^), norm, distance, normalize, dot, Quaternion(Quaternion), V2(V2), V3(V3))

-- | The distance a point p is from a line segment (a,b)
distFromLineSeg :: ℝ2 -> (ℝ2, ℝ2) -> ℝ
distFromLineSeg :: ℝ2 -> (ℝ2, ℝ2) -> ℝ
distFromLineSeg ℝ2
p (ℝ2
a,ℝ2
b) = ℝ2 -> ℝ2 -> ℝ
forall (f :: * -> *) a. (Metric f, Floating a) => f a -> f a -> a
distance ℝ2
p ℝ2
closest
    where
        ab :: ℝ2
ab = ℝ2
b ℝ2 -> ℝ2 -> ℝ2
forall a. Num a => a -> a -> a
- ℝ2
a
        ap :: ℝ2
ap = ℝ2
p ℝ2 -> ℝ2 -> ℝ2
forall a. Num a => a -> a -> a
- ℝ2
a
        d :: ℝ
        d :: ℝ
d  = ℝ2 -> ℝ2
forall a (f :: * -> *).
(Floating a, Metric f, Epsilon a) =>
f a -> f a
normalize ℝ2
ab ℝ2 -> ℝ2 -> ℝ
forall (f :: * -> *) a. (Metric f, Num a) => f a -> f a -> a
`dot` ℝ2
ap
        -- the closest point to p on the line segment.
        closest :: ℝ2
        closest :: ℝ2
closest
            | ℝ
d ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
< ℝ
0 = ℝ2
a
            | ℝ
d ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
> ℝ2 -> ℝ
forall (f :: * -> *) a. (Metric f, Floating a) => f a -> a
norm ℝ2
ab = ℝ2
b
            | Bool
otherwise = ℝ2
a ℝ2 -> ℝ2 -> ℝ2
forall a. Num a => a -> a -> a
+ ℝ
d ℝ -> ℝ2 -> ℝ2
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ ℝ2 -> ℝ2
forall a (f :: * -> *).
(Floating a, Metric f, Epsilon a) =>
f a -> f a
normalize ℝ2
ab

box3sWithin :: ℝ -> (ℝ3, ℝ3) -> (ℝ3, ℝ3) -> Bool
box3sWithin :: ℝ -> (ℝ3, ℝ3) -> (ℝ3, ℝ3) -> Bool
box3sWithin ℝ
r (V3 ℝ
ax1 ℝ
ay1 ℝ
az1, V3 ℝ
ax2 ℝ
ay2 ℝ
az2) (V3 ℝ
bx1 ℝ
by1 ℝ
bz1, V3 ℝ
bx2 ℝ
by2 ℝ
bz2) =
    let
        near :: (ℝ, ℝ) -> (ℝ, ℝ) -> Bool
near (ℝ
a1, ℝ
a2) (ℝ
b1, ℝ
b2) = Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ (ℝ
a2 ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ ℝ
r ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
< ℝ
b1) Bool -> Bool -> Bool
|| (ℝ
b2 ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ ℝ
r ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
< ℝ
a1)
    in
           (ℝ
ax1,ℝ
ax2) (ℝ, ℝ) -> (ℝ, ℝ) -> Bool
`near` (ℝ
bx1, ℝ
bx2)
        Bool -> Bool -> Bool
&& (ℝ
ay1,ℝ
ay2) (ℝ, ℝ) -> (ℝ, ℝ) -> Bool
`near` (ℝ
by1, ℝ
by2)
        Bool -> Bool -> Bool
&& (ℝ
az1,ℝ
az2) (ℝ, ℝ) -> (ℝ, ℝ) -> Bool
`near` (ℝ
bz1, ℝ
bz2)

-- | Rounded Maximum
-- Consider  max(x,y) = 0, the generated curve
-- has a square-like corner. We replace it with a
-- quarter of a circle
--
-- NOTE: rmax is not associative!
rmax ::
    ℝ     -- ^ radius
    -> ℝ  -- ^ first number to round maximum
    -> ℝ  -- ^ second number to round maximum
    -> ℝ  -- ^ resulting number
rmax :: ℝ -> ℝ -> ℝ -> ℝ
rmax ℝ
r ℝ
x ℝ
y = if ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
xℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y) ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
< ℝ
r Bool -> Bool -> Bool
&& ℝ
r ℝ -> ℝ -> Bool
forall a. Eq a => a -> a -> Bool
/= ℝ
0
                then ℝ
y ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
- ℝ
rℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
*ℝ -> ℝ
forall a. Floating a => a -> a
sin(ℝ
forall a. Floating a => a
piℝ -> ℝ -> ℝ
forall a. Fractional a => a -> a -> a
/ℝ
4ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ -> ℝ
forall a. Floating a => a -> a
asin((ℝ
xℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y)ℝ -> ℝ -> ℝ
forall a. Fractional a => a -> a -> a
/ℝ
rℝ -> ℝ -> ℝ
forall a. Fractional a => a -> a -> a
/ℝ -> ℝ
forall a. Floating a => a -> a
sqrt ℝ
2)) ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ ℝ
r
                else ℝ -> ℝ -> ℝ
forall a. Ord a => a -> a -> a
max ℝ
x ℝ
y

-- | Rounded minimum
--
-- NOTE: rmin is not associative!
rmin ::
    ℝ     -- ^ radius
    -> ℝ  -- ^ first number to round minimum
    -> ℝ  -- ^ second number to round minimum
    -> ℝ  -- ^ resulting number
rmin :: ℝ -> ℝ -> ℝ -> ℝ
rmin ℝ
r ℝ
x ℝ
y = if ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
xℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y) ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
< ℝ
r Bool -> Bool -> Bool
&& ℝ
r ℝ -> ℝ -> Bool
forall a. Eq a => a -> a -> Bool
/= ℝ
0
    then ℝ
y ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ ℝ
rℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
*ℝ -> ℝ
forall a. Floating a => a -> a
sin(ℝ
forall a. Floating a => a
piℝ -> ℝ -> ℝ
forall a. Fractional a => a -> a -> a
/ℝ
4ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ℝ -> ℝ
forall a. Floating a => a -> a
asin((ℝ
xℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y)ℝ -> ℝ -> ℝ
forall a. Fractional a => a -> a -> a
/ℝ
rℝ -> ℝ -> ℝ
forall a. Fractional a => a -> a -> a
/ℝ -> ℝ
forall a. Floating a => a -> a
sqrt ℝ
2)) ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
- ℝ
r
    else ℝ -> ℝ -> ℝ
forall a. Ord a => a -> a -> a
min ℝ
x ℝ
y

-- | Like rmax, but on a list instead of two.
-- Just as maximum is.
-- The implementation is to take the maximum two
-- and rmax those.
rmaximum ::
    ℝ      -- ^ radius
    -> [ℝ] -- ^ numbers to take round maximum
    -> ℝ   -- ^ resulting number
rmaximum :: ℝ -> [ℝ] -> ℝ
rmaximum ℝ
_ [] = ℝ
0
rmaximum ℝ
_ [ℝ
a] = ℝ
a
rmaximum ℝ
r [ℝ
a,ℝ
b] = ℝ -> ℝ -> ℝ -> ℝ
rmax ℝ
r ℝ
a ℝ
b
rmaximum ℝ
r ((ℝ -> ℝ -> Ordering) -> [ℝ] -> [ℝ]
forall a. (a -> a -> Ordering) -> [a] -> [a]
sortBy ((ℝ -> ℝ -> Ordering) -> ℝ -> ℝ -> Ordering
forall a b c. (a -> b -> c) -> b -> a -> c
flip ℝ -> ℝ -> Ordering
forall a. Ord a => a -> a -> Ordering
compare) -> (ℝ
a:ℝ
b:ℝ
_:[ℝ]
_)) = ℝ -> ℝ -> ℝ -> ℝ
rmax ℝ
r ℝ
a ℝ
b
rmaximum ℝ
_ [ℝ]
_ = [Char] -> ℝ
forall a. HasCallStack => [Char] -> a
error [Char]
"impossible."  -- (and with dependent types we could prove it!)

-- | Like rmin but on a list.
rminimum ::
    ℝ      -- ^ radius
    -> [ℝ] -- ^ numbers to take round minimum
    -> ℝ   -- ^ resulting number
rminimum :: ℝ -> [ℝ] -> ℝ
rminimum ℝ
_ [] = ℝ
0
rminimum ℝ
_ [ℝ
a] = ℝ
a
rminimum ℝ
r [ℝ
a,ℝ
b] = ℝ -> ℝ -> ℝ -> ℝ
rmin ℝ
r ℝ
a ℝ
b
rminimum ℝ
r ([ℝ] -> [ℝ]
forall a. Ord a => [a] -> [a]
sort -> (ℝ
a:ℝ
b:ℝ
_:[ℝ]
_)) = ℝ -> ℝ -> ℝ -> ℝ
rmin ℝ
r ℝ
a ℝ
b
rminimum ℝ
_ [ℝ]
_ = [Char] -> ℝ
forall a. HasCallStack => [Char] -> a
error [Char]
"impossible."

-- | Pack the given objects in a box the given size.
pack ::
    Box2           -- ^ The box to pack within
    -> ℝ           -- ^ The space seperation between items
    -> [(Box2, a)] -- ^ Objects with their boxes
    -> ([(ℝ2, a)], [(Box2, a)] ) -- ^ Packed objects with their positions, objects that could be packed
pack :: (ℝ2, ℝ2) -> ℝ -> [((ℝ2, ℝ2), a)] -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
pack (ℝ2
dx, ℝ2
dy) ℝ
sep [((ℝ2, ℝ2), a)]
objs = [((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall a.
[((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
packSome [((ℝ2, ℝ2), a)]
sortedObjs (ℝ2
dx, ℝ2
dy)
    where
        compareBoxesByY :: Box2 -> Box2 -> Ordering
        compareBoxesByY :: (ℝ2, ℝ2) -> (ℝ2, ℝ2) -> Ordering
compareBoxesByY  (V2 ℝ
_ ℝ
ay1, V2 ℝ
_ ℝ
ay2)  (V2 ℝ
_ ℝ
by1, V2 ℝ
_ ℝ
by2) =
                ℝ -> ℝ -> Ordering
forall a. Ord a => a -> a -> Ordering
compare (ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ -> ℝ) -> ℝ -> ℝ
forall a b. (a -> b) -> a -> b
$ ℝ
by2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
by1) (ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ -> ℝ) -> ℝ -> ℝ
forall a b. (a -> b) -> a -> b
$ ℝ
ay2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
ay1)

        sortedObjs :: [((ℝ2, ℝ2), a)]
sortedObjs = (((ℝ2, ℝ2), a) -> ((ℝ2, ℝ2), a) -> Ordering)
-> [((ℝ2, ℝ2), a)] -> [((ℝ2, ℝ2), a)]
forall a. (a -> a -> Ordering) -> [a] -> [a]
sortBy
            (\((ℝ2, ℝ2)
boxa, a
_) ((ℝ2, ℝ2)
boxb, a
_) -> (ℝ2, ℝ2) -> (ℝ2, ℝ2) -> Ordering
compareBoxesByY (ℝ2, ℝ2)
boxa (ℝ2, ℝ2)
boxb )
            [((ℝ2, ℝ2), a)]
objs

        tmap1 :: (t2 -> t) -> (t2, t1) -> (t, t1)
        tmap1 :: (t2 -> t) -> (t2, t1) -> (t, t1)
tmap1 t2 -> t
f (t2
a,t1
b) = (t2 -> t
f t2
a, t1
b)
        tmap2 :: (t2 -> t1) -> (t, t2) -> (t, t1)
        tmap2 :: (t2 -> t1) -> (t, t2) -> (t, t1)
tmap2 t2 -> t1
f (t
a,t2
b) = (t
a, t2 -> t1
f t2
b)

        packSome :: [(Box2,a)] -> Box2 -> ([(ℝ2,a)], [(Box2,a)])
        packSome :: [((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
packSome (presObj :: ((ℝ2, ℝ2), a)
presObj@((V2 ℝ
x1 ℝ
y1,V2 ℝ
x2 ℝ
y2),a
obj):[((ℝ2, ℝ2), a)]
otherBoxedObjs) box :: (ℝ2, ℝ2)
box@(V2 ℝ
bx1 ℝ
by1, V2 ℝ
bx2 ℝ
by2) =
            if ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
x2 ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
- ℝ
x1) ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
<= ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
bx2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
bx1) Bool -> Bool -> Bool
&& ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
y2 ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
- ℝ
y1) ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
<= ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
by2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
by1)
            then
                let
                    row :: ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
row = ([(ℝ2, a)] -> [(ℝ2, a)])
-> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall t2 t t1. (t2 -> t) -> (t2, t1) -> (t, t1)
tmap1 ((ℝ -> ℝ -> ℝ2
forall a. a -> a -> V2 a
V2 (ℝ
bx1ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
x1) (ℝ
by1ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y1), a
obj)(ℝ2, a) -> [(ℝ2, a)] -> [(ℝ2, a)]
forall a. a -> [a] -> [a]
:) (([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]))
-> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall a b. (a -> b) -> a -> b
$
                        [((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall a.
[((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
packSome [((ℝ2, ℝ2), a)]
otherBoxedObjs (ℝ -> ℝ -> ℝ2
forall a. a -> a -> V2 a
V2 (ℝ
bx1ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ℝ
x2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
x1ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ℝ
sep) ℝ
by1, ℝ -> ℝ -> ℝ2
forall a. a -> a -> V2 a
V2 ℝ
bx2 (ℝ
by1 ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ ℝ
y2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y1))
                    rowAndUp :: ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
rowAndUp =
                        if ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
by2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
by1) ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
- ℝ -> ℝ
forall a. Num a => a -> a
abs (ℝ
y2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y1) ℝ -> ℝ -> Bool
forall a. Ord a => a -> a -> Bool
> ℝ
sep
                        then ([(ℝ2, a)] -> [(ℝ2, a)])
-> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall t2 t t1. (t2 -> t) -> (t2, t1) -> (t, t1)
tmap1 (([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> [(ℝ2, a)]
forall a b. (a, b) -> a
fst ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
row [(ℝ2, a)] -> [(ℝ2, a)] -> [(ℝ2, a)]
forall a. Semigroup a => a -> a -> a
<> ) (([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]))
-> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall a b. (a -> b) -> a -> b
$
                            [((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall a.
[((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
packSome (([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> [((ℝ2, ℝ2), a)]
forall a b. (a, b) -> b
snd ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
row) (ℝ -> ℝ -> ℝ2
forall a. a -> a -> V2 a
V2 ℝ
bx1 (ℝ
by1 ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ ℝ
y2ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
-ℝ
y1ℝ -> ℝ -> ℝ
forall a. Num a => a -> a -> a
+ℝ
sep), ℝ -> ℝ -> ℝ2
forall a. a -> a -> V2 a
V2 ℝ
bx2 ℝ
by2)
                        else ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
row
                in
                    ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
rowAndUp
            else
                ([((ℝ2, ℝ2), a)] -> [((ℝ2, ℝ2), a)])
-> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall t2 t1 t. (t2 -> t1) -> (t, t2) -> (t, t1)
tmap2 (((ℝ2, ℝ2), a)
presObj((ℝ2, ℝ2), a) -> [((ℝ2, ℝ2), a)] -> [((ℝ2, ℝ2), a)]
forall a. a -> [a] -> [a]
:) (([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]))
-> ([(ℝ2, a)], [((ℝ2, ℝ2), a)]) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall a b. (a -> b) -> a -> b
$ [((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
forall a.
[((ℝ2, ℝ2), a)] -> (ℝ2, ℝ2) -> ([(ℝ2, a)], [((ℝ2, ℝ2), a)])
packSome [((ℝ2, ℝ2), a)]
otherBoxedObjs (ℝ2, ℝ2)
box
        packSome [] (ℝ2, ℝ2)
_ = ([], [])

-- | Reflect a vector across a hyperplane defined by its normal vector.
--
-- From https://en.wikipedia.org/wiki/Reflection_(mathematics)#Reflection_through_a_hyperplane_in_n_dimensions
reflect
    :: (Num (f a), Fractional a, Metric f)
    => f a  -- ^ Mirror axis
    -> f a  -- ^ Vector to transform
    -> f a
reflect :: f a -> f a -> f a
reflect f a
a f a
v = f a
v f a -> f a -> f a
forall a. Num a => a -> a -> a
- (a
2 a -> a -> a
forall a. Num a => a -> a -> a
* ((f a
v f a -> f a -> a
forall (f :: * -> *) a. (Metric f, Num a) => f a -> f a -> a
`dot` f a
a) a -> a -> a
forall a. Fractional a => a -> a -> a
/ (f a
a f a -> f a -> a
forall (f :: * -> *) a. (Metric f, Num a) => f a -> f a -> a
`dot` f a
a))) a -> f a -> f a
forall (f :: * -> *) a. (Functor f, Num a) => a -> f a -> f a
*^ f a
a

-- | Lift a function over 'V3' into a function over 'ℝ3'.
alaV3 :: (V3 a -> V3 a) -> (a, a, a) -> (a, a, a)
alaV3 :: (V3 a -> V3 a) -> (a, a, a) -> (a, a, a)
alaV3 V3 a -> V3 a
f = V3 a -> (a, a, a)
forall a. V3 a -> (a, a, a)
unpackV3 (V3 a -> (a, a, a))
-> ((a, a, a) -> V3 a) -> (a, a, a) -> (a, a, a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. V3 a -> V3 a
f (V3 a -> V3 a) -> ((a, a, a) -> V3 a) -> (a, a, a) -> V3 a
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (a, a, a) -> V3 a
forall a. (a, a, a) -> V3 a
packV3
{-# INLINABLE alaV3 #-}

packV3 :: (a, a, a) -> V3 a
packV3 :: (a, a, a) -> V3 a
packV3 (a
x, a
y, a
z) = a -> a -> a -> V3 a
forall a. a -> a -> a -> V3 a
V3 a
x a
y a
z
{-# INLINABLE packV3 #-}

unpackV3 :: V3 a -> (a, a, a)
unpackV3 :: V3 a -> (a, a, a)
unpackV3 (V3 a
a a
a2 a
a3) = (a
a, a
a2, a
a3)
{-# INLINABLE unpackV3 #-}

-- | Convert a 'Quaternion' to its constituent euler angles.
--
-- From https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2
quaternionToEuler :: RealFloat a => Quaternion a -> (a, a, a)
quaternionToEuler :: Quaternion a -> (a, a, a)
quaternionToEuler (Quaternion a
w (V3 a
x a
y a
z))=
  let sinr_cosp :: a
sinr_cosp = a
2 a -> a -> a
forall a. Num a => a -> a -> a
* (a
w a -> a -> a
forall a. Num a => a -> a -> a
* a
x a -> a -> a
forall a. Num a => a -> a -> a
+ a
y a -> a -> a
forall a. Num a => a -> a -> a
* a
z)
      cosr_cosp :: a
cosr_cosp = a
1 a -> a -> a
forall a. Num a => a -> a -> a
- a
2 a -> a -> a
forall a. Num a => a -> a -> a
* (a
x a -> a -> a
forall a. Num a => a -> a -> a
* a
x a -> a -> a
forall a. Num a => a -> a -> a
+ a
y a -> a -> a
forall a. Num a => a -> a -> a
* a
y)
      sinp :: a
sinp = a
2 a -> a -> a
forall a. Num a => a -> a -> a
* (a
w a -> a -> a
forall a. Num a => a -> a -> a
* a
y a -> a -> a
forall a. Num a => a -> a -> a
- a
z a -> a -> a
forall a. Num a => a -> a -> a
* a
x);
      siny_cosp :: a
siny_cosp = a
2 a -> a -> a
forall a. Num a => a -> a -> a
* (a
w a -> a -> a
forall a. Num a => a -> a -> a
* a
z a -> a -> a
forall a. Num a => a -> a -> a
+ a
x a -> a -> a
forall a. Num a => a -> a -> a
* a
y);
      cosy_cosp :: a
cosy_cosp = a
1 a -> a -> a
forall a. Num a => a -> a -> a
- a
2 a -> a -> a
forall a. Num a => a -> a -> a
* (a
y a -> a -> a
forall a. Num a => a -> a -> a
* a
y a -> a -> a
forall a. Num a => a -> a -> a
+ a
z a -> a -> a
forall a. Num a => a -> a -> a
* a
z);
      pitch :: a
pitch = if a
sinp a -> a -> Bool
forall a. Ord a => a -> a -> Bool
>= a
1
              then a -> a
forall a. Num a => a -> a
signum a
sinp a -> a -> a
forall a. Num a => a -> a -> a
* a
forall a. Floating a => a
pi a -> a -> a
forall a. Fractional a => a -> a -> a
/ a
2
              else a -> a
forall a. Floating a => a -> a
asin a
sinp
      roll :: a
roll = a -> a -> a
forall a. RealFloat a => a -> a -> a
atan2 a
sinr_cosp a
cosr_cosp
      yaw :: a
yaw = a -> a -> a
forall a. RealFloat a => a -> a -> a
atan2 a
siny_cosp a
cosy_cosp
   in (a
roll, a
pitch, a
yaw)

------------------------------------------------------------------------------
-- | Haskell's standard library doesn't make floating-point infinity available
-- in any convenient way, so we define it here.
infty :: (Fractional t) => t
infty :: t
infty = t
1t -> t -> t
forall a. Fractional a => a -> a -> a
/t
0
{-# INLINABLE infty #-}