-- SPDX-License-Identifier: MPL-2.0

{- | Finite Hamilton quaternions in scalar-first @(w,x,y,z)@ order. Rotations
are active, right-handed, column-vector rotations in radians. Product @a*b@
applies @b@ first. Euler angles are intrinsic ZYX (yaw, pitch, roll), returned
as @(roll,pitch,yaw)@ with pitch in [-pi/2,pi/2]. At gimbal lock roll is zero.
Unit quaternions differing by sign represent the same rotation. Interpolation
chooses the shortest sign-equivalent arc; SQUAD tangents use quaternion logs.
-}
module HMath.Quaternion (
    Quaternion,
    quaternion,
    components,
    norm,
    normalize,
    inverse,
    conjugate,
    multiply,
    add,
    scale,
    rotateVector,
    axisAngle,
    toAxisAngle,
    fromEuler,
    toEuler,
    toMatrix,
    fromMatrix,
    nlerp,
    slerp,
    squad,
    exponential,
    logarithm,
    swingTwist,
) where

import HMath.Error (MathError (..))
import qualified HMath.Matrix as M

-- | A finite scalar-first Hamilton quaternion. Construction is checked.
data Quaternion = Q Double Double Double Double deriving (Quaternion -> Quaternion -> Bool
(Quaternion -> Quaternion -> Bool)
-> (Quaternion -> Quaternion -> Bool) -> Eq Quaternion
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: Quaternion -> Quaternion -> Bool
== :: Quaternion -> Quaternion -> Bool
$c/= :: Quaternion -> Quaternion -> Bool
/= :: Quaternion -> Quaternion -> Bool
Eq, Int -> Quaternion -> ShowS
[Quaternion] -> ShowS
Quaternion -> String
(Int -> Quaternion -> ShowS)
-> (Quaternion -> String)
-> ([Quaternion] -> ShowS)
-> Show Quaternion
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> Quaternion -> ShowS
showsPrec :: Int -> Quaternion -> ShowS
$cshow :: Quaternion -> String
show :: Quaternion -> String
$cshowList :: [Quaternion] -> ShowS
showList :: [Quaternion] -> ShowS
Show)

finite :: Double -> Bool
finite :: Double -> Bool
finite Double
a = Bool -> Bool
not (Double -> Bool
forall a. RealFloat a => a -> Bool
isNaN Double
a Bool -> Bool -> Bool
|| Double -> Bool
forall a. RealFloat a => a -> Bool
isInfinite Double
a)
check :: Quaternion -> Either MathError Quaternion
check :: Quaternion -> Either MathError Quaternion
check Quaternion
q = if (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite (Quaternion -> [Double]
components Quaternion
q) then Quaternion -> Either MathError Quaternion
forall a b. b -> Either a b
Right Quaternion
q else MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left MathError
NonFiniteResult

-- | Finite scalar-first constructor.
quaternion :: Double -> Double -> Double -> Double -> Either MathError Quaternion
quaternion :: Double -> Double -> Double -> Double -> Either MathError Quaternion
quaternion Double
w Double
x Double
y Double
z = Quaternion -> Either MathError Quaternion
check (Double -> Double -> Double -> Double -> Quaternion
Q Double
w Double
x Double
y Double
z)

-- | Scalar, x, y, z components.
components :: Quaternion -> [Double]
components :: Quaternion -> [Double]
components (Q Double
w Double
x Double
y Double
z) = [Double
w, Double
x, Double
y, Double
z]

-- | Scaled Euclidean norm (may overflow only when the true norm is unrepresentable).
norm :: Quaternion -> Double
norm :: Quaternion -> Double
norm Quaternion
q =
    let xs :: [Double]
xs = Quaternion -> [Double]
components Quaternion
q; s :: Double
s = [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
abs [Double]
xs)
     in if Double
s Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 then Double
0 else Double
s Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sqrt ([Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [(Double
x Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) Double -> Int -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^ (Int
2 :: Int) | Double
x <- [Double]
xs])

-- | Normalize without forming a squared norm; zero has no normalization.
normalize :: Quaternion -> Either MathError Quaternion
normalize :: Quaternion -> Either MathError Quaternion
normalize Quaternion
q =
    let s :: Double
s = [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
abs (Quaternion -> [Double]
components Quaternion
q))
     in if Double
s Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0
            then MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"zero quaternion norm")
            else
                let Q Double
a Double
b Double
c Double
e = Quaternion
q
                    w :: Double
w = Double
a Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s
                    x :: Double
x = Double
b Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s
                    y :: Double
y = Double
c Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s
                    z :: Double
z = Double
e Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s
                    d :: Double
d = Double -> Double
forall a. Floating a => a -> a
sqrt (Double
w Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
z Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z)
                 in Quaternion -> Either MathError Quaternion
check (Double -> Double -> Double -> Double -> Quaternion
Q (Double
w Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
d) (Double
x Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
d) (Double
y Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
d) (Double
z Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
d))

-- | Quaternion conjugation.
conjugate :: Quaternion -> Quaternion
conjugate :: Quaternion -> Quaternion
conjugate (Q Double
w Double
x Double
y Double
z) = Double -> Double -> Double -> Double -> Quaternion
Q Double
w (-Double
x) (-Double
y) (-Double
z)

-- | Multiplicative inverse; uses component scaling to avoid norm overflow.
inverse :: Quaternion -> Either MathError Quaternion
inverse :: Quaternion -> Either MathError Quaternion
inverse Quaternion
q = do
    u <- Quaternion -> Either MathError Quaternion
normalize Quaternion
q
    let s = [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
abs (Quaternion -> [Double]
components Quaternion
q))
        d = Double -> Double
forall a. Floating a => a -> a
sqrt ([Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [(Double
x Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) Double -> Int -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^ (Int
2 :: Int) | Double
x <- Quaternion -> [Double]
components Quaternion
q])
    let Q w x y z = conjugate u
    check (Q ((w / d) / s) ((x / d) / s) ((y / d) / s) ((z / d) / s))

rawScale :: Double -> Quaternion -> Quaternion
rawScale :: Double -> Quaternion -> Quaternion
rawScale Double
a (Q Double
w Double
x Double
y Double
z) = Double -> Double -> Double -> Double -> Quaternion
Q (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w) (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x) (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y) (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z)
rawAdd :: Quaternion -> Quaternion -> Quaternion
rawAdd :: Quaternion -> Quaternion -> Quaternion
rawAdd (Q Double
a Double
b Double
c Double
d) (Q Double
w Double
x Double
y Double
z) = Double -> Double -> Double -> Double -> Quaternion
Q (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
w) (Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
x) (Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
y) (Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
z)
rawMul :: Quaternion -> Quaternion -> Quaternion
rawMul :: Quaternion -> Quaternion -> Quaternion
rawMul (Q Double
a Double
b Double
c Double
d) (Q Double
w Double
x Double
y Double
z) =
    Double -> Double -> Double -> Double -> Quaternion
Q
        (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z)
        (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y)
        (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x)
        (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w)

-- | Hamilton product.
multiply :: Quaternion -> Quaternion -> Either MathError Quaternion
multiply :: Quaternion -> Quaternion -> Either MathError Quaternion
multiply Quaternion
a Quaternion
b = Quaternion -> Either MathError Quaternion
check (Quaternion -> Quaternion -> Quaternion
rawMul Quaternion
a Quaternion
b)

-- | Component sum.
add :: Quaternion -> Quaternion -> Either MathError Quaternion
add :: Quaternion -> Quaternion -> Either MathError Quaternion
add Quaternion
a Quaternion
b = Quaternion -> Either MathError Quaternion
check (Quaternion -> Quaternion -> Quaternion
rawAdd Quaternion
a Quaternion
b)

-- | Checked real scaling.
scale :: Double -> Quaternion -> Either MathError Quaternion
scale :: Double -> Quaternion -> Either MathError Quaternion
scale Double
a Quaternion
q = if Double -> Bool
finite Double
a then Quaternion -> Either MathError Quaternion
check (Double -> Quaternion -> Quaternion
rawScale Double
a Quaternion
q) else MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"nonfinite quaternion scale")

-- | Rotate a finite three-vector; any nonzero quaternion is normalized first.
rotateVector :: Quaternion -> [Double] -> Either MathError [Double]
rotateVector :: Quaternion -> [Double] -> Either MathError [Double]
rotateVector Quaternion
q [Double]
v = do
    u <- Quaternion -> Either MathError Quaternion
normalize Quaternion
q
    case v of
        [Double
x, Double
y, Double
z] | (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double]
v -> case Quaternion -> Quaternion -> Quaternion
rawMul (Quaternion -> Quaternion -> Quaternion
rawMul Quaternion
u (Double -> Double -> Double -> Double -> Quaternion
Q Double
0 Double
x Double
y Double
z)) (Quaternion -> Quaternion
conjugate Quaternion
u) of
            Q Double
_ Double
a Double
b Double
c -> if (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double
a, Double
b, Double
c] then [Double] -> Either MathError [Double]
forall a b. b -> Either a b
Right [Double
a, Double
b, Double
c] else MathError -> Either MathError [Double]
forall a b. a -> Either a b
Left MathError
NonFiniteResult
        [Double]
_ -> MathError -> Either MathError [Double]
forall a b. a -> Either a b
Left (String -> MathError
InvalidArgument String
"rotation requires a finite three-vector")

-- | Axis-angle conversion. The axis must be nonzero even for a zero angle.
axisAngle :: [Double] -> Double -> Either MathError Quaternion
axisAngle :: [Double] -> Double -> Either MathError Quaternion
axisAngle [Double]
axis Double
angle = case [Double]
axis of
    [Double
x, Double
y, Double
z] | (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double]
axis Bool -> Bool -> Bool
&& Double -> Bool
finite Double
angle -> do
        Q _ a b c <- Quaternion -> Either MathError Quaternion
normalize (Double -> Double -> Double -> Double -> Quaternion
Q Double
0 Double
x Double
y Double
z)
        check (Q (cos (angle / 2)) (a * sin (angle / 2)) (b * sin (angle / 2)) (c * sin (angle / 2)))
    [Double]
_ -> MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
InvalidArgument String
"invalid axis-angle")

canonical :: Quaternion -> Quaternion
canonical :: Quaternion -> Quaternion
canonical q :: Quaternion
q@(Q Double
w Double
x Double
y Double
z) = if Double
w Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
0 Bool -> Bool -> Bool
|| (Double
w Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 Bool -> Bool -> Bool
&& [Double
x, Double
y, Double
z] [Double] -> [Double] -> Bool
forall a. Ord a => a -> a -> Bool
< [Double
0, Double
0, Double
0]) then Double -> Quaternion -> Quaternion
rawScale (-Double
1) Quaternion
q else Quaternion
q

-- | Principal rotation angle in [0,pi]; identity uses the x axis.
toAxisAngle :: Quaternion -> Either MathError ([Double], Double)
toAxisAngle :: Quaternion -> Either MathError ([Double], Double)
toAxisAngle Quaternion
q = do
    Q w x y z <- Quaternion -> Quaternion
canonical (Quaternion -> Quaternion)
-> Either MathError Quaternion -> Either MathError Quaternion
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Quaternion -> Either MathError Quaternion
normalize Quaternion
q
    let s = Double -> Double
forall a. Floating a => a -> a
sqrt (Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
z Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z)
    Right (if s < 1e-15 then ([1, 0, 0], 0) else ([x / s, y / s, z / s], 2 * atan2 s w))

-- | Intrinsic ZYX rotation from @(roll,pitch,yaw)@.
fromEuler :: Double -> Double -> Double -> Either MathError Quaternion
fromEuler :: Double -> Double -> Double -> Either MathError Quaternion
fromEuler Double
roll Double
pitch Double
yaw
    | Bool -> Bool
not ((Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double
roll, Double
pitch, Double
yaw]) = MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"nonfinite Euler angle")
    | Bool
otherwise =
        let cr :: Double
cr = Double -> Double
forall a. Floating a => a -> a
cos (Double
roll Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
            sr :: Double
sr = Double -> Double
forall a. Floating a => a -> a
sin (Double
roll Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
            cp :: Double
cp = Double -> Double
forall a. Floating a => a -> a
cos (Double
pitch Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
            sp :: Double
sp = Double -> Double
forall a. Floating a => a -> a
sin (Double
pitch Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
            cy :: Double
cy = Double -> Double
forall a. Floating a => a -> a
cos (Double
yaw Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
            sy :: Double
sy = Double -> Double
forall a. Floating a => a -> a
sin (Double
yaw Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
         in Quaternion -> Either MathError Quaternion
normalize
                ( Double -> Double -> Double -> Double -> Quaternion
Q
                    (Double
cr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy)
                    (Double
sr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
cr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy)
                    (Double
cr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy)
                    (Double
cr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
sr Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sp Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy)
                )

-- | Intrinsic ZYX angles, choosing roll zero at gimbal lock.
toEuler :: Quaternion -> Either MathError (Double, Double, Double)
toEuler :: Quaternion -> Either MathError (Double, Double, Double)
toEuler Quaternion
q = do
    m <- Quaternion -> Either MathError (Matrix Double)
toMatrix Quaternion
q
    let a = Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
m
        s = Double -> Double -> Double
forall a. Ord a => a -> a -> a
max (-Double
1) (Double -> Double -> Double
forall a. Ord a => a -> a -> a
min Double
1 (-([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
6)))
        cp = Double -> Double
forall a. Floating a => a -> a
sqrt (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0) Double -> Int -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^ (Int
2 :: Int) Double -> Double -> Double
forall a. Num a => a -> a -> a
+ ([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
3) Double -> Int -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^ (Int
2 :: Int))
        pitch = Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
s Double
cp
    Right $
        if cp < 1e-12
            then (0, if s >= 0 then pi / 2 else -pi / 2, atan2 (-(a !! 1)) (a !! 4))
            else (atan2 (a !! 7) (a !! 8), pitch, atan2 (a !! 3) (a !! 0))

-- | Active rotation as a row-major 3x3 matrix.
toMatrix :: Quaternion -> Either MathError (M.Matrix Double)
toMatrix :: Quaternion -> Either MathError (Matrix Double)
toMatrix Quaternion
q = do
    Q w x y z <- Quaternion -> Either MathError Quaternion
normalize Quaternion
q
    M.fromListChecked
        3
        3
        [ 1 - 2 * (y * y + z * z)
        , 2 * (x * y - z * w)
        , 2 * (x * z + y * w)
        , 2 * (x * y + z * w)
        , 1 - 2 * (x * x + z * z)
        , 2 * (y * z - x * w)
        , 2 * (x * z - y * w)
        , 2 * (y * z + x * w)
        , 1 - 2 * (x * x + y * y)
        ]

-- | Validate SO(3) within 1e-9, then use the largest-diagonal conversion near pi.
fromMatrix :: M.Matrix Double -> Either MathError Quaternion
fromMatrix :: Matrix Double -> Either MathError Quaternion
fromMatrix Matrix Double
matrix
    | Matrix Double -> (Int, Int)
forall a. Matrix a -> (Int, Int)
M.shape Matrix Double
matrix (Int, Int) -> (Int, Int) -> Bool
forall a. Eq a => a -> a -> Bool
/= (Int
3, Int
3) = MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
InvalidArgument String
"rotation matrix must be 3x3")
    | Bool -> Bool
not ((Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double]
a) = MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"nonfinite rotation matrix")
    | [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
abs [Double]
orth) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
1e-9 Bool -> Bool -> Bool
|| Double -> Double
forall a. Num a => a -> a
abs (Double
det Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
1) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
1e-9 = MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"matrix is not SO(3)")
    | Bool
otherwise =
        Quaternion -> Either MathError Quaternion
normalize (Quaternion -> Either MathError Quaternion)
-> Quaternion -> Either MathError Quaternion
forall a b. (a -> b) -> a -> b
$
            if Double
tr Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0
                then
                    let s :: Double
s = Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sqrt (Double
tr Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
1)
                     in Double -> Double -> Double -> Double -> Quaternion
Q (Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
4) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
7 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
5) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
6) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
3 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
1) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s)
                else
                    if [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0 Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4 Bool -> Bool -> Bool
&& [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0 Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8
                        then
                            let s :: Double
s = Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sqrt (Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8)
                             in Double -> Double -> Double -> Double -> Quaternion
Q (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
7 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
5) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
4) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
3) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
6) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s)
                        else
                            if [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4 Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8
                                then
                                    let s :: Double
s = Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sqrt (Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8)
                                     in Double -> Double -> Double -> Double -> Quaternion
Q (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
6) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
3) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
4) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
5 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
7) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s)
                                else
                                    let s :: Double
s = Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sqrt (Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4)
                                     in Double -> Double -> Double -> Double -> Quaternion
Q (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
3 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
1) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
6) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
5 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
7) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) (Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
4)
  where
    a :: [Double]
a = Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
matrix
    tr :: Double
tr = [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8
    orth :: [Double]
orth =
        [ [Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [[Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! (Int
3 Int -> Int -> Int
forall a. Num a => a -> a -> a
* Int
k Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
i) Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! (Int
3 Int -> Int -> Int
forall a. Num a => a -> a -> a
* Int
k Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
j) | Int
k <- [Int
0 .. Int
2]] Double -> Double -> Double
forall a. Num a => a -> a -> a
- if Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
j then Double
1 else Double
0
        | Int
i <- [Int
0 .. Int
2]
        , Int
j <- [Int
0 .. Int
2]
        ]
    det :: Double
det =
        [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
0 Double -> Double -> Double
forall a. Num a => a -> a -> a
* ([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4 Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
5 Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
7)
            Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* ([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
3 Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
8 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
5 Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
6)
            Double -> Double -> Double
forall a. Num a => a -> a -> a
+ [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* ([Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
3 Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
7 Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
4 Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
6)

shortPair :: Quaternion -> Quaternion -> Either MathError (Quaternion, Quaternion, Double)
shortPair :: Quaternion
-> Quaternion -> Either MathError (Quaternion, Quaternion, Double)
shortPair Quaternion
a Quaternion
b = do
    u <- Quaternion -> Either MathError Quaternion
normalize Quaternion
a
    v <- normalize b
    let d = [Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum ((Double -> Double -> Double) -> [Double] -> [Double] -> [Double]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith Double -> Double -> Double
forall a. Num a => a -> a -> a
(*) (Quaternion -> [Double]
components Quaternion
u) (Quaternion -> [Double]
components Quaternion
v)); w = if Double
d Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
0 then Double -> Quaternion -> Quaternion
rawScale (-Double
1) Quaternion
v else Quaternion
v
    Right (u, w, min 1 (abs d))
interpolation :: Double -> Either MathError ()
interpolation :: Double -> Either MathError ()
interpolation Double
t =
    if Double -> Bool
finite Double
t Bool -> Bool -> Bool
&& Double
t Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0 Bool -> Bool -> Bool
&& Double
t Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
1
        then () -> Either MathError ()
forall a b. b -> Either a b
Right ()
        else MathError -> Either MathError ()
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"interpolation parameter outside [0,1]")

-- | Shortest-arc normalized linear interpolation on [0,1].
nlerp :: Double -> Quaternion -> Quaternion -> Either MathError Quaternion
nlerp :: Double -> Quaternion -> Quaternion -> Either MathError Quaternion
nlerp Double
t Quaternion
a Quaternion
b = do
    Double -> Either MathError ()
interpolation Double
t
    (u, v, _) <- Quaternion
-> Quaternion -> Either MathError (Quaternion, Quaternion, Double)
shortPair Quaternion
a Quaternion
b
    normalize (rawAdd (rawScale (1 - t) u) (rawScale t v))

-- | Shortest-arc spherical interpolation, with stable NLERP near equal rotations.
slerp :: Double -> Quaternion -> Quaternion -> Either MathError Quaternion
slerp :: Double -> Quaternion -> Quaternion -> Either MathError Quaternion
slerp Double
t Quaternion
a Quaternion
b = do
    Double -> Either MathError ()
interpolation Double
t
    (u, v, d) <- Quaternion
-> Quaternion -> Either MathError (Quaternion, Quaternion, Double)
shortPair Quaternion
a Quaternion
b
    if d > 1 - 1e-10
        then nlerp t u v
        else
            let theta = Double -> Double
forall a. Floating a => a -> a
acos Double
d; s = Double -> Double
forall a. Floating a => a -> a
sin Double
theta
             in normalize (rawAdd (rawScale (sin ((1 - t) * theta) / s) u) (rawScale (sin (t * theta) / s) v))

-- | General quaternion exponential; sinc expansion stabilizes a tiny vector part.
exponential :: Quaternion -> Either MathError Quaternion
exponential :: Quaternion -> Either MathError Quaternion
exponential (Q Double
w Double
x Double
y Double
z) =
    let v :: Double
v = Quaternion -> Double
norm (Double -> Double -> Double -> Double -> Quaternion
Q Double
0 Double
x Double
y Double
z); s :: Double
s = if Double
v Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-8 then Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
v Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
6 else Double -> Double
forall a. Floating a => a -> a
sin Double
v Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
v
     in Quaternion -> Either MathError Quaternion
check (Double -> Quaternion -> Quaternion
rawScale (Double -> Double
forall a. Floating a => a -> a
exp Double
w) (Double -> Double -> Double -> Double -> Quaternion
Q (Double -> Double
forall a. Floating a => a -> a
cos Double
v) (Double
s Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x) (Double
s Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y) (Double
s Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z)))

-- | Principal quaternion logarithm; the negative real axis chooses positive x.
logarithm :: Quaternion -> Either MathError Quaternion
logarithm :: Quaternion -> Either MathError Quaternion
logarithm q :: Quaternion
q@(Q Double
w Double
_ Double
_ Double
_) = do
    u <- Quaternion -> Either MathError Quaternion
normalize Quaternion
q
    let Q a b c d = u
        s = Quaternion -> Double
norm (Double -> Double -> Double -> Double -> Quaternion
Q Double
0 Double
b Double
c Double
d)
        size = [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
abs (Quaternion -> [Double]
components Quaternion
q))
        logNorm = Double -> Double
forall a. Floating a => a -> a
log Double
size Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double
forall a. Floating a => a -> a
log (Double -> Double
forall a. Floating a => a -> a
sqrt ([Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [(Double
v Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size) Double -> Int -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^ (Int
2 :: Int) | Double
v <- Quaternion -> [Double]
components Quaternion
q]))
    check $
        if s == 0
            then Q logNorm (if w < 0 then pi else 0) 0 0
            else let angle = Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
s Double
a in Q logNorm (b / s * angle) (c / s * angle) (d / s * angle)

-- | Shoemake SQUAD through q1 to q2 using neighboring q0 and q3.
squad ::
    Double -> Quaternion -> Quaternion -> Quaternion -> Quaternion -> Either MathError Quaternion
squad :: Double
-> Quaternion
-> Quaternion
-> Quaternion
-> Quaternion
-> Either MathError Quaternion
squad Double
t Quaternion
q0 Quaternion
q1 Quaternion
q2 Quaternion
q3 = do
    Double -> Either MathError ()
interpolation Double
t
    (_, a, _) <- Quaternion
-> Quaternion -> Either MathError (Quaternion, Quaternion, Double)
shortPair Quaternion
q1 Quaternion
q0
    (b, c, _) <- shortPair q1 q2
    (_, d, _) <- shortPair c q3
    s1 <- tangent b a c
    s2 <- tangent c b d
    p <- slerp t b c
    q <- slerp t s1 s2
    slerp (2 * t * (1 - t)) p q
  where
    tangent :: Quaternion
-> Quaternion -> Quaternion -> Either MathError Quaternion
tangent Quaternion
q Quaternion
before Quaternion
after = do
        l <- Quaternion -> Either MathError Quaternion
logarithm (Quaternion -> Quaternion -> Quaternion
rawMul (Quaternion -> Quaternion
conjugate Quaternion
q) Quaternion
before)
        r <- logarithm (rawMul (conjugate q) after)
        e <- exponential (rawScale (-0.25) (rawAdd l r))
        normalize (rawMul q e)

{- | Decompose q = swing * twist around a nonzero axis. A pi swing with zero
twist projection uses identity twist, a deterministic singular convention.
-}
swingTwist :: [Double] -> Quaternion -> Either MathError (Quaternion, Quaternion)
swingTwist :: [Double] -> Quaternion -> Either MathError (Quaternion, Quaternion)
swingTwist [Double]
axis Quaternion
q = do
    Q _ ax ay az <- case [Double]
axis of
        [Double
a, Double
b, Double
c] | (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double]
axis -> Quaternion -> Either MathError Quaternion
normalize (Double -> Double -> Double -> Double -> Quaternion
Q Double
0 Double
a Double
b Double
c)
        [Double]
_ -> MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
InvalidArgument String
"twist axis must be a finite three-vector")
    u@(Q w x y z) <- normalize q
    let d = Double
ax Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
ay Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
az Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z; candidate = Double -> Double -> Double -> Double -> Quaternion
Q Double
w (Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ax) (Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ay) (Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
az)
    twist <- if norm candidate < 1e-14 then Right (Q 1 0 0 0) else normalize candidate
    swing <- normalize (rawMul u (conjugate twist))
    Right (swing, twist)