-- | -- Module: Data.Geo.Jord.LocalFrames -- Copyright: (c) 2020 Cedric Liegeois -- License: BSD3 -- Maintainer: Cedric Liegeois -- 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 Data.Geo.Jord.LocalFrames -- import Data.Geo.Jord.Position -- @ -- -- All functions are implemented using the vector-based approached described in -- -- -- 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.LocalFrames ( -- * Local Reference frame LocalFrame(..) -- * 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 , delta , deltaMetres , dx , dy , dz -- * Delta in the north, east, down frame , Ned , ned , nedMetres , north , east , down , bearing , elevation , slantRange -- * Calculations , deltaBetween , nedBetween , target , targetN -- * re-exported for convenience , module Data.Geo.Jord.Rotation ) where import Data.Geo.Jord.Position 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 LocalFrame 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 a = FrameB { yaw :: Angle -- ^ body yaw angle (vertical axis). , pitch :: Angle -- ^ body pitch angle (transverse axis). , roll :: Angle -- ^ body roll angle (longitudinal axis). , bOrigin :: Position a -- ^ frame origin. } deriving (Eq, Show) -- | 'FrameB' from given yaw, pitch, roll, position (origin). frameB :: (Model a) => Angle -> Angle -> Angle -> Position a -> FrameB a frameB = FrameB -- | R_EB: frame B to Earth instance LocalFrame (FrameB a) 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 a = FrameL { wanderAzimuth :: Angle -- ^ wander azimuth: angle between x-axis of the frame L and the north direction. , lOrigin :: Position a -- ^ frame origin. } deriving (Eq, Show) -- | R_EL: frame L to Earth instance LocalFrame (FrameL m) where rEF (FrameL w o) = rm where lat = latitude o lon = longitude o r = xyz2r lon (negate' lat) 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). frameL :: (Model a) => Angle -> Position a -> FrameL a frameL = 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. -- newtype FrameN a = FrameN { nOrigin :: Position a -- ^ frame origin. } deriving (Eq, Show) -- | R_EN: frame N to Earth instance LocalFrame (FrameN a) where rEF (FrameN o) = transpose rm where vo = nvec o np = nvNorthPole rd = vscale vo (-1) -- down (pointing opposite to n-vector) re = vunit (vcross np vo) -- east (pointing perpendicular to the plane) rn = vcross re rd -- north (by right hand rule) rm = [rn, re, rd] -- | 'FrameN' from given position (origin). frameN :: (Model a) => Position a -> FrameN a frameN = FrameN -- | 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) -- | x component of given 'Delta'. dx :: Delta -> Length dx (Delta v) = metres (vx v) -- | y component of given 'Delta'. dy :: Delta -> Length dy (Delta v) = metres (vy v) -- | z component of given 'Delta'. dz :: Delta -> Length dz (Delta v) = metres (vz v) -- | 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)) -- | @slantRange v@ computes the distance from origin in the local system of the NED vector @v@. slantRange :: Ned -> Length slantRange (Ned v) = metres (vnorm v) -- | @deltaBetween p1 p2 f@ computes the exact 'Delta' between the two -- positions @p1@ and @p2@ in local frame @f@. -- -- ==== __Examples__ -- -- >>> import Data.Geo.Jord.LocalFrames -- >>> import Data.Geo.Jord.Position -- >>> -- >>> let p1 = wgs84Pos 1 2 (metres (-3)) -- >>> let p2 = wgs84Pos 4 5 (metres (-6)) -- >>> let w = decimalDegrees 5 -- wander azimuth -- >>> deltaBetween p1 p2 (frameL w) -- Delta (Vector3d {vx = 359490.5782, vy = 302818.5225, vz = 17404.2714}) -- deltaBetween :: (LocalFrame a, Model b) => Position b -> Position b -> (Position b -> a) -> Delta deltaBetween p1 p2 f = deltaMetres (vx d) (vy d) (vz d) where g1 = gcvec p1 g2 = gcvec p2 de = vsub g2 g1 -- rotation matrix to go from Earth Frame to Frame at p1 rm = transpose (rEF (f p1)) d = vmultm de rm -- | @nedBetween p1 p2@ computes the exact 'Ned' vector between the two -- positions @p1@ and @p2@, in north, east, and down. -- -- 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: -- -- @ -- 'deltaBetween' p1 p2 'frameN' -- @ -- -- ==== __Examples__ -- -- >>> import Data.Geo.Jord.LocalFrames -- >>> import Data.Geo.Jord.Position -- >>> -- >>> let p1 = wgs84Pos 1 2 (metres (-3)) -- >>> let p2 = wgs84Pos 4 5 (metres (-6)) -- >>> nedBetween p1 p2 -- Ned (Vector3d {vx = 331730.2348, vy = 332997.875, vz = 17404.2714}) -- nedBetween :: (Model a) => Position a -> Position a -> Ned nedBetween p1 p2 = nedMetres (vx d) (vy d) (vz d) where (Delta d) = deltaBetween p1 p2 frameN -- | @target p0 f d@ computes the target position from position @p0@ and delta @d@ in local frame @f@. -- -- ==== __Examples__ -- -- >>> import Data.Geo.Jord.LocalFrames -- >>> import Data.Geo.Jord.Position -- >>> -- >>> let p0 = wgs84Pos 49.66618 3.45063 zero -- >>> let y = decimalDegrees 10 -- yaw -- >>> let r = decimalDegrees 20 -- roll -- >>> let p = decimalDegrees 30 -- pitch -- >>> let d = deltaMetres 3000 2000 100 -- >>> target p0 (frameB y r p) d -- 49°41'30.486"N,3°28'52.561"E 6.0077m (WGS84) -- target :: (LocalFrame a, Model b) => Position b -> (Position b -> a) -> Delta -> Position b target p0 f (Delta d) = geocentricMetresPos x y z (model p0) where g0 = gcvec p0 rm = rEF (f p0) c = vmultm d rm (Vector3d x y z) = vadd g0 c -- | @targetN p0 d@ computes the target position from position @p0@ and north, east, down @d@. -- -- This is equivalent to: -- -- @ -- 'target' p0 'frameN' ('Delta' d) -- @ -- -- ==== __Examples__ -- -- >>> import Data.Geo.Jord.LocalFrames -- >>> import Data.Geo.Jord.Position -- >>> -- >>> let p0 = wgs84Pos 49.66618 3.45063 zero -- >>> targetN p0 (nedMeters 100 200 300) -- 49°40'1.485"N,3°27'12.242"E -299.9961m (WGS84) -- targetN :: (Model a) => Position a -> Ned -> Position a targetN p0 (Ned d) = target p0 frameN (Delta d)