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
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
}