diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,2 @@ +# 12-20-2023 +- Initial release diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -0,0 +1,20 @@ +Copyright (c) 2021 Travis Whitaker + +Permission is hereby granted, free of charge, to any person obtaining +a copy of this software and associated documentation files (the +"Software"), to deal in the Software without restriction, including +without limitation the rights to use, copy, modify, merge, publish, +distribute, sublicense, and/or sell copies of the Software, and to +permit persons to whom the Software is furnished to do so, subject to +the following conditions: + +The above copyright notice and this permission notice shall be included +in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY +CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, +TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 --- /dev/null +++ b/README.md @@ -0,0 +1,9 @@ +# linear-geo + +Geographic coordinates, built on the linear package. + +This package provides types and functions for dealing with coordinates in +geodetic, ECEF, and ENU coordinate systems. A particular emphasis is placed on +numerical stability, especially for complex conversions like converting between +geodetic and ECEF coordinates. However, not every part of every function has +machine checked for numerical stability. diff --git a/linear-geo.cabal b/linear-geo.cabal new file mode 100644 --- /dev/null +++ b/linear-geo.cabal @@ -0,0 +1,71 @@ +cabal-version: 2.4 +name: linear-geo +version: 0.1.0.0 +synopsis: Geographic coordinates, built on the linear package. +description: + Geographic coordinates, built on the linear package. + . + This package provides types and functions for dealing with coordinates in + geodetic, ECEF, and ENU coordinate systems. A particular emphasis is placed + on numerical stability, especially for complex conversions like converting + between geodetic and ECEF coordinates. However, not every part of every + function has machine checked for numerical stability. + + +homepage: https://github.com/TravisWhitaker/linear-geo +bug-reports: https://github.com/TravisWhitaker/linear-geo +license: MIT +license-file: LICENSE +author: Travis Whitaker +maintainer: pi.boy.travis@gmail.com + +copyright: Travis Whitaker 2023 +category: Math +extra-source-files: + CHANGELOG.md + README.md + +common common + default-language: Haskell2010 + ghc-options: -O2 + -Weverything + -Wcompat + -- -Werror + -Wno-unsafe + -Wno-type-defaults + -Wno-missing-safe-haskell-mode + -Wno-implicit-prelude + -Wno-missing-import-lists + -Wno-prepositive-qualified-module + -Wno-missing-kind-signatures + -Wno-monomorphism-restriction + +library + import: common + exposed-modules: Linear.Geo + Linear.Geo.ECEF + Linear.Geo.ENU + Linear.Geo.Geodetic + Linear.Geo.PlaneAngle + --other-modules: + build-depends: base >=4.14 && <5 + , deepseq >= 1.4 && < 2 + , distributive >= 0.6 && < 1 + , linear >= 1.20 && < 2 + , vector >= 0.13 && < 1 + hs-source-dirs: src + +test-suite props + import: common + type: exitcode-stdio-1.0 + hs-source-dirs: test + main-is: Main.hs + build-depends: base >=4.14 && <5 + , hedgehog >= 1.2 && < 2 + , linear >= 1.20 && < 2 + , linear-geo + , reflection >= 2.1 && < 3 + ghc-options: -threaded + -rtsopts + "-with-rtsopts=-N" + -Wno-all-missed-specialisations diff --git a/src/Linear/Geo.hs b/src/Linear/Geo.hs new file mode 100644 --- /dev/null +++ b/src/Linear/Geo.hs @@ -0,0 +1,22 @@ +{-| +Copyright : Travis Whitaker 2023 +License : MIT +Maintainer : pi.boy.travis@gmail.com +Stability : Provisional +Portability : Portable (Windows, POSIX) + +Various Earth-centric coordinate systems with utilities. + +-} + +module Linear.Geo ( + module Linear.Geo.ECEF + , module Linear.Geo.ENU + , module Linear.Geo.Geodetic + , module Linear.Geo.PlaneAngle + ) where + +import Linear.Geo.ECEF +import Linear.Geo.ENU +import Linear.Geo.Geodetic +import Linear.Geo.PlaneAngle diff --git a/src/Linear/Geo/ECEF.hs b/src/Linear/Geo/ECEF.hs new file mode 100644 --- /dev/null +++ b/src/Linear/Geo/ECEF.hs @@ -0,0 +1,117 @@ +{-| +Module : Linear.Geo.ECEF +Copyright : Travis Whitaker 2023 +License : MIT +Maintainer : pi.boy.travis@gmail.com +Stability : Provisional +Portability : Portable (Windows, POSIX) + +Earth-centered Earth-fixed (ECEF) coordinates. + +-} + +{-# LANGUAGE DataKinds + , DeriveDataTypeable + , DeriveGeneric + , DerivingStrategies + , GeneralizedNewtypeDeriving + , TypeFamilies + #-} + +module Linear.Geo.ECEF ( + ECEF(..) + , cross + , triple + ) where + +import Control.DeepSeq (NFData) + +import Control.Monad.Fix (MonadFix) +import Control.Monad.Zip (MonadZip) + +import Data.Coerce + +import Data.Data (Data) + +import Data.Distributive + +import qualified Data.Vector as V + +import GHC.Generics (Generic) + +import qualified Linear.Affine as L +import qualified Linear.Epsilon as L +import qualified Linear.Matrix as L +import qualified Linear.Metric as L +import qualified Linear.V as L +import qualified Linear.V2 as L +import qualified Linear.V3 as L +import qualified Linear.Vector as L + +-- | R3 vector with the origin at the Earth's center of mass, first basis vector +-- through the intersection of the prime meridian and the equator, and the +-- third basis vector through True North. The origin and basis vectors move +-- and rotate with the Earth through space. +newtype ECEF a = ECEF (L.V3 a) + deriving stock ( Eq + , Ord + , Show + , Generic + , Data + , Bounded + ) + deriving newtype ( Num + , Fractional + , Floating + , Functor + , Applicative + , Monad + , MonadFix + , MonadZip + , Foldable + , L.Additive + , L.Metric + , L.Trace + , L.Epsilon + , NFData + ) + +instance Traversable ECEF where + traverse f ecef = traverse f (coerce ecef) + +instance Distributive ECEF where + distribute f = ECEF $ L.V3 (fmap (\(ECEF (L.V3 x _ _)) -> x) f) + (fmap (\(ECEF (L.V3 _ y _)) -> y) f) + (fmap (\(ECEF (L.V3 _ _ z)) -> z) f) + +instance L.Finite ECEF where + type Size ECEF = 3 + toV (ECEF (L.V3 x y z)) = L.V (V.fromListN 3 [x, y, z]) + fromV (L.V v) = ECEF $ L.V3 (v V.! 0) (v V.! 1) (v V.! 2) + +instance L.R1 ECEF where + _x f (ECEF (L.V3 x y z)) = (\x' -> ECEF (L.V3 x' y z)) <$> f x + +instance L.R2 ECEF where + _y f (ECEF (L.V3 x y z)) = (\y' -> ECEF (L.V3 x y' z)) <$> f y + _xy f (ECEF (L.V3 x y z)) = (\(L.V2 x' y') -> ECEF (L.V3 x' y' z)) + <$> f (L.V2 x y) + +instance L.R3 ECEF where + _z f (ECEF (L.V3 x y z)) = (\z' -> ECEF (L.V3 x y z')) <$> f z + _xyz f (ECEF v) = ECEF <$> f v + +instance L.Affine ECEF where + type Diff ECEF = L.V3 + (ECEF x) .-. (ECEF y) = x L..-. y + (ECEF x) .+^ y = ECEF (x L..+^ y) + (ECEF x) .-^ y = ECEF (x L..-^ y) + +-- | Right-handed orthogonal vector with magnitude equal to the area of the +-- subtended parallelogram. +cross :: Num a => ECEF a -> ECEF a -> ECEF a +cross x y = ECEF $ L.cross (coerce x) (coerce y) + +-- | Scalar triple product. +triple :: Num a => ECEF a -> ECEF a -> ECEF a -> a +triple x y z = L.triple (coerce x) (coerce y) (coerce z) diff --git a/src/Linear/Geo/ENU.hs b/src/Linear/Geo/ENU.hs new file mode 100644 --- /dev/null +++ b/src/Linear/Geo/ENU.hs @@ -0,0 +1,213 @@ +{-| +Module : Linear.Geo.ENU +Copyright : Travis Whitaker 2023 +License : MIT +Maintainer : pi.boy.travis@gmail.com +Stability : Provisional +Portability : Portable (Windows, POSIX) + +East-North-Up coordinates. + +-} + +{-# LANGUAGE DataKinds + , DeriveAnyClass + , DeriveDataTypeable + , DeriveGeneric + , DerivingStrategies + , MagicHash + , ScopedTypeVariables + , TypeFamilies + #-} + +module Linear.Geo.ENU ( + ENU(..) + , alignOrigin + , liftAO2 + , liftAO2V + , rotNormToECEF + , rotNormToECEFFromENU + , enuToECEF + , rotECEFToNorm + , rotECEFToNormFromENU + , ecefToENU + , disp + , diff + , lerp + , dot + , quadrance + , norm + , distance + , normalize + , project + ) where + +import Control.DeepSeq (NFData) + +import Data.Data (Data) + +import GHC.Generics (Generic) + +import GHC.Exts + +import qualified Linear.Affine as L +import qualified Linear.Epsilon as L +import qualified Linear.Matrix as L +import qualified Linear.Metric as L +import qualified Linear.V2 as L +import qualified Linear.V3 as L +import qualified Linear.Vector as L + +import Linear.Geo.ECEF +import Linear.Geo.Geodetic +import Linear.Geo.PlaneAngle + +-- | R3 vector with the origin located at some arbitrary 'ECEF' position vector, +-- first basis pointing east at the origin, second basis vector pointing north +-- at the origin, and third basis vector normal to the plane tangent to the +-- ellipsoid at the origin. +-- +-- Each value records both the ENU vector and the ENU origin. Most functions +-- of multiple ENU values will require the points to occupy coordinal frames. +-- Binary operations on ENU values should preserve the coordinate frame of the +-- /left/ value. +-- +-- The 'Eq' and 'Ord' instances for this type implement structural equality, +-- i.e. ENU points with different 'enuOrigin' values will never be equal. +-- Floating point errors limit the usefulness of +-- exact-equality-as-coincidence. +-- +-- Operations on ENU points use the uncorrected WGS84 geoid model. +data ENU a = ENU { + enuOrigin :: ECEF a + , enuPoint :: L.V3 a + } deriving stock ( Eq + , Ord + , Show + , Generic + , Data + , Bounded + ) + deriving anyclass (NFData) + +instance L.R1 ENU where + _x f (ENU o (L.V3 x y z)) = (\x' -> ENU o (L.V3 x' y z)) <$> f x + +instance L.R2 ENU where + _y f (ENU o (L.V3 x y z)) = (\y' -> ENU o (L.V3 x y' z)) <$> f y + _xy f (ENU o (L.V3 x y z)) = (\(L.V2 x' y') -> ENU o (L.V3 x' y' z)) + <$> f (L.V2 x y) + +instance L.R3 ENU where + _z f (ENU o (L.V3 x y z)) = (\z' -> ENU o (L.V3 x y z')) <$> f z + _xyz f (ENU o v) = ENU o <$> f v + +-- | Align the second argument with the coordinate system of the first. +alignOrigin :: RealFloat a => ENU a -> ENU a -> ENU a +alignOrigin (ENU xo _) y@(ENU yo _) + | isTrue# (reallyUnsafePtrEquality# xo yo) = y + | xo == yo = y + | otherwise = ecefToENU xo (enuToECEF y) + +-- | Lift a function on vectors to a function on origin-aligned ENU points. +liftAO2 :: RealFloat a => (L.V3 a -> L.V3 a -> b) -> ENU a -> ENU a -> b +liftAO2 f x@(ENU _ xp) y = let (ENU _ y'p) = alignOrigin x y + in f xp y'p + +-- | Lift a binary operation on vectors to a binary operation on origin-aligned +-- ENU points. +liftAO2V :: RealFloat a + => (L.V3 a -> L.V3 a -> L.V3 a) + -> ENU a + -> ENU a + -> ENU a +liftAO2V f x@(ENU xo xp) y = let (ENU _ y'p) = alignOrigin x y + in ENU xo (f xp y'p) + +-- | Rotation matrix that rotates the ENU coordinate frame at the provided +-- latitude and longitude to the ECEF coordinate frame. +rotNormToECEF :: Floating a + => Radians a -- ^ lat + -> Radians a -- ^ lon + -> L.M33 a +rotNormToECEF (Radians po) (Radians lo) = + L.V3 (L.V3 (-(sin lo)) ((-(cos lo)) * (sin po)) ((cos lo) * (cos po))) + (L.V3 (cos lo) ((- (sin lo)) * (sin po)) ((sin lo) * (cos po))) + (L.V3 0 (cos po) (sin po) ) + +-- | Do 'rotNormToECEF', but get the lat and lon from some 'ENU's origin. +rotNormToECEFFromENU :: RealFloat a => ENU a -> L.M33 a +rotNormToECEFFromENU (ENU o _) = + let (Geo po lo _) = ecefToGeo o + in rotNormToECEF po lo + +-- | Convert an 'ENU' to an 'ECEF' by adding the rotated position vector to the +-- origin. +enuToECEF :: RealFloat a => ENU a -> ECEF a +enuToECEF enu@(ENU o x) = + let rot = rotNormToECEFFromENU enu + in o L..+^ (rot L.!* x) + +-- | Rotation matrix that rotates the ECEF coordinate frame to the ENU +-- coordinate frame at the provided latitude and longitude. +rotECEFToNorm :: Floating a + => Radians a -- ^ lat + -> Radians a -- ^ lon + -> L.M33 a +rotECEFToNorm (Radians po) (Radians lo) = + L.V3 (L.V3 (-(sin lo)) (cos lo) 0 ) + (L.V3 ((-(cos lo)) * (sin po)) ((-(sin lo)) * (sin po)) (cos po)) + (L.V3 ((cos lo) * (cos po)) ((sin lo) * (cos po)) (sin po)) + +-- | Do 'rotECEFToNorm', but get the lat and lon from some 'ENU's origin. +rotECEFToNormFromENU :: RealFloat a => ENU a -> L.M33 a +rotECEFToNormFromENU (ENU o _) = + let (Geo po lo _) = ecefToGeo o + in rotECEFToNorm po lo + +-- | Pack an 'ECEF' origin and point into an 'ENU'. +ecefToENU :: RealFloat a + => ECEF a -- ^ Origin + -> ECEF a -- ^ Point + -> ENU a +ecefToENU o@(ECEF vo) (ECEF vp) = + let (Geo po lo _) = ecefToGeo o + rot = rotECEFToNorm po lo + x = rot L.!* (vp - vo) + in ENU o x + +-- | Affine addition. Apply a displacement vector. +disp :: Num a => ENU a -> L.V3 a -> ENU a +disp (ENU o p) v = (ENU o (p + v)) + +-- | Affine subtraction. Get the vector from the first to the second ENU point. +diff :: RealFloat a => ENU a -> ENU a -> L.V3 a +diff x y = enuPoint $ liftAO2V (L..-.) x y + +-- | Linearly interpolate between two points. +lerp :: RealFloat a => a -> ENU a -> ENU a -> ENU a +lerp f = liftAO2V (L.lerp f) + +-- | Lifted dot. +dot :: RealFloat a => ENU a -> ENU a -> a +dot = liftAO2 L.dot + +-- | Lifted quadrance. +quadrance :: Num a => ENU a -> a +quadrance = L.quadrance . enuPoint + +-- | Lifted norm. +norm :: Floating a => ENU a -> a +norm = L.norm . enuPoint + +-- | Lifted distance. +distance :: RealFloat a => ENU a -> ENU a -> a +distance = liftAO2 L.distance + +-- | Lifted normalize. +normalize :: (Floating a, L.Epsilon a) => ENU a -> ENU a +normalize (ENU xo xp) = ENU xo (L.normalize xp) + +-- | Lifted project. +project :: RealFloat a => ENU a -> ENU a -> ENU a +project = liftAO2V L.project diff --git a/src/Linear/Geo/Geodetic.hs b/src/Linear/Geo/Geodetic.hs new file mode 100644 --- /dev/null +++ b/src/Linear/Geo/Geodetic.hs @@ -0,0 +1,191 @@ +{-| +Module : Linear.Geo.Geodetic +Copyright : Travis Whitaker 2023 +License : MIT +Maintainer : pi.boy.travis@gmail.com +Stability : Provisional +Portability : Portable (Windows, POSIX) + +Geodetic coordinates. The ellipsoid is not indexed explicitly, but conversion functions +for WGS84 are provided. + +-} + +{-# LANGUAGE BangPatterns + , DeriveAnyClass + , DeriveDataTypeable + , DeriveFunctor + , DeriveGeneric + , DerivingStrategies + #-} + +module Linear.Geo.Geodetic ( + Geo(..) + , normalizeGeo + , fromLatLonAlt + , toLatLonAlt + , simpleEllipsoid + , earthEllipsoid + , ecefToGeoFerrariEllipsoid + , ecefToGeoFerrariEarth + , geoToECEF + , ecefToGeo + ) where + +import Control.Applicative + +import Control.DeepSeq (NFData) + +import Control.Monad.Fix +import Control.Monad.Zip + +import Data.Data (Data) + +import GHC.Generics (Generic) + +import qualified Linear.V3 as L + +import Linear.Geo.ECEF +import Linear.Geo.PlaneAngle + +-- | A point in some geodetic coordinate system, where 'geoLat' is the angle +-- between the normal at the specified point on the ellipsoid and the +-- equatorial plane (north positive, south negative), 'geoLon' is the angle +-- formed by the intersection of the parallel and the prime meridian and the +-- specified point on the parallel, and 'geoAlt' is the magnitude of the +-- position vector minus the magnitude of the unique vector colinear and +-- coordinal with the position vector impingent on the ellipsoid's surface +-- (i.e. height above ellipsoid). Angles are in radians. +data Geo a = Geo { + geoLat :: !(Radians a) + , geoLon :: !(Radians a) + , geoAlt :: !a + } deriving stock ( Eq + , Ord + , Show + , Generic + , Data + , Bounded + , Functor + ) + deriving anyclass (NFData) + +instance Applicative Geo where + pure x = Geo (pure x) (pure x) x + (Geo pf lf hf) <*> (Geo p l h) = Geo (pf <*> p) (lf <*> l) (hf h) + +instance Monad Geo where + return = pure + (Geo (Radians p) (Radians l) h) >>= f = + let Geo p' _ _ = f p + Geo _ l' _ = f l + Geo _ _ h' = f h + in Geo p' l' h' + +instance MonadZip Geo where + mzipWith = liftA2 + +instance MonadFix Geo where + mfix f = Geo (let Geo (Radians p) _ _ = f p in Radians p) + (let Geo _ (Radians l) _ = f l in Radians l) + (let Geo _ _ h = f h in h) + +instance Foldable Geo where + foldMap f (Geo p l h) = foldMap f p <> foldMap f l <> f h + +instance Traversable Geo where + traverse f (Geo p l h) = Geo <$> traverse f p <*> traverse f l <*> f h + +-- | Normalize the two angle components of a `Geo`. +normalizeGeo :: (Floating a, Real a) => Geo a -> Geo a +normalizeGeo (Geo p l a) = Geo (normalizeAngle p) (normalizeAngle l) a + +-- | Convert a pair of angles and a height above the ellipsoid into a 'Geo'. +fromLatLonAlt :: (PlaneAngle lat, PlaneAngle lon, Floating a, Real a) + => lat a -- ^ Latitude + -> lon a -- ^ Longitude + -> a -- ^ Altitude + -> Geo a +fromLatLonAlt lat lon alt = Geo (toRadians lat) (toRadians lon) alt + +-- | Unpack a 'Geo' into latitude, longitude, and height above the ellipsoid. +toLatLonAlt :: (PlaneAngle lat, PlaneAngle lon, Floating a, Real a) + => Geo a + -> (lat a, lon a, a) +toLatLonAlt (Geo p l a) = (fromRadians p, fromRadians l, a) + +-- | Convert from geodetic coordinates to ECEF by assuming the earth is an +-- ellipsoid. +simpleEllipsoid :: Floating a + => a -- ^ Semi-major axis. + -> a -- ^ Semi-minor axis. + -> Geo a + -> ECEF a +simpleEllipsoid a b = + let -- coefficient for adjusted prime vertical radius + dpvr = (b ^ 2) / (a ^ 2) + -- square of first eccentricity + esqr = 1 - dpvr + -- prime vertical radius as function of latitude + pvr p = a / (sqrt (1 - (esqr * ((sin p) ^ 2)))) + proj (Geo (Radians p) (Radians l) h) = + let n = pvr p + nh = n + h + nd = (dpvr * n) + h + in ECEF (L.V3 (nh * cos p * cos l) + (nh * cos p * sin l) + (nd * sin p) + ) + in proj + +-- | Standard WGS84 ellipsoid. +earthEllipsoid :: RealFloat a + => Geo a + -> ECEF a +earthEllipsoid = simpleEllipsoid 6378137 6356752.314245 + +-- | Conversion from ECEF to geodetic coordinates via a numerically stable +-- formulation of Ferrari's closed-form solution to the quartic polynomial. +-- See https://ieeexplore.ieee.org/document/303772/ +ecefToGeoFerrariEllipsoid :: RealFloat a + => a -- ^ Semi-major axis. + -> a -- ^ Semi-minor axis. + -> ECEF a + -> Geo a +ecefToGeoFerrariEllipsoid a b (ECEF (L.V3 x y z)) = + let r = sqrt ((x ^ 2) + (y ^ 2)) + dpvr = (b ^ 2) / (a ^ 2) + esqr = 1 - dpvr + e'sqr = ((a ^ 2) - (b ^ 2)) / (b ^ 2) + eesqr = (a ^ 2) - (b ^ 2) + ff = 54 * (b ^ 2) * (z ^ 2) + gg = (r ^ 2) + ((1 - esqr) * (z ^ 2)) - (esqr * eesqr) + cc = ((esqr ^ 2) * ff * (r ^ 2)) / (gg ^ 3) + ss = (1 + cc + sqrt ((cc ^ 2) + (2 * cc))) ** (1 / 3) + pp = ff / (3 * (((ss + (1 / ss) + 1)) ^ 2) * (gg ^ 2)) + qq = sqrt (1 + (2 * (esqr ^ 2) * pp)) + r0 = ((-(pp * esqr * r)) / (1 + qq)) + + (sqrt ( ((1 / 2) * (a ^ 2) * (1 + (1 / qq))) + - ((pp * (1 - esqr) * (z ^ 2)) / (qq * (1 + qq))) + - ((1 / 2) * pp * (r ^ 2)) + ) + ) + uu = sqrt (((r - (esqr * r0)) ^ 2) + (z ^ 2)) + vv = sqrt (((r - esqr * r0) ^ 2) + ((1 - esqr) * (z ^ 2))) + zz0 = ((b ^ 2) * z) / (a * vv) + h = uu * (1 - ((b ^ 2) / (a * vv))) + p = atan ((z + (e'sqr * zz0)) / r) + l = atan2 y x + in Geo (Radians p) (Radians l) h + +-- | Standard WGS84 ellipsoid. +ecefToGeoFerrariEarth :: RealFloat a => ECEF a -> Geo a +ecefToGeoFerrariEarth = ecefToGeoFerrariEllipsoid 6378137 6356752.314245 + +-- | Synonym for 'earthEllipsoid'. +geoToECEF :: RealFloat a => Geo a -> ECEF a +geoToECEF = earthEllipsoid + +-- | Synonym for 'ecefToGeoFerrariEarth'. +ecefToGeo :: RealFloat a => ECEF a -> Geo a +ecefToGeo = ecefToGeoFerrariEarth diff --git a/src/Linear/Geo/PlaneAngle.hs b/src/Linear/Geo/PlaneAngle.hs new file mode 100644 --- /dev/null +++ b/src/Linear/Geo/PlaneAngle.hs @@ -0,0 +1,288 @@ +{-| +Module : Linear.Geo.PlaneAngle +Copyright : Travis Whitaker 2023 +License : MIT +Maintainer : pi.boy.travis@gmail.com +Stability : Provisional +Portability : Portable (Windows, POSIX) + +Types for dealing with different representations of angles in the plane. + +-} + +{-# LANGUAGE BangPatterns + , DeriveAnyClass + , DeriveDataTypeable + , DeriveFunctor + , DeriveGeneric + , DerivingStrategies + , GeneralizedNewtypeDeriving + #-} + +module Linear.Geo.PlaneAngle ( + PlaneAngle(..) + , Radians(..) + , Degrees(..) + , DMS(..) + , dmsToDegrees + , degreesToDMS + , DM(..) + , dmToDegrees + , degreesToDM + ) where + +import Control.Applicative + +import Control.DeepSeq (NFData) + +import Control.Monad.Fix +import Control.Monad.Zip + +import Data.Coerce + +import Data.Data (Data) + +import Data.Distributive + +import Data.Fixed (divMod', mod') + +import GHC.Generics (Generic) + +-- | Plane angles. +class PlaneAngle ang where + -- | Put the angle into the canonical range 0 to 2*pi. + normalizeAngle :: (Floating a, Real a) => ang a -> ang a + -- | Convert the angle to radians. + toRadians :: (Floating a, Real a) => ang a -> Radians a + -- | Convert the angle from radians. + fromRadians :: (Floating a, Real a) => Radians a -> ang a + +-- | A quantity representing a plane angle that satisfies the equation +-- @S = r * a@ where @r@ is the radius of a circle, @a@ is the measure of some +-- angle subtending the circle, and @S@ is the length of the subtended arc. +newtype Radians a = Radians a + deriving stock ( Eq + , Ord + , Show + , Generic + , Data + , Bounded + , Functor + ) + deriving newtype ( Num + , Fractional + , Floating + , Real + , RealFrac + , RealFloat + , NFData + ) + +instance Applicative Radians where + pure = coerce + Radians f <*> Radians x = Radians (f x) + +instance Monad Radians where + return = pure + Radians x >>= f = f x + +instance MonadZip Radians where + mzipWith = liftA2 + +instance MonadFix Radians where + mfix f = Radians (let Radians x = f x in x) + +instance Foldable Radians where + foldMap f (Radians x) = f x + +instance Traversable Radians where + traverse f (Radians x) = Radians <$> f x + +instance Distributive Radians where + distribute f = Radians (fmap coerce f) + +instance PlaneAngle Radians where + normalizeAngle = coerce . (`mod'` (2 * pi)) + toRadians = id + fromRadians = id + {-# INLINEABLE normalizeAngle #-} + {-# INLINEABLE toRadians #-} + {-# INLINEABLE fromRadians #-} + +-- | One degree is @pi / 180@ radians. +newtype Degrees a = Degrees a + deriving stock ( Eq + , Ord + , Show + , Generic + , Data + , Bounded + , Functor + ) + deriving newtype ( Num + , Fractional + , Floating + , Real + , RealFrac + , RealFloat + , NFData + ) + +instance Applicative Degrees where + pure = coerce + Degrees f <*> Degrees x = Degrees (f x) + +instance Monad Degrees where + return = pure + Degrees x >>= f = f x + +instance MonadZip Degrees where + mzipWith = liftA2 + +instance MonadFix Degrees where + mfix f = Degrees (let Degrees x = f x in x) + +instance Foldable Degrees where + foldMap f (Degrees x) = f x + +instance Traversable Degrees where + traverse f (Degrees x) = Degrees <$> f x + +instance Distributive Degrees where + distribute f = Degrees (fmap coerce f) + +instance PlaneAngle Degrees where + normalizeAngle = coerce . (`mod'` 360) + toRadians (Degrees d) = Radians (pi * (d / 180)) + fromRadians (Radians r) = Degrees ((r / pi) * 180) + {-# INLINEABLE normalizeAngle #-} + {-# INLINEABLE toRadians #-} + {-# INLINEABLE fromRadians #-} + +-- | An angle represented as degrees, minutes, and seconds of arc. +data DMS a = DMS { + dmsDeg :: !a + , dmsMin :: !a + , dmsSec :: !a + } deriving stock ( Eq + , Ord + , Show + , Generic + , Data + , Bounded + , Functor + ) + deriving anyclass (NFData) + +instance Applicative DMS where + pure x = DMS x x x + (DMS df mf sf) <*> (DMS d m s) = DMS (df d) (mf m) (sf s) + +instance Monad DMS where + return = pure + (DMS d m s) >>= f = let DMS d' _ _ = f d + DMS _ m' _ = f m + DMS _ _ s' = f s + in DMS d' m' s' + +instance MonadZip DMS where + mzipWith = liftA2 + +instance MonadFix DMS where + mfix f = DMS (let DMS d _ _ = f d in d) + (let DMS _ m _ = f m in m) + (let DMS _ _ s = f s in s) + +instance Foldable DMS where + foldMap f (DMS d m s) = f d <> f m <> f s + +instance Traversable DMS where + traverse f (DMS d m s) = DMS <$> f d <*> f m <*> f s + +instance Distributive DMS where + distribute f = DMS (fmap (\(DMS d _ _) -> d) f) + (fmap (\(DMS _ m _) -> m) f) + (fmap (\(DMS _ _ s) -> s) f) + +-- | Convert DMS to Degrees. This does not normalize the angle. +dmsToDegrees :: Fractional a => DMS a -> Degrees a +dmsToDegrees (DMS d m s) = Degrees (d + (m * (1 / 60)) + (s * (1 / 3600))) +{-# INLINEABLE dmsToDegrees #-} + +-- | Convert degrees to DMS. This does not normalize the angle. +degreesToDMS :: (Real a, Fractional a) => Degrees a -> DMS a +degreesToDMS (Degrees d) = + let (dint, dleft) = divMod' d 1 + (mint, mleft) = divMod' dleft (1 / 60) + sleft = mleft / (1 / 3600) + in DMS (fromIntegral dint) (fromIntegral mint) sleft +{-# INLINEABLE degreesToDMS #-} + +instance PlaneAngle DMS where + normalizeAngle = degreesToDMS . normalizeAngle . dmsToDegrees + toRadians = toRadians . dmsToDegrees + fromRadians = degreesToDMS . fromRadians + {-# INLINEABLE normalizeAngle #-} + {-# INLINEABLE toRadians #-} + {-# INLINEABLE fromRadians #-} + +-- | An angle represented as degrees and minutes of arc. +data DM a = DM { + dmDeg :: !a + , dmMin :: !a + } deriving stock ( Eq + , Ord + , Show + , Generic + , Data + , Bounded + , Functor + ) + deriving anyclass (NFData) + +instance Applicative DM where + pure x = DM x x + (DM df mf) <*> (DM d m) = DM (df d) (mf m) + +instance Monad DM where + return = pure + (DM d m) >>= f = let DM d' _ = f d + DM _ m' = f m + in DM d' m' + +instance MonadZip DM where + mzipWith = liftA2 + +instance MonadFix DM where + mfix f = DM (let DM d _ = f d in d) + (let DM _ m = f m in m) + +instance Foldable DM where + foldMap f (DM d m) = f d <> f m + +instance Traversable DM where + traverse f (DM d m) = DM <$> f d <*> f m + +instance Distributive DM where + distribute f = DM (fmap (\(DM d _) -> d) f) + (fmap (\(DM _ m) -> m) f) + +-- | Convert DM to degrees. This does not normalize the angle. +dmToDegrees :: Fractional a => DM a -> Degrees a +dmToDegrees (DM d m) = Degrees (d + (m * (1 / 60))) +{-# INLINEABLE dmToDegrees #-} + +-- | Convert degrees to DM. This does not normalize the angle. +degreesToDM :: (Fractional a, Real a) => Degrees a -> DM a +degreesToDM (Degrees d) = + let (dint, m) = divMod' d 1 + in DM (fromIntegral dint) (m / (1 / 60)) +{-# INLINEABLE degreesToDM #-} + +instance PlaneAngle DM where + normalizeAngle = degreesToDM . normalizeAngle . dmToDegrees + toRadians = toRadians . dmToDegrees + fromRadians = degreesToDM . fromRadians + {-# INLINEABLE normalizeAngle #-} + {-# INLINEABLE toRadians #-} + {-# INLINEABLE fromRadians #-} diff --git a/test/Main.hs b/test/Main.hs new file mode 100644 --- /dev/null +++ b/test/Main.hs @@ -0,0 +1,274 @@ +{-# LANGUAGE OverloadedStrings + , TypeFamilies + , FlexibleContexts + , ConstrainedClassMethods + , UndecidableInstances + #-} + +module Main (main) where + +import Data.Reflection + +import Hedgehog +import Hedgehog.Main +import qualified Hedgehog.Gen as HG +import qualified Hedgehog.Range as HR + +import qualified Linear as L +import Linear.Geo hiding (diff) + +-- from the numeric-limits package: + +maxValue :: (RealFloat a) => a +maxValue = x + where n = floatDigits x + b = floatRadix x + (_, u) = floatRange x + x = encodeFloat (b ^ n - 1) (u - n) + +minValue :: (RealFloat a) => a +minValue = -maxValue + +hugeValFRange :: Range Double +hugeValFRange = HR.exponentialFloatFrom 0 minValue maxValue + +hugeValFs :: MonadGen m => m Double +hugeValFs = HG.double hugeValFRange + +modBugRange :: Range Double +modBugRange = + let l = 1e15 + in HR.exponentialFloatFrom 0 (-l) l + +modBug :: MonadGen m => m Double +modBug = HG.double modBugRange + +class ApproxEq a where + type family Atom a + (~=~) :: Given (Atom a) => a -> a -> Bool + +instance ApproxEq Double where + type Atom Double = Double + x ~=~ y = (abs (x - y)) / ((x + y + given) / 2) <= given + +instance (ApproxEq a, Given (Atom a)) => ApproxEq (Radians a) where + type Atom (Radians a) = a + (Radians x) ~=~ (Radians y) = x ~=~ y + +instance (ApproxEq a, Given (Atom a)) => ApproxEq (Degrees a) where + type Atom (Degrees a) = a + (Degrees x) ~=~ (Degrees y) = x ~=~ y + +instance (ApproxEq a, Given (Atom a)) => ApproxEq (DMS a) where + type Atom (DMS a) = a + (DMS xd xm xs) ~=~ (DMS yd ym ys) = (xd ~=~ yd) + && (xm ~=~ ym) + && (xs ~=~ ys) + +instance (ApproxEq a, Given (Atom a)) => ApproxEq (DM a) where + type Atom (DM a) = a + (DM xd xm) ~=~ (DM yd ym) = (xd ~=~ yd) + && (xm ~=~ ym) + +instance (ApproxEq a, Given (Atom a)) => ApproxEq (L.V3 a) where + type Atom (L.V3 a) = a + (L.V3 ax ay az) ~=~ (L.V3 bx by bz) = (ax ~=~ bx) + && (ay ~=~ by) + && (az ~=~ bz) + +instance (ApproxEq a, Given (Atom a)) => ApproxEq (ECEF a) where + type Atom (ECEF a) = a + (ECEF x) ~=~ (ECEF y) = x ~=~ y + +instance (ApproxEq a, Given (Atom a)) => ApproxEq (Geo a) where + type Atom (Geo a) = a + (Geo ap al ah) ~=~ (Geo bp bl bh) = (ap ~=~ bp) + && (al ~=~ bl) + && (ah ~=~ bh) + +instance (RealFloat a, ApproxEq a, Given (Atom a)) => ApproxEq (ENU a) where + type Atom (ENU a) = a + x@(ENU _ xp) ~=~ y = let ENU _ y'p = alignOrigin x y + in xp ~=~ y'p + +(=~=) :: (ApproxEq a, Given (Atom a), Show a, MonadTest m) => a -> a -> m () +(=~=) a b = diff a (~=~) b + +--genECEF :: MonadGen m => m Double -> m (ECEF Double) +--genECEF gd = ECEF <$> (L.V3 <$> gd <*> gd <*> gd) + +--genENU :: MonadGen m => m Double -> m (ENU Double) +--genENU gd = do +-- og <- genGeo gd +-- pv <- L.V3 <$> gd <*> gd <*> gd +-- pure (ENU (geoToECEF og) pv) + +genGeo :: MonadGen m => m Double -> m (Geo Double) +genGeo gd = Geo <$> (normalizeAngle <$> genRad gd) + <*> (normalizeAngle <$> genRad gd) + <*> gd + +genRad :: MonadGen m => m Double -> m (Radians Double) +genRad = fmap Radians + +genDeg :: MonadGen m => m Double -> m (Degrees Double) +genDeg = fmap Degrees + +genDMS :: MonadGen m => m Double -> m (DMS Double) +genDMS gd = DMS <$> gd <*> gd <*> gd + +genDM :: MonadGen m => m Double -> m (DM Double) +genDM gd = DM <$> gd <*> gd + +radNormRange :: Property +radNormRange = property $ do + r <- forAll $ genRad modBug + let (Radians r') = normalizeAngle r + assert ((r' >= 0) && (r' < (2 * pi))) + +radNormIdemp :: Property +radNormIdemp = property $ do + r <- forAll $ genRad modBug + let r' = normalizeAngle r + r' === normalizeAngle r' + +radToRadFromRadIdemp :: Property +radToRadFromRadIdemp = property $ do + r <- forAll $ genRad hugeValFs + r === fromRadians (toRadians r) + +degNormRange :: Property +degNormRange = property $ do + d <- forAll $ genDeg modBug + let (Degrees d') = normalizeAngle d + assert ((d' >= 0) && (d' < 360)) + +degNormIdemp :: Property +degNormIdemp = property $ do + d <- forAll $ genDeg modBug + let d' = normalizeAngle d + d' === normalizeAngle d' + +degToRadFromRadIdemp :: Property +degToRadFromRadIdemp = property $ do + d <- forAll $ genDeg hugeValFs + let r = toRadians d + give 1e-15 (d =~= fromRadians r) + +dmsNormRange :: Property +dmsNormRange = property $ do + d <- forAll $ genDMS modBug + let dmsn = normalizeAngle d + (Degrees d') = dmsToDegrees dmsn + assert ((d' >= 0) && (d' < 360)) + +dmsNormIdemp :: Property +dmsNormIdemp = property $ do + d <- forAll $ genDMS modBug + let d' = normalizeAngle d + give 1e-10 (d' =~= normalizeAngle d') + +-- Stability is so bad, practically this does not hold. +--dmsToRadFromRadIdemp :: Property +--dmsToRadFromRadIdemp = property $ do +-- d <- forAll $ genDMS modBug +-- let r = toRadians d +-- give 1e-2 (normalizeAngle d =~= normalizeAngle (fromRadians r)) + +dmNormRange :: Property +dmNormRange = property $ do + d <- forAll $ genDM modBug + let dmn = normalizeAngle d + (Degrees d') = dmToDegrees dmn + assert ((d' >= 0) && (d' < 360)) + +dmNormIdemp :: Property +dmNormIdemp = property $ do + d <- forAll $ genDM modBug + let d' = normalizeAngle d + d' === normalizeAngle d' + +-- Stability is so bad, practically this does not hold. +--dmToRadFromRadIdemp :: Property +--dmToRadFromRadIdemp = property $ do +-- d <- forAll $ genDM modBug +-- let r = toRadians d +-- give 1e-2 (normalizeAngle d =~= normalizeAngle (fromRadians r)) + +radToFromLatLonIdemp :: Property +radToFromLatLonIdemp = property $ do + g <- forAll $ genGeo hugeValFs + let p :: Radians Double + l :: Radians Double + h :: Double + (p, l, h) = toLatLonAlt g + g' = fromLatLonAlt p l h + g === g' + +degToFromLatLonIdemp :: Property +degToFromLatLonIdemp = property $ do + g <- forAll $ genGeo hugeValFs + let p :: Degrees Double + l :: Degrees Double + h :: Double + (p, l, h) = toLatLonAlt g + g' = fromLatLonAlt p l h + give 1e-8 (g =~= g') + +dmsToFromLatLonIdemp :: Property +dmsToFromLatLonIdemp = property $ do + g <- forAll $ genGeo hugeValFs + let p :: DMS Double + l :: DMS Double + h :: Double + (p, l, h) = toLatLonAlt g + g' = fromLatLonAlt p l h + give 1e-8 (g =~= g') + +dmToFromLatLonIdemp :: Property +dmToFromLatLonIdemp = property $ do + g <- forAll $ genGeo hugeValFs + let p :: DM Double + l :: DM Double + h :: Double + (p, l, h) = toLatLonAlt g + g' = fromLatLonAlt p l h + give 1e-8 (g =~= g') + +-- | Not true due to NaNs, need to figure out why... +--geoToFromECEFIdemp :: Property +--geoToFromECEFIdemp = property $ do +-- g <- forAll $ genGeo hugeValFs +-- give 1e-7 +-- (g =~= normalizeGeo (ecefToGeo (geoToECEF g))) + +-- | Not true due to NaNs, need to figure out why... +--enuToFromECEFIdemp :: Property +--enuToFromECEFIdemp = property $ do +-- p@(ENU o _) <- forAll $ genENU hugeValFs +-- give 1e-8 (p =~= ecefToENU o (enuToECEF p)) + +main :: IO () +main = defaultMain $ (:[]) $ checkParallel $ Group "Linear.Geo" + [ ("normalizeAngle range check @Radians", radNormRange) + , ("normalizeAngle idempotent @Radians", radNormIdemp) + , ("(fromRadians . toRadians) == id @Radians", radToRadFromRadIdemp) + , ("normalizeAngle range check @Degrees", degNormRange) + , ("normalizeAngle idempotent @Degrees", degNormIdemp) + , ("(fromRadians . toRadians) == id @Degrees", degToRadFromRadIdemp) + , ("normalizeAngle range check @DMS", dmsNormRange) + , ("normalizeAngle idempotent @DMS", dmsNormIdemp) + -- Does not hold in practice due to stability + --, ("(fromRadians . toRadians) == id @DMS", dmsToRadFromRadIdemp) + , ("normalizeAngle range check @DM", dmNormRange) + , ("normalizeAngle idempotent @DM", dmNormIdemp) + -- Does not hold in practice due to stability + --, ("(fromRadians . toRadians) == id @DM", dmToRadFromRadIdemp) + , ("(fromLatLon . toLatLon) == id @Radians", radToFromLatLonIdemp) + , ("(fromLatLon . toLatLon) == id @Degrees", degToFromLatLonIdemp) + , ("(fromLatLon . toLatLon) == id @DMS", dmsToFromLatLonIdemp) + , ("(fromLatLon . toLatLon) == id @DM", dmToFromLatLonIdemp) + -- Not true due to NaNs, need to figure out why... + --, ("(ecefToGeo . geoToECEF) == id", geoToFromECEFIdemp) + --, ("(ecefToENU . enuToECEF) == id", enuToFromECEFIdemp) + ]