-- SPDX-License-Identifier: MPL-2.0

{- | SO(3), SE(3), and rigid dual quaternions. Active right-handed transforms
act on column vectors; @compose a b@ applies b first. Translation shares the
coordinate unit of points, angles are radians, and twists are @(omega,v)@.
SO(3) logs choose angles in [0,pi]; pi-axis sign is the quaternion convention.
SE(3) exp/log use the SO(3) left Jacobian, with series near identity. Dual
quaternion interpolation is screw-linear ScLERP through SE(3), including the
pure-translation limit. These APIs model rigid transforms, not scale/shear.
-}
module HMath.Rotation (
    SO3,
    so3,
    rotationQuaternion,
    identityRotation,
    composeRotation,
    inverseRotation,
    rotate,
    expSO3,
    logSO3,
    SE3,
    se3,
    rotationPart,
    translationPart,
    identityTransform,
    compose,
    inverse,
    transformPoint,
    expSE3,
    logSE3,
    toHomogeneous,
    fromHomogeneous,
    DualQuaternion,
    dualQuaternion,
    dualParts,
    toDualQuaternion,
    fromDualQuaternion,
    multiplyDual,
    inverseDual,
    interpolateDual,
) where

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

-- | Normalized representative of SO(3).
newtype SO3 = SO3 Q.Quaternion deriving (SO3 -> SO3 -> Bool
(SO3 -> SO3 -> Bool) -> (SO3 -> SO3 -> Bool) -> Eq SO3
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: SO3 -> SO3 -> Bool
== :: SO3 -> SO3 -> Bool
$c/= :: SO3 -> SO3 -> Bool
/= :: SO3 -> SO3 -> Bool
Eq, Int -> SO3 -> ShowS
[SO3] -> ShowS
SO3 -> String
(Int -> SO3 -> ShowS)
-> (SO3 -> String) -> ([SO3] -> ShowS) -> Show SO3
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> SO3 -> ShowS
showsPrec :: Int -> SO3 -> ShowS
$cshow :: SO3 -> String
show :: SO3 -> String
$cshowList :: [SO3] -> ShowS
showList :: [SO3] -> ShowS
Show)

-- | Rigid transformation x -> R*x+t.
data SE3 = SE3 SO3 [Double] deriving (SE3 -> SE3 -> Bool
(SE3 -> SE3 -> Bool) -> (SE3 -> SE3 -> Bool) -> Eq SE3
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: SE3 -> SE3 -> Bool
== :: SE3 -> SE3 -> Bool
$c/= :: SE3 -> SE3 -> Bool
/= :: SE3 -> SE3 -> Bool
Eq, Int -> SE3 -> ShowS
[SE3] -> ShowS
SE3 -> String
(Int -> SE3 -> ShowS)
-> (SE3 -> String) -> ([SE3] -> ShowS) -> Show SE3
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> SE3 -> ShowS
showsPrec :: Int -> SE3 -> ShowS
$cshow :: SE3 -> String
show :: SE3 -> String
$cshowList :: [SE3] -> ShowS
showList :: [SE3] -> ShowS
Show)

-- | Unit real part and Study-orthogonal dual part.
data DualQuaternion = DQ Q.Quaternion Q.Quaternion deriving (DualQuaternion -> DualQuaternion -> Bool
(DualQuaternion -> DualQuaternion -> Bool)
-> (DualQuaternion -> DualQuaternion -> Bool) -> Eq DualQuaternion
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: DualQuaternion -> DualQuaternion -> Bool
== :: DualQuaternion -> DualQuaternion -> Bool
$c/= :: DualQuaternion -> DualQuaternion -> Bool
/= :: DualQuaternion -> DualQuaternion -> Bool
Eq, Int -> DualQuaternion -> ShowS
[DualQuaternion] -> ShowS
DualQuaternion -> String
(Int -> DualQuaternion -> ShowS)
-> (DualQuaternion -> String)
-> ([DualQuaternion] -> ShowS)
-> Show DualQuaternion
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> DualQuaternion -> ShowS
showsPrec :: Int -> DualQuaternion -> ShowS
$cshow :: DualQuaternion -> String
show :: DualQuaternion -> String
$cshowList :: [DualQuaternion] -> ShowS
showList :: [DualQuaternion] -> ShowS
Show)

finite :: Double -> Bool
finite :: Double -> Bool
finite Double
x = Bool -> Bool
not (Double -> Bool
forall a. RealFloat a => a -> Bool
isNaN Double
x Bool -> Bool -> Bool
|| Double -> Bool
forall a. RealFloat a => a -> Bool
isInfinite Double
x)
vector :: [Double] -> Either MathError ()
vector :: [Double] -> Either MathError ()
vector [Double]
xs =
    if [Double] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Double]
xs Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
3 Bool -> Bool -> Bool
&& (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double]
xs
        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
InvalidArgument String
"expected finite three-vector")
checkV :: [Double] -> Either MathError [Double]
checkV :: [Double] -> Either MathError [Double]
checkV [Double]
xs = if (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double]
xs then [Double] -> Either MathError [Double]
forall a b. b -> Either a b
Right [Double]
xs else MathError -> Either MathError [Double]
forall a b. a -> Either a b
Left MathError
NonFiniteResult
qFrom :: [Double] -> Either MathError Q.Quaternion
qFrom :: [Double] -> Either MathError Quaternion
qFrom [Double
w, Double
x, Double
y, Double
z] = Double -> Double -> Double -> Double -> Either MathError Quaternion
Q.quaternion Double
w Double
x Double
y Double
z
qFrom [Double]
_ = MathError -> Either MathError Quaternion
forall a b. a -> Either a b
Left (String -> MathError
InvalidArgument String
"quaternion component count")

-- | Normalize a nonzero quaternion into SO(3).
so3 :: Q.Quaternion -> Either MathError SO3
so3 :: Quaternion -> Either MathError SO3
so3 = (Quaternion -> SO3)
-> Either MathError Quaternion -> Either MathError SO3
forall a b. (a -> b) -> Either MathError a -> Either MathError b
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Quaternion -> SO3
SO3 (Either MathError Quaternion -> Either MathError SO3)
-> (Quaternion -> Either MathError Quaternion)
-> Quaternion
-> Either MathError SO3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Quaternion -> Either MathError Quaternion
Q.normalize

-- | Underlying unit Hamilton quaternion.
rotationQuaternion :: SO3 -> Q.Quaternion
rotationQuaternion :: SO3 -> Quaternion
rotationQuaternion (SO3 Quaternion
q) = Quaternion
q

-- | Identity rotation.
identityRotation :: SO3
identityRotation :: SO3
identityRotation = Quaternion -> SO3
SO3 Quaternion
one

one :: Q.Quaternion
one :: Quaternion
one = case Double -> Double -> Double -> Double -> Either MathError Quaternion
Q.quaternion Double
1 Double
0 Double
0 Double
0 of Right Quaternion
q -> Quaternion
q; Left MathError
_ -> String -> Quaternion
forall a. HasCallStack => String -> a
error String
"finite quaternion constant"
zero :: Q.Quaternion
zero :: Quaternion
zero = case Double -> Double -> Double -> Double -> Either MathError Quaternion
Q.quaternion Double
0 Double
0 Double
0 Double
0 of Right Quaternion
q -> Quaternion
q; Left MathError
_ -> String -> Quaternion
forall a. HasCallStack => String -> a
error String
"finite quaternion constant"

-- | Composition applying the right rotation first.
composeRotation :: SO3 -> SO3 -> Either MathError SO3
composeRotation :: SO3 -> SO3 -> Either MathError SO3
composeRotation (SO3 Quaternion
a) (SO3 Quaternion
b) = Quaternion -> Quaternion -> Either MathError Quaternion
Q.multiply Quaternion
a Quaternion
b Either MathError Quaternion
-> (Quaternion -> Either MathError SO3) -> Either MathError SO3
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= Quaternion -> Either MathError SO3
so3

-- | Inverse of a unit rotation.
inverseRotation :: SO3 -> SO3
inverseRotation :: SO3 -> SO3
inverseRotation (SO3 Quaternion
q) = Quaternion -> SO3
SO3 (Quaternion -> Quaternion
Q.conjugate Quaternion
q)

-- | Apply a rotation to a finite three-vector.
rotate :: SO3 -> [Double] -> Either MathError [Double]
rotate :: SO3 -> [Double] -> Either MathError [Double]
rotate (SO3 Quaternion
q) = Quaternion -> [Double] -> Either MathError [Double]
Q.rotateVector Quaternion
q

norm3 :: [Double] -> Double
norm3 :: [Double] -> Double
norm3 [Double]
xs =
    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 [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])
cross :: [Double] -> [Double] -> [Double]
cross :: [Double] -> [Double] -> [Double]
cross [Double
a, Double
b, Double
c] [Double
x, Double
y, Double
z] = [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
y, 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
a 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
y Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x]
cross [Double]
_ [Double]
_ = [Double
0, Double
0, Double
0]

-- | Exponential of a rotation vector; vector norm is the angle in radians.
expSO3 :: [Double] -> Either MathError SO3
expSO3 :: [Double] -> Either MathError SO3
expSO3 [Double]
omega = do
    [Double] -> Either MathError ()
vector [Double]
omega
    let theta :: Double
theta = [Double] -> Double
norm3 [Double]
omega
    if Bool -> Bool
not (Double -> Bool
finite Double
theta)
        then MathError -> Either MathError SO3
forall a b. a -> Either a b
Left MathError
NonFiniteResult
        else
            if Double
theta Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 then SO3 -> Either MathError SO3
forall a b. b -> Either a b
Right SO3
identityRotation else [Double] -> Double -> Either MathError Quaternion
Q.axisAngle ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
theta) [Double]
omega) Double
theta Either MathError Quaternion
-> (Quaternion -> Either MathError SO3) -> Either MathError SO3
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= Quaternion -> Either MathError SO3
so3

-- | Principal logarithm as a rotation vector.
logSO3 :: SO3 -> Either MathError [Double]
logSO3 :: SO3 -> Either MathError [Double]
logSO3 (SO3 Quaternion
q) = do
    (axis, angle) <- Quaternion -> Either MathError ([Double], Double)
Q.toAxisAngle Quaternion
q
    checkV (map (* angle) axis)

-- | Construct a rigid transform from a rotation and finite translation.
se3 :: SO3 -> [Double] -> Either MathError SE3
se3 :: SO3 -> [Double] -> Either MathError SE3
se3 SO3
r [Double]
t = [Double] -> Either MathError ()
vector [Double]
t Either MathError () -> Either MathError SE3 -> Either MathError SE3
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> SE3 -> Either MathError SE3
forall a b. b -> Either a b
Right (SO3 -> [Double] -> SE3
SE3 SO3
r [Double]
t)

-- | Rotation component.
rotationPart :: SE3 -> SO3
rotationPart :: SE3 -> SO3
rotationPart (SE3 SO3
r [Double]
_) = SO3
r

-- | Translation component.
translationPart :: SE3 -> [Double]
translationPart :: SE3 -> [Double]
translationPart (SE3 SO3
_ [Double]
t) = [Double]
t

-- | Identity rigid transform.
identityTransform :: SE3
identityTransform :: SE3
identityTransform = SO3 -> [Double] -> SE3
SE3 SO3
identityRotation [Double
0, Double
0, Double
0]

-- | Compose transforms; b acts first.
compose :: SE3 -> SE3 -> Either MathError SE3
compose :: SE3 -> SE3 -> Either MathError SE3
compose (SE3 SO3
a [Double]
x) (SE3 SO3
b [Double]
y) = do
    r <- SO3 -> SO3 -> Either MathError SO3
composeRotation SO3
a SO3
b
    y' <- rotate a y
    t <- checkV (zipWith (+) x y')
    se3 r t

-- | Inverse rigid transform.
inverse :: SE3 -> Either MathError SE3
inverse :: SE3 -> Either MathError SE3
inverse (SE3 SO3
r [Double]
t) = let ri :: SO3
ri = SO3 -> SO3
inverseRotation SO3
r in SO3 -> [Double] -> Either MathError [Double]
rotate SO3
ri ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
negate [Double]
t) Either MathError [Double]
-> ([Double] -> Either MathError SE3) -> Either MathError SE3
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= SO3 -> [Double] -> Either MathError SE3
se3 SO3
ri

-- | Apply a rigid transform.
transformPoint :: SE3 -> [Double] -> Either MathError [Double]
transformPoint :: SE3 -> [Double] -> Either MathError [Double]
transformPoint (SE3 SO3
r [Double]
t) [Double]
x = SO3 -> [Double] -> Either MathError [Double]
rotate SO3
r [Double]
x Either MathError [Double]
-> ([Double] -> Either MathError [Double])
-> Either MathError [Double]
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= [Double] -> Either MathError [Double]
checkV ([Double] -> Either MathError [Double])
-> ([Double] -> [Double]) -> [Double] -> Either MathError [Double]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (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
(+) [Double]
t

-- | SE(3) exponential. The translational twist v is integrated by J(omega).
expSE3 :: ([Double], [Double]) -> Either MathError SE3
expSE3 :: ([Double], [Double]) -> Either MathError SE3
expSE3 ([Double]
omega, [Double]
v) = do
    [Double] -> Either MathError ()
vector [Double]
omega
    [Double] -> Either MathError ()
vector [Double]
v
    r <- [Double] -> Either MathError SO3
expSO3 [Double]
omega
    let theta = [Double] -> Double
norm3 [Double]
omega
        t2 = Double
theta Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
theta
        a = if Double
theta Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-4 then Double
0.5 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
t2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
24 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
t2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
t2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
720 else (Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double -> Double
forall a. Floating a => a -> a
cos Double
theta) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
t2
        b = if Double
theta Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-4 then Double
1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
6 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
t2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
120 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
t2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
t2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
5040 else (Double
theta Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double -> Double
forall a. Floating a => a -> a
sin Double
theta) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
t2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
theta)
        wv = [Double] -> [Double] -> [Double]
cross [Double]
omega [Double]
v
        wwv = [Double] -> [Double] -> [Double]
cross [Double]
omega [Double]
wv
    t <- checkV (zipWith3 (\Double
x Double
y Double
z -> Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ 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) v wv wwv)
    se3 r t

-- | Principal SE(3) logarithm, stable at identity and at rotations near pi.
logSE3 :: SE3 -> Either MathError ([Double], [Double])
logSE3 :: SE3 -> Either MathError ([Double], [Double])
logSE3 (SE3 SO3
r [Double]
t) = do
    omega <- SO3 -> Either MathError [Double]
logSO3 SO3
r
    let theta = [Double] -> Double
norm3 [Double]
omega
        t2 = Double
theta Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
theta
        a =
            if Double
theta Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-4
                then Double
1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
12 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
t2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
720 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
t2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
t2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
30240
                else (Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
theta Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
cos (Double
theta Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double -> Double
forall a. Floating a => a -> a
sin (Double
theta Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
t2
        wt = [Double] -> [Double] -> [Double]
cross [Double]
omega [Double]
t
        wwt = [Double] -> [Double] -> [Double]
cross [Double]
omega [Double]
wt
    v <- checkV (zipWith3 (\Double
x Double
y Double
z -> Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
y Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
z) t wt wwt)
    Right (omega, v)

-- | Row-major homogeneous 4x4 matrix, bottom row [0,0,0,1].
toHomogeneous :: SE3 -> Either MathError (M.Matrix Double)
toHomogeneous :: SE3 -> Either MathError (Matrix Double)
toHomogeneous (SE3 (SO3 Quaternion
q) [Double]
t) = do
    r <- Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList (Matrix Double -> [Double])
-> Either MathError (Matrix Double) -> Either MathError [Double]
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Quaternion -> Either MathError (Matrix Double)
Q.toMatrix Quaternion
q
    M.fromListChecked 4 4 (concat [take 3 (drop (3 * i) r) ++ [t !! i] | i <- [0 .. 2]] ++ [0, 0, 0, 1])

-- | Validate a homogeneous rigid matrix (bottom row tolerance 1e-12, rotation 1e-9).
fromHomogeneous :: M.Matrix Double -> Either MathError SE3
fromHomogeneous :: Matrix Double -> Either MathError SE3
fromHomogeneous Matrix Double
m
    | Matrix Double -> (Int, Int)
forall a. Matrix a -> (Int, Int)
M.shape Matrix Double
m (Int, Int) -> (Int, Int) -> Bool
forall a. Eq a => a -> a -> Bool
/= (Int
4, Int
4) = MathError -> Either MathError SE3
forall a b. a -> Either a b
Left (String -> MathError
InvalidArgument String
"rigid matrix must be 4x4")
    | Bool -> Bool
not ((Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite [Double]
a) Bool -> Bool -> Bool
|| [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 -> Double -> Double) -> [Double] -> [Double] -> [Double]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith (-) (Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
drop Int
12 [Double]
a) [Double
0, Double
0, Double
0, Double
1])) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
1e-12 =
        MathError -> Either MathError SE3
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"invalid homogeneous bottom row")
    | Bool
otherwise = do
        r <-
            Int -> Int -> [Double] -> Either MathError (Matrix Double)
forall a. Int -> Int -> [a] -> Either MathError (Matrix a)
M.fromListChecked Int
3 Int
3 [[Double]
a [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! (Int
4 Int -> Int -> Int
forall a. Num a => a -> a -> a
* Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
j) | Int
i <- [Int
0 .. Int
2], Int
j <- [Int
0 .. Int
2]] Either MathError (Matrix Double)
-> (Matrix Double -> Either MathError Quaternion)
-> Either MathError Quaternion
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= Matrix Double -> Either MathError Quaternion
Q.fromMatrix Either MathError Quaternion
-> (Quaternion -> Either MathError SO3) -> Either MathError SO3
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= Quaternion -> Either MathError SO3
so3
        se3 r [a !! 3, a !! 7, a !! 11]
  where
    a :: [Double]
a = Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
m

{- | Construct a rigid dual quaternion. Real norm must be nonzero; both parts
are scaled together and the Study dot-product condition checked at 1e-9.
-}
dualQuaternion :: Q.Quaternion -> Q.Quaternion -> Either MathError DualQuaternion
dualQuaternion :: Quaternion -> Quaternion -> Either MathError DualQuaternion
dualQuaternion Quaternion
real Quaternion
dual = do
    unit <- Quaternion -> Either MathError Quaternion
Q.normalize Quaternion
real
    let n = Quaternion -> Double
Q.norm Quaternion
real
    if not (finite n)
        then Left (DomainError "dual quaternion norm is unrepresentable")
        else do
            d <- qFrom (map (/ n) (Q.components dual))
            if abs (sum (zipWith (*) (Q.components unit) (Q.components d))) > 1e-9
                then Left (DomainError "dual quaternion violates Study condition")
                else Right (DQ unit d)

-- | Real and dual Hamilton parts.
dualParts :: DualQuaternion -> (Q.Quaternion, Q.Quaternion)
dualParts :: DualQuaternion -> (Quaternion, Quaternion)
dualParts (DQ Quaternion
r Quaternion
d) = (Quaternion
r, Quaternion
d)

-- | Encode translation as 0.5*(0,t)*r.
toDualQuaternion :: SE3 -> Either MathError DualQuaternion
toDualQuaternion :: SE3 -> Either MathError DualQuaternion
toDualQuaternion (SE3 (SO3 Quaternion
r) [Double
x, Double
y, Double
z]) = do
    t <- Double -> Double -> Double -> Double -> Either MathError Quaternion
Q.quaternion Double
0 Double
x Double
y Double
z
    d <- Q.multiply t r >>= Q.scale 0.5
    Right (DQ r d)
toDualQuaternion SE3
_ = DualQuaternion -> Either MathError DualQuaternion
forall a b. b -> Either a b
Right (Quaternion -> Quaternion -> DualQuaternion
DQ Quaternion
one Quaternion
zero)

-- | Decode translation from 2*d*conjugate(r).
fromDualQuaternion :: DualQuaternion -> Either MathError SE3
fromDualQuaternion :: DualQuaternion -> Either MathError SE3
fromDualQuaternion (DQ Quaternion
r Quaternion
d) = do
    t <- Quaternion -> Quaternion -> Either MathError Quaternion
Q.multiply Quaternion
d (Quaternion -> Quaternion
Q.conjugate Quaternion
r) Either MathError Quaternion
-> (Quaternion -> Either MathError Quaternion)
-> Either MathError Quaternion
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= Double -> Quaternion -> Either MathError Quaternion
Q.scale Double
2
    se3 (SO3 r) (drop 1 (Q.components t))

-- | Rigid dual-quaternion product.
multiplyDual :: DualQuaternion -> DualQuaternion -> Either MathError DualQuaternion
multiplyDual :: DualQuaternion -> DualQuaternion -> Either MathError DualQuaternion
multiplyDual (DQ Quaternion
ar Quaternion
ad) (DQ Quaternion
br Quaternion
bd) = do
    r <- Quaternion -> Quaternion -> Either MathError Quaternion
Q.multiply Quaternion
ar Quaternion
br
    a <- Q.multiply ar bd
    b <- Q.multiply ad br
    d <- Q.add a b
    dualQuaternion r d

-- | Inverse rigid dual quaternion.
inverseDual :: DualQuaternion -> Either MathError DualQuaternion
inverseDual :: DualQuaternion -> Either MathError DualQuaternion
inverseDual DualQuaternion
d = DualQuaternion -> Either MathError SE3
fromDualQuaternion DualQuaternion
d Either MathError SE3
-> (SE3 -> Either MathError SE3) -> Either MathError SE3
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= SE3 -> Either MathError SE3
inverse Either MathError SE3
-> (SE3 -> Either MathError DualQuaternion)
-> Either MathError DualQuaternion
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= SE3 -> Either MathError DualQuaternion
toDualQuaternion

-- | Shortest-rotation screw-linear interpolation on [0,1].
interpolateDual :: Double -> DualQuaternion -> DualQuaternion -> Either MathError DualQuaternion
interpolateDual :: Double
-> DualQuaternion
-> DualQuaternion
-> Either MathError DualQuaternion
interpolateDual Double
t DualQuaternion
a DualQuaternion
b
    | Bool -> Bool
not (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 = MathError -> Either MathError DualQuaternion
forall a b. a -> Either a b
Left (String -> MathError
DomainError String
"dual interpolation outside [0,1]")
    | Bool
otherwise = do
        x <- DualQuaternion -> Either MathError SE3
fromDualQuaternion DualQuaternion
a
        y <- fromDualQuaternion b
        xi <- inverse x
        relative <- compose xi y
        (w, v) <- logSE3 relative
        step <- expSE3 (map (* t) w, map (* t) v)
        compose x step >>= toDualQuaternion