{-# LANGUAGE TypeOperators, TypeFamilies, FlexibleContexts #-}
-- | The implementation assumes IEEE 754 arithmetic.

module Geodetics.Path where

import Control.Monad
import Geodetics.Ellipsoids
import Geodetics.Geodetic


-- | Lower and upper exclusive distance bounds within which a path is valid. 
type PathValidity = (Double, Double)

-- | A path is a parametric function of distance along the path. The result is the
-- position, and the direction of the path at that point as heading and elevation angles.
--
-- A well-behaved path must be continuous and monotonic (that is, if the distance increases
-- then the result is further along the path) for all distances within its validity range.
-- Ideally the physical distance along the path from the origin to the result should be equal
-- to the argument. If this is not practical then a reasonably close approximation may be used,
-- such as a spheroid instead of an ellipsoid, provided that this is documented. 
-- Outside its validity the path function may
-- return anything or bottom.
data Path e = Path {
      forall e. Path e -> Double -> (Geodetic e, Double, Double)
pathFunc :: Double -> (Geodetic e, Double, Double),
         -- ^ Takes a length and returns a position, and direction as heading and elevation angles.
      forall e. Path e -> PathValidity
pathValidity :: PathValidity
   }
   
-- | Convenience value for paths that are valid for all distances.
alwaysValid :: PathValidity
alwaysValid :: PathValidity
alwaysValid = (Double -> Double
forall a. Num a => a -> a
negate Double
inf, Double
inf) where
   inf :: Double
inf = Double
1.0 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
0  -- Assumes IEE arithmetic.


-- | True if the path is valid at that distance.
pathValidAt :: Path e -> Double -> Bool
pathValidAt :: forall e. Path e -> Double -> Bool
pathValidAt Path e
path Double
d = Double
d Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
x1 Bool -> Bool -> Bool
&& Double
d Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
x2
   where (Double
x1,Double
x2) = Path e -> PathValidity
forall e. Path e -> PathValidity
pathValidity Path e
path


-- | Find where a path meets a given condition using bisection. This is not the
-- most efficient algorithm, but it will always produce a result if there is one
-- within the initial bounds. If there is more than one result then an arbitrary
-- one will be returned.
-- 
-- The initial bounds must return one GT or EQ value and one LT or EQ value. If they
-- do not then @Nothing@ is returned.
bisect :: 
   Path e 
   -> (Geodetic e -> Ordering)        -- ^ Evaluation function.
   -> Double                          -- ^ Required accuracy in terms of distance along the path.
   -> Double -> Double                -- ^ Initial bounds.
   -> Maybe Double
bisect :: forall e.
Path e
-> (Geodetic e -> Ordering)
-> Double
-> Double
-> Double
-> Maybe Double
bisect Path e
path Geodetic e -> Ordering
f Double
t Double
b1 Double
b2 = do
      Bool -> Maybe ()
forall (f :: * -> *). Alternative f => Bool -> f ()
guard (Bool -> Maybe ()) -> Bool -> Maybe ()
forall a b. (a -> b) -> a -> b
$ Path e -> Double -> Bool
forall e. Path e -> Double -> Bool
pathValidAt Path e
path Double
b1
      Bool -> Maybe ()
forall (f :: * -> *). Alternative f => Bool -> f ()
guard (Bool -> Maybe ()) -> Bool -> Maybe ()
forall a b. (a -> b) -> a -> b
$ Path e -> Double -> Bool
forall e. Path e -> Double -> Bool
pathValidAt Path e
path Double
b2
      let r :: ((Double, Ordering), (Double, Ordering))
r = Double -> Double -> ((Double, Ordering), (Double, Ordering))
pairResults Double
b1 Double
b2
      Bool -> Maybe ()
forall (f :: * -> *). Alternative f => Bool -> f ()
guard (Bool -> Maybe ()) -> Bool -> Maybe ()
forall a b. (a -> b) -> a -> b
$ ((Double, Ordering), (Double, Ordering)) -> Bool
forall {a} {a}. ((a, Ordering), (a, Ordering)) -> Bool
hasRoot ((Double, Ordering), (Double, Ordering))
r
      ((Double, Ordering), (Double, Ordering)) -> Maybe Double
bisect1 (((Double, Ordering), (Double, Ordering)) -> Maybe Double)
-> ((Double, Ordering), (Double, Ordering)) -> Maybe Double
forall a b. (a -> b) -> a -> b
$ ((Double, Ordering), (Double, Ordering))
-> ((Double, Ordering), (Double, Ordering))
forall {a} {a}. Ord a => ((a, a), (a, a)) -> ((a, a), (a, a))
sortPair ((Double, Ordering), (Double, Ordering))
r
   where
      f' :: Double -> Ordering
f' Double
d = let (Geodetic e
p, Double
_, Double
_) = Path e -> Double -> (Geodetic e, Double, Double)
forall e. Path e -> Double -> (Geodetic e, Double, Double)
pathFunc Path e
path Double
d in Geodetic e -> Ordering
f Geodetic e
p 
      pairResults :: Double -> Double -> ((Double, Ordering), (Double, Ordering))
pairResults Double
d1 Double
d2 = ((Double
d1, Double -> Ordering
f' Double
d1), (Double
d2, Double -> Ordering
f' Double
d2))
      hasRoot :: ((a, Ordering), (a, Ordering)) -> Bool
hasRoot ((a, Ordering)
v1, (a, Ordering)
v2) = (a, Ordering) -> Ordering
forall a b. (a, b) -> b
snd (a, Ordering)
v1 Ordering -> Ordering -> Bool
forall a. Ord a => a -> a -> Bool
<= Ordering
EQ Bool -> Bool -> Bool
&& Ordering
EQ Ordering -> Ordering -> Bool
forall a. Ord a => a -> a -> Bool
<= (a, Ordering) -> Ordering
forall a b. (a, b) -> b
snd (a, Ordering)
v2
      sortPair :: ((a, a), (a, a)) -> ((a, a), (a, a))
sortPair ((a, a)
v1, (a, a)
v2) = if (a, a) -> a
forall a b. (a, b) -> b
snd (a, a)
v1 a -> a -> Bool
forall a. Ord a => a -> a -> Bool
<= (a, a) -> a
forall a b. (a, b) -> b
snd (a, a)
v2 then ((a, a)
v1, (a, a)
v2) else ((a, a)
v2, (a, a)
v1)
      bisect1 :: ((Double, Ordering), (Double, Ordering)) -> Maybe Double
bisect1 ((Double
d1, Ordering
r1), (Double
d2, Ordering
r2)) =
         let d3 :: Double
d3 = (Double
d1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d2) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2
             r3 :: Ordering
r3 = Double -> Ordering
f' Double
d3
             c1 :: ((Double, Ordering), (Double, Ordering))
c1 = ((Double
d1, Ordering
r1), (Double
d3, Ordering
r3))
             c2 :: ((Double, Ordering), (Double, Ordering))
c2 = ((Double
d3, Ordering
r3), (Double
d2, Ordering
r2))
         in if Double -> Double
forall a. Num a => a -> a
abs (Double
d1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
d2) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
t 
            then Double -> Maybe Double
forall a. a -> Maybe a
forall (m :: * -> *) a. Monad m => a -> m a
return Double
d3
            else ((Double, Ordering), (Double, Ordering)) -> Maybe Double
bisect1 (((Double, Ordering), (Double, Ordering)) -> Maybe Double)
-> ((Double, Ordering), (Double, Ordering)) -> Maybe Double
forall a b. (a -> b) -> a -> b
$ if ((Double, Ordering), (Double, Ordering)) -> Bool
forall {a} {a}. ((a, Ordering), (a, Ordering)) -> Bool
hasRoot ((Double, Ordering), (Double, Ordering))
c1 then ((Double, Ordering), (Double, Ordering))
c1 else ((Double, Ordering), (Double, Ordering))
c2


-- | Try to find the intersection point of two ground paths (i.e. ignoring 
-- altitude). Returns the distance of the intersection point along each path
-- using a modified Newton-Raphson method. If the two paths are 
-- fairly straight and not close to parallel then this will converge rapidly.
--
-- The algorithm projects great-circle paths forwards using the bearing at the 
-- estimate to find the estimated intersection, and then uses the distances to this 
-- intersection as the next estimates.
--
-- If either estimate departs from its path validity then @Nothing@ is returned.
intersect :: (Ellipsoid e) =>
   Double -> Double                   -- ^ Starting estimates.
   -> Double                          -- ^ Required accuracy.
   -> Int                             -- ^ Iteration limit. Returns @Nothing@ if this is reached.  
   -> Path e -> Path e                -- ^ Paths to intersect.
   -> Maybe (Double, Double)
intersect :: forall e.
Ellipsoid e =>
Double
-> Double
-> Double
-> Int
-> Path e
-> Path e
-> Maybe PathValidity
intersect Double
d1 Double
d2 Double
accuracy Int
n Path e
path1 Path e
path2
   | Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ Path e -> Double -> Bool
forall e. Path e -> Double -> Bool
pathValidAt Path e
path1 Double
d1     = Maybe PathValidity
forall a. Maybe a
Nothing
   | Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ Path e -> Double -> Bool
forall e. Path e -> Double -> Bool
pathValidAt Path e
path2 Double
d2     = Maybe PathValidity
forall a. Maybe a
Nothing
   | Int
n Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
0                         = Maybe PathValidity
forall a. Maybe a
Nothing
   | Double
mag Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-15                    = Maybe PathValidity
forall a. Maybe a
Nothing
   | (Double, Double, Double) -> Double
forall {a}. Floating a => (a, a, a) -> a
mag3 ((Double, Double, Double)
nv1 (Double, Double, Double)
-> (Double, Double, Double) -> (Double, Double, Double)
forall a. Num a => Vec3 a -> Vec3 a -> Vec3 a
`cross3` (Double, Double, Double)
nv2) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
r Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
accuracy = PathValidity -> Maybe PathValidity
forall a. a -> Maybe a
Just (Double
d1, Double
d2)
       -- Assumes that sin (accuracy/r) == accuracy/r
   | Bool
otherwise = 
      if Double -> Double
forall a. Num a => a -> a
abs Double
d1a Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double
forall a. Num a => a -> a
abs Double
d2a Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double -> Double
forall a. Num a => a -> a
abs Double
d1b Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double
forall a. Num a => a -> a
abs Double
d2b
         then Double
-> Double
-> Double
-> Int
-> Path e
-> Path e
-> Maybe PathValidity
forall e.
Ellipsoid e =>
Double
-> Double
-> Double
-> Int
-> Path e
-> Path e
-> Maybe PathValidity
intersect (Double
d1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d1a) (Double
d2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d2a) Double
accuracy (Int -> Int
forall a. Enum a => a -> a
pred Int
n) Path e
path1 Path e
path2
         else Double
-> Double
-> Double
-> Int
-> Path e
-> Path e
-> Maybe PathValidity
forall e.
Ellipsoid e =>
Double
-> Double
-> Double
-> Int
-> Path e
-> Path e
-> Maybe PathValidity
intersect (Double
d1 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d1b) (Double
d2 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
d2b) Double
accuracy (Int -> Int
forall a. Enum a => a -> a
pred Int
n) Path e
path1 Path e
path2
   where
      (Geodetic e
pt1, Double
h1, Double
_) = Path e -> Double -> (Geodetic e, Double, Double)
forall e. Path e -> Double -> (Geodetic e, Double, Double)
pathFunc Path e
path1 Double
d1
      (Geodetic e
pt2, Double
h2, Double
_) = Path e -> Double -> (Geodetic e, Double, Double)
forall e. Path e -> Double -> (Geodetic e, Double, Double)
pathFunc Path e
path2 Double
d2
      vectors :: Double -> Double -> Double 
                 -> (Vec3 Double, Vec3 Double)
      vectors :: Double
-> Double
-> Double
-> ((Double, Double, Double), (Double, Double, Double))
vectors Double
lat Double
lon Double
b = (
          -- Unit vector of normal to surface at (lat,lon)
         (Double
cosLatDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cosLon, Double
cosLatDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
sinLon, Double
sinLat),
         -- Normal of great circle defined by bearing b at (lat,lon)
         (Double
sinLon Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cosB Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
sinLat Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cosLon Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinB,
          Double -> Double
forall a. Num a => a -> a
negate Double
cosLon Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cosB Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
sinLat Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinLon Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinB,
           Double
cosLat Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinB))
         where
            sinLon :: Double
sinLon = Double -> Double
forall a. Floating a => a -> a
sin Double
lon
            sinLat :: Double
sinLat = Double -> Double
forall a. Floating a => a -> a
sin Double
lat
            cosLon :: Double
cosLon = Double -> Double
forall a. Floating a => a -> a
cos Double
lon
            cosLat :: Double
cosLat = Double -> Double
forall a. Floating a => a -> a
cos Double
lat
            sinB :: Double
sinB = Double -> Double
forall a. Floating a => a -> a
sin Double
b
            cosB :: Double
cosB = Double -> Double
forall a. Floating a => a -> a
cos Double
b
      mag3 :: (a, a, a) -> a
mag3 (a
x,a
y,a
z) = a -> a
forall a. Floating a => a -> a
sqrt (a -> a) -> a -> a
forall a b. (a -> b) -> a -> b
$ a
xa -> a -> a
forall a. Num a => a -> a -> a
*a
x a -> a -> a
forall a. Num a => a -> a -> a
+ a
ya -> a -> a
forall a. Num a => a -> a -> a
*a
y a -> a -> a
forall a. Num a => a -> a -> a
+ a
za -> a -> a
forall a. Num a => a -> a -> a
*a
z
      ((Double, Double, Double)
nv1, (Double, Double, Double)
gc1) = Double
-> Double
-> Double
-> ((Double, Double, Double), (Double, Double, Double))
vectors (Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt1) (Geodetic e -> Double
forall e. Geodetic e -> Double
longitude Geodetic e
pt1) Double
h1
      ((Double, Double, Double)
nv2, (Double, Double, Double)
gc2) = Double
-> Double
-> Double
-> ((Double, Double, Double), (Double, Double, Double))
vectors (Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt2) (Geodetic e -> Double
forall e. Geodetic e -> Double
longitude Geodetic e
pt2) Double
h2
      nv3 :: (Double, Double, Double)
nv3 = (Double, Double, Double)
gc1 (Double, Double, Double)
-> (Double, Double, Double) -> (Double, Double, Double)
forall a. Num a => Vec3 a -> Vec3 a -> Vec3 a
`cross3` (Double, Double, Double)
gc2         -- Intersection of the great circles
      mag :: Double
mag = (Double, Double, Double) -> Double
forall {a}. Floating a => (a, a, a) -> a
mag3 (Double, Double, Double)
nv3
      nv3a :: (Double, Double, Double)
nv3a = (Double, Double, Double) -> Double -> (Double, Double, Double)
forall a. Num a => Vec3 a -> a -> Vec3 a
scale3 (Double, Double, Double)
nv3 (Double
1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
mag)   -- Scale to unit. See outer function for case when mag3 == 0
      nv3b :: (Double, Double, Double)
nv3b = (Double, Double, Double) -> (Double, Double, Double)
forall a. Num a => Vec3 a -> Vec3 a
negate3 (Double, Double, Double)
nv3a            -- Antipodal result. Take the closest.
      -- Find "nearest" intersection, defined as smaller of sum of distances to current points.
      d1a :: Double
d1a = (Double, Double, Double)
-> (Double, Double, Double) -> (Double, Double, Double) -> Double
forall {b}. RealFloat b => Vec3 b -> Vec3 b -> Vec3 b -> b
gcDist (Double, Double, Double)
gc1 (Double, Double, Double)
nv1 (Double, Double, Double)
nv3a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
r
      d2a :: Double
d2a = (Double, Double, Double)
-> (Double, Double, Double) -> (Double, Double, Double) -> Double
forall {b}. RealFloat b => Vec3 b -> Vec3 b -> Vec3 b -> b
gcDist (Double, Double, Double)
gc2 (Double, Double, Double)
nv2 (Double, Double, Double)
nv3a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
r
      d1b :: Double
d1b = (Double, Double, Double)
-> (Double, Double, Double) -> (Double, Double, Double) -> Double
forall {b}. RealFloat b => Vec3 b -> Vec3 b -> Vec3 b -> b
gcDist (Double, Double, Double)
gc1 (Double, Double, Double)
nv1 (Double, Double, Double)
nv3b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
r
      d2b :: Double
d2b = (Double, Double, Double)
-> (Double, Double, Double) -> (Double, Double, Double) -> Double
forall {b}. RealFloat b => Vec3 b -> Vec3 b -> Vec3 b -> b
gcDist (Double, Double, Double)
gc2 (Double, Double, Double)
nv2 (Double, Double, Double)
nv3b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
r
      -- Signed angle between v1 and v2, 
      gcDist :: Vec3 b -> Vec3 b -> Vec3 b -> b
gcDist Vec3 b
norm Vec3 b
v1 Vec3 b
v2 = 
         let c :: Vec3 b
c = Vec3 b
v1 Vec3 b -> Vec3 b -> Vec3 b
forall a. Num a => Vec3 a -> Vec3 a -> Vec3 a
`cross3` Vec3 b
v2 
         in (if Vec3 b
c Vec3 b -> Vec3 b -> b
forall a. Num a => Vec3 a -> Vec3 a -> a
`dot3` Vec3 b
norm b -> b -> Bool
forall a. Ord a => a -> a -> Bool
< b
0 then b -> b
forall a. Num a => a -> a
negate else b -> b
forall a. a -> a
id) (b -> b) -> b -> b
forall a b. (a -> b) -> a -> b
$ b -> b -> b
forall a. RealFloat a => a -> a -> a
atan2 (Vec3 b -> b
forall {a}. Floating a => (a, a, a) -> a
mag3 Vec3 b
c) (Vec3 b
v1 Vec3 b -> Vec3 b -> b
forall a. Num a => Vec3 a -> Vec3 a -> a
`dot3` Vec3 b
v2) 
      r :: Double
r = e -> Double
forall a. Ellipsoid a => a -> Double
majorRadius (e -> Double) -> e -> Double
forall a b. (a -> b) -> a -> b
$ Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt1
          
{- Note on derivation of "intersect"

The algorithm is a variant of the Newton-Raphson method, and shares its advantage
of rapid convergence in many useful cases. Each path has a current approximation to
the intersection, and the next approximation is computed by projecting both paths 
along great circles from the current approximation and finding the point where those
great circles intersect. A spherical Earth is assumed for simplicity. 

The Great Circle calculations use a vector method rather than spherical trigonometry.
This avoids a lot of transcendental functions and also the singularities inherent in 
polar coordinate systems. This implementation is based on formulae from 
<http://www.movable-type.co.uk/scripts/latlong-vectors.html>, which in turn is based on 
"A Non-singular Horizontal Position Representation" by Kenneth Gade, THE JOURNAL OF 
NAVIGATION (2010), 63, 395–417.
<http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf>


"pt1" is the current approximation for the result on "path1".  The vector "nv1" is the 
unit vector pointing "pt1". "gc1" is the normal to the great circle tangent to 
"path1" at "pt1". "nv2" and "gc2" are similarly derived from "path2".

The intersection of the planes of "gc1" and "gc2" is given by their cross product, "nv3".
This is scaled to a unit vector "nv3a", and the other intersection of the great circles is
at "nv3b", opposite. The distances from nv1 and nv2 to nv3a and nv3b are computed using
the corresponding great-circle normals to determine whether the distances are positive or 
negative along the paths. The nearest solution (defined in terms of great-circle distance) 
is taken as the basis for the next approximation.
-}

-- | A ray from a point heading in a straight line in 3 dimensions. 
rayPath :: (Ellipsoid e) => 
   Geodetic e          -- ^ Start point.
   -> Double           -- ^ Bearing.
   -> Double           -- ^ Elevation.
   -> Path e
rayPath :: forall e. Ellipsoid e => Geodetic e -> Double -> Double -> Path e
rayPath Geodetic e
pt1 Double
bearing Double
elevation = (Double -> (Geodetic e, Double, Double)) -> PathValidity -> Path e
forall e.
(Double -> (Geodetic e, Double, Double)) -> PathValidity -> Path e
Path Double -> (Geodetic e, Double, Double)
ray PathValidity
alwaysValid
   where
      ray :: Double -> (Geodetic e, Double, Double)
ray Double
distance = (Double -> Double -> Double -> e -> Geodetic e
forall e. Double -> Double -> Double -> e -> Geodetic e
Geodetic Double
lat Double
long Double
alt (Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt1), Double
bearing2, Double
elevation2)
         where
            pt2' :: (Double, Double, Double)
pt2' = (Double, Double, Double)
pt1' (Double, Double, Double)
-> (Double, Double, Double) -> (Double, Double, Double)
forall a. Num a => Vec3 a -> Vec3 a -> Vec3 a
`add3` ((Double, Double, Double)
delta (Double, Double, Double) -> Double -> (Double, Double, Double)
forall a. Num a => Vec3 a -> a -> Vec3 a
`scale3` Double
distance)      -- ECEF of result point.
            (Double
lat,Double
long,Double
alt) = e -> (Double, Double, Double) -> (Double, Double, Double)
forall e.
Ellipsoid e =>
e -> (Double, Double, Double) -> (Double, Double, Double)
earthToGeo (Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt1) (Double, Double, Double)
pt2'  -- Geodetic of result point.
            (Double
dE,Double
dN,Double
dU) = Matrix3 Double
-> (Double, Double, Double) -> (Double, Double, Double)
forall a. Num a => Matrix3 a -> Vec3 a -> Vec3 a
transform3 (Matrix3 Double -> Matrix3 Double
forall a. Matrix3 a -> Matrix3 a
trans3 (Matrix3 Double -> Matrix3 Double)
-> Matrix3 Double -> Matrix3 Double
forall a b. (a -> b) -> a -> b
$ Double -> Double -> Matrix3 Double
forall {c} {a}.
(Floating c, Num a) =>
c -> c -> ((c, c, c), (c, c, c), (a, c, c))
ecefMatrix Double
lat Double
long) (Double, Double, Double)
delta  -- Direction of ray at result point.
            elevation2 :: Double
elevation2 = Double -> Double
forall a. Floating a => a -> a
asin Double
dU
            bearing2 :: Double
bearing2 = if Double
dE Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 Bool -> Bool -> Bool
&& Double
dN Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 then Double
bearing else Double -> Double -> Double
forall a. RealFloat a => a -> a -> a
atan2 Double
dE Double
dN  -- Allow for vertical elevation.
            
      ecefMatrix :: c -> c -> ((c, c, c), (c, c, c), (a, c, c))
ecefMatrix c
lat c
long =   -- Transform matrix for vectors from (East, North, Up) to (X,Y,Z).
         ((c -> c
forall a. Num a => a -> a
negate c
sinLong, c -> c
forall a. Num a => a -> a
negate c
cosLongc -> c -> c
forall a. Num a => a -> a -> a
*c
sinLat, c
cosLongc -> c -> c
forall a. Num a => a -> a -> a
*c
cosLat),
              --    East X      North X               Up X
          (       c
cosLong, c -> c
forall a. Num a => a -> a
negate c
sinLongc -> c -> c
forall a. Num a => a -> a -> a
*c
sinLat, c
sinLongc -> c -> c
forall a. Num a => a -> a -> a
*c
cosLat),
              --    East Y      North Y               Up Y
          (             a
0,      c
cosLat         , c
sinLat))
              --    East Z      North Z               Up Z
         where
            sinLong :: c
sinLong = c -> c
forall a. Floating a => a -> a
sin c
long
            cosLong :: c
cosLong = c -> c
forall a. Floating a => a -> a
cos c
long
            sinLat :: c
sinLat = c -> c
forall a. Floating a => a -> a
sin c
lat
            cosLat :: c
cosLat = c -> c
forall a. Floating a => a -> a
cos c
lat
      
      direction :: (Double, Double, Double)
direction = (Double
sinBDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cosE, Double
cosBDouble -> Double -> Double
forall a. Num a => a -> a -> a
*Double
cosE, Double
sinE)  -- Direction of ray in ENU
      delta :: (Double, Double, Double)
delta = Matrix3 Double
-> (Double, Double, Double) -> (Double, Double, Double)
forall a. Num a => Matrix3 a -> Vec3 a -> Vec3 a
transform3 (Double -> Double -> Matrix3 Double
forall {c} {a}.
(Floating c, Num a) =>
c -> c -> ((c, c, c), (c, c, c), (a, c, c))
ecefMatrix (Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt1) (Geodetic e -> Double
forall e. Geodetic e -> Double
longitude Geodetic e
pt1)) (Double, Double, Double)
direction  -- Convert to ECEF
      pt1' :: (Double, Double, Double)
pt1' = Geodetic e -> (Double, Double, Double)
forall e. Ellipsoid e => Geodetic e -> (Double, Double, Double)
geoToEarth Geodetic e
pt1    -- ECEF of origin point.
      sinB :: Double
sinB = Double -> Double
forall a. Floating a => a -> a
sin Double
bearing
      cosB :: Double
cosB = Double -> Double
forall a. Floating a => a -> a
cos Double
bearing
      sinE :: Double
sinE = Double -> Double
forall a. Floating a => a -> a
sin Double
elevation
      cosE :: Double
cosE = Double -> Double
forall a. Floating a => a -> a
cos Double
elevation
      
-- | Rhumb line: path following a constant course. Also known as a loxodrome.
--
-- The valid range stops a few arc-minutes short of the poles to ensure that the 
-- polar singularities are not included. Anyone using a rhumb line that close to a pole
-- must be going round the twist anyway.
--
-- Based on /Practical Sailing Formulas for Rhumb-Line Tracks on an Oblate Earth/
-- by G.H. Kaplan, U.S. Naval Observatory. Except for points close to the poles 
-- the approximation is accurate to within a few meters over 1000km.
rhumbPath :: (Ellipsoid e) =>
   Geodetic e            -- ^ Start point.
   -> Double             -- ^ Course.
   -> Path e
rhumbPath :: forall e. Ellipsoid e => Geodetic e -> Double -> Path e
rhumbPath Geodetic e
pt Double
course = (Double -> (Geodetic e, Double, Double)) -> PathValidity -> Path e
forall e.
(Double -> (Geodetic e, Double, Double)) -> PathValidity -> Path e
Path Double -> (Geodetic e, Double, Double)
rhumb PathValidity
validity
   where
      rhumb :: Double -> (Geodetic e, Double, Double)
rhumb Double
distance = (Double -> Double -> Double -> e -> Geodetic e
forall e. Double -> Double -> Double -> e -> Geodetic e
Geodetic Double
lat (Double -> Double
properAngle Double
lon) Double
0 (Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt), Double
course, Double
0)
         where
            lat' :: Double
lat' = Double
lat0 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
distance Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
cosC Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
m0   -- Kaplan Eq 13.
            lat :: Double
lat = Double
lat0 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ (Double
m0 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
aDouble -> Double -> Double
forall a. Num a => a -> a -> a
*(Double
1Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
e2))) Double -> Double -> Double
forall a. Num a => a -> a -> a
* ((Double
1Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
3Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e2Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
4)Double -> Double -> Double
forall a. Num a => a -> a -> a
*(Double
lat'Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
lat0)
                                            Double -> Double -> Double
forall a. Num a => a -> a -> a
+ (Double
3Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
e2Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
8)Double -> Double -> Double
forall a. Num a => a -> a -> a
*(Double -> Double
forall a. Floating a => a -> a
sin (Double
2Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
lat') Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double -> Double
forall a. Floating a => a -> a
sin (Double
2Double -> Double -> Double
forall a. Num a => a -> a -> a
*Double
lat0)))
            lon :: Double
lon | Double -> Double
forall a. Num a => a -> a
abs Double
cosC Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
1e-7
                     = Double
lon0 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
tanC Double -> Double -> Double
forall a. Num a => a -> a -> a
* (Double -> Double
q Double
lat Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
q0)     -- Kaplan Eq 16.
                | Bool
otherwise
                     = Double
lon0 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
distance Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sinC Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ e -> Double -> Double
forall e. Ellipsoid e => e -> Double -> Double
latitudeRadius (Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt) ((Double
lat0 Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
lat')Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2)
      validity :: PathValidity
validity
         | Double
cosC Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0  = ((Double -> Double
forall a. Num a => a -> a
negate Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
b Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
cosC, (Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
b Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
cosC)
         | Bool
otherwise  = ((Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
b Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
cosC, (Double -> Double
forall a. Num a => a -> a
negate Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
b Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
cosC)
      q0 :: Double
q0 = Double -> Double
q Double
lat0
      q :: Double -> Double
q Double
phi = Double -> Double
forall a. Floating a => a -> a
log (Double -> Double
forall a. Floating a => a -> a
tan (Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
4Double -> Double -> Double
forall a. Num a => a -> a -> a
+Double
phiDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2)) Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
e Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
log ((Double
1Double -> Double -> Double
forall a. Num a => a -> a -> a
-Double
eSinPhi)Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/(Double
1Double -> Double -> Double
forall a. Num a => a -> a -> a
+Double
eSinPhi)) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2
         where                                -- Factor out expression from Eq 16 of Kaplan
            eSinPhi :: Double
eSinPhi = Double
e Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double
forall a. Floating a => a -> a
sin Double
phi
      sinC :: Double
sinC = Double -> Double
forall a. Floating a => a -> a
sin Double
course
      cosC :: Double
cosC = Double -> Double
forall a. Floating a => a -> a
cos Double
course
      tanC :: Double
tanC = Double -> Double
forall a. Floating a => a -> a
tan Double
course
      lat0 :: Double
lat0 = Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt
      lon0 :: Double
lon0 = Geodetic e -> Double
forall e. Geodetic e -> Double
longitude Geodetic e
pt
      e2 :: Double
e2 = e -> Double
forall a. Ellipsoid a => a -> Double
eccentricity2 (e -> Double) -> e -> Double
forall a b. (a -> b) -> a -> b
$ Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt
      e :: Double
e = Double -> Double
forall a. Floating a => a -> a
sqrt Double
e2
      m0 :: Double
m0 = e -> Double -> Double
forall e. Ellipsoid e => e -> Double -> Double
meridianRadius (Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt) Double
lat0
      a :: Double
a = e -> Double
forall a. Ellipsoid a => a -> Double
majorRadius (e -> Double) -> e -> Double
forall a b. (a -> b) -> a -> b
$ Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt
      b :: Double
b = e -> Double
forall a. Ellipsoid a => a -> Double
minorRadius (e -> Double) -> e -> Double
forall a b. (a -> b) -> a -> b
$ Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt
   

-- | A path following the line of latitude around the Earth eastwards.
--
-- This is equivalent to @rhumbPath pt (pi/2)@
latitudePath :: (Ellipsoid e) =>
   Geodetic e           -- ^ Start point.
   -> Path e
latitudePath :: forall e. Ellipsoid e => Geodetic e -> Path e
latitudePath Geodetic e
pt = (Double -> (Geodetic e, Double, Double)) -> PathValidity -> Path e
forall e.
(Double -> (Geodetic e, Double, Double)) -> PathValidity -> Path e
Path Double -> (Geodetic e, Double, Double)
line PathValidity
alwaysValid
   where
      line :: Double -> (Geodetic e, Double, Double)
line Double
distance = (Geodetic e
pt2, Double
forall a. Floating a => a
piDouble -> Double -> Double
forall a. Fractional a => a -> a -> a
/Double
2, Double
0)
         where
            pt2 :: Geodetic e
pt2 = Double -> Double -> Double -> e -> Geodetic e
forall e. Double -> Double -> Double -> e -> Geodetic e
Geodetic 
               (Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt) (Geodetic e -> Double
forall e. Geodetic e -> Double
longitude Geodetic e
pt Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
distance Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
r)
               Double
0 (Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt)
      r :: Double
r = e -> Double -> Double
forall e. Ellipsoid e => e -> Double -> Double
latitudeRadius (Geodetic e -> e
forall e. Geodetic e -> e
ellipsoid Geodetic e
pt) (Geodetic e -> Double
forall e. Geodetic e -> Double
latitude Geodetic e
pt)


-- | A path from the specified point to the North Pole. Use negative distances
-- for the southward path.
--
-- This is equivalent to @rhumbPath pt _0@
longitudePath :: (Ellipsoid e) =>
   Geodetic e    -- ^ Start point.
   -> Path e
longitudePath :: forall e. Ellipsoid e => Geodetic e -> Path e
longitudePath Geodetic e
pt = Geodetic e -> Double -> Path e
forall e. Ellipsoid e => Geodetic e -> Double -> Path e
rhumbPath Geodetic e
pt Double
0