-- |

-- Module:      Data.Geo.Jord.Kinematics

-- Copyright:   (c) 2020 Cedric Liegeois

-- License:     BSD3

-- Maintainer:  Cedric Liegeois <ofmooseandmen@yahoo.fr>

-- Stability:   experimental

-- Portability: portable

--

-- Types and functions for working with kinematics calculations assuming a __spherical__ celestial body.

--

-- In order to use this module you should start with the following imports:

--

-- @

-- import qualified Data.Geo.Jord.Geodetic as Geodetic

-- import qualified Data.Geo.Jord.Kinematics as Kinematics

-- @

--

-- All functions are implemented using the vector-based approached described in

-- <http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf Gade, K. (2010). A Non-singular Horizontal Position Representation>

-- and in <https://calhoun.nps.edu/bitstream/handle/10945/29516/sometacticalalgo00shud.pdf Shudde, Rex H. (1986). Some tactical algorithms for spherical geometry>

--

module Data.Geo.Jord.Kinematics
    (
    -- * The 'Track' type.

      Track(..)
    -- * The 'Course' type.

    , Course
    -- * The 'Cpa' type.

    , Cpa
    , timeToCpa
    , distanceAtCpa
    , cpaOwnshipPosition
    , cpaIntruderPosition
    -- * The 'Intercept' type.

    , Intercept
    , timeToIntercept
    , distanceToIntercept
    , interceptPosition
    , interceptorBearing
    , interceptorSpeed
    -- * Calculations

    , course
    , positionAfter
    , positionAfter'
    , trackPositionAfter
    , cpa
    , intercept
    , interceptBySpeed
    , interceptByTime
    -- * re-exported for convenience

    , module Data.Geo.Jord.Duration
    , module Data.Geo.Jord.Speed
    ) where

import Control.Applicative ((<|>))
import Data.Maybe (fromJust, isNothing)

import Data.Geo.Jord.Angle (Angle)
import qualified Data.Geo.Jord.Angle as Angle
import Data.Geo.Jord.Duration (Duration)
import qualified Data.Geo.Jord.Duration as Duration (seconds, toMilliseconds, toSeconds, zero)
import Data.Geo.Jord.Ellipsoid (equatorialRadius)
import Data.Geo.Jord.Geodetic (HorizontalPosition)
import qualified Data.Geo.Jord.Geodetic as Geodetic
import qualified Data.Geo.Jord.GreatCircle as GreatCircle (distance, initialBearing)
import Data.Geo.Jord.Length (Length)
import qualified Data.Geo.Jord.Length as Length (toMetres, zero)
import qualified Data.Geo.Jord.Math3d as Math3d
import Data.Geo.Jord.Model (Spherical, surface)
import Data.Geo.Jord.Speed (Speed)
import qualified Data.Geo.Jord.Speed as Speed (average, toMetresPerSecond)

-- | 'Track' represents the state of a vehicle by its current horizontal position, bearing and speed.

data Track a =
    Track
        { Track a -> HorizontalPosition a
trackPosition :: HorizontalPosition a -- ^ horizontal position of the track.

        , Track a -> Angle
trackBearing :: Angle -- ^ bearing of the track.

        , Track a -> Speed
trackSpeed :: Speed -- ^ speed of the track.

        }
    deriving (Track a -> Track a -> Bool
(Track a -> Track a -> Bool)
-> (Track a -> Track a -> Bool) -> Eq (Track a)
forall a. Model a => Track a -> Track a -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: Track a -> Track a -> Bool
$c/= :: forall a. Model a => Track a -> Track a -> Bool
== :: Track a -> Track a -> Bool
$c== :: forall a. Model a => Track a -> Track a -> Bool
Eq, Int -> Track a -> ShowS
[Track a] -> ShowS
Track a -> String
(Int -> Track a -> ShowS)
-> (Track a -> String) -> ([Track a] -> ShowS) -> Show (Track a)
forall a. Model a => Int -> Track a -> ShowS
forall a. Model a => [Track a] -> ShowS
forall a. Model a => Track a -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Track a] -> ShowS
$cshowList :: forall a. Model a => [Track a] -> ShowS
show :: Track a -> String
$cshow :: forall a. Model a => Track a -> String
showsPrec :: Int -> Track a -> ShowS
$cshowsPrec :: forall a. Model a => Int -> Track a -> ShowS
Show)

-- | 'Course' represents the cardinal direction in which the vehicle is to be steered.

newtype Course =
    Course Math3d.V3
    deriving (Course -> Course -> Bool
(Course -> Course -> Bool)
-> (Course -> Course -> Bool) -> Eq Course
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: Course -> Course -> Bool
$c/= :: Course -> Course -> Bool
== :: Course -> Course -> Bool
$c== :: Course -> Course -> Bool
Eq, Int -> Course -> ShowS
[Course] -> ShowS
Course -> String
(Int -> Course -> ShowS)
-> (Course -> String) -> ([Course] -> ShowS) -> Show Course
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Course] -> ShowS
$cshowList :: [Course] -> ShowS
show :: Course -> String
$cshow :: Course -> String
showsPrec :: Int -> Course -> ShowS
$cshowsPrec :: Int -> Course -> ShowS
Show)

-- | Time to, and distance at, closest point of approach (CPA) as well as position of both tracks at CPA.

data Cpa a =
    Cpa
        { Cpa a -> Duration
timeToCpa :: Duration -- ^ time to CPA.

        , Cpa a -> Length
distanceAtCpa :: Length -- ^ distance at CPA.

        , Cpa a -> HorizontalPosition a
cpaOwnshipPosition :: HorizontalPosition a -- ^ horizontal position of ownship CPA.

        , Cpa a -> HorizontalPosition a
cpaIntruderPosition :: HorizontalPosition a -- ^ horizontal position of intruder at CPA.

        }
    deriving (Cpa a -> Cpa a -> Bool
(Cpa a -> Cpa a -> Bool) -> (Cpa a -> Cpa a -> Bool) -> Eq (Cpa a)
forall a. Model a => Cpa a -> Cpa a -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: Cpa a -> Cpa a -> Bool
$c/= :: forall a. Model a => Cpa a -> Cpa a -> Bool
== :: Cpa a -> Cpa a -> Bool
$c== :: forall a. Model a => Cpa a -> Cpa a -> Bool
Eq, Int -> Cpa a -> ShowS
[Cpa a] -> ShowS
Cpa a -> String
(Int -> Cpa a -> ShowS)
-> (Cpa a -> String) -> ([Cpa a] -> ShowS) -> Show (Cpa a)
forall a. Model a => Int -> Cpa a -> ShowS
forall a. Model a => [Cpa a] -> ShowS
forall a. Model a => Cpa a -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Cpa a] -> ShowS
$cshowList :: forall a. Model a => [Cpa a] -> ShowS
show :: Cpa a -> String
$cshow :: forall a. Model a => Cpa a -> String
showsPrec :: Int -> Cpa a -> ShowS
$cshowsPrec :: forall a. Model a => Int -> Cpa a -> ShowS
Show)

-- | Time, distance and position of intercept as well as speed and initial bearing of interceptor.

data Intercept a =
    Intercept
        { Intercept a -> Duration
timeToIntercept :: Duration -- ^ time to intercept.

        , Intercept a -> Length
distanceToIntercept :: Length -- ^ distance travelled to intercept.

        , Intercept a -> HorizontalPosition a
interceptPosition :: HorizontalPosition a -- ^ horizontal position of intercept.

        , Intercept a -> Angle
interceptorBearing :: Angle -- ^ initial bearing of interceptor.

        , Intercept a -> Speed
interceptorSpeed :: Speed -- ^ speed of interceptor.

        }
    deriving (Intercept a -> Intercept a -> Bool
(Intercept a -> Intercept a -> Bool)
-> (Intercept a -> Intercept a -> Bool) -> Eq (Intercept a)
forall a. Model a => Intercept a -> Intercept a -> Bool
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
/= :: Intercept a -> Intercept a -> Bool
$c/= :: forall a. Model a => Intercept a -> Intercept a -> Bool
== :: Intercept a -> Intercept a -> Bool
$c== :: forall a. Model a => Intercept a -> Intercept a -> Bool
Eq, Int -> Intercept a -> ShowS
[Intercept a] -> ShowS
Intercept a -> String
(Int -> Intercept a -> ShowS)
-> (Intercept a -> String)
-> ([Intercept a] -> ShowS)
-> Show (Intercept a)
forall a. Model a => Int -> Intercept a -> ShowS
forall a. Model a => [Intercept a] -> ShowS
forall a. Model a => Intercept a -> String
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
showList :: [Intercept a] -> ShowS
$cshowList :: forall a. Model a => [Intercept a] -> ShowS
show :: Intercept a -> String
$cshow :: forall a. Model a => Intercept a -> String
showsPrec :: Int -> Intercept a -> ShowS
$cshowsPrec :: forall a. Model a => Int -> Intercept a -> ShowS
Show)

-- | @course p b@ computes the course of a vehicle currently at position @p@ and following bearing @b@.

course :: (Spherical a) => HorizontalPosition a -> Angle -> Course
course :: HorizontalPosition a -> Angle -> Course
course HorizontalPosition a
p Angle
b = V3 -> Course
Course (Double -> Double -> Double -> V3
Math3d.vec3 (V3 -> Double
Math3d.v3z ([V3] -> V3
forall a. [a] -> a
head [V3]
r)) (V3 -> Double
Math3d.v3z ([V3]
r [V3] -> Int -> V3
forall a. [a] -> Int -> a
!! Int
1)) (V3 -> Double
Math3d.v3z ([V3]
r [V3] -> Int -> V3
forall a. [a] -> Int -> a
!! Int
2)))
  where
    lat :: Angle
lat = HorizontalPosition a -> Angle
forall a. HasCoordinates a => a -> Angle
Geodetic.latitude HorizontalPosition a
p
    lon :: Angle
lon = HorizontalPosition a -> Angle
forall a. HasCoordinates a => a -> Angle
Geodetic.longitude HorizontalPosition a
p
    r :: [V3]
r = [V3] -> [V3] -> [V3]
Math3d.dotM ([V3] -> [V3] -> [V3]
Math3d.dotM (Angle -> [V3]
rz (Angle -> Angle
Angle.negate Angle
lon)) (Angle -> [V3]
ry Angle
lat)) (Angle -> [V3]
rx Angle
b)

-- | @positionAfter p b s d@ computes the horizontal position of a vehicle currently at position @p@ following

-- bearing @b@ and travelling at speed @s@ after duration @d@ has elapsed. For example:

--

-- >>> let p = Geodetic.s84Pos 53.321 (-1.729)

-- >>> let b = Angle.decimalDegrees 96.0217

-- >>> let s = Speed.kilometresPerHour 124.8

-- >>> Kinematics.positionAfter p b s (Duration.hours 1)

-- 53°11'19.367"N,0°8'2.456"E (S84)

--

-- This is equivalent to:

--

-- > Kinematics.positionAfter' p (Kinematics.course p b) s d

positionAfter ::
       (Spherical a) => HorizontalPosition a -> Angle -> Speed -> Duration -> HorizontalPosition a
positionAfter :: HorizontalPosition a
-> Angle -> Speed -> Duration -> HorizontalPosition a
positionAfter HorizontalPosition a
p Angle
b Speed
s Duration
d = HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
forall a.
Spherical a =>
HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
position' HorizontalPosition a
p (HorizontalPosition a -> Angle -> Course
forall a. Spherical a => HorizontalPosition a -> Angle -> Course
course HorizontalPosition a
p Angle
b) Speed
s (Duration -> Double
Duration.toSeconds Duration
d)

-- | @positionAfter' p c s d@ computes the horizontal position of a vehicle currently at position @p@ on course @c@ and

-- travelling at speed @s@ after duration @d@ has elapsed.

-- Note: course must have been calculated from position @p@.

positionAfter' ::
       (Spherical a) => HorizontalPosition a -> Course -> Speed -> Duration -> HorizontalPosition a
positionAfter' :: HorizontalPosition a
-> Course -> Speed -> Duration -> HorizontalPosition a
positionAfter' HorizontalPosition a
p Course
c Speed
s Duration
d = HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
forall a.
Spherical a =>
HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
position' HorizontalPosition a
p Course
c Speed
s (Duration -> Double
Duration.toSeconds Duration
d)

-- | @trackPositionAfter t d@ computes the horizontal position of a track @t@ after duration @d@ has elapsed. For example:

--

-- >>> let p = Geodetic.s84Pos 53.321 (-1.729)

-- >>> let b = Angle.decimalDegrees 96.0217

-- >>> let s = Speed.kilometresPerHour 124.8

-- >>> Kinematics.trackPositionAfter (Kinematics.Track p b s) (Duration.hours 1)

-- 53°11'19.367"N,0°8'2.456"E (S84)

trackPositionAfter :: (Spherical a) => Track a -> Duration -> HorizontalPosition a
trackPositionAfter :: Track a -> Duration -> HorizontalPosition a
trackPositionAfter (Track HorizontalPosition a
p Angle
b Speed
s) = HorizontalPosition a
-> Course -> Speed -> Duration -> HorizontalPosition a
forall a.
Spherical a =>
HorizontalPosition a
-> Course -> Speed -> Duration -> HorizontalPosition a
positionAfter' HorizontalPosition a
p (HorizontalPosition a -> Angle -> Course
forall a. Spherical a => HorizontalPosition a -> Angle -> Course
course HorizontalPosition a
p Angle
b) Speed
s

-- | @cpa ownship intruder@ computes the closest point of approach between tracks @ownship@ and @intruder@.

-- The closest point of approach is calculated assuming both ships maintain a constant course and speed.

--

-- >>> let ownship = Kinematics.Track (Geodetic.s84Pos 20 (-60)) (Angle.decimalDegrees 10) (Speed.knots 15)

-- >>> let intruder = Kinematics.Track (Geodetic.s84Pos 34 (-50)) (Angle.decimalDegrees 220) (Speed.knots 300)

-- >>> let cpa = Kinematics.cpa ownship intruder

-- Just (Cpa { timeToCpa = 3H9M56.155S

--           , distanceAtCpa = 124.231730834km

--           , cpaOwnshipPosition = 20°46'43.641"N,59°51'11.225"W (S84)

--           , cpaIntruderPosition = 21°24'8.523"N,60°50'48.159"W (S84)})

cpa :: (Spherical a) => Track a -> Track a -> Maybe (Cpa a)
cpa :: Track a -> Track a -> Maybe (Cpa a)
cpa (Track HorizontalPosition a
p1 Angle
b1 Speed
s1) (Track HorizontalPosition a
p2 Angle
b2 Speed
s2)
    | HorizontalPosition a
p1 HorizontalPosition a -> HorizontalPosition a -> Bool
forall a. Eq a => a -> a -> Bool
== HorizontalPosition a
p2 = Cpa a -> Maybe (Cpa a)
forall a. a -> Maybe a
Just (Duration
-> Length -> HorizontalPosition a -> HorizontalPosition a -> Cpa a
forall a.
Duration
-> Length -> HorizontalPosition a -> HorizontalPosition a -> Cpa a
Cpa Duration
Duration.zero Length
Length.zero HorizontalPosition a
p1 HorizontalPosition a
p2)
    | Double
t Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
0 = Maybe (Cpa a)
forall a. Maybe a
Nothing
    | Bool
otherwise = Cpa a -> Maybe (Cpa a)
forall a. a -> Maybe a
Just (Duration
-> Length -> HorizontalPosition a -> HorizontalPosition a -> Cpa a
forall a.
Duration
-> Length -> HorizontalPosition a -> HorizontalPosition a -> Cpa a
Cpa (Double -> Duration
Duration.seconds Double
t) Length
d HorizontalPosition a
cp1 HorizontalPosition a
cp2)
  where
    c1 :: Course
c1 = HorizontalPosition a -> Angle -> Course
forall a. Spherical a => HorizontalPosition a -> Angle -> Course
course HorizontalPosition a
p1 Angle
b1
    c2 :: Course
c2 = HorizontalPosition a -> Angle -> Course
forall a. Spherical a => HorizontalPosition a -> Angle -> Course
course HorizontalPosition a
p2 Angle
b2
    t :: Double
t = HorizontalPosition a
-> Course
-> Speed
-> HorizontalPosition a
-> Course
-> Speed
-> Double
forall a.
Spherical a =>
HorizontalPosition a
-> Course
-> Speed
-> HorizontalPosition a
-> Course
-> Speed
-> Double
timeToCpa' HorizontalPosition a
p1 Course
c1 Speed
s1 HorizontalPosition a
p2 Course
c2 Speed
s2
    cp1 :: HorizontalPosition a
cp1 = HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
forall a.
Spherical a =>
HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
position' HorizontalPosition a
p1 Course
c1 Speed
s1 Double
t
    cp2 :: HorizontalPosition a
cp2 = HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
forall a.
Spherical a =>
HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
position' HorizontalPosition a
p2 Course
c2 Speed
s2 Double
t
    d :: Length
d = HorizontalPosition a -> HorizontalPosition a -> Length
forall a.
Spherical a =>
HorizontalPosition a -> HorizontalPosition a -> Length
GreatCircle.distance HorizontalPosition a
cp1 HorizontalPosition a
cp2

-- | @intercept t p@ computes the __minimum__ speed of interceptor at position @p@ needed for an intercept with target

-- track @t@ to take place. Intercept time, position, distance and interceptor bearing are derived from this minimum

-- speed. For example:

--

-- >>> let t = Kinematics.Track (Geodetic.s84Pos 34 (-50)) (Angle.decimalDegrees 220) (Speed.knots 600)

-- >>> let ip = Geodetic.s84Pos 20 (-60)

-- >>> Kinematics.intercept t ip

-- Just (Intercept { timeToIntercept = 1H39M53.831S

--                 , distanceToIntercept = 162.294627463km

--                 , interceptPosition = 20°43'42.305"N,61°20'56.848"W (S84)

--                 , interceptorBearing = 300°10'18.053"

--                 , interceptorSpeed = 97.476999km/h})

--

-- Returns 'Nothing' if intercept cannot be achieved e.g.:

--

--     * interceptor and target are at the same position

--

--     * interceptor is "behind" the target

--

intercept :: (Spherical a) => Track a -> HorizontalPosition a -> Maybe (Intercept a)
intercept :: Track a -> HorizontalPosition a -> Maybe (Intercept a)
intercept Track a
t HorizontalPosition a
p = Track a -> HorizontalPosition a -> Duration -> Maybe (Intercept a)
forall a.
Spherical a =>
Track a -> HorizontalPosition a -> Duration -> Maybe (Intercept a)
interceptByTime Track a
t HorizontalPosition a
p (Double -> Duration
Duration.seconds (Track a -> HorizontalPosition a -> Double
forall a. Spherical a => Track a -> HorizontalPosition a -> Double
timeToIntercept' Track a
t HorizontalPosition a
p))

-- | @interceptBySpeed t p s@ computes the time needed by interceptor at position @p@ and travelling at speed @s@ to

-- intercept target track @t@.

--

-- Returns 'Nothing' if intercept cannot be achieved e.g.:

--

--     * interceptor and target are at the same position

--

--     * interceptor speed is below minimum speed returned by 'intercept'

interceptBySpeed :: (Spherical a) => Track a -> HorizontalPosition a -> Speed -> Maybe (Intercept a)
interceptBySpeed :: Track a -> HorizontalPosition a -> Speed -> Maybe (Intercept a)
interceptBySpeed Track a
t HorizontalPosition a
p Speed
s
    | Maybe (Intercept a) -> Bool
forall a. Maybe a -> Bool
isNothing Maybe (Intercept a)
minInt = Maybe (Intercept a)
forall a. Maybe a
Nothing
    | (Intercept a -> Speed) -> Maybe (Intercept a) -> Maybe Speed
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Intercept a -> Speed
forall a. Intercept a -> Speed
interceptorSpeed Maybe (Intercept a)
minInt Maybe Speed -> Maybe Speed -> Bool
forall a. Eq a => a -> a -> Bool
== Speed -> Maybe Speed
forall a. a -> Maybe a
Just Speed
s = Maybe (Intercept a)
minInt
    | Bool
otherwise = Track a -> HorizontalPosition a -> Duration -> Maybe (Intercept a)
forall a.
Spherical a =>
Track a -> HorizontalPosition a -> Duration -> Maybe (Intercept a)
interceptByTime Track a
t HorizontalPosition a
p (Double -> Duration
Duration.seconds (Track a -> HorizontalPosition a -> Speed -> Double
forall a.
Spherical a =>
Track a -> HorizontalPosition a -> Speed -> Double
timeToInterceptSpeed Track a
t HorizontalPosition a
p Speed
s))
  where
    minInt :: Maybe (Intercept a)
minInt = Track a -> HorizontalPosition a -> Maybe (Intercept a)
forall a.
Spherical a =>
Track a -> HorizontalPosition a -> Maybe (Intercept a)
intercept Track a
t HorizontalPosition a
p

-- | @interceptByTime t p d@ computes the speed of interceptor at position @p@ needed for an intercept with target

-- track @t@ to take place after duration @d@.For example:

--

-- >>> let t = Kinematics.Track (Geodetic.s84Pos 34 (-50)) (Angle.decimalDegrees 220) (Speed.knots 600)

-- >>> let ip = Geodetic.s84Pos 20 (-60)

-- >>> let d = Duration.seconds 2700

-- >>> interceptByTime t ip d

-- Just (Intercept { timeToIntercept = 0H45M0.000S

--                 , distanceToIntercept = 1015.302358852km

--                 , interceptPosition = 28°8'12.046"N,55°27'21.411"W (S84)

--                 , interceptorBearing = 26°7'11.649"

--                 , interceptorSpeed = 1353.736478km/h})

--

-- Returns 'Nothing' if given duration is <= 0 or interceptor and target are at the same position. Contrary to

-- 'intercept' and 'interceptBySpeed' this function handles cases where the interceptor has to catch up the target.

interceptByTime ::
       (Spherical a) => Track a -> HorizontalPosition a -> Duration -> Maybe (Intercept a)
interceptByTime :: Track a -> HorizontalPosition a -> Duration -> Maybe (Intercept a)
interceptByTime Track a
t HorizontalPosition a
p Duration
d
    | Duration -> Int
Duration.toMilliseconds Duration
d Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
0 = Maybe (Intercept a)
forall a. Maybe a
Nothing
    | Track a -> HorizontalPosition a
forall a. Track a -> HorizontalPosition a
trackPosition Track a
t HorizontalPosition a -> HorizontalPosition a -> Bool
forall a. Eq a => a -> a -> Bool
== HorizontalPosition a
p = Maybe (Intercept a)
forall a. Maybe a
Nothing
    | Maybe Angle -> Bool
forall a. Maybe a -> Bool
isNothing Maybe Angle
ib = Maybe (Intercept a)
forall a. Maybe a
Nothing
    | Bool
otherwise =
        let is :: Speed
is = Length -> Duration -> Speed
Speed.average Length
idist Duration
d
         in Intercept a -> Maybe (Intercept a)
forall a. a -> Maybe a
Just (Duration
-> Length -> HorizontalPosition a -> Angle -> Speed -> Intercept a
forall a.
Duration
-> Length -> HorizontalPosition a -> Angle -> Speed -> Intercept a
Intercept Duration
d Length
idist HorizontalPosition a
ipos (Maybe Angle -> Angle
forall a. HasCallStack => Maybe a -> a
fromJust Maybe Angle
ib) Speed
is)
  where
    ipos :: HorizontalPosition a
ipos = Track a -> Duration -> HorizontalPosition a
forall a.
Spherical a =>
Track a -> Duration -> HorizontalPosition a
trackPositionAfter Track a
t Duration
d
    idist :: Length
idist = HorizontalPosition a -> HorizontalPosition a -> Length
forall a.
Spherical a =>
HorizontalPosition a -> HorizontalPosition a -> Length
GreatCircle.distance HorizontalPosition a
p HorizontalPosition a
ipos
    ib :: Maybe Angle
ib = HorizontalPosition a -> HorizontalPosition a -> Maybe Angle
forall a.
Spherical a =>
HorizontalPosition a -> HorizontalPosition a -> Maybe Angle
GreatCircle.initialBearing HorizontalPosition a
p HorizontalPosition a
ipos Maybe Angle -> Maybe Angle -> Maybe Angle
forall (f :: * -> *) a. Alternative f => f a -> f a -> f a
<|> HorizontalPosition a -> HorizontalPosition a -> Maybe Angle
forall a.
Spherical a =>
HorizontalPosition a -> HorizontalPosition a -> Maybe Angle
GreatCircle.initialBearing HorizontalPosition a
p (Track a -> HorizontalPosition a
forall a. Track a -> HorizontalPosition a
trackPosition Track a
t)

-- private

-- | position from speed course and seconds.

position' ::
       (Spherical a) => HorizontalPosition a -> Course -> Speed -> Double -> HorizontalPosition a
position' :: HorizontalPosition a
-> Course -> Speed -> Double -> HorizontalPosition a
position' HorizontalPosition a
p0 (Course V3
c) Speed
s Double
sec = V3 -> a -> HorizontalPosition a
forall a. Model a => V3 -> a -> HorizontalPosition a
Geodetic.nvectorPos' V3
v1 (HorizontalPosition a -> a
forall a. Model a => HorizontalPosition a -> a
Geodetic.model HorizontalPosition a
p0)
  where
    nv0 :: V3
nv0 = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector HorizontalPosition a
p0
    v1 :: V3
v1 = V3 -> V3 -> Speed -> Double -> Double -> V3
position'' V3
nv0 V3
c Speed
s Double
sec (HorizontalPosition a -> Double
forall a. Spherical a => HorizontalPosition a -> Double
radiusM HorizontalPosition a
p0)

-- | position from course, speed and seconds.

position'' :: Math3d.V3 -> Math3d.V3 -> Speed -> Double -> Double -> Math3d.V3
position'' :: V3 -> V3 -> Speed -> Double -> Double -> V3
position'' V3
v0 V3
c Speed
s Double
sec Double
rm = V3
v1
  where
    a :: Double
a = Speed -> Double
Speed.toMetresPerSecond Speed
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
rm Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sec
    v1 :: V3
v1 = V3 -> V3 -> V3
Math3d.add (V3 -> Double -> V3
Math3d.scale V3
v0 (Double -> Double
forall a. Floating a => a -> a
cos Double
a)) (V3 -> Double -> V3
Math3d.scale V3
c (Double -> Double
forall a. Floating a => a -> a
sin Double
a))

-- | time to CPA.

timeToCpa' ::
       (Spherical a)
    => HorizontalPosition a
    -> Course
    -> Speed
    -> HorizontalPosition a
    -> Course
    -> Speed
    -> Double
timeToCpa' :: HorizontalPosition a
-> Course
-> Speed
-> HorizontalPosition a
-> Course
-> Speed
-> Double
timeToCpa' HorizontalPosition a
p1 (Course V3
c10) Speed
s1 HorizontalPosition a
p2 (Course V3
c20) Speed
s2 = V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double -> Int -> Double
cpaNrRec V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 Double
0 Int
0
  where
    v10 :: V3
v10 = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector HorizontalPosition a
p1
    rm :: Double
rm = HorizontalPosition a -> Double
forall a. Spherical a => HorizontalPosition a -> Double
radiusM HorizontalPosition a
p1
    w1 :: Double
w1 = Speed -> Double
Speed.toMetresPerSecond Speed
s1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
rm
    v20 :: V3
v20 = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector HorizontalPosition a
p2
    w2 :: Double
w2 = Speed -> Double
Speed.toMetresPerSecond Speed
s2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
rm

-- | time to intercept with minimum speed

timeToIntercept' :: (Spherical a) => Track a -> HorizontalPosition a -> Double
timeToIntercept' :: Track a -> HorizontalPosition a -> Double
timeToIntercept' (Track HorizontalPosition a
p2 Angle
b2 Speed
s2) HorizontalPosition a
p1 = Double
-> Double
-> Double
-> (Double -> Double)
-> Double
-> Int
-> Double
intMinNrRec Double
v10v20 Double
v10c2 Double
w2 (V3 -> V3 -> V3 -> Speed -> Double -> Double -> Double
sep V3
v10 V3
v20 V3
c2 Speed
s2 Double
rm) Double
t0 Int
0
  where
    v10 :: V3
v10 = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector HorizontalPosition a
p1
    v20 :: V3
v20 = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector HorizontalPosition a
p2
    (Course V3
c2) = HorizontalPosition a -> Angle -> Course
forall a. Spherical a => HorizontalPosition a -> Angle -> Course
course HorizontalPosition a
p2 Angle
b2
    v10v20 :: Double
v10v20 = V3 -> V3 -> Double
Math3d.dot V3
v10 V3
v20
    v10c2 :: Double
v10c2 = V3 -> V3 -> Double
Math3d.dot V3
v10 V3
c2
    s2mps :: Double
s2mps = Speed -> Double
Speed.toMetresPerSecond Speed
s2
    rm :: Double
rm = HorizontalPosition a -> Double
forall a. Spherical a => HorizontalPosition a -> Double
radiusM HorizontalPosition a
p1
    w2 :: Double
w2 = Double
s2mps Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
rm
    s0 :: Double
s0 = V3 -> V3 -> Double
angleBetweenRadians V3
v10 V3
v20 -- initial angular distance between target and interceptor

    t0 :: Double
t0 = Double
rm Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
s0 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s2mps -- assume target is travelling towards interceptor


-- | time to intercept with speed.

timeToInterceptSpeed :: (Spherical a) => Track a -> HorizontalPosition a -> Speed -> Double
timeToInterceptSpeed :: Track a -> HorizontalPosition a -> Speed -> Double
timeToInterceptSpeed (Track HorizontalPosition a
p2 Angle
b2 Speed
s2) HorizontalPosition a
p1 Speed
s1 =
    Double
-> Double
-> Double
-> Double
-> (Double -> Double)
-> Double
-> Int
-> Double
intSpdNrRec Double
v10v20 Double
v10c2 Double
w1 Double
w2 (V3 -> V3 -> V3 -> Speed -> Double -> Double -> Double
sep V3
v10 V3
v20 V3
c2 Speed
s2 Double
rm) Double
t0 Int
0
  where
    v10 :: V3
v10 = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector HorizontalPosition a
p1
    v20 :: V3
v20 = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector HorizontalPosition a
p2
    (Course V3
c2) = HorizontalPosition a -> Angle -> Course
forall a. Spherical a => HorizontalPosition a -> Angle -> Course
course HorizontalPosition a
p2 Angle
b2
    v10v20 :: Double
v10v20 = V3 -> V3 -> Double
Math3d.dot V3
v10 V3
v20
    v10c2 :: Double
v10c2 = V3 -> V3 -> Double
Math3d.dot V3
v10 V3
c2
    rm :: Double
rm = HorizontalPosition a -> Double
forall a. Spherical a => HorizontalPosition a -> Double
radiusM HorizontalPosition a
p1
    w1 :: Double
w1 = Speed -> Double
Speed.toMetresPerSecond Speed
s1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
rm
    w2 :: Double
w2 = Speed -> Double
Speed.toMetresPerSecond Speed
s2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
rm
    t0 :: Double
t0 = Double
0.1

rx :: Angle -> [Math3d.V3]
rx :: Angle -> [V3]
rx Angle
a = [Double -> Double -> Double -> V3
Math3d.vec3 Double
1 Double
0 Double
0, Double -> Double -> Double -> V3
Math3d.vec3 Double
0 Double
c Double
s, Double -> Double -> Double -> V3
Math3d.vec3 Double
0 (-Double
s) Double
c]
  where
    c :: Double
c = Angle -> Double
Angle.cos Angle
a
    s :: Double
s = Angle -> Double
Angle.sin Angle
a

ry :: Angle -> [Math3d.V3]
ry :: Angle -> [V3]
ry Angle
a = [Double -> Double -> Double -> V3
Math3d.vec3 Double
c Double
0 (-Double
s), Double -> Double -> Double -> V3
Math3d.vec3 Double
0 Double
1 Double
0, Double -> Double -> Double -> V3
Math3d.vec3 Double
s Double
0 Double
c]
  where
    c :: Double
c = Angle -> Double
Angle.cos Angle
a
    s :: Double
s = Angle -> Double
Angle.sin Angle
a

rz :: Angle -> [Math3d.V3]
rz :: Angle -> [V3]
rz Angle
a = [Double -> Double -> Double -> V3
Math3d.vec3 Double
c Double
s Double
0, Double -> Double -> Double -> V3
Math3d.vec3 (-Double
s) Double
c Double
0, Double -> Double -> Double -> V3
Math3d.vec3 Double
0 Double
0 Double
1]
  where
    c :: Double
c = Angle -> Double
Angle.cos Angle
a
    s :: Double
s = Angle -> Double
Angle.sin Angle
a

cpaA :: Math3d.V3 -> Math3d.V3 -> Double -> Math3d.V3 -> Math3d.V3 -> Double -> Double
cpaA :: V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaA V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 =
    Double -> Double
forall a. Num a => a -> a
negate (V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
v10 Double
w1) V3
c20 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
v20 Double
w2) V3
c10)

cpaB :: Math3d.V3 -> Math3d.V3 -> Double -> Math3d.V3 -> Math3d.V3 -> Double -> Double
cpaB :: V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaB V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 =
    V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
c10 Double
w1) V3
v20 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
c20 Double
w2) V3
v10

cpaC :: Math3d.V3 -> Math3d.V3 -> Double -> Math3d.V3 -> Math3d.V3 -> Double -> Double
cpaC :: V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaC V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 =
    Double -> Double
forall a. Num a => a -> a
negate (V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
v10 Double
w1) V3
v20 Double -> Double -> Double
forall a. Num a => a -> a -> a
- V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
c20 Double
w2) V3
c10)

cpaD :: Math3d.V3 -> Math3d.V3 -> Double -> Math3d.V3 -> Math3d.V3 -> Double -> Double
cpaD :: V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaD V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 =
    V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
c10 Double
w1) V3
c20 Double -> Double -> Double
forall a. Num a => a -> a -> a
- V3 -> V3 -> Double
Math3d.dot (V3 -> Double -> V3
Math3d.scale V3
v20 Double
w2) V3
v10

cpaFt :: Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double -> Double
cpaFt :: Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
cpaFt Double
cw1t Double
cw2t Double
sw1t Double
sw2t Double
a Double
b Double
c Double
d =
    Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw2t

cpaDft ::
       Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
    -> Double
cpaDft :: Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
cpaDft Double
w1 Double
w2 Double
cw1t Double
cw2t Double
sw1t Double
sw2t Double
a Double
b Double
c Double
d =
    Double -> Double
forall a. Num a => a -> a
negate ((Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w1) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw2t) Double -> Double -> Double
forall a. Num a => a -> a -> a
+ (Double
d Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
c Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w1) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
+
    (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w1) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
-
    (Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w1) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cw1t Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sw2t

cpaStep :: Math3d.V3 -> Math3d.V3 -> Double -> Math3d.V3 -> Math3d.V3 -> Double -> Double -> Double
cpaStep :: V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double -> Double
cpaStep V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 Double
t =
    Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
cpaFt Double
cw1t Double
cw2t Double
sw1t Double
sw2t Double
a Double
b Double
c Double
d Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Double
cpaDft Double
w1 Double
w2 Double
cw1t Double
cw2t Double
sw1t Double
sw2t Double
a Double
b Double
c Double
d
  where
    cw1t :: Double
cw1t = Double -> Double
forall a. Floating a => a -> a
cos (Double
w1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
t)
    cw2t :: Double
cw2t = Double -> Double
forall a. Floating a => a -> a
cos (Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
t)
    sw1t :: Double
sw1t = Double -> Double
forall a. Floating a => a -> a
sin (Double
w1 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
t)
    sw2t :: Double
sw2t = Double -> Double
forall a. Floating a => a -> a
sin (Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
t)
    a :: Double
a = V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaA V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2
    b :: Double
b = V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaB V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2
    c :: Double
c = V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaC V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2
    d :: Double
d = V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double
cpaD V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2

-- | Newton-Raphson for CPA time.

cpaNrRec ::
       Math3d.V3
    -> Math3d.V3
    -> Double
    -> Math3d.V3
    -> Math3d.V3
    -> Double
    -> Double
    -> Int
    -> Double
cpaNrRec :: V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double -> Int -> Double
cpaNrRec V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 Double
ti Int
i
    | Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
50 = -Double
1.0 -- no convergence

    | Double -> Double
forall a. Num a => a -> a
abs Double
fi Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-11 = Double
ti1
    | Bool
otherwise = V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double -> Int -> Double
cpaNrRec V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 Double
ti1 (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
  where
    fi :: Double
fi = V3 -> V3 -> Double -> V3 -> V3 -> Double -> Double -> Double
cpaStep V3
v10 V3
c10 Double
w1 V3
v20 V3
c20 Double
w2 Double
ti
    ti1 :: Double
ti1 = Double
ti Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
fi

-- | Newton-Raphson for min speed intercept.

intMinNrRec :: Double -> Double -> Double -> (Double -> Double) -> Double -> Int -> Double
intMinNrRec :: Double
-> Double
-> Double
-> (Double -> Double)
-> Double
-> Int
-> Double
intMinNrRec Double
v10v20 Double
v10c2 Double
w2 Double -> Double
st Double
ti Int
i
    | Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
50 = -Double
1.0 -- no convergence

    | Double -> Double
forall a. Num a => a -> a
abs Double
fi Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-11 = Double
ti1
    | Bool
otherwise = Double
-> Double
-> Double
-> (Double -> Double)
-> Double
-> Int
-> Double
intMinNrRec Double
v10v20 Double
v10c2 Double
w2 Double -> Double
st Double
ti1 (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
  where
    cosw2t :: Double
cosw2t = Double -> Double
forall a. Floating a => a -> a
cos (Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ti)
    sinw2t :: Double
sinw2t = Double -> Double
forall a. Floating a => a -> a
sin (Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ti)
    v10dv2dt :: Double
v10dv2dt = (-Double
w2) Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
v10v20 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
v10c2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cosw2t)
    v10d2v2dt2 :: Double
v10d2v2dt2 = (-Double
1.0 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
w2) Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
v10v20 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cosw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v10c2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinw2t)
    si :: Double
si = Double -> Double
st Double
ti
    sinS :: Double
sinS = Double -> Double
forall a. Floating a => a -> a
sin Double
si
    a :: Double
a = (-Double
1.0) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
sinS
    b :: Double
b = Double -> Double
forall a. Floating a => a -> a
cos Double
si Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
sinS Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinS)
    f :: Double
f = Double
ti Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v10dv2dt Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
si
    d2sdt2 :: Double
d2sdt2 = Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v10dv2dt Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v10dv2dt Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v10d2v2dt2)
    df :: Double
df = Double
ti Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
d2sdt2
    fi :: Double
fi = Double
f Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
df
    ti1 :: Double
ti1 = Double
ti Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
fi

-- | Newton-Raphson for speed intercept.

intSpdNrRec :: Double -> Double -> Double -> Double -> (Double -> Double) -> Double -> Int -> Double
intSpdNrRec :: Double
-> Double
-> Double
-> Double
-> (Double -> Double)
-> Double
-> Int
-> Double
intSpdNrRec Double
v10v20 Double
v10c2 Double
w1 Double
w2 Double -> Double
st Double
ti Int
i
    | Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
50 = -Double
1.0 -- no convergence

    | Double -> Double
forall a. Num a => a -> a
abs Double
fi Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-11 = Double
ti1
    | Bool
otherwise = Double
-> Double
-> Double
-> Double
-> (Double -> Double)
-> Double
-> Int
-> Double
intSpdNrRec Double
v10v20 Double
v10c2 Double
w1 Double
w2 Double -> Double
st Double
ti1 (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
  where
    cosw2t :: Double
cosw2t = Double -> Double
forall a. Floating a => a -> a
cos (Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ti)
    sinw2t :: Double
sinw2t = Double -> Double
forall a. Floating a => a -> a
sin (Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ti)
    si :: Double
si = Double -> Double
st Double
ti
    f :: Double
f = Double
si Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
ti Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
w1
    dsdt :: Double
dsdt = (Double
w2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double
v10v20 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinw2t Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
v10c2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cosw2t)) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double -> Double
forall a. Floating a => a -> a
sin Double
si
    df :: Double
df = (Double
dsdt Double -> Double -> Double
forall a. Num a => a -> a -> a
- (Double
si Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
ti)) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
ti
    fi :: Double
fi = Double
f Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
df
    ti1 :: Double
ti1 = Double
ti Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
fi

-- | angular separation in radians at ti between v10 and track with initial position v20,

-- course c2 and speed s2.

sep :: Math3d.V3 -> Math3d.V3 -> Math3d.V3 -> Speed -> Double -> Double -> Double
sep :: V3 -> V3 -> V3 -> Speed -> Double -> Double -> Double
sep V3
v10 V3
v20 V3
c2 Speed
s2 Double
r Double
ti = V3 -> V3 -> Double
angleBetweenRadians V3
v10 (V3 -> V3 -> Speed -> Double -> Double -> V3
position'' V3
v20 V3
c2 Speed
s2 Double
ti Double
r)

-- | reference sphere radius.

radius :: (Spherical a) => HorizontalPosition a -> Length
radius :: HorizontalPosition a -> Length
radius = Ellipsoid -> Length
equatorialRadius (Ellipsoid -> Length)
-> (HorizontalPosition a -> Ellipsoid)
-> HorizontalPosition a
-> Length
forall b c a. (b -> c) -> (a -> b) -> a -> c
. a -> Ellipsoid
forall a. Model a => a -> Ellipsoid
surface (a -> Ellipsoid)
-> (HorizontalPosition a -> a) -> HorizontalPosition a -> Ellipsoid
forall b c a. (b -> c) -> (a -> b) -> a -> c
. HorizontalPosition a -> a
forall a. Model a => HorizontalPosition a -> a
Geodetic.model

-- | reference sphere radius in metres.

radiusM :: (Spherical a) => HorizontalPosition a -> Double
radiusM :: HorizontalPosition a -> Double
radiusM = Length -> Double
Length.toMetres (Length -> Double)
-> (HorizontalPosition a -> Length)
-> HorizontalPosition a
-> Double
forall b c a. (b -> c) -> (a -> b) -> a -> c
. HorizontalPosition a -> Length
forall a. Spherical a => HorizontalPosition a -> Length
radius

-- angle between 2 vectors in radians - this is duplicated with GreatCircle but

-- does not return an Angle - truncating to microarcsecond resolution can be

-- detrimental to the convergence of Newtow-Raphson.

angleBetweenRadians :: Math3d.V3 -> Math3d.V3 -> Double
angleBetweenRadians :: V3 -> V3 -> Double
angleBetweenRadians V3
v1 V3
v2 = Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
sinO Double
cosO
  where
    sinO :: Double
sinO = V3 -> Double
Math3d.norm (V3 -> V3 -> V3
Math3d.cross V3
v1 V3
v2)
    cosO :: Double
cosO = V3 -> V3 -> Double
Math3d.dot V3
v1 V3
v2