{-# LANGUAGE FlexibleInstances #-} -- | -- Module: Data.Geo.Jord.Frames -- Copyright: (c) 2018 Cedric Liegeois -- License: BSD3 -- Maintainer: Cedric Liegeois <ofmooseandmen@yahoo.fr> -- 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 -- <http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf Gade, K. (2010). A Non-singular Horizontal Position Representation> -- 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 , dx , dy , dz -- * Delta in the north, east, down frame , Ned , ned , nedMetres , north , east , down , bearing , elevation , slantRange -- * 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.Transformation 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 { yaw :: Angle -- ^ body yaw angle (vertical axis). , pitch :: Angle -- ^ body pitch angle (transverse axis). , roll :: Angle -- ^ body roll angle (longitudinal axis). , bOrg :: Vector3d -- ^ frame origin (n-vector). } deriving (Eq, Show) -- | '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 { wanderAzimuth :: Angle -- ^ wander azimuth: angle between x-axis of the frame L and the north direction. , lOrg :: LatLong -- ^ frame origin (latlong). } deriving (Eq, Show) -- | 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 { nOrg :: Vector3d -- ^ frame origin (n-vector). } 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) -- | 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 e@ computes the exact 'Delta' between the two positions @p1@ and @p2@ in frame @f@ -- using earth model @e@. -- -- @ -- let p1 = decimalLatLongHeight 1 2 (metres (-3)) -- let p2 = decimalLatLongHeight 4 5 (metres (-6)) -- let w = decimalDegrees 5 -- wander azimuth -- let d = deltaBetween p1 p2 (frameL w) wgs84 -- d = deltaMetres 359490.579 302818.523 17404.272 -- @ 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. -- -- @ -- let p1 = decimalLatLongHeight 1 2 (metres (-3)) -- let p2 = decimalLatLongHeight 4 5 (metres (-6)) -- let d1 = nedBetween p1 p2 wgs84 -- let d2 = deltaBetween p1 p2 frameN wgs84 -- north d1 = dx d2 -- east d1 = dy d2 -- down d1 = dz d2 -- @ 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@. -- -- @ -- let p0 = decimalLatLongHeight 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 wgs84 = decimalLatLongHeight 49.6918016 3.4812669 (metres 6.007) -- @ 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@. -- -- @ -- let p0 = decimalLatLongHeight 49.66618 3.45063 zero -- targetN p0 (nedMeters 100 200 300) wgs84 = target p0 frameN (deltaMetres 100 200 300) wgs84 -- @ 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))