{-# 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))