;------------------------------------------------------------------------------------------------------;
; InvVincenty by ymg ;
; ;
; Calculates geodetic distance between two points specified by latitude/longitude using ;
; Vincenty inverse formula for ellipsoids ;
; ;
; Arguments: lat1, Latitude of First Point in Decimal Degrees. ;
; lon1, Longitude of First Point in Decimal Degrees. ;
; lat2, Latitude of Second Point in Decimal Degrees. ;
; lon2, Longitude of Second Point in Decimal Degrees. ;
; ;
; Returns: List (Distance Forward_Azimut Reverse_Azimut) ;
; ;
; Translated from Vincenty Inverse Solution of Geodesics on the Ellipsoid (c) Chris Veness 2002-2012 ;
; Some modifications as per Wikipedia on Calculation of MajA and MajB ;
;------------------------------------------------------------------------------------------------------;
(defun InvVincenty
(lat1 lon1 lat2 lon2
/)
; // WGS-84 ellipsoid params
f (/ 1 298.257223563)
b (* a (- f 1) -1) ; eq. 6356752.314245179
eSq (* f (- 2 f)) ; eq. (/ (- (* a a) (* b b)) (* a a))
e*Sq (/ eSq (- 1 eSq)) ; eq. (/ (- (* a a) (* b b)) (* b b))
L (dtr (- lon2 lon1))
u1
(atan (* (- 1 f
) (tan
(dtr lat1
)))) u2
(atan (* (- 1 f
) (tan
(dtr lat2
)))) lambdas L iterlimit 100
)
(while (and (> (abs (- lambdas lambdaP
)) 0.00000000000001) (> iterLimit
0))
sinSigma
(sqrt (+ (* cosU2 sinLambda cosU2 sinLambda
) (* (- (* cosU1 sinU2
) (* sinU1 cosU2 cosLambda
)) (- (* cosU1 sinU2
) (* sinU1 cosU2 cosLambda
))))) )
(setq cosSigma
(+ (* sinU1 sinU2
) (* cosU1 cosU2 cosLambda
)) sigma
(atan sinSigma cosSigma
) sinAlpha (/ (* cosU1 cosU2 sinLambda) sinSigma)
cosSqAlpha (- 1 (* sinAlpha sinAlpha))
cos2SigmaM (- cosSigma (/ (* 2 sinU1 sinU2) cosSqAlpha))
)
(setq c
(* (/ f
16) cosSqAlpha
(+ 4 (* f
(- 4 (* 3 cosSqAlpha
))))) lambdaP lambdas
lambdas (+ L (* (- 1 c) f sinAlpha (+ sigma (* c sinSigma (+ cos2SigmaM (* c cosSigma (- (* 2 cos2SigmaM cos2SigmaM) 1)))))))
)
)
(setq iterlimit
0) ; Else sinSigma is equal to 0 so we exit the loop. )
)
((= 0 sinSigma
) (list 0.0 nil nil)) ; Returns List (0 nil nil) ((= iterLimit
0) (alert "The Equations Did Not Nonverge In\n" (atoi iterlimit
) "Iterations.")) ; Returns nil (t
(setq uSq
(* cosSqAlpha e
*Sq
) k1
(/ (- (sqrt (+ 1 uSq
)) 1) (+ (sqrt (+ 1 uSq
)) 1)) MajA (/ (+ 1 (* k1 k1 0.25)) (- 1 k1))
MajB (* k1 (- 1 (* k1 k1 0.375)))
deltaSigma (* majB sinSigma (+ cos2SigmaM (* (/ majB 4) (- (* cosSigma (- (* 2. cos2SigmaM cos2SigmaM) 1.)) (* (/ majB 6.) cos2SigmaM (- (* 4. sinSigma sinSigma) 3.) (- (* 4. cos2SigmaM cos2SigmaM) 3.))))))
s (* b MajA (- sigma deltaSigma))
fwdAz
(rtd
(atan (* cosU2 sinLambda
) (- (* cosU1 sinU2
) (* sinU1 cosU2 cosLambda
)))) revAz
(rtd
(atan (* cosU1 sinLambda
) (+ (* sinU1 cosU2
-1) (* cosU1 sinU2 cosLambda
)))) )
(list s fwdAz revAz
) ; Returns List (Distance Forward_Azimut Reverse_Azimut) )
)
)
;------------------------------------------------------------------------------------------------------;
; DirVincenty by ymg ;
; ;
; Calculates Destination Point Given Start Point lat/long, Initial Azimut & Distance, ;
; using Vincenty inverse formula for ellipsoids. ;
; ;
; Arguments: lat1, Latitude of Origin Point in Decimal Degrees. ;
; lon1, Longitude of Origin Point in Decimal Degrees. ;
; InitAz, Initial Azimut of Travel in Decimal Degrees. ;
; dist, Distance along bearing in metres. ;
; ;
; Returns: A List (Latitude Longitude FinalAzimut) ;
; ;
; Translated from Vincenty Inverse Solution of Geodesics on the Ellipsoid (c) Chris Veness 2002-2012 ;
; Some modifications as per Wikipedia on Calculation of MajA and MajB ;
; See http://www.movable-type.co.uk/scripts/latlong-vincenty-direct.html ;
;------------------------------------------------------------------------------------------------------;
(defun dirVincenty
(lat1 lon1 InitAz dist
/)
; // WGS-84 ellipsoid params
f (/ 1 298.257223563)
b (* a (- f 1) -1) ; eq. 6356752.314245179
eSq (* f (- 2 f)) ; eq. (/ (- (* a a) (* b b)) (* a a))
e*Sq (/ eSq (- 1 eSq)) ; eq. (/ (- (* a a) (* b b)) (* b b))
s dist
U1
(atan (* (- 1 f
) (tan
(dtr lat1
)))) tanU1 (/ sinU1 cosU1)
alpha1 (dtr InitAz)
sigma1
(atan tanU1 cosAlpha1
) sinAlpha (* cosU1 sinAlpha1)
cosSqAlpha (- 1 (* sinAlpha sinAlpha))
uSq (* cosSqAlpha e*Sq)
k1
(/ (- (sqrt (+ 1 uSq
)) 1) (+ (sqrt (+ 1 uSq
)) 1)) MajA (/ (+ 1 (* k1 k1 0.25)) (- 1 k1))
MajB (* k1 (- 1 (* k1 k1 0.375)))
sigma (/ s (* b MajA))
sigmaP (* 2 pi)
)
(while (> (abs (- sigma sigmaP
)) 0.00000000000001) (setq cos2SigmaM
(cos (+ (* 2 sigma1
) sigma
)) deltaSigma (* majB sinSigma (+ cos2SigmaM (* (/ majB 4) (- (* cosSigma (- (* 2. cos2SigmaM cos2SigmaM) 1.)) (* (/ majB 6.) cos2SigmaM (- (* 4. sinSigma sinSigma) 3.) (- (* 4. cos2SigmaM cos2SigmaM) 3.))))))
sigmaP sigma
sigma (+ (/ s (* b MajA)) deltaSigma)
)
)
(setq tmp
(- (* sinU1 sinSigma
) (* cosU1 cosSigma cosAlpha1
)) lat2
(rtd
(atan (+ (* sinU1 cosSigma
) (* cosU1 sinSigma cosAlpha1
)) (* (- 1 f
) (sqrt (+ (* sinAlpha sinAlpha
) (* tmp tmp
)))))) lambdas
(atan (* sinSigma sinAlpha1
) (- (* cosU1 cosSigma
) (* sinU1 sinSigma cosAlpha1
))) C (* (/ f 16) cosSqAlpha (+ 4 (* f (- 4 (* 3 cosSqAlpha)))))
L (- lambdas (* (- 1 C) f sinAlpha (+ sigma (* C sinSigma (+ cos2SigmaM (* C cosSigma (- (* 2 cos2SigmaM cos2SigmaM) 1)))))))
lon2 (rtd (+ (dtr lon1) L))
FinalAz
(rtd
(atan sinAlpha
(* tmp
-1))) )
)
; Helper Functions ;
(defun dtr
(a
) (* pi
(/ a
180.0))) (defun rtd
(a
) (/ (* a
180.0) pi
))
