{-# LANGUAGE FlexibleInstances #-}
-- |
-- Module: Data.Geo.Jord.Frames
-- Copyright: (c) 2018 Cedric Liegeois
-- License: BSD3
-- Maintainer: Cedric Liegeois
-- Stability: experimental
-- Portability: portable
--
-- Type and functions for working with delta vectors in different reference frames.
--
-- All functions are implemented using the vector-based approached described in
--
--
module Data.Geo.Jord.Frames
(
-- * Reference Frames
Frame(..)
-- ** Body frame
, FrameB
, yaw
, pitch
, roll
, frameB
-- ** Local frame
, FrameL
, wanderAzimuth
, frameL
-- ** North-East-Down frame
, FrameN
, frameN
-- * Deltas
, Delta
, delta
, deltaMetres
-- * Delta in the north, east, down frame
, Ned
, ned
, nedMetres
, north
, east
, down
, bearing
, elevation
, norm
-- * Calculations
, deltaBetween
, nedBetween
, target
, targetN
) where
import Data.Geo.Jord.Angle
import Data.Geo.Jord.AngularPosition
import Data.Geo.Jord.Earth
import Data.Geo.Jord.EcefPosition
import Data.Geo.Jord.LatLong
import Data.Geo.Jord.Length
import Data.Geo.Jord.NVector
import Data.Geo.Jord.Rotation
import Data.Geo.Jord.Transform
import Data.Geo.Jord.Vector3d
-- | class for reference frames.
--
-- Supported frames:
--
-- * 'FrameB': 'rEF' returns R_EB
--
-- * 'FrameL': 'rEF' returns R_EL
--
-- * 'FrameN': 'rEF' returns R_EN
--
class Frame a where
rEF :: a -> [Vector3d] -- ^ 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 =
FrameB Angle
Angle
Angle
Vector3d
deriving (Eq, Show)
-- | body yaw angle (vertical axis).
yaw :: FrameB -> Angle
yaw (FrameB a _ _ _) = a
-- | body pitch angle (transverse axis).
pitch :: FrameB -> Angle
pitch (FrameB _ a _ _) = a
-- | body roll angle (longitudinal axis).
roll :: FrameB -> Angle
roll (FrameB _ _ a _) = a
-- | 'FrameB' from given yaw, pitch, roll, position (origin) and earth model.
frameB :: (ETransform a) => Angle -> Angle -> Angle -> a -> Earth -> FrameB
frameB yaw' pitch' roll' p e = FrameB yaw' pitch' roll' (nvec p e)
-- | R_EB: frame B to Earth
instance Frame FrameB where
rEF (FrameB y p r o) = rm
where
rNB = zyx2r y p r
n = FrameN o
rEN = rEF n
rm = mdot rEN 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 =
FrameL Angle
LatLong
deriving (Eq, Show)
-- | wander azimuth: angle between x-axis of the frame L and the north direction.
wanderAzimuth :: FrameL -> Angle
wanderAzimuth (FrameL a _) = a
-- | R_EL: frame L to Earth
instance Frame FrameL where
rEF (FrameL w o) = rm
where
r = xyz2r (longitude o) (negate' (latitude o)) w
rEe' = [Vector3d 0 0 (-1), Vector3d 0 1 0, Vector3d 1 0 0]
rm = mdot rEe' r
-- | 'FrameL' from given wander azimuth, position (origin) and earth model.
frameL :: (ETransform a) => Angle -> a -> Earth -> FrameL
frameL w p e = FrameL w ll
where
v = pos (ecefToNVector (toEcef p e) e)
ll = nvectorToLatLong v
-- | 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.
--
newtype FrameN =
FrameN Vector3d
deriving (Eq, Show)
-- | R_EN: frame N to Earth
instance Frame FrameN where
rEF (FrameN o) = transpose rm
where
np = vec northPole
rd = vscale o (-1) -- down (pointing opposite to n-vector)
re = vunit (vcross np o) -- east (pointing perpendicular to the plane)
rn = vcross re rd -- north (by right hand rule)
rm = [rn, re, rd]
-- | 'FrameN' from given position (origin) and earth model.
frameN :: (ETransform a) => a -> Earth -> FrameN
frameN p e = FrameN (nvec p e)
-- | delta between position in one of the reference frames.
newtype Delta =
Delta Vector3d
deriving (Eq, Show)
-- | 'Delta' from given x, y and z length.
delta :: Length -> Length -> Length -> Delta
delta x y z = Delta (Vector3d (toMetres x) (toMetres y) (toMetres z))
-- | 'Delta' from given x, y and z length in _metres_.
deltaMetres :: Double -> Double -> Double -> Delta
deltaMetres x y z = delta (metres x) (metres y) (metres z)
-- | North, east and down delta (thus in frame 'FrameN').
newtype Ned =
Ned Vector3d
deriving (Eq, Show)
-- | 'Ned' from given north, east and down.
ned :: Length -> Length -> Length -> Ned
ned n e d = Ned (Vector3d (toMetres n) (toMetres e) (toMetres d))
-- | 'Ned' from given north, east and down in _metres_.
nedMetres :: Double -> Double -> Double -> Ned
nedMetres n e d = ned (metres n) (metres e) (metres d)
-- | North component of given 'Ned'.
north :: Ned -> Length
north (Ned v) = metres (vx v)
-- | East component of given 'Ned'.
east :: Ned -> Length
east (Ned v) = metres (vy v)
-- | Down component of given 'Ned'.
down :: Ned -> Length
down (Ned v) = metres (vz v)
-- | @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 v =
let a = atan2' (toMetres (east v)) (toMetres (north v))
in normalise a (decimalDegrees 360.0)
-- | @elevation v@ computes the elevation of the NED vector @v@ from horizontal (ie tangent to ellipsoid surface).
elevation :: Ned -> Angle
elevation (Ned v) = negate' (asin' (vz v / vnorm v))
-- | @norm v@ computes the norm of the NED vector @v@.
norm :: Ned -> Length
norm (Ned v) = metres (vnorm v)
-- | @deltaBetween p1 p2 f e@ computes the exact 'Delta' between the two positions @p1@ and @p2@ in frame @f@
-- using earth model @e@.
deltaBetween :: (ETransform a, Frame c) => a -> a -> (a -> Earth -> c) -> Earth -> Delta
deltaBetween p1 p2 f e = deltaMetres (vx d) (vy d) (vz d)
where
e1 = ecefvec p1 e
e2 = ecefvec p2 e
de = vsub e2 e1
-- rotation matrix to go from Earth Frame to Frame at p1
rm = transpose (rEF (f p1 e))
d = vrotate de rm
-- | @nedBetween p1 p2 e@ computes the exact 'Ned' vector between the two positions @p1@ and @p2@, in north, east, and down
-- using earth model @e@.
--
-- Produced '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.
nedBetween :: (ETransform a) => a -> a -> Earth -> Ned
nedBetween p1 p2 e = nedMetres (vx d) (vy d) (vz d)
where
(Delta d) = deltaBetween p1 p2 frameN e
-- | @target p0 f d e@ computes the target position from position @p0@ and delta @d@ using in frame @f@
-- and using earth model @e@.
target :: (ETransform a, Frame c) => a -> (a -> Earth -> c) -> Delta -> Earth -> a
target p0 f (Delta d) e = fromEcef (ecefMetres (vx e0 + vx c) (vy e0 + vy c) (vz e0 + vz c)) e
where
e0 = ecefvec p0 e
rm = rEF (f p0 e)
c = vrotate d rm
-- | @targetN p0 d e@ computes the target position from position @p0@ and north, east, down @d@ using earth model @e@.
targetN :: (ETransform a) => a -> Ned -> Earth -> a
targetN p0 (Ned d) = target p0 frameN (Delta d)
-- | ECEF position (as a 'Vector3d') from given position.
ecefvec :: (ETransform a) => a -> Earth -> Vector3d
ecefvec p m = vec (toEcef p m)
-- | NVector (as a 'Vector3d') from given positon.
nvec :: (ETransform a) => a -> Earth -> Vector3d
nvec p e = vec (pos (ecefToNVector (toEcef p e) e))