-- |

-- Module:      Data.Geo.Jord.Local

-- Copyright:   (c) 2020 Cedric Liegeois

-- License:     BSD3

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

-- Stability:   experimental

-- Portability: portable

--

-- Type and functions for working with delta vectors in different local reference frames: all frames are location dependent.

--

-- 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.Local as Local

-- @

--

-- 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>

--

-- Notes:

--

--     * The term Earth is used to be consistent with the paper. However any celestial body reference frame can be used.

--

--     * Though the API accept spherical models, doing so defeats the purpose of this module

--       which is to find exact solutions. Prefer using ellipsoidal models.

module Data.Geo.Jord.Local
    (
    -- * Local Reference frame

      Frame(..)
    -- * Body frame

    , FrameB
    , yaw
    , pitch
    , roll
    , bOrigin
    , frameB
    -- * Local level/wander azimuth frame

    , FrameL
    , wanderAzimuth
    , lOrigin
    , frameL
    -- * North-East-Down frame

    , FrameN
    , nOrigin
    , frameN
    -- * Deltas

    , Delta(..)
    , deltaMetres
    -- * Delta in the north, east, down frame

    , Ned(..)
    , nedMetres
    , bearing
    , elevation
    , slantRange
    -- * Calculations

    , deltaBetween
    , nedBetween
    , destination
    , destinationN
    ) where

import Data.Geo.Jord.Angle (Angle)
import qualified Data.Geo.Jord.Angle as Angle
import qualified Data.Geo.Jord.Geocentric as Geocentric
import qualified Data.Geo.Jord.Geodetic as Geodetic
import Data.Geo.Jord.Length (Length)
import qualified Data.Geo.Jord.Length as Length (metres, toMetres)
import qualified Data.Geo.Jord.Math3d as Math3d
import Data.Geo.Jord.Model (Model)
import Data.Geo.Jord.Positions
import Data.Geo.Jord.Rotation

-- | class for local reference frames: a reference frame which is location dependant.

--

-- Supported frames:

--

--     * 'FrameB': 'rEF' returns R_EB

--

--     * 'FrameL': 'rEF' returns R_EL

--

--     * 'FrameN': 'rEF' returns R_EN

class Frame a where
    rEF :: a -> [Math3d.V3] -- ^ rotation matrix to transform vectors decomposed in frame @a@ to vectors decomposed Earth-Fixed frame.


-- | Body frame (typically of a vehicle).

--

--     * Position: The origin is in the vehicle’s reference point.

--

--     * Orientation: The x-axis points forward, the y-axis to the right (starboard) and the z-axis in the vehicle’s down direction.

--

--     * Comments: The frame is fixed to the vehicle.

data FrameB a =
    FrameB
        { FrameB a -> Angle
yaw :: Angle -- ^ body yaw angle (vertical axis).

        , FrameB a -> Angle
pitch :: Angle -- ^ body pitch angle (transverse axis).

        , FrameB a -> Angle
roll :: Angle -- ^ body roll angle (longitudinal axis).

        , FrameB a -> Position a
bOrigin :: Geodetic.Position a -- ^ frame origin.

        , FrameB a -> V3
bNorth :: Math3d.V3 -- ^ position of the north pole as /n/-vector.

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

-- | 'FrameB' from given yaw, pitch, roll, position (origin).

frameB :: (Model a) => Angle -> Angle -> Angle -> Geodetic.Position a -> FrameB a
frameB :: Angle -> Angle -> Angle -> Position a -> FrameB a
frameB Angle
y Angle
p Angle
r Position a
o = Angle -> Angle -> Angle -> Position a -> V3 -> FrameB a
forall a. Angle -> Angle -> Angle -> Position a -> V3 -> FrameB a
FrameB Angle
y Angle
p Angle
r Position a
o (Position a -> V3
forall a. Model a => Position a -> V3
northPole Position a
o)

-- | R_EB: frame B to Earth

instance Frame (FrameB a) where
    rEF :: FrameB a -> [V3]
rEF (FrameB Angle
y Angle
p Angle
r Position a
o V3
np) = [V3]
rm
      where
        rNB :: [V3]
rNB = Angle -> Angle -> Angle -> [V3]
zyx2r Angle
y Angle
p Angle
r
        n :: FrameN a
n = Position a -> V3 -> FrameN a
forall a. Position a -> V3 -> FrameN a
FrameN Position a
o V3
np
        rEN :: [V3]
rEN = FrameN a -> [V3]
forall a. Frame a => a -> [V3]
rEF FrameN a
n
        rm :: [V3]
rm = [V3] -> [V3] -> [V3]
Math3d.dotM [V3]
rEN [V3]
rNB -- closest frames cancel: N


-- | Local level, Wander azimuth frame.

--

--     * Position: The origin is directly beneath or above the vehicle (B), at Earth’s surface (surface

-- of ellipsoid model).

--

--     * Orientation: The z-axis is pointing down. Initially, the x-axis points towards north, and the

-- y-axis points towards east, but as the vehicle moves they are not rotating about the z-axis

-- (their angular velocity relative to the Earth has zero component along the z-axis).

-- (Note: Any initial horizontal direction of the x- and y-axes is valid for L, but if the

-- initial position is outside the poles, north and east are usually chosen for convenience.)

--

--     * Comments: The L-frame is equal to the N-frame except for the rotation about the z-axis,

-- which is always zero for this frame (relative to Earth). Hence, at a given time, the only

-- difference between the frames is an angle between the x-axis of L and the north direction;

-- this angle is called the wander azimuth angle. The L-frame is well suited for general

-- calculations, as it is non-singular.

data FrameL a =
    FrameL
        { FrameL a -> Angle
wanderAzimuth :: Angle -- ^ wander azimuth: angle between x-axis of the frame L and the north direction.

        , FrameL a -> Position a
lOrigin :: Geodetic.Position a -- ^ frame origin.

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

-- | R_EL: frame L to Earth

instance Frame (FrameL m) where
    rEF :: FrameL m -> [V3]
rEF (FrameL Angle
w Position m
o) = [V3]
rm
      where
        lat :: Angle
lat = Position m -> Angle
forall a. HasCoordinates a => a -> Angle
Geodetic.latitude Position m
o
        lon :: Angle
lon = Position m -> Angle
forall a. HasCoordinates a => a -> Angle
Geodetic.longitude Position m
o
        r :: [V3]
r = Angle -> Angle -> Angle -> [V3]
xyz2r Angle
lon (Angle -> Angle
Angle.negate Angle
lat) Angle
w
        rEe' :: [V3]
rEe' = [Double -> Double -> Double -> V3
Math3d.vec3 Double
0 Double
0 (-Double
1), Double -> Double -> Double -> V3
Math3d.vec3 Double
0 Double
1 Double
0, Double -> Double -> Double -> V3
Math3d.vec3 Double
1 Double
0 Double
0]
        rm :: [V3]
rm = [V3] -> [V3] -> [V3]
Math3d.dotM [V3]
rEe' [V3]
r

-- | 'FrameL' from given wander azimuth, position (origin).

frameL :: (Model a) => Angle -> Geodetic.Position a -> FrameL a
frameL :: Angle -> Position a -> FrameL a
frameL = Angle -> Position a -> FrameL a
forall a. Angle -> Position a -> FrameL a
FrameL

-- | North-East-Down (local level) frame.

--

--     * Position: The origin is directly beneath or above the vehicle (B), at Earth’s surface (surface

-- of ellipsoid model).

--

--     * Orientation: The x-axis points towards north, the y-axis points towards east (both are

-- horizontal), and the z-axis is pointing down.

--

--     * Comments: When moving relative to the Earth, the frame rotates about its z-axis to allow the

-- x-axis to always point towards north. When getting close to the poles this rotation rate

-- will increase, being infinite at the poles. The poles are thus singularities and the direction of

-- the x- and y-axes are not defined here. Hence, this coordinate frame is not suitable for

-- general calculations.

data FrameN a =
    FrameN
        { FrameN a -> Position a
nOrigin :: Geodetic.Position a -- ^ frame origin.

        , FrameN a -> V3
nNorth :: Math3d.V3 -- ^ position of the north pole as /n/-vector.

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

-- | R_EN: frame N to Earth

instance Frame (FrameN a) where
    rEF :: FrameN a -> [V3]
rEF (FrameN Position a
o V3
np) = [V3] -> [V3]
Math3d.transposeM [V3]
rm
      where
        vo :: V3
vo = Position a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector Position a
o
        rd :: V3
rd = V3 -> Double -> V3
Math3d.scale V3
vo (-Double
1.0) -- down (pointing opposite to n-vector)

        re :: V3
re = V3 -> V3
Math3d.unit (V3 -> V3 -> V3
Math3d.cross V3
np V3
vo) -- east (pointing perpendicular to the plane)

        rn :: V3
rn = V3 -> V3 -> V3
Math3d.cross V3
re V3
rd -- north (by right hand rule)

        rm :: [V3]
rm = [V3
rn, V3
re, V3
rd]

-- | 'FrameN' from given position (origin).

frameN :: (Model a) => Geodetic.Position a -> FrameN a
frameN :: Position a -> FrameN a
frameN Position a
p = Position a -> V3 -> FrameN a
forall a. Position a -> V3 -> FrameN a
FrameN Position a
p (Position a -> V3
forall a. Model a => Position a -> V3
northPole Position a
p)

-- | delta between position in one of the reference frames.

data Delta =
    Delta
        { Delta -> Length
dx :: Length -- ^ x component.

        , Delta -> Length
dy :: Length -- ^ y component.

        , Delta -> Length
dz :: Length -- ^ z component.

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

-- | 'Delta' from given x, y and z length in __metres__.

deltaMetres :: Double -> Double -> Double -> Delta
deltaMetres :: Double -> Double -> Double -> Delta
deltaMetres Double
x Double
y Double
z = Length -> Length -> Length -> Delta
Delta (Double -> Length
Length.metres Double
x) (Double -> Length
Length.metres Double
y) (Double -> Length
Length.metres Double
z)

-- | North, east and down delta (thus in frame 'FrameN').

data Ned =
    Ned
        { Ned -> Length
north :: Length -- ^ North component.

        , Ned -> Length
east :: Length -- ^ East component.

        , Ned -> Length
down :: Length -- ^ Down component.

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

-- | 'Ned' from given north, east and down in __metres__.

nedMetres :: Double -> Double -> Double -> Ned
nedMetres :: Double -> Double -> Double -> Ned
nedMetres Double
n Double
e Double
d = Length -> Length -> Length -> Ned
Ned (Double -> Length
Length.metres Double
n) (Double -> Length
Length.metres Double
e) (Double -> Length
Length.metres Double
d)

-- | @bearing v@ computes the bearing in compass angle of the NED vector @v@ from north.

--

-- Compass angles are clockwise angles from true north: 0 = north, 90 = east, 180 = south, 270 = west.

bearing :: Ned -> Angle
bearing :: Ned -> Angle
bearing (Ned Length
n Length
e Length
_) =
    let a :: Angle
a = Double -> Double -> Angle
Angle.atan2 (Length -> Double
Length.toMetres Length
e) (Length -> Double
Length.toMetres Length
n)
     in Angle -> Angle -> Angle
Angle.normalise Angle
a (Double -> Angle
Angle.decimalDegrees Double
360.0)

-- | @elevation v@ computes the elevation of the NED vector @v@ from horizontal (ie tangent to ellipsoid surface).

elevation :: Ned -> Angle
elevation :: Ned -> Angle
elevation Ned
n = Angle -> Angle
Angle.negate (Double -> Angle
Angle.asin (V3 -> Double
Math3d.v3z V3
v Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ V3 -> Double
Math3d.norm V3
v))
  where
    v :: V3
v = Ned -> V3
nedV3 Ned
n

-- | @slantRange v@ computes the distance from origin in the local system of the NED vector @v@.

slantRange :: Ned -> Length
slantRange :: Ned -> Length
slantRange = Double -> Length
Length.metres (Double -> Length) -> (Ned -> Double) -> Ned -> Length
forall b c a. (b -> c) -> (a -> b) -> a -> c
. V3 -> Double
Math3d.norm (V3 -> Double) -> (Ned -> V3) -> Ned -> Double
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Ned -> V3
nedV3

-- | @deltaBetween p1 p2 f@ computes the exact 'Delta' between the two

-- positions @p1@ and @p2@ in local frame @f@. For example:

--

-- >>> let p1 = Geodetic.latLongHeightPos 1 2 (Length.metres (-3)) WGS84

-- >>> let p2 = Geodetic.latLongHeightPos 4 5 (Length.metres (-6)) WGS84

-- >>> let w = Angle.decimalDegrees 5 -- wander azimuth

-- >>> Local.deltaBetween p1 p2 (Local.frameL w)

-- Delta {dx = 359.490578214km, dy = 302.818522536km, dz = 17.404271362km}

deltaBetween ::
       (Frame a, Model b)
    => Geodetic.Position b
    -> Geodetic.Position b
    -> (Geodetic.Position b -> a)
    -> Delta
deltaBetween :: Position b -> Position b -> (Position b -> a) -> Delta
deltaBetween Position b
p1 Position b
p2 Position b -> a
f = Double -> Double -> Double -> Delta
deltaMetres (V3 -> Double
Math3d.v3x V3
d) (V3 -> Double
Math3d.v3y V3
d) (V3 -> Double
Math3d.v3z V3
d)
  where
    g1 :: V3
g1 = Position b -> V3
forall a. Model a => Position a -> V3
Geocentric.metresCoords (Position b -> V3)
-> (Position b -> Position b) -> Position b -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position b -> Position b
forall m. Model m => Position m -> Position m
toGeocentric (Position b -> V3) -> Position b -> V3
forall a b. (a -> b) -> a -> b
$ Position b
p1
    g2 :: V3
g2 = Position b -> V3
forall a. Model a => Position a -> V3
Geocentric.metresCoords (Position b -> V3)
-> (Position b -> Position b) -> Position b -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position b -> Position b
forall m. Model m => Position m -> Position m
toGeocentric (Position b -> V3) -> Position b -> V3
forall a b. (a -> b) -> a -> b
$ Position b
p2
    de :: V3
de = V3 -> V3 -> V3
Math3d.subtract V3
g2 V3
g1
    -- rotation matrix to go from Earth Frame to Frame at p1

    rm :: [V3]
rm = [V3] -> [V3]
Math3d.transposeM (a -> [V3]
forall a. Frame a => a -> [V3]
rEF (Position b -> a
f Position b
p1))
    d :: V3
d = V3 -> [V3] -> V3
Math3d.multM V3
de [V3]
rm

-- | @nedBetween p1 p2@ computes the exact 'Ned' vector between the two

-- positions @p1@ and @p2@, in north, east, and down. For example:

--

-- >>> let p1 = Geodetic.latLongHeightPos 1 2 (Length.metres (-3)) WGS84

-- >>> let p2 = Geodetic.latLongHeightPos 4 5 (Length.metres (-6)) WGS84

-- >>> Local.nedBetween p1 p2

-- Ned {north = 331.730234781km, east = 332.997874989km, down = 17.404271362km}

--

-- Resulting 'Ned' delta is relative to @p1@: Due to the curvature of Earth and

-- different directions to the North Pole, the north, east, and down directions

-- will change (relative to Earth) for different places.

--

-- Position @p1@ must be outside the poles for the north and east directions to be defined.

--

-- This is equivalent to:

--

-- > Local.deltaBetween p1 p2 Local.frameN

nedBetween :: (Model a) => Geodetic.Position a -> Geodetic.Position a -> Ned
nedBetween :: Position a -> Position a -> Ned
nedBetween Position a
p1 Position a
p2 = Length -> Length -> Length -> Ned
Ned Length
n Length
e Length
d
  where
    (Delta Length
n Length
e Length
d) = Position a -> Position a -> (Position a -> FrameN a) -> Delta
forall a b.
(Frame a, Model b) =>
Position b -> Position b -> (Position b -> a) -> Delta
deltaBetween Position a
p1 Position a
p2 Position a -> FrameN a
forall a. Model a => Position a -> FrameN a
frameN

-- | @destination p0 f d@ computes the destination position from position @p0@ and delta @d@ in local frame @f@. For

-- example:

--

-- >>> let p0 = Geodetic.latLongHeightPos 49.66618 3.45063 Length.zero WGS84

-- >>> let y = Angle.decimalDegrees 10 -- yaw

-- >>> let r = Angle.decimalDegrees 20 -- roll

-- >>> let p = Angle.decimalDegrees 30 -- pitch

-- >>> let d = Local.deltaMetres 3000 2000 100

-- >>> Local.destination p0 (Local.frameB y r p) d

-- 49°41'30.485"N,3°28'52.561"E 6.007735m (WGS84)

destination ::
       (Frame a, Model b)
    => Geodetic.Position b
    -> (Geodetic.Position b -> a)
    -> Delta
    -> Geodetic.Position b
destination :: Position b -> (Position b -> a) -> Delta -> Position b
destination Position b
p0 Position b -> a
f Delta
d = Position b -> Position b
forall m. Model m => Position m -> Position m
toGeodetic Position b
gt
  where
    g0 :: V3
g0 = Position b -> V3
forall a. Model a => Position a -> V3
Geocentric.metresCoords (Position b -> V3)
-> (Position b -> Position b) -> Position b -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position b -> Position b
forall m. Model m => Position m -> Position m
toGeocentric (Position b -> V3) -> Position b -> V3
forall a b. (a -> b) -> a -> b
$ Position b
p0
    rm :: [V3]
rm = a -> [V3]
forall a. Frame a => a -> [V3]
rEF (Position b -> a
f Position b
p0)
    c :: V3
c = V3 -> [V3] -> V3
Math3d.multM (Delta -> V3
deltaV3 Delta
d) [V3]
rm
    v :: V3
v = V3 -> V3 -> V3
Math3d.add V3
g0 V3
c
    gt :: Position b
gt = V3 -> b -> Position b
forall a. Model a => V3 -> a -> Position a
Geocentric.metresPos' V3
v (Position b -> b
forall a. Model a => Position a -> a
Geodetic.model' Position b
p0)

-- | @destinationN p0 d@ computes the destination position from position @p0@ and north, east, down @d@. For example:

--

-- >>> let p0 = Geodetic.latLongHeightPos 49.66618 3.45063 Length.zero WGS84

-- >>> Local.destinationN p0 (Local.nedMetres 100 200 300)

-- 49°40'1.484"N,3°27'12.242"E -299.996086m (WGS84)

-- This is equivalent to:

--

-- > Local.destination p0 Local.frameN

destinationN :: (Model a) => Geodetic.Position a -> Ned -> Geodetic.Position a
destinationN :: Position a -> Ned -> Position a
destinationN Position a
p0 (Ned Length
n Length
e Length
d) = Position a -> (Position a -> FrameN a) -> Delta -> Position a
forall a b.
(Frame a, Model b) =>
Position b -> (Position b -> a) -> Delta -> Position b
destination Position a
p0 Position a -> FrameN a
forall a. Model a => Position a -> FrameN a
frameN (Length -> Length -> Length -> Delta
Delta Length
n Length
e Length
d)

nedV3 :: Ned -> Math3d.V3
nedV3 :: Ned -> V3
nedV3 (Ned Length
n Length
e Length
d) = Double -> Double -> Double -> V3
Math3d.vec3 (Length -> Double
Length.toMetres Length
n) (Length -> Double
Length.toMetres Length
e) (Length -> Double
Length.toMetres Length
d)

deltaV3 :: Delta -> Math3d.V3
deltaV3 :: Delta -> V3
deltaV3 (Delta Length
x' Length
y' Length
z') =
    Double -> Double -> Double -> V3
Math3d.vec3 (Length -> Double
Length.toMetres Length
x') (Length -> Double
Length.toMetres Length
y') (Length -> Double
Length.toMetres Length
z')

northPole :: (Model a) => Geodetic.Position a -> Math3d.V3
northPole :: Position a -> V3
northPole = HorizontalPosition a -> V3
forall a. HasCoordinates a => a -> V3
Geodetic.nvector (HorizontalPosition a -> V3)
-> (Position a -> HorizontalPosition a) -> Position a -> V3
forall b c a. (b -> c) -> (a -> b) -> a -> c
. a -> HorizontalPosition a
forall a. Model a => a -> HorizontalPosition a
Geodetic.northPole (a -> HorizontalPosition a)
-> (Position a -> a) -> Position a -> HorizontalPosition a
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Position a -> a
forall a. Model a => Position a -> a
Geodetic.model'