{-# LANGUAGE FlexibleInstances #-} -- | An implementation of Thaddeus Vincenty's direct and inverse geodetic algorithms. module Data.Geo.Vincenty( convergence, VincentyDirect, direct, VincentyInverse, inverse ) where import Data.Geo.Ellipsoid import Data.Geo.Coord import Data.Geo.Bearing import Data.Geo.Radians import Data.Geo.GeodeticCurve import Data.Geo.Accessor.Lat import Data.Geo.Accessor.Lon import Control.Arrow import Control.Monad -- | An acceptable convergence value. convergence :: Double convergence = 0.0000000000001 class VincentyDirect a where direct :: a -> Coord -> Bearing -> Double -> (Coord, Bearing) instance VincentyDirect (Double, Ellipsoid) where direct (conv, e) start bear dist = let sMnr = semiMinor e flat = flattening e alpha = toRadians bear cosAlpha = cos alpha sinAlpha = sin alpha tanu1 = (1.0 - flat) * tan (toRadians . lat $ start) cosu1 = 1.0 / sqrt (1.0 + square tanu1) sinu1 = tanu1 * cosu1 sigma1 = atan2 tanu1 cosAlpha csa = cosu1 * sinAlpha sin2Alpha = square csa cos2Alpha = 1 - sin2Alpha ab d f g h i = let s = cos2Alpha * (square (semiMajor e / sMnr) - 1) in (s / d) * (f + s * (g + s * (h - i * s))) a = 1 + ab 16384 4096 (-768) 320 175 b = ab 1024 256 (-128) 74 47 end = let begin = ps (dist / sMnr / a) iter p = let tf d = -3 + 4 * d cosSigma'' = cosSigma' p sinSigma'' = sinSigma' p cosSigmaM2'' = cosSigmaM2' sigma1 p cos2SigmaM2'' = cos2SigmaM2' sigma1 p deltaSigma = b * sinSigma'' * (cosSigmaM2'' + b / 4.0 * (cosSigma'' * (-1 + 2 * cos2SigmaM2'') - (b / 6.0) * cosSigmaM2'' * tf (square sinSigma'') * tf cos2SigmaM2'')) in transition p deltaSigma pred' p = abs (sigma' p - prevSigma' p) >= conv in doWhile iter pred' begin sigma'' = sigma' end sinSigma = sinSigma' end cosSigmaM2 = cosSigmaM2' sigma1 end cos2SigmaM2 = cos2SigmaM2' sigma1 end cosSigma = cos sigma'' c = flat / 16 * cos2Alpha * (4 + flat * (4 - 3 * cos2Alpha)) cc = cosu1 * cosSigma ccca = cc * cosAlpha sss = sinu1 * sinSigma latitude' = fromRadians (atan2 (sinu1 * cosSigma + cosu1 * sinSigma * cosAlpha) ((1.0 - flat) * sqrt (sin2Alpha + (sss - ccca) ** 2.0))) longitude = lon start + fromRadians (atan2 (sinSigma * sinAlpha) (cc - sss * cosAlpha) - (1 - c) * flat * csa * (sigma'' + c * sinSigma * (cosSigmaM2 + c * cosSigma * (-1 + 2 * cos2SigmaM2)))) in (latitude' |.| longitude, fromRadians (atan2 csa (ccca - sss))) instance VincentyDirect Double where direct d = direct (d, wgs84) instance VincentyDirect Ellipsoid where direct e = direct (convergence, e) instance VincentyDirect () where direct _ = direct (convergence, wgs84) class VincentyInverse a where inverse :: a -> Coord -> Coord -> GeodeticCurve instance VincentyInverse (Double, Ellipsoid) where inverse (conv, e) start end = let b = semiMinor e f = flattening e (phi1, phi2) = let rl k = toRadians . lat $ k in (rl start, rl end) a2b2b2 = let ss z = square (z e) in ss semiMajor / ss semiMinor - 1 omega = let rl k = toRadians . lon $ k in rl end - rl start (u1, u2) = let at = atan . ((1 - f) *) . tan in (at phi1, at phi2) sinu1 = sin u1 cosu1 = cos u1 sinu2 = sin u2 cosu2 = cos u2 sinu1sinu2 = sinu1 * sinu2 cosu1sinu2 = cosu1 * sinu2 sinu1cosu2 = sinu1 * cosu2 cosu1cosu2 = cosu1 * cosu2 begin = Q 0 Continue omega 0 0 0 iter q = let sinlambda = sin (lambda q) coslambda = cos (lambda q) sin2sigma = square cosu2 * square sinlambda + square (cosu1sinu2 - sinu1cosu2 * coslambda) sinsigma = sqrt sin2sigma cossigma = sinu1sinu2 + cosu1cosu2 * coslambda sigma'' = atan2 sinsigma cossigma sinalpha = if sin2sigma == 0.0 then 0.0 else cosu1cosu2 * sinlambda / sinsigma alpha = asin sinalpha cos2alpha = square (cos alpha) cos2sigmam = if cos2alpha == 0.0 then 0.0 else cossigma - 2 * sinu1sinu2 / cos2alpha u2' = cos2alpha * a2b2b2 cos2sigmam2 = square cos2sigmam a = 1.0 + u2' / 16384 * (4096 + u2' * (u2' * (320 - 175 * u2') - 768)) b' = u2' / 1024 * (256 + u2' * (u2' * (74 - 47 * u2') - 128)) deltasigma' = b' * sinsigma * (cos2sigmam + b' / 4 * (cossigma * (2 * cos2sigmam2 - 1) - b' / 6 * cos2sigmam * (4 * sin2sigma - 3) * (cos2sigmam2 * 4 - 3))) c' = f / 16 * cos2alpha * (4 + f * (4 - 3 * cos2alpha)) l = omega + (1 - c') * f * sinalpha * (sigma'' + c' * sinsigma * (cos2sigmam + c' * cossigma * (2 * cos2sigmam2 - 1))) r = let c = count q in if c == 20 then Limit else if c > 1 && cos alpha < conv then Converge else Continue in Q (count q + 1) r l a sigma'' deltasigma' pred' = (== Continue) . result ed = whileDo iter pred' begin ifi p t a = if p a then t a else a (alpha1, alpha2) = let alphaNoConverge c cp x y = join (***) (ifi (>= 360) (subtract 360)) (if c then (x, y) else if cp == GT then (180.0, 0.0) else if cp == LT then (0.0, 180.0) else let nan = 0/0 in (nan, nan)) in alphaNoConverge (result ed == Converge) (compare phi1 phi2) 0 0 in geodeticCurve (b * a' ed * (sigma ed - deltasigma ed)) alpha1 alpha2 instance VincentyInverse Double where inverse d = inverse (d, wgs84) instance VincentyInverse Ellipsoid where inverse e = inverse (convergence, e) instance VincentyInverse () where inverse _ = inverse (convergence, wgs84) data P = P { origSigma' :: Double, sigma' :: Double, prevSigma' :: Double } deriving Show ps :: Double -> P ps s = P s s s transition :: P -> Double -> P transition p d = P (origSigma' p) (d + origSigma' p) (sigma' p) sinSigma' :: P -> Double sinSigma' = sin . sigma' cosSigma' :: P -> Double cosSigma' = cos . sigma' sigmaM2' :: Double -> P -> Double sigmaM2' s p = 2.0 * s + sigma' p cosSigmaM2' :: Double -> P -> Double cosSigmaM2' s p = cos (sigmaM2' s p) cos2SigmaM2' :: Double -> P -> Double cos2SigmaM2' s p = square (cosSigmaM2' s p) square :: (Num a) => a -> a square = join (*) doWhile :: (a -> a) -> (a -> Bool) -> a -> a doWhile f p a = let x = f a in if p x then doWhile f p x else x whileDo :: (a -> a) -> (a -> Bool) -> a -> a whileDo f p a = if p a then whileDo f p $! (f a) else a data InverseResult = Continue | Limit | Converge deriving Eq data Q = Q { count :: Int, result :: InverseResult, lambda :: Double, a' :: Double, sigma :: Double, deltasigma :: Double }