{-# LANGUAGE TypeFamilies #-}
{-# LANGUAGE FlexibleInstances #-}
module Math.Rotations.Class ( Rotatable (..)
, xAxis, yAxis, zAxis
, (°)
, rotateViaEulerAnglesYZ
, rotateℝ³AboutCenteredAxis
, rotmatrixForAxis
, eulerAnglesZYZForMatrix
, rotmatrixForEulerAnglesZYZ
) where
import Math.Manifold.Core.Types
import Math.Manifold.Core.PseudoAffine
import Data.VectorSpace
import Linear.V3 (V3(V3))
class Rotatable m where
type AxisSpace m :: *
rotateAbout :: AxisSpace m -> S¹ -> m -> m
instance (Rotatable m) => Rotatable (m -> Double) where
type AxisSpace (m -> Double) = AxisSpace m
rotateAbout :: AxisSpace (m -> Double) -> S¹ -> (m -> Double) -> m -> Double
rotateAbout AxisSpace (m -> Double)
ax (S¹Polar Double
δφ) m -> Double
f = m -> Double
f (m -> Double) -> (m -> m) -> m -> Double
forall b c a. (b -> c) -> (a -> b) -> a -> c
. AxisSpace m -> S¹ -> m -> m
forall m. Rotatable m => AxisSpace m -> S¹ -> m -> m
rotateAbout AxisSpace m
AxisSpace (m -> Double)
ax (Double -> S¹
forall r. r -> S¹_ r
S¹Polar (Double -> S¹) -> Double -> S¹
forall a b. (a -> b) -> a -> b
$ -Double
δφ)
instance Rotatable S¹ where
type AxisSpace S¹ = ℝP⁰
rotateAbout :: AxisSpace S¹ -> S¹ -> S¹ -> S¹
rotateAbout AxisSpace S¹
ℝPZero (S¹Polar Double
δφ) (S¹Polar Double
φ)
| Double
φ' Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
forall a. Floating a => a
pi = Double -> S¹
forall r. r -> S¹_ r
S¹Polar (Double -> S¹) -> Double -> S¹
forall a b. (a -> b) -> a -> b
$ Double
φ'Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
forall r. RealFloat r => r
tau
| Double
φ' Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< -Double
forall a. Floating a => a
pi = Double -> S¹
forall r. r -> S¹_ r
S¹Polar (Double -> S¹) -> Double -> S¹
forall a b. (a -> b) -> a -> b
$ Double
φ'Double -> Double -> Double
forall a. Num a => a -> a -> a
+Double
forall r. RealFloat r => r
tau
| Bool
otherwise = Double -> S¹
forall r. r -> S¹_ r
S¹Polar Double
φ'
where φ' :: Double
φ' = Double
φ Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
δφ
rotateViaEulerAnglesYZ
:: (S¹ -> m -> m)
-> (S¹ -> m -> m)
-> (ℝP² -> S¹ -> m -> m)
rotateViaEulerAnglesYZ :: (S¹ -> m -> m) -> (S¹ -> m -> m) -> ℝP² -> S¹ -> m -> m
rotateViaEulerAnglesYZ S¹ -> m -> m
yRot S¹ -> m -> m
zRot ℝP²
ax = [[Double]] -> m -> m
rotAroundAxis ([[Double]] -> m -> m) -> (S¹ -> [[Double]]) -> S¹ -> m -> m
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ℝP² -> S¹ -> [[Double]]
rotmatrixForAxis ℝP²
ax
where rotAroundAxis :: [[Double]] -> m -> m
rotAroundAxis [[Double]]
mat = case [[Double]] -> [Double]
eulerAnglesZYZForMatrix [[Double]]
mat of
[Double
θz₀, Double
θy, Double
θz₁] -> S¹ -> m -> m
zRot (Double -> S¹
forall r. r -> S¹_ r
S¹Polar Double
θz₁) (m -> m) -> (m -> m) -> m -> m
forall b c a. (b -> c) -> (a -> b) -> a -> c
. S¹ -> m -> m
yRot (Double -> S¹
forall r. r -> S¹_ r
S¹Polar Double
θy) (m -> m) -> (m -> m) -> m -> m
forall b c a. (b -> c) -> (a -> b) -> a -> c
. S¹ -> m -> m
zRot (Double -> S¹
forall r. r -> S¹_ r
S¹Polar Double
θz₀)
rotmatrixForAxis :: ℝP² -> S¹ -> [[ℝ]]
rotmatrixForAxis :: ℝP² -> S¹ -> [[Double]]
rotmatrixForAxis (HemisphereℝP²Polar Double
θax Double
φax) = S¹ -> [[Double]]
rotAroundAxis
where rotAroundAxis :: S¹ -> [[Double]]
rotAroundAxis (S¹Polar Double
θ) = [[Double
r₀₀,Double
r₀₁,Double
r₀₂]
,[Double
r₁₀,Double
r₁₁,Double
r₁₂]
,[Double
r₂₀,Double
r₂₁,Double
r₂₂]]
where r₀₀ :: Double
r₀₀ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₀Double -> Integer -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cosθ
r₀₁ :: Double
r₀₁ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₁ Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
e₂Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sinθ
r₀₂ :: Double
r₀₂ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₂ Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
e₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sinθ
r₁₀ :: Double
r₁₀ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
e₂Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sinθ
r₁₁ :: Double
r₁₁ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₁Double -> Integer -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cosθ
r₁₂ :: Double
r₁₂ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₂ Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
e₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sinθ
r₂₀ :: Double
r₂₀ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₂Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
e₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sinθ
r₂₁ :: Double
r₂₁ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₂Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₁ Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
e₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sinθ
r₂₂ :: Double
r₂₂ = Double
one_cosθDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e₂Double -> Integer -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
cosθ
cosθ :: Double
cosθ = Double -> Double
forall a. Floating a => a -> a
cos Double
θ
sinθ :: Double
sinθ = Double -> Double
forall a. Floating a => a -> a
sin Double
θ
one_cosθ :: Double
one_cosθ = Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double -> Double
forall a. Floating a => a -> a
cos Double
θ
e₀ :: Double
e₀ = Double -> Double
forall a. Floating a => a -> a
cos Double
φax Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sin Double
θax
e₁ :: Double
e₁ = Double -> Double
forall a. Floating a => a -> a
sin Double
φax Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sin Double
θax
e₂ :: Double
e₂ = Double -> Double
forall a. Floating a => a -> a
cos Double
θax
rotmatrixForEulerAnglesZYZ :: [ℝ] -> [[ℝ]]
rotmatrixForEulerAnglesZYZ :: [Double] -> [[Double]]
rotmatrixForEulerAnglesZYZ [Double]
angles
= [[ Double
cyDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₁Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
sz₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₁, -Double
cyDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₁Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
cz₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₁, Double
syDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₁ ]
,[ Double
cyDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₁Double -> Double -> Double
forall a. Num a => a -> a -> a
+Double
cz₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₀, Double
cz₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₁Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
cyDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₁, Double
syDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₁ ]
,[ -Double
syDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₀ , Double
syDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₀ , Double
cy ]]
where [Double
cz₀,Double
cy,Double
cz₁] = Double -> Double
forall a. Floating a => a -> a
cos(Double -> Double) -> [Double] -> [Double]
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$>[Double]
angles
[Double
sz₀,Double
sy,Double
sz₁] = Double -> Double
forall a. Floating a => a -> a
sin(Double -> Double) -> [Double] -> [Double]
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$>[Double]
angles
eulerAnglesZYZForMatrix :: [[ℝ]] -> [ℝ]
eulerAnglesZYZForMatrix :: [[Double]] -> [Double]
eulerAnglesZYZForMatrix [[Double
r₀₀,Double
r₀₁,Double
r₀₂]
,[Double
r₁₀,Double
r₁₁,Double
r₁₂]
,[Double
r₂₀,Double
r₂₁,Double
r₂₂]]
= [Double
θz₀,Double
θy,Double
θz₁]
where
cy :: Double
cy = Double
r₂₂
sy :: Double
sy = Double -> Double
forall a. Floating a => a -> a
sqrt (Double -> Double) -> Double -> Double
forall a b. (a -> b) -> a -> b
$ Double
r₂₀Double -> Integer -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2Double -> Double -> Double
forall a. Num a => a -> a -> a
+Double
r₂₁Double -> Integer -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2
θy :: Double
θy = Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
sy Double
cy
θz₀ :: Double
θz₀ = Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
r₂₁ (-Double
r₂₀) ; sz₀ :: Double
sz₀ = Double -> Double
forall a. Floating a => a -> a
sin Double
θz₀; cz₀ :: Double
cz₀ = Double -> Double
forall a. Floating a => a -> a
cos Double
θz₀
θz₁ :: Double
θz₁ = Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 (-Double
r₀₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
r₀₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₀)
( Double
r₁₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sz₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
r₁₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cz₀)
instance Rotatable S² where
type AxisSpace S² = ℝP²
rotateAbout :: AxisSpace S² -> S¹ -> S² -> S²
rotateAbout = (S¹ -> S² -> S²) -> (S¹ -> S² -> S²) -> ℝP² -> S¹ -> S² -> S²
forall m. (S¹ -> m -> m) -> (S¹ -> m -> m) -> ℝP² -> S¹ -> m -> m
rotateViaEulerAnglesYZ
(\(S¹Polar Double
β) (S²Polar Double
θ Double
φ)
-> let x₀ :: Double
x₀ = Double -> Double
forall a. Floating a => a -> a
cos Double
φ Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sin Double
θ
y :: Double
y = Double -> Double
forall a. Floating a => a -> a
sin Double
φ Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sin Double
θ
z₀ :: Double
z₀ = Double -> Double
forall a. Floating a => a -> a
cos Double
θ
x₁ :: Double
x₁ = Double
x₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
cos Double
β Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
z₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sin Double
β
z₁ :: Double
z₁ = -Double
x₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sin Double
β Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
z₀ Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
cos Double
β
rxy :: Double
rxy = Double -> Double
forall a. Floating a => a -> a
sqrt (Double -> Double) -> Double -> Double
forall a b. (a -> b) -> a -> b
$ Double
x₁Double -> Integer -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
yDouble -> Integer -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^Integer
2
in Double -> Double -> S²
forall r. r -> r -> S²_ r
S²Polar (Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
rxy Double
z₁) (Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
y Double
x₁) )
(\S¹
γ (S²Polar Double
θ Double
φ) -> case AxisSpace S¹ -> S¹ -> S¹ -> S¹
forall m. Rotatable m => AxisSpace m -> S¹ -> m -> m
rotateAbout AxisSpace S¹
forall r. ℝP⁰_ r
ℝPZero S¹
γ (Double -> S¹
forall r. r -> S¹_ r
S¹Polar Double
φ) of
S¹Polar Double
φ' -> Double -> Double -> S²
forall r. r -> r -> S²_ r
S²Polar Double
θ Double
φ')
xAxis, yAxis, zAxis :: ℝP²
xAxis :: ℝP²
xAxis = Double -> Double -> ℝP²
forall r. r -> r -> ℝP²_ r
HemisphereℝP²Polar (Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2) Double
0
yAxis :: ℝP²
yAxis = Double -> Double -> ℝP²
forall r. r -> r -> ℝP²_ r
HemisphereℝP²Polar (Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2) (Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2)
zAxis :: ℝP²
zAxis = Double -> Double -> ℝP²
forall r. r -> r -> ℝP²_ r
HemisphereℝP²Polar Double
0 Double
0
infix 5 °
(°) :: Rotatable m => ℝ -> AxisSpace m -> m -> m
Double
angle° :: Double -> AxisSpace m -> m -> m
° AxisSpace m
axis = AxisSpace m -> S¹ -> m -> m
forall m. Rotatable m => AxisSpace m -> S¹ -> m -> m
rotateAbout AxisSpace m
axis (S¹ -> m -> m) -> (Double -> S¹) -> Double -> m -> m
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Double -> S¹
forall r. r -> S¹_ r
S¹Polar (Double -> m -> m) -> Double -> m -> m
forall a b. (a -> b) -> a -> b
$ Double
angle Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
180
rotateℝ³AboutCenteredAxis :: ℝP² -> S¹ -> V3 ℝ -> V3 ℝ
rotateℝ³AboutCenteredAxis :: ℝP² -> S¹ -> V3 Double -> V3 Double
rotateℝ³AboutCenteredAxis ℝP²
axis S¹
angle = case ℝP² -> S¹ -> [[Double]]
rotmatrixForAxis ℝP²
axis S¹
angle of
[ [Double
r₀₀,Double
r₀₁,Double
r₀₂]
,[Double
r₁₀,Double
r₁₁,Double
r₁₂]
,[Double
r₂₀,Double
r₂₁,Double
r₂₂] ] -> \(V3 Double
x Double
y Double
z) -> Double -> Double -> Double -> V3 Double
forall a. a -> a -> a -> V3 a
V3 (Double
r₀₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
r₀₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
r₀₂Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
z)
(Double
r₁₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
r₁₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
r₁₂Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
z)
(Double
r₂₀Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
r₂₁Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
r₂₂Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
z)