{-# language QuasiQuotes #-}

module Physics.Orbit.StateVectors
  ( -- *** Types
    StateVectors(..)
  , Position
  , Velocity
    -- *** Conversion to state vectors
  , stateVectorsAtTrueAnomaly
  , positionAtTrueAnomaly
  , positionInPlaneAtTrueAnomaly
  , velocityAtTrueAnomaly
  , velocityInPlaneAtTrueAnomaly
    -- *** Conversion from state vectors
  , elementsFromStateVectors
  , eccentricityVector
  , trueAnomalyAtPosition
    -- *** Rotations to and from orbital plane
  , orbitalPlaneQuaternion
  , rotateToPlane
  , rotateFromPlane
    -- *** other utilities
  , flightPathAngleAtTrueAnomaly
  , specificAngularMomentumVector
  ) where

import           Control.Lens.Operators         ( (^.) )
import           Data.Coerce
import           Data.Constants.Mechanics.Extra
import           Data.Metrology
import           Data.Metrology.Extra
import           Data.Metrology.Unsafe          ( Qu(..) )
import           Data.Units.SI.Parser
import           Linear.Conjugate
import           Linear.Quaternion
import           Linear.V3
import           Physics.Orbit

type Position a = V3 (Distance a)
type Velocity a = V3 (Speed a)

data StateVectors a = StateVectors
  { StateVectors a -> Position a
position :: Position a
  , StateVectors a -> Velocity a
velocity :: Velocity a
  }
  deriving (Int -> StateVectors a -> ShowS
[StateVectors a] -> ShowS
StateVectors a -> String
(Int -> StateVectors a -> ShowS)
-> (StateVectors a -> String)
-> ([StateVectors a] -> ShowS)
-> Show (StateVectors a)
forall a. Show a => Int -> StateVectors a -> ShowS
forall a. Show a => [StateVectors a] -> ShowS
forall a. Show a => StateVectors a -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [StateVectors a] -> ShowS
$cshowList :: forall a. Show a => [StateVectors a] -> ShowS
show :: StateVectors a -> String
$cshow :: forall a. Show a => StateVectors a -> String
showsPrec :: Int -> StateVectors a -> ShowS
$cshowsPrec :: forall a. Show a => Int -> StateVectors a -> ShowS
Show, StateVectors a -> StateVectors a -> Bool
(StateVectors a -> StateVectors a -> Bool)
-> (StateVectors a -> StateVectors a -> Bool)
-> Eq (StateVectors a)
forall a. Eq a => StateVectors a -> StateVectors a -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: StateVectors a -> StateVectors a -> Bool
$c/= :: forall a. Eq a => StateVectors a -> StateVectors a -> Bool
== :: StateVectors a -> StateVectors a -> Bool
$c== :: forall a. Eq a => StateVectors a -> StateVectors a -> Bool
Eq)

----------------------------------------------------------------
-- Conversiont to state vectors
----------------------------------------------------------------

-- | Get the position in space of a body after rotating it according to the
-- inclination and periapsis specifier.
positionAtTrueAnomaly
  :: (Conjugate a, RealFloat a) => Orbit a -> Angle a -> Position a
positionAtTrueAnomaly :: Orbit a -> Angle a -> Position a
positionAtTrueAnomaly o :: Orbit a
o = Orbit a
-> V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
-> V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
forall a (u :: [Factor *]) (l :: LCSU *).
(Conjugate a, RealFloat a) =>
Orbit a -> V3 (Qu u l a) -> V3 (Qu u l a)
rotateFromPlane Orbit a
o (V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
 -> V3 (Qu '[ 'F Length One] 'DefaultLCSU a))
-> (Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
    -> V3 (Qu '[ 'F Length One] 'DefaultLCSU a))
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Orbit a -> Angle a -> Position a
forall a. (Ord a, Floating a) => Orbit a -> Angle a -> Position a
positionInPlaneAtTrueAnomaly Orbit a
o

-- | Get the position of a body relative to the orbital plane
positionInPlaneAtTrueAnomaly
  :: (Ord a, Floating a) => Orbit a -> Angle a -> Position a
positionInPlaneAtTrueAnomaly :: Orbit a -> Angle a -> Position a
positionInPlaneAtTrueAnomaly o :: Orbit a
o ν :: Angle a
ν = V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
Position a
r
 where
  radius :: Distance a
radius = Orbit a -> Angle a -> Distance a
forall a. (Ord a, Floating a) => Orbit a -> Angle a -> Distance a
radiusAtTrueAnomaly Orbit a
o Angle a
ν
  r :: V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
r      = Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu '[ 'F Length One] 'DefaultLCSU a
-> V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
forall a. a -> a -> a -> V3 a
V3 (Angle a -> Unitless a
forall a. Floating a => Angle a -> Unitless a
qCos Angle a
ν Qu '[] 'DefaultLCSU a
-> Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu (Normalize ('[] @+ '[ 'F Length One])) 'DefaultLCSU a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[ 'F Length One] 'DefaultLCSU a
Distance a
radius) (Angle a -> Unitless a
forall a. Floating a => Angle a -> Unitless a
qSin Angle a
ν Qu '[] 'DefaultLCSU a
-> Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu (Normalize ('[] @+ '[ 'F Length One])) 'DefaultLCSU a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[ 'F Length One] 'DefaultLCSU a
Distance a
radius) Qu '[ 'F Length One] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero

-- | Get the velocity in space of a body after rotating it according to the
-- inclination and periapsis specifier.
velocityAtTrueAnomaly
  :: (Conjugate a, RealFloat a) => Orbit a -> Angle a -> Velocity a
velocityAtTrueAnomaly :: Orbit a -> Angle a -> Velocity a
velocityAtTrueAnomaly o :: Orbit a
o = Orbit a
-> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall a (u :: [Factor *]) (l :: LCSU *).
(Conjugate a, RealFloat a) =>
Orbit a -> V3 (Qu u l a) -> V3 (Qu u l a)
rotateFromPlane Orbit a
o (V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
 -> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a))
-> (Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
    -> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a))
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Orbit a -> Angle a -> Velocity a
forall a. (Ord a, Floating a) => Orbit a -> Angle a -> Velocity a
velocityInPlaneAtTrueAnomaly Orbit a
o

-- | The in-plane velocity of a body
velocityInPlaneAtTrueAnomaly
  :: (Ord a, Floating a) => Orbit a -> Angle a -> Velocity a
velocityInPlaneAtTrueAnomaly :: Orbit a -> Angle a -> Velocity a
velocityInPlaneAtTrueAnomaly o :: Orbit a
o ν :: Angle a
ν = V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
Velocity a
v
 where
  μ :: Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ    = Orbit a
-> Quantity
     ((Meter :^ Succ (Succ (Succ 'Zero)))
      :* (Second :^ Pred (Pred 'Zero)))
     a
forall a.
Orbit a
-> Quantity
     ((Meter :^ Succ (Succ (Succ 'Zero)))
      :* (Second :^ Pred (Pred 'Zero)))
     a
primaryGravitationalParameter Orbit a
o
  e :: Unitless a
e    = Orbit a -> Unitless a
forall a. Orbit a -> Unitless a
eccentricity Orbit a
o
  r :: Distance a
r    = Orbit a -> Angle a -> Distance a
forall a. (Ord a, Floating a) => Orbit a -> Angle a -> Distance a
radiusAtTrueAnomaly Orbit a
o Angle a
ν
  h :: Quantity ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
h    = Orbit a
-> Quantity
     ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
forall a.
Floating a =>
Orbit a
-> Quantity
     ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
specificAngularMomentum Orbit a
o
  cosν :: Unitless a
cosν = Angle a -> Unitless a
forall a. Floating a => Angle a -> Unitless a
qCos Angle a
ν
  sinν :: Unitless a
sinν = Angle a -> Unitless a
forall a. Floating a => Angle a -> Unitless a
qSin Angle a
ν
  vr :: Qu
  (Normalize
     ('[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
vr   = Qu
  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ Qu
  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] @+ '[]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[] 'DefaultLCSU a
Unitless a
e Qu
  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] @+ '[]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[] 'DefaultLCSU a
Unitless a
sinν Qu
  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]
         @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
Quantity ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
h
  vtA :: Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)] @- '[ 'F Length One]))
  'DefaultLCSU
  a
vtA  = Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
Quantity ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
h Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S One), 'F Time ('P 'Zero)] @- '[ 'F Length One]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| Qu '[ 'F Length One] 'DefaultLCSU a
Distance a
r
  v :: V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
v    = Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall a. a -> a -> a -> V3 a
V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
Qu
  (Normalize
     ('[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
vr Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a
-> Qu
     (Normalize ('[ 'F Length One, 'F Time ('P 'Zero)] @+ '[]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[] 'DefaultLCSU a
Unitless a
cosν Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
forall (d1 :: [Factor *]) (d2 :: [Factor *]) n (l :: LCSU *).
(d1 @~ d2, Num n) =>
Qu d1 l n -> Qu d2 l n -> Qu d1 l n
|-| Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)] @- '[ 'F Length One]))
  'DefaultLCSU
  a
vtA Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a
-> Qu
     (Normalize ('[ 'F Length One, 'F Time ('P 'Zero)] @+ '[]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[] 'DefaultLCSU a
Unitless a
sinν) (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
Qu
  (Normalize
     ('[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
vr Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a
-> Qu
     (Normalize ('[ 'F Length One, 'F Time ('P 'Zero)] @+ '[]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[] 'DefaultLCSU a
Unitless a
sinν Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
forall (d1 :: [Factor *]) (d2 :: [Factor *]) n (l :: LCSU *).
(d1 @~ d2, Num n) =>
Qu d1 l n -> Qu d2 l n -> Qu d1 l n
|+| Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)] @- '[ 'F Length One]))
  'DefaultLCSU
  a
vtA Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a
-> Qu
     (Normalize ('[ 'F Length One, 'F Time ('P 'Zero)] @+ '[]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[] 'DefaultLCSU a
Unitless a
cosν) Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero

stateVectorsAtTrueAnomaly
  :: (Conjugate a, RealFloat a) => Orbit a -> Angle a -> StateVectors a
stateVectorsAtTrueAnomaly :: Orbit a -> Angle a -> StateVectors a
stateVectorsAtTrueAnomaly o :: Orbit a
o ν :: Angle a
ν = Position a -> Velocity a -> StateVectors a
forall a. Position a -> Velocity a -> StateVectors a
StateVectors Position a
r Velocity a
v
 where
  r :: Position a
r = Orbit a -> Angle a -> Position a
forall a.
(Conjugate a, RealFloat a) =>
Orbit a -> Angle a -> Position a
positionAtTrueAnomaly Orbit a
o Angle a
ν
  v :: Velocity a
v = Orbit a -> Angle a -> Velocity a
forall a.
(Conjugate a, RealFloat a) =>
Orbit a -> Angle a -> Velocity a
velocityAtTrueAnomaly Orbit a
o Angle a
ν

----------------------------------------------------------------
-- Conversion from state vectors
----------------------------------------------------------------

-- Thanks to https://downloads.rene-schwarz.com/download/M002-Cartesian_State_Vectors_to_Keplerian_Orbit_Elements.pdf
elementsFromStateVectors
  :: (Ord a, Floating a, Conjugate a, RealFloat a, Show a)
  => Quantity [si| m^3 s^-2 |] a
  -> StateVectors a
  -> (Orbit a, Angle a)
elementsFromStateVectors :: Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
-> StateVectors a -> (Orbit a, Angle a)
elementsFromStateVectors μ :: Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ sv :: StateVectors a
sv@(StateVectors r :: Position a
r v :: Velocity a
v) = (Orbit a
o, Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
ν)
 where
  o :: Orbit a
o     = Unitless a
-> Distance a
-> InclinationSpecifier a
-> PeriapsisSpecifier a
-> Quantity
     ((Meter :^ Succ (Succ (Succ 'Zero)))
      :* (Second :^ Pred (Pred 'Zero)))
     a
-> Orbit a
forall a.
Unitless a
-> Distance a
-> InclinationSpecifier a
-> PeriapsisSpecifier a
-> Quantity
     ((Meter :^ Succ (Succ (Succ 'Zero)))
      :* (Second :^ Pred (Pred 'Zero)))
     a
-> Orbit a
Orbit Qu '[] 'DefaultLCSU a
Unitless a
e Qu ('F Length One : Normalize '[ 'F Time 'Zero]) 'DefaultLCSU a
Distance a
q InclinationSpecifier a
inclinationSpecifier' PeriapsisSpecifier a
periapsisSpecifier' Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ

  h :: V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h     = StateVectors a
-> V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
forall a.
Num a =>
StateVectors a
-> V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
specificAngularMomentumVector StateVectors a
sv
  n :: V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
n     = Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> V3
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall a. a -> a -> a -> V3 a
V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall n (d :: [Factor *]) (l :: LCSU *).
Num n =>
Qu d l n -> Qu d l n
qNegate (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Getting
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
     (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
  (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall (t :: * -> *) a. R2 t => Lens' (t a) a
_y)) (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Getting
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
     (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
  (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall (t :: * -> *) a. R1 t => Lens' (t a) a
_x) Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero

  e' :: V3 (Unitless a)
e'    = Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
-> StateVectors a -> V3 (Unitless a)
forall a.
Floating a =>
Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
-> StateVectors a -> V3 (Unitless a)
eccentricityVector Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ StateVectors a
sv
  e :: Qu '[] 'DefaultLCSU a
e     = V3 (Qu '[] 'DefaultLCSU a) -> Qu '[] 'DefaultLCSU a
forall (u :: [Factor *]) (l :: LCSU *) a.
Floating a =>
V3 (Qu u l a) -> Qu u l a
qNorm V3 (Qu '[] 'DefaultLCSU a)
V3 (Unitless a)
e'
  eNorm :: V3 (Qu '[] 'DefaultLCSU a)
eNorm = (Qu '[] 'DefaultLCSU a -> Qu '[] 'DefaultLCSU a
forall a. Fractional a => a -> a
recip Qu '[] 'DefaultLCSU a
e Qu '[] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a -> Qu '[] 'DefaultLCSU a
forall a. Num a => a -> a -> a
*) (Qu '[] 'DefaultLCSU a -> Qu '[] 'DefaultLCSU a)
-> V3 (Qu '[] 'DefaultLCSU a) -> V3 (Qu '[] 'DefaultLCSU a)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> V3 (Qu '[] 'DefaultLCSU a)
V3 (Unitless a)
e'

  aInv :: Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
aInv  = (2 Qu '[] 'DefaultLCSU a
-> Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu (Normalize ('[] @- '[ 'F Length One])) 'DefaultLCSU a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
-> Qu '[ 'F Length One] 'DefaultLCSU a
forall (u :: [Factor *]) (l :: LCSU *) a.
Floating a =>
V3 (Qu u l a) -> Qu u l a
qNorm V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
Position a
r) Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
forall (d1 :: [Factor *]) (d2 :: [Factor *]) n (l :: LCSU *).
(d1 @~ d2, Num n) =>
Qu d1 l n -> Qu d2 l n -> Qu d1 l n
|-| (V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu
     (Normalize
        ('[ 'F Length One, 'F Time ('P 'Zero)]
         @@+ Reorder
               '[ 'F Length One, 'F Time ('P 'Zero)]
               '[ 'F Length One, 'F Time ('P 'Zero)]))
     'DefaultLCSU
     a
forall (u :: [Factor *]) (l :: LCSU *) a.
Num a =>
V3 (Qu u l a) -> Qu (Normalize (u @@+ Reorder u u)) l a
qQuadrance V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
Velocity a
v Qu '[ 'F Length ('S One), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> Qu
     '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S One), 'F Time ('P ('P 'Zero))]
         @- '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| Qu
  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ)
  a :: Qu (Normalize ('[] @- '[ 'F Length ('P 'Zero)])) 'DefaultLCSU a
a     = Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
-> Qu (Normalize ('[] @- '[ 'F Length ('P 'Zero)])) 'DefaultLCSU a
forall (u :: [Factor *]) (l :: LCSU *) a.
Fractional a =>
Qu u l a -> Qu (Normalize ('[] @- u)) l a
qRecip Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
aInv
  q :: Qu ('F Length One : Normalize '[ 'F Time 'Zero]) 'DefaultLCSU a
q     = if Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
aInv Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a -> Bool
forall a. Eq a => a -> a -> Bool
== Qu '[ 'F Length ('P 'Zero)] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero -- parabolic trajectory
    then V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu
     (Normalize
        ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
         @@+ Reorder
               '[ 'F Length ('S One), 'F Time ('P 'Zero)]
               '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
     'DefaultLCSU
     a
forall (u :: [Factor *]) (l :: LCSU *) a.
Num a =>
V3 (Qu u l a) -> Qu (Normalize (u @@+ Reorder u u)) l a
qQuadrance V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h Qu
  '[ 'F Length ('S ('S ('S One))), 'F Time ('P ('P 'Zero))]
  'DefaultLCSU
  a
-> Qu
     '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S ('S ('S One))), 'F Time ('P ('P 'Zero))]
         @- '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| (2 Qu '[] 'DefaultLCSU a
-> Qu
     '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[] @+ '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu
  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ)
    else Qu '[ 'F Length One] 'DefaultLCSU a
Qu (Normalize ('[] @- '[ 'F Length ('P 'Zero)])) 'DefaultLCSU a
a Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a
-> Qu (Normalize ('[ 'F Length One] @+ '[])) 'DefaultLCSU a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| (1 Qu '[] 'DefaultLCSU a
-> Qu '[] 'DefaultLCSU a -> Qu '[] 'DefaultLCSU a
forall a. Num a => a -> a -> a
- Qu '[] 'DefaultLCSU a
e)

  ν :: Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
ν = if Qu '[] 'DefaultLCSU a
e Qu '[] 'DefaultLCSU a -> Qu '[] 'DefaultLCSU a -> Bool
forall a. Eq a => a -> a -> Bool
== Qu '[] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero
    then -- fall back to the slower version if this is a circular orbit
         Orbit a -> Position a -> Angle a
forall a.
(Conjugate a, RealFloat a) =>
Orbit a -> Position a -> Angle a
trueAnomalyAtPosition Orbit a
o Position a
r
    else
      let cosν :: Qu (Normalize ('[] @@+ Reorder '[] '[])) 'DefaultLCSU a
cosν = V3 (Qu '[] 'DefaultLCSU a)
eNorm V3 (Qu '[] 'DefaultLCSU a)
-> V3 (Qu '[] 'DefaultLCSU a)
-> Qu (Normalize ('[] @@+ Reorder '[] '[])) 'DefaultLCSU a
forall (u :: [Factor *]) (v :: [Factor *]) (l :: LCSU *) a.
Num a =>
V3 (Qu u l a)
-> V3 (Qu v l a) -> Qu (Normalize (u @@+ Reorder v u)) l a
`qDot` V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
-> V3
     (Qu
        (Normalize
           (Normalize ('[] @- '[ 'F Length One])
            @@+ Reorder
                  '[ 'F Length One] (Normalize ('[] @- '[ 'F Length One]))))
        'DefaultLCSU
        a)
forall n (b :: [Factor *]) (l :: LCSU *).
Floating n =>
V3 (Qu b l n)
-> V3
     (Qu
        (Normalize
           (Normalize ('[] @- b) @@+ Reorder b (Normalize ('[] @- b))))
        l
        n)
qNormalize V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
Position a
r
      in  if V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
Position a
r V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
-> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu
     (Normalize
        ('[ 'F Length One]
         @@+ Reorder
               '[ 'F Length One, 'F Time ('P 'Zero)] '[ 'F Length One]))
     'DefaultLCSU
     a
forall (u :: [Factor *]) (v :: [Factor *]) (l :: LCSU *) a.
Num a =>
V3 (Qu u l a)
-> V3 (Qu v l a) -> Qu (Normalize (u @@+ Reorder v u)) l a
`qDot` V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
Velocity a
v Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Bool
forall a. Ord a => a -> a -> Bool
>= Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero then Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos Unitless a
Qu (Normalize ('[] @@+ Reorder '[] '[])) 'DefaultLCSU a
cosν else Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a. Floating a => PlaneAngle a
turn Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall (d1 :: [Factor *]) (d2 :: [Factor *]) n (l :: LCSU *).
(d1 @~ d2, Num n) =>
Qu d1 l n -> Qu d2 l n -> Qu d1 l n
|-| Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos Unitless a
Qu (Normalize ('[] @@+ Reorder '[] '[])) 'DefaultLCSU a
cosν

  inclinationSpecifier' :: InclinationSpecifier a
inclinationSpecifier' =
    let i :: Angle a
i    = Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos ((V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Getting
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
     (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
  (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall (t :: * -> *) a. R3 t => Lens' (t a) a
_z) Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
         @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall (u :: [Factor *]) (l :: LCSU *) a.
Floating a =>
V3 (Qu u l a) -> Qu u l a
qNorm V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h)
        cosΩ :: Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
cosΩ = V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
n V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Getting
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
     (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
  (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall (t :: * -> *) a. R1 t => Lens' (t a) a
_x Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
         @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall (u :: [Factor *]) (l :: LCSU *) a.
Floating a =>
V3 (Qu u l a) -> Qu u l a
qNorm V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
n
        _Ω :: Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
_Ω   = if V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
n V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Getting
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
     (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
  (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall (t :: * -> *) a. R2 t => Lens' (t a) a
_y Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Bool
forall a. Ord a => a -> a -> Bool
>= Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero then Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos Unitless a
Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
cosΩ else Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a. Floating a => PlaneAngle a
turn Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall (d1 :: [Factor *]) (d2 :: [Factor *]) n (l :: LCSU *).
(d1 @~ d2, Num n) =>
Qu d1 l n -> Qu d2 l n -> Qu d1 l n
|-| Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos Unitless a
Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
cosΩ
    in  if V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Getting
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
     (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
  (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall (t :: * -> *) a. R1 t => Lens' (t a) a
_x Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Bool
forall a. Eq a => a -> a -> Bool
== Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero Bool -> Bool -> Bool
&& V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Getting
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
     (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
  (V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a))
  (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
forall (t :: * -> *) a. R2 t => Lens' (t a) a
_y Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Bool
forall a. Eq a => a -> a -> Bool
== Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero
          then InclinationSpecifier a
forall a. InclinationSpecifier a
NonInclined
          else Angle a -> Angle a -> InclinationSpecifier a
forall a. Angle a -> Angle a -> InclinationSpecifier a
Inclined Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
_Ω Angle a
i

  -- If the orbit is not inclined, ω is relative to the reference direction
  -- [1,0,0]
  periapsisSpecifier' :: PeriapsisSpecifier a
periapsisSpecifier' =
    let cosω :: Qu '[] 'DefaultLCSU a
cosω = case InclinationSpecifier a
inclinationSpecifier' of
          Inclined _ _ -> V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> V3
     (Qu
        (Normalize
           (Normalize ('[] @- '[ 'F Length ('S One), 'F Time ('P 'Zero)])
            @@+ Reorder
                  '[ 'F Length ('S One), 'F Time ('P 'Zero)]
                  (Normalize ('[] @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))))
        'DefaultLCSU
        a)
forall n (b :: [Factor *]) (l :: LCSU *).
Floating n =>
V3 (Qu b l n)
-> V3
     (Qu
        (Normalize
           (Normalize ('[] @- b) @@+ Reorder b (Normalize ('[] @- b))))
        l
        n)
qNormalize V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
n V3 (Qu '[] 'DefaultLCSU a)
-> V3 (Qu '[] 'DefaultLCSU a)
-> Qu (Normalize ('[] @@+ Reorder '[] '[])) 'DefaultLCSU a
forall (u :: [Factor *]) (v :: [Factor *]) (l :: LCSU *) a.
Num a =>
V3 (Qu u l a)
-> V3 (Qu v l a) -> Qu (Normalize (u @@+ Reorder v u)) l a
`qDot` V3 (Qu '[] 'DefaultLCSU a)
eNorm
          NonInclined  -> V3 (Qu '[] 'DefaultLCSU a)
eNorm V3 (Qu '[] 'DefaultLCSU a)
-> Getting
     (Qu '[] 'DefaultLCSU a)
     (V3 (Qu '[] 'DefaultLCSU a))
     (Qu '[] 'DefaultLCSU a)
-> Qu '[] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[] 'DefaultLCSU a)
  (V3 (Qu '[] 'DefaultLCSU a))
  (Qu '[] 'DefaultLCSU a)
forall (t :: * -> *) a. R1 t => Lens' (t a) a
_x
        -- ω = if (e' ^. _z) >= zero then qArcCos cosω else turn |-| qArcCos cosω
        ω :: Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
ω = case InclinationSpecifier a
inclinationSpecifier' of
          Inclined _ _ ->
            if (V3 (Qu '[] 'DefaultLCSU a)
V3 (Unitless a)
e' V3 (Qu '[] 'DefaultLCSU a)
-> Getting
     (Qu '[] 'DefaultLCSU a)
     (V3 (Qu '[] 'DefaultLCSU a))
     (Qu '[] 'DefaultLCSU a)
-> Qu '[] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[] 'DefaultLCSU a)
  (V3 (Qu '[] 'DefaultLCSU a))
  (Qu '[] 'DefaultLCSU a)
forall (t :: * -> *) a. R3 t => Lens' (t a) a
_z) Qu '[] 'DefaultLCSU a -> Qu '[] 'DefaultLCSU a -> Bool
forall a. Ord a => a -> a -> Bool
>= Qu '[] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero then Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos Qu '[] 'DefaultLCSU a
Unitless a
cosω else Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a. Floating a => PlaneAngle a
turn Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall (d1 :: [Factor *]) (d2 :: [Factor *]) n (l :: LCSU *).
(d1 @~ d2, Num n) =>
Qu d1 l n -> Qu d2 l n -> Qu d1 l n
|-| Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos Qu '[] 'DefaultLCSU a
Unitless a
cosω
          NonInclined ->
            let sinω :: Qu '[] 'DefaultLCSU a
sinω = V3 (Qu '[] 'DefaultLCSU a)
eNorm V3 (Qu '[] 'DefaultLCSU a)
-> Getting
     (Qu '[] 'DefaultLCSU a)
     (V3 (Qu '[] 'DefaultLCSU a))
     (Qu '[] 'DefaultLCSU a)
-> Qu '[] 'DefaultLCSU a
forall s a. s -> Getting a s a -> a
^. Getting
  (Qu '[] 'DefaultLCSU a)
  (V3 (Qu '[] 'DefaultLCSU a))
  (Qu '[] 'DefaultLCSU a)
forall (t :: * -> *) a. R2 t => Lens' (t a) a
_y in Unitless a -> Unitless a -> Angle a
forall a. RealFloat a => Unitless a -> Unitless a -> Angle a
qArcTan2 Qu '[] 'DefaultLCSU a
Unitless a
sinω Qu '[] 'DefaultLCSU a
Unitless a
cosω Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a (u :: [Factor *]) (l :: LCSU *).
Real a =>
Qu u l a -> Qu u l a -> Qu u l a
`mod'` Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a. Floating a => PlaneAngle a
turn
    in  if Qu '[] 'DefaultLCSU a
e Qu '[] 'DefaultLCSU a -> Qu '[] 'DefaultLCSU a -> Bool
forall a. Eq a => a -> a -> Bool
== Qu '[] 'DefaultLCSU a
forall n (dimspec :: [Factor *]) (l :: LCSU *).
Num n =>
Qu dimspec l n
zero then PeriapsisSpecifier a
forall a. PeriapsisSpecifier a
Circular else Angle a -> PeriapsisSpecifier a
forall a. Angle a -> PeriapsisSpecifier a
Eccentric Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
ω

-- | Calculate the true anomaly, ν, of a body at position, r, given its orbital
-- elements.
trueAnomalyAtPosition
  :: (Conjugate a, RealFloat a) => Orbit a -> Position a -> Angle a
trueAnomalyAtPosition :: Orbit a -> Position a -> Angle a
trueAnomalyAtPosition o :: Orbit a
o r :: Position a
r = Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
ν
 where
  V3 (Qu x :: a
x) (Qu y :: a
y) _ = Orbit a
-> V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
-> V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
forall a (u :: [Factor *]) (l :: LCSU *).
(Conjugate a, RealFloat a) =>
Orbit a -> V3 (Qu u l a) -> V3 (Qu u l a)
rotateToPlane Orbit a
o V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
Position a
r
  ν :: Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
ν                  = a -> a -> a
forall a. RealFloat a => a -> a -> a
atan2 a
y a
x a -> Radian -> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall (dim :: [Factor *]) unit n.
(ValidDLU dim 'DefaultLCSU unit, Fractional n) =>
n -> unit -> Qu dim 'DefaultLCSU n
% [si|rad|]

-- | Calculate the momentum vector, h, given state vectors
specificAngularMomentumVector
  :: Num a => StateVectors a -> V3 (Quantity [si|m^2 / s|] a)
specificAngularMomentumVector :: StateVectors a
-> V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
specificAngularMomentumVector (StateVectors r :: Position a
r v :: Velocity a
v) = V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
Position a
r V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
-> V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> V3
     (Qu
        (Normalize
           ('[ 'F Length One]
            @@+ Reorder
                  '[ 'F Length One, 'F Time ('P 'Zero)] '[ 'F Length One]))
        'DefaultLCSU
        a)
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
V3 (Qu a l n)
-> V3 (Qu b l n) -> V3 (Qu (Normalize (a @@+ Reorder b a)) l n)
`qCross` V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
Velocity a
v

-- | Calculate the eccentricity vector, e, given state vectors
eccentricityVector
  :: Floating a
  => Quantity [si| m^3 s^-2 |] a
  -> StateVectors a
  -> V3 (Unitless a)
eccentricityVector :: Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
-> StateVectors a -> V3 (Unitless a)
eccentricityVector μ :: Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ sv :: StateVectors a
sv@(StateVectors r :: Position a
r v :: Velocity a
v) = V3 (Qu '[] 'DefaultLCSU a)
V3 (Unitless a)
e
 where
  e :: V3 (Qu '[] 'DefaultLCSU a)
e = (V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
Velocity a
v V3 (Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> V3
     (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
-> V3
     (Qu
        (Normalize
           ('[ 'F Length One, 'F Time ('P 'Zero)]
            @@+ Reorder
                  '[ 'F Length ('S One), 'F Time ('P 'Zero)]
                  '[ 'F Length One, 'F Time ('P 'Zero)]))
        'DefaultLCSU
        a)
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
V3 (Qu a l n)
-> V3 (Qu b l n) -> V3 (Qu (Normalize (a @@+ Reorder b a)) l n)
`qCross` V3 (Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a)
V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h) V3
  (Qu
     '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]
     'DefaultLCSU
     a)
-> Qu
     '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
-> V3
     (Qu
        (Normalize
           (Normalize
              ('[] @- '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))])
            @@+ Reorder
                  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]
                  (Normalize
                     ('[] @- '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))]))))
        'DefaultLCSU
        a)
forall (f :: * -> *) n (b :: [Factor *]) (l :: LCSU *)
       (u :: [Factor *]).
(Functor f, Fractional n) =>
f (Qu b l n)
-> Qu u l n
-> f (Qu
        (Normalize
           (Normalize ('[] @- u) @@+ Reorder b (Normalize ('[] @- u))))
        l
        n)
|^/| Qu
  '[ 'F Length ('S ('S One)), 'F Time ('P ('P 'Zero))] 'DefaultLCSU a
Quantity
  ((Meter :^ Succ (Succ (Succ 'Zero)))
   :* (Second :^ Pred (Pred 'Zero)))
  a
μ V3 (Qu '[] 'DefaultLCSU a)
-> V3 (Qu '[] 'DefaultLCSU a) -> V3 (Qu '[] 'DefaultLCSU a)
forall (f :: * -> *) (u :: [Factor *]) (l :: LCSU *) a.
(Additive f, Applicative f, Num a) =>
f (Qu u l a) -> f (Qu u l a) -> f (Qu u l a)
|^-^| V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
-> V3
     (Qu
        (Normalize
           (Normalize ('[] @- '[ 'F Length One])
            @@+ Reorder
                  '[ 'F Length One] (Normalize ('[] @- '[ 'F Length One]))))
        'DefaultLCSU
        a)
forall n (b :: [Factor *]) (l :: LCSU *).
Floating n =>
V3 (Qu b l n)
-> V3
     (Qu
        (Normalize
           (Normalize ('[] @- b) @@+ Reorder b (Normalize ('[] @- b))))
        l
        n)
qNormalize V3 (Qu '[ 'F Length One] 'DefaultLCSU a)
Position a
r
  h :: V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
h = StateVectors a
-> V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
forall a.
Num a =>
StateVectors a
-> V3 (Quantity ((Meter :^ Succ (Succ 'Zero)) :/ Second) a)
specificAngularMomentumVector StateVectors a
sv

----------------------------------------------------------------
-- Rotations to and from the orbital plane
----------------------------------------------------------------

-- | Rotate a position relative to the orbital plane according to the
-- inclination specifier and periapsis specifier.
--
-- The orbital plane is perpendicular to the z axis
rotateFromPlane
  :: (Conjugate a, RealFloat a)
  => Orbit a
  -> V3 (Qu u l a)
  -> V3 (Qu u l a)
rotateFromPlane :: Orbit a -> V3 (Qu u l a) -> V3 (Qu u l a)
rotateFromPlane = Quaternion a -> V3 (Qu u l a) -> V3 (Qu u l a)
forall a (q :: * -> *).
(Coercible (q a) a, Conjugate a, RealFloat a) =>
Quaternion a -> V3 (q a) -> V3 (q a)
qRotate (Quaternion a -> V3 (Qu u l a) -> V3 (Qu u l a))
-> (Orbit a -> Quaternion a)
-> Orbit a
-> V3 (Qu u l a)
-> V3 (Qu u l a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Orbit a -> Quaternion a
forall a. RealFloat a => Orbit a -> Quaternion a
orbitalPlaneQuaternion

-- | Rotate a position such that is is relative to the orbital plane according
-- to the inclination specifier and periapsis specifier.
--
-- The orbital plane is perpendicular to the z axis
rotateToPlane
  :: (Conjugate a, RealFloat a) => Orbit a -> V3 (Qu u l a) -> V3 (Qu u l a)
rotateToPlane :: Orbit a -> V3 (Qu u l a) -> V3 (Qu u l a)
rotateToPlane = Quaternion a -> V3 (Qu u l a) -> V3 (Qu u l a)
forall a (q :: * -> *).
(Coercible (q a) a, Conjugate a, RealFloat a) =>
Quaternion a -> V3 (q a) -> V3 (q a)
qRotate (Quaternion a -> V3 (Qu u l a) -> V3 (Qu u l a))
-> (Orbit a -> Quaternion a)
-> Orbit a
-> V3 (Qu u l a)
-> V3 (Qu u l a)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Quaternion a -> Quaternion a
forall a. Conjugate a => a -> a
conjugate (Quaternion a -> Quaternion a)
-> (Orbit a -> Quaternion a) -> Orbit a -> Quaternion a
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Orbit a -> Quaternion a
forall a. RealFloat a => Orbit a -> Quaternion a
orbitalPlaneQuaternion

-- | A quaternion representing the rotation of the orbital plane
orbitalPlaneQuaternion :: RealFloat a => Orbit a -> Quaternion a
orbitalPlaneQuaternion :: Orbit a -> Quaternion a
orbitalPlaneQuaternion o :: Orbit a
o = Quaternion a
lon Quaternion a -> Quaternion a -> Quaternion a
forall a. Num a => a -> a -> a
* Quaternion a
per
 where
  per :: Quaternion a
per = case Orbit a -> PeriapsisSpecifier a
forall a. Orbit a -> PeriapsisSpecifier a
periapsisSpecifier Orbit a
o of
    Eccentric ω :: Angle a
ω -> Angle a -> Quaternion a
forall a. Floating a => Angle a -> Quaternion a
rotateZ Angle a
ω
    Circular    -> 1
  lon :: Quaternion a
lon = case Orbit a -> InclinationSpecifier a
forall a. Orbit a -> InclinationSpecifier a
inclinationSpecifier Orbit a
o of
    Inclined _Ω :: Angle a
_Ω i :: Angle a
i -> Angle a -> Quaternion a
forall a. Floating a => Angle a -> Quaternion a
rotateZ Angle a
_Ω Quaternion a -> Quaternion a -> Quaternion a
forall a. Num a => a -> a -> a
* Angle a -> Quaternion a
forall a. Floating a => Angle a -> Quaternion a
rotateX Angle a
i
    NonInclined   -> 1

----------------------------------------------------------------
-- Orbit Utils
----------------------------------------------------------------

-- | Get the flight path angle, φ, of a body a a specific true anomaly. This is
-- the angle of the body's motion relative to a vector perpendicular to the
-- radius.
flightPathAngleAtTrueAnomaly
  :: (Real a, Floating a) => Orbit a -> Angle a -> Angle a
flightPathAngleAtTrueAnomaly :: Orbit a -> Angle a -> Angle a
flightPathAngleAtTrueAnomaly o :: Orbit a
o ν :: Angle a
ν = Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
sign (Unitless a -> Angle a
forall a. Floating a => Unitless a -> Angle a
qArcCos Unitless a
Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
cosφ)
 where
  cosφ :: Qu
  (Normalize
     ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
      @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
  'DefaultLCSU
  a
cosφ = Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
Quantity ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
h Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu '[ 'F Length ('S One), 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length ('S One), 'F Time ('P 'Zero)]
         @- '[ 'F Length ('S One), 'F Time ('P 'Zero)]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Fractional n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @- b)) l n
|/| (Qu '[ 'F Length One] 'DefaultLCSU a
Distance a
r Qu '[ 'F Length One] 'DefaultLCSU a
-> Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
-> Qu
     (Normalize
        ('[ 'F Length One] @+ '[ 'F Length One, 'F Time ('P 'Zero)]))
     'DefaultLCSU
     a
forall n (a :: [Factor *]) (l :: LCSU *) (b :: [Factor *]).
Num n =>
Qu a l n -> Qu b l n -> Qu (Normalize (a @+ b)) l n
|*| Qu '[ 'F Length One, 'F Time ('P 'Zero)] 'DefaultLCSU a
Speed a
v)
  sign :: Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
sign = if (Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
ν Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a (u :: [Factor *]) (l :: LCSU *).
Real a =>
Qu u l a -> Qu u l a -> Qu u l a
`mod'` Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a. Floating a => PlaneAngle a
turn) Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a -> Bool
forall a. Ord a => a -> a -> Bool
< Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a. Floating a => PlaneAngle a
halfTurn then Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall a. a -> a
id else Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
-> Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
forall n (d :: [Factor *]) (l :: LCSU *).
Num n =>
Qu d l n -> Qu d l n
qNegate
  r :: Distance a
r    = Orbit a -> Angle a -> Distance a
forall a. (Ord a, Floating a) => Orbit a -> Angle a -> Distance a
radiusAtTrueAnomaly Orbit a
o Angle a
ν
  v :: Speed a
v    = Orbit a -> Angle a -> Speed a
forall a. (Ord a, Floating a) => Orbit a -> Angle a -> Speed a
speedAtTrueAnomaly Orbit a
o Angle a
ν
  h :: Quantity ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
h    = Orbit a
-> Quantity
     ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
forall a.
Floating a =>
Orbit a
-> Quantity
     ((Meter :^ Succ (Succ 'Zero)) :* (Second :^ Pred 'Zero)) a
specificAngularMomentum Orbit a
o

----------------------------------------------------------------
-- Utils
----------------------------------------------------------------

qRotate
  :: forall a q
   . (Coercible (q a) a, Conjugate a, RealFloat a)
  => Quaternion a
  -> V3 (q a)
  -> V3 (q a)
qRotate :: Quaternion a -> V3 (q a) -> V3 (q a)
qRotate = (Quaternion a -> V3 a -> V3 a)
-> Quaternion a -> V3 (q a) -> V3 (q a)
forall a b. Coercible a b => a -> b
coerce ((Conjugate a, RealFloat a) => Quaternion a -> V3 a -> V3 a
forall a.
(Conjugate a, RealFloat a) =>
Quaternion a -> V3 a -> V3 a
rotate @a)

rotateX :: Floating a => Angle a -> Quaternion a
rotateX :: Angle a -> Quaternion a
rotateX θ :: Angle a
θ = a -> V3 a -> Quaternion a
forall a. a -> V3 a -> Quaternion a
Quaternion (a -> a
forall a. Floating a => a -> a
cos a
half) (a -> a -> a -> V3 a
forall a. a -> a -> a -> V3 a
V3 (a -> a
forall a. Floating a => a -> a
sin a
half) 0 0)
  where half :: a
half = (Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
θ Qu '[ 'F PlaneAngle One] 'DefaultLCSU a -> Radian -> a
forall (dim :: [Factor *]) unit n.
(ValidDLU dim 'DefaultLCSU unit, Fractional n) =>
Qu dim 'DefaultLCSU n -> unit -> n
# [si|rad|]) a -> a -> a
forall a. Fractional a => a -> a -> a
/ 2

_rotateY :: Floating a => Angle a -> Quaternion a
_rotateY :: Angle a -> Quaternion a
_rotateY θ :: Angle a
θ = a -> V3 a -> Quaternion a
forall a. a -> V3 a -> Quaternion a
Quaternion (a -> a
forall a. Floating a => a -> a
cos a
half) (a -> a -> a -> V3 a
forall a. a -> a -> a -> V3 a
V3 0 (a -> a
forall a. Floating a => a -> a
sin a
half) 0)
  where half :: a
half = (Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
θ Qu '[ 'F PlaneAngle One] 'DefaultLCSU a -> Radian -> a
forall (dim :: [Factor *]) unit n.
(ValidDLU dim 'DefaultLCSU unit, Fractional n) =>
Qu dim 'DefaultLCSU n -> unit -> n
# [si|rad|]) a -> a -> a
forall a. Fractional a => a -> a -> a
/ 2

rotateZ :: Floating a => Angle a -> Quaternion a
rotateZ :: Angle a -> Quaternion a
rotateZ θ :: Angle a
θ = a -> V3 a -> Quaternion a
forall a. a -> V3 a -> Quaternion a
Quaternion (a -> a
forall a. Floating a => a -> a
cos a
half) (a -> a -> a -> V3 a
forall a. a -> a -> a -> V3 a
V3 0 0 (a -> a
forall a. Floating a => a -> a
sin a
half))
  where half :: a
half = (Qu '[ 'F PlaneAngle One] 'DefaultLCSU a
Angle a
θ Qu '[ 'F PlaneAngle One] 'DefaultLCSU a -> Radian -> a
forall (dim :: [Factor *]) unit n.
(ValidDLU dim 'DefaultLCSU unit, Fractional n) =>
Qu dim 'DefaultLCSU n -> unit -> n
# [si|rad|]) a -> a -> a
forall a. Fractional a => a -> a -> a
/ 2

{-


orbitalPlaneQuaternion :: Orbit -> Quaternion Double
orbitalPlaneQuaternion Elliptic{..} = l * p
  where p = case periapsisSpecifier of
              Eccentric ω -> rotateZ ω
              Circular -> noRotation
        l = case longitudeSpecifier of
              Inclined{..} -> rotateZ longitudeOfAscendingNode * rotateX inclination
              NonInclined -> noRotation

rotateToWorld :: Orbit -> V3 Double -> V3 Double
rotateToWorld orbit = rotate (orbitalPlaneQuaternion orbit)

rotateToPlane :: Orbit -> V3 Double -> V3 Double
rotateToPlane orbit = rotate (conjugate (orbitalPlaneQuaternion orbit))

positionAtTrueAnomaly :: Orbit -> Angle -> V3 Double
positionAtTrueAnomaly orbit trueAnomaly = rotateToWorld orbit r
  where ν = trueAnomaly
        d = radiusAtTrueAnomaly orbit ν
        r = V3 (cos ν) (sin ν) 0 ^* d

velocityAtTrueAnomaly :: Orbit -> Angle -> V3 Double
velocityAtTrueAnomaly orbit trueAnomaly = rotateToWorld orbit v
  where ν = trueAnomaly
        μ = primaryGravitationalParameter orbit
        e = eccentricity orbit
        h = sqrt (μ * a * (1 - e^2))
        a = semiMajorAxis orbit
        r = radiusAtTrueAnomaly orbit trueAnomaly
        vr = μ * e * sin ν / h
        vtA = h / r
        v = V3 (vr * cos ν - vtA * sin ν) (vr * sin ν + vtA * cos ν) 0

trueAnomalyAtPosition :: Orbit -> V3 Double -> Angle
trueAnomalyAtPosition orbit r = ν
  where V3 x y _ = rotateToPlane orbit r
        ν = atan2 y x

-- also equal to sqrt(μ/a^3)
averageAngularVelocity :: Orbit -> Angle
averageAngularVelocity orbit = 2 * pi / p
  where p = period orbit

distance :: Orbit -> Angle -> Distance
distance orbit@Elliptic{..} trueAnomaly = semilatusRectum orbit / (1 + eccentricity * cos trueAnomaly)


{-
eccentricAnomaly :: Orbit -> Angle -> Angle
eccentricAnomaly Elliptic{..} trueAnomaly = acos ((eccentricity + cosTrue)/(1 + eccentricity * cosTrue))
  where cosTrue = cos trueAnomaly

meanAnomaly :: Orbit -> Angle -> Angle
meanAnomaly orbit@Elliptic{..} trueAnomaly = e - eccentricity * sin e
  where e = eccentricAnomaly orbit trueAnomaly
  -}

eccentricityVector :: Orbit -> Angle -> V3 Double
eccentricityVector orbit trueAnomaly = eccentricityVectorFromState μ sv
  where sv = stateVectorsFromOrbit orbit trueAnomaly
        μ = primaryGravitationalParameter orbit

eccentricityVectorFromState :: Double -> StateVectors -> V3 Double
eccentricityVectorFromState primaryGravitationalParameter StateVectors{..} = (v `cross` h) ^/ μ - normalize r
  where μ = primaryGravitationalParameter
        r = position
        v = velocity
        h = r `cross` v

trueAnomalyFromState :: Orbit -> StateVectors -> Angle
trueAnomalyFromState orbit stateVectors = if r `dot` v >= 0 then ν else 2 * pi - ν
  where e = eccentricityVectorFromState μ stateVectors
        r = position stateVectors
        v = velocity stateVectors
        ν = acos $ (e `dot` r) / (norm e * norm r)
        μ = primaryGravitationalParameter orbit

orbitalSpeed :: Orbit -> Angle -> Double
orbitalSpeed orbit trueAnomaly = v
  where d = Orbit.distance orbit trueAnomaly
        --ν = trueAnomaly
        μ = primaryGravitationalParameter orbit
        a = semiMajorAxis orbit
        v = if | isElliptic orbit ||
                 isHyperbolic orbit -> sqrt (μ * (2 / d - 1 / a))
               | isParabolic orbit -> sqrt (μ * 2 / d)

velocityAngleFromPrograde :: Orbit -> Angle -> Angle
velocityAngleFromPrograde orbit trueAnomaly = φ
  where ν = trueAnomaly
        e = eccentricity orbit
        φ = if | isElliptic orbit ||
                 isHyperbolic orbit -> atan2 (e * sin ν) (1 + e * cos ν)
               | isParabolic orbit -> ν / 2

-- | Calculate the state vectors relative to the orbital plane
--
-- The Z dimension is perpendicular to the orbital plane and hence is
-- always zero
orbitalPlaneStateVectors :: Orbit -> Angle -> StateVectors
orbitalPlaneStateVectors orbit trueAnomaly = StateVectors r v
  where d = Orbit.distance orbit trueAnomaly
        ν = trueAnomaly
        r = V3 (d * cos ν) (d * sin ν) 0
        e = eccentricity orbit
        --a = semiMajorAxis orbit
        u = V3 (1 + e * cos ν) (e * sin ν) 0
        v = u ^* orbitalSpeed orbit trueAnomaly
        --n = averageAngularVelocity orbit
        --v = V3 (- sin ν) (e + cos ν) 0 ^* (n * a / sqrt (1 - e^2))

rotateX :: Angle -> Quaternion Double
rotateX = axisAngle $ V3 1 0 0

rotateY :: Angle -> Quaternion Double
rotateY = axisAngle $ V3 0 1 0

rotateZ :: Angle -> Quaternion Double
rotateZ = axisAngle $ V3 0 0 1

noRotation :: Num a => Quaternion a
noRotation = Quaternion 1 (V3 0 0 0)

stateVectorsFromOrbit :: Orbit -> Angle -> StateVectors
stateVectorsFromOrbit orbit trueAnomaly = StateVectors r v
  where ν = trueAnomaly
        r = positionAtTrueAnomaly orbit ν
        v = velocityAtTrueAnomaly orbit ν
        {-
        o = orbitalPlaneStateVectors orbit trueAnomaly
        r' = position o
        v' = velocity o
        p = case periapsisSpecifier of
              Eccentric ω -> rotateZ ω
              Circular -> noRotation
        l = case longitudeSpecifier of
              Inclined{..} -> rotateZ longitudeOfAscendingNode * rotateX inclination
              NonInclined -> noRotation
        r = (l * p) `rotate` r'
        v = (l * p) `rotate` v'
        -}

orbitFromStateVectors :: StateVectors -> Double -> (Orbit, Angle)
orbitFromStateVectors sv@StateVectors{..} primaryGravitationalParameter = (orbit, ν)
  where r = position
        v = velocity
        μ = primaryGravitationalParameter
        -- `h` is the specific relative angular momentum
        h@(V3 _ _ hz) = r `cross` v
        -- `an` is the vector pointing towards the ascending node
        -- Todo, handle inclinations of 90 degrees here
        an@(V3 anx any _) = let an' = V3 0 0 1 `cross` h
                            in if nearZero (norm an') then V3 1 0 0 else an'
        -- `ev` is the eccentricity vector
        ev@(V3 evx evy evz) = eccentricityVectorFromState μ sv
        -- ε is the specific orbital energY
        ε = quadrance v / 2 - μ / norm r
        -- `a` is the semimajor axis
        --a = μ * norm r / (2 * μ - norm r * quadrance v)
        a = let a' = μ / (2 * ε)
            in if | isElliptic orbit ||
                    isHyperbolic orbit -> -a'
                  | isParabolic orbit -> error "parabolic orbits don't have a well defined semi-major axis"
        -- `e` is the eccentricity, Sometimes these numbers come out a tiny
        -- bit negative so clamp with 0
        --e = sqrt (max 0 $ 1 - quadrance h / (μ * a))
        e = norm ev
        -- `i` is the inclination
        i = acos $ hz / norm h
        -- `lan` is the longitude of the ascending node, sometimes known as Ω
        lan = let lan' = acos (anx / norm an)
              in if any >= 0 then lan' else 2 * pi - lan'
        -- `ω` is the argument of periapsis
        ω = let ω' = acos ((an `dot` ev)/(norm an * norm ev))
            in if evz < 0 then 2 * pi - ω' else ω'
        --ω = let ω' = atan2 evy evx
            --in if (r `cross` v < 0) then 2 * pi - ω' else ω'
        -- `ν` is the true anomaly
        ν = let ν' = acos ((ev `dot` r)/(norm ev * norm r))
            in if | isElliptic orbit -> if r `dot` v < 0 then 2 * pi - ν' else ν'
                  | isHyperbolic orbit -> if r `dot` v < 0 then -ν' else ν'
        orbit = Elliptic{ eccentricity = e
               , semiMajorAxis = a
               , longitudeSpecifier = if nearZero i then NonInclined
                                                    else Inclined { inclination = i
                                                                  , longitudeOfAscendingNode = lan
                                                                  }
               , periapsisSpecifier = if nearZero e then Circular
                                                    else Eccentric{argumentOfPeriapsis = ω}
               , primaryGravitationalParameter = μ}

lambert :: V3 Double -> V3 Double -> Double -> Double -> [(V3 Double, V3 Double)]
lambert r1 r2 primaryGravitationalParameter transferTime = [(v1, v2)]
  where μ = primaryGravitationalParameter
        h = r1 `cross` r2
        cosθ = (r1 `dot` r2) / (norm r1 * norm r2)
        θ = let θ' = acos cosθ
            in if | (h^._z) >= 0 -> θ' -- Todo, fixme
                  | otherwise -> 2 * π - θ'
        d = if | 0 <= θ && θ <= π -> 1
               | π < θ && θ <= 2 * π -> -1
        τ = d * sqrt (norm r1 * norm r2 * (1 + cosθ)) / (norm r1 + norm r2)
        s = sqrt $ ((norm r1 + norm r2)^3) / μ
        n = 0
        wse k = let v = k - sqrt 2
                in sqrt 2/3 - v/5 + 2/35*sqrt 2*v^2 - 2/63*v^3 + 2/231*sqrt 2*v^4 -
                   2/429*v^5 + 8/6435*sqrt 2*v^6 - 8/12155*v^7 + 8/46189*sqrt 2*v^8 -
                   8/88179*v^9 + 16/676039*sqrt 2*v^10 - 16/1300075*v^11 +
                   16/5014575*sqrt 2*v^12 - 16/9694845*v^13 +
                   128/300540195*sqrt 2*v^14 - 128/583401555*v^15 +
                   128/2268783825*sqrt 2*v^16
        tof n k = (tofk, tof'k, tof''k)
                  where tofk = s * sqrt (1 - k * τ) * (τ + (1 - k * τ) * w) -- 26
                        tof'k = -tofk / (2 * c) + s * τ * sqrt (c * τ) * (w' * c - w)
                        tof''k = -tofk / (4 * c^2) + s * τ * sqrt (c * τ) * (w / c + c * w'' - 3 * w')
                        c = (1 - k * τ) / τ
                        ε = 2e-2
                        w = if | k < sqrt 2 - ε ->
                                  ((1 - signum k) * π + signum k * acos (1 - m) + 2 * π * n) /
                                  sqrt (m^3) -
                                  k/m
                               | k > sqrt 2 + ε -> - acosh (1 - m) / sqrt (-m^3) - k / m
                               | otherwise -> ws -- 27
                        w' = if | k < sqrt 2 - ε -> (-2 + 3 * w * k) / m
                                | k > sqrt 2 + ε -> (-2 + 3 * w * k) / (-m)
                                | otherwise -> ws'
                        w'' = if | k < sqrt 2 - ε -> (5 * w' * k + 3 * w) / m
                                 | k > sqrt 2 + ε -> (5 * w' * k + 3 * w) / (-m)
                                 | otherwise -> ws''
                        (ws:ws':ws'':_) = diffs wse k
                        m = 2 - k^2
        --Right k = traceShowId $ newton (\k -> let (a, b, _) = tof (traceShowId k) in (a - transferTime, b)) (-sqrt 2) (sqrt 2) 1e-6
        initialGuess = 0
        --isValid = (&&) <$> (not . isNaN) <*> (-sqrt 2<)
        --ks = filter (isValid . snd) . zip initialGuesses $ halley (\k -> let (a,b,c) = tof n k in (a - transferTime, b, c)) <$> initialGuesses
        --Right k = newton (\k -> let (a,b,_) = tof n k in (a - transferTime, b)) (-sqrt 2) (sqrt 2) 1e-6
        --k = -1.414284878632464
        --k = snd . head $ ks
        k = halley (\k -> let (a,b,c) = tof n k in (a - transferTime, b, c)) initialGuess
        --kMinTime n = (\(Right y) -> y) $ newton (\k -> let (_, y',y'') = tof n k in (y', y'')) (-1) 1 1e-6
        --kbs = kMinTime <$> [1..]
        --tbs = (^._1) . uncurry tof <$> zip [1..] kbs
        f = 1 - (1 - k * τ) * (norm r1 + norm r2) / norm r1 -- 1 - (1 - k * τ) / norm r1
        --g' = 1 - (1 - k * τ) / norm r2
        g' = 1 - (1 - k * τ) * (norm r1 + norm r2) / norm r2
        --g = s * τ * sqrt ((1 - k * τ) * μ) -- τ * (norm r1 + norm r2) * sqrt (1 - k * τ)
        g = s * τ * sqrt (1 - k * τ)
        v1 = (r2 - f *^ r1) ^/ g -- ^* sqrt μ
        v2 = (g' *^ r2 - r1) ^/ g
        {-debugInfo = "kbs: " ++ show (take 5 kbs) ++
                    "\ntbs: " ++ show (take 5 tbs) ++
                    -- "\nks: " ++ show ks ++
                    "\nd: " ++ show d ++
                    "\nτ (tau): " ++ show τ ++
                    "\nθ (theta): " ++ show θ ++
                    "\nk: " ++ show k ++
                    "\nn: " ++ show n ++
                    "\ntof: " ++ show (tof n k) ++
                    "\nf: " ++ show f ++
                    "\ng: " ++ show g ++
                    "\nr1: " ++ show r1 ++
                    "\nr2: " ++ show r2 ++
                    "\nv1: " ++ show v1 ++
                    "\nv2: " ++ show v2 ++
                    "\norbits: "-}
        (orbit1, ν1) = traceShowId $ Debug.Trace.trace debugInfo $ orbitFromStateVectors (StateVectors r1 v1) μ
        (orbit2, ν2) = traceShowId $ orbitFromStateVectors (StateVectors r2 v2) μ
        ma1 = meanAnomalyAtTrueAnomaly orbit1 ν1
        ma2 = meanAnomalyAtTrueAnomaly orbit2 ν2

isValid :: V3 Double -> Bool
isValid = noneOf each isNaN

ballisticTransfer :: (Double -> StateVectors) -> (Double -> StateVectors) -> Double -> Double -> Double -> Double -> (Burn, Burn)
ballisticTransfer fo1 fo2 primaryGravitationalParameter departureMin departureMax maxTransferTime = (b1, b2)
  where (b1, b2, _) = minimumBy (compare `on` (^._3)) ts
        ts = do let numDepartureSamples = 100
                    numArrivalSamples = 100
                    departureInterval = departureMax - departureMin
                d <- [0..numDepartureSamples-1]
                a <- [0..numArrivalSamples-1]
                let departureTime = departureMin + departureInterval * d / (numDepartureSamples - 1)
                    transferTime = maxTransferTime * a / (numArrivalSamples - 1)
                    arrivalTime = departureTime + transferTime
                    StateVectors{position = r1, velocity = v1} = fo1 departureTime
                    StateVectors{position = r2, velocity = v2} = fo2 arrivalTime
                (v1', v2') <- lambert r1 r2 primaryGravitationalParameter transferTime
                guard $ noneOf each isNaN v1'
                guard $ noneOf each isNaN v2'
                let b1 = v1' - v1
                    b2 = v2' - v2
                    δv1 = norm b1
                    δv2 = norm b2
                    δv = δv1 + δv2
                pure (Burn departureTime b1, Burn arrivalTime b2, δv)

toManoeuvreReferenceFrame :: Orbit -> Angle -> V3 Double -> V3 Double
toManoeuvreReferenceFrame orbit trueAnomaly = (m !*)
  where ν = trueAnomaly
        r = normalize $ positionAtTrueAnomaly orbit ν
        v = normalize $ velocityAtTrueAnomaly orbit ν
        prograde = normalize $ v
        normal = normalize $ prograde `cross` (-r)
        radial = normalize $ prograde `cross` normal
        m = V3 prograde normal radial

-}