module Data.Geo.Jord.Rotation
( r2xyz
, r2zyx
, xyz2r
, zyx2r
) where
import Data.Geo.Jord.Angle (Angle)
import qualified Data.Geo.Jord.Angle as Angle (atan2, cos, negate, sin)
import qualified Data.Geo.Jord.Math3d as Math3d (V3, transposeM, v3x, v3y, v3z, vec3)
r2xyz :: [Math3d.V3] -> [Angle]
r2xyz :: [V3] -> [Angle]
r2xyz [V3
v0, V3
v1, V3
v2] = [Angle
x, Angle
y, Angle
z]
where
v00 :: Double
v00 = V3 -> Double
Math3d.v3x V3
v0
v01 :: Double
v01 = V3 -> Double
Math3d.v3y V3
v0
v12 :: Double
v12 = V3 -> Double
Math3d.v3z V3
v1
v22 :: Double
v22 = V3 -> Double
Math3d.v3z V3
v2
z :: Angle
z = Double -> Double -> Angle
Angle.atan2 (-Double
v01) Double
v00
x :: Angle
x = Double -> Double -> Angle
Angle.atan2 (-Double
v12) Double
v22
sy :: Double
sy = V3 -> Double
Math3d.v3z V3
v0
cy :: Double
cy = Double -> Double
forall a. Floating a => a -> a
sqrt ((Double
v00 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v00 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v01 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v01 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v12 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v12 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v22 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v22) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2.0)
y :: Angle
y = Double -> Double -> Angle
Angle.atan2 Double
sy Double
cy
r2xyz [V3]
m = [Char] -> [Angle]
forall a. HasCallStack => [Char] -> a
error ([Char]
"Invalid rotation matrix " [Char] -> [Char] -> [Char]
forall a. [a] -> [a] -> [a]
++ [V3] -> [Char]
forall a. Show a => a -> [Char]
show [V3]
m)
r2zyx :: [Math3d.V3] -> [Angle]
r2zyx :: [V3] -> [Angle]
r2zyx [V3]
m = [Angle
z, Angle
y, Angle
x]
where
[Angle
x, Angle
y, Angle
z] = (Angle -> Angle) -> [Angle] -> [Angle]
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Angle -> Angle
Angle.negate ([V3] -> [Angle]
r2xyz ([V3] -> [V3]
Math3d.transposeM [V3]
m))
xyz2r :: Angle -> Angle -> Angle -> [Math3d.V3]
xyz2r :: Angle -> Angle -> Angle -> [V3]
xyz2r Angle
x Angle
y Angle
z = [V3
v1, V3
v2, V3
v3]
where
cx :: Double
cx = Angle -> Double
Angle.cos Angle
x
sx :: Double
sx = Angle -> Double
Angle.sin Angle
x
cy :: Double
cy = Angle -> Double
Angle.cos Angle
y
sy :: Double
sy = Angle -> Double
Angle.sin Angle
y
cz :: Double
cz = Angle -> Double
Angle.cos Angle
z
sz :: Double
sz = Angle -> Double
Angle.sin Angle
z
v1 :: V3
v1 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz) ((-Double
cy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz) Double
sy
v2 :: V3
v2 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz) ((-Double
sy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz) ((-Double
cy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx)
v3 :: V3
v3 = Double -> Double -> Double -> V3
Math3d.vec3 ((-Double
sy) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz) (Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cz) (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx)
zyx2r :: Angle -> Angle -> Angle -> [Math3d.V3]
zyx2r :: Angle -> Angle -> Angle -> [V3]
zyx2r Angle
z Angle
y Angle
x = [V3
v1, V3
v2, V3
v3]
where
cx :: Double
cx = Angle -> Double
Angle.cos Angle
x
sx :: Double
sx = Angle -> Double
Angle.sin Angle
x
cy :: Double
cy = Angle -> Double
Angle.cos Angle
y
sy :: Double
sy = Angle -> Double
Angle.sin Angle
y
cz :: Double
cz = Angle -> Double
Angle.cos Angle
z
sz :: Double
sz = Angle -> Double
Angle.sin Angle
z
v1 :: V3
v1 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy) ((-Double
sz) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx) (Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx)
v2 :: V3
v2 = Double -> Double -> Double -> V3
Math3d.vec3 (Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cy) (Double
cz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx) ((-Double
cz) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sz Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx)
v3 :: V3
v3 = Double -> Double -> Double -> V3
Math3d.vec3 (-Double
sy) (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sx) (Double
cy Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cx)