# ===============================================================================
# =========== Comparaison distance sphérique - ellisoide WSG84 =============
# Function Source : https://www.johndcook.com/blog/2018/11/24/spheroid-distance/
# ===============================================================================
# ---- Start Source Functions
#
from scipy import sin, cos, tan, arctan, arctan2, arccos, pi
#
# spherical_distance
def spherical_distance(lat1, long1, lat2, long2):
phi1 = 0.5*pi - lat1
phi2 = 0.5*pi - lat2
r = 0.5*(6378137 + 6356752) # mean radius in meters
t = sin(phi1)*sin(phi2)*cos(long1-long2) + cos(phi1)*cos(phi2)
return r * arccos(t)
#
# ellipsoidal_distance
def ellipsoidal_distance(lat1, long1, lat2, long2):
a = 6378137.0 # equatorial radius in meters
f = 1/298.257223563 # ellipsoid flattening
b = (1 - f)*a
tolerance = 1e-11 # to stop iteration
phi1, phi2 = lat1, lat2
U1 = arctan((1-f)*tan(phi1))
U2 = arctan((1-f)*tan(phi2))
L1, L2 = long1, long2
L = L2 - L1
lambda_old = L + 0
while True:
t = (cos(U2)*sin(lambda_old))**2
t += (cos(U1)*sin(U2) - sin(U1)*cos(U2)*cos(lambda_old))**2
sin_sigma = t**0.5
cos_sigma = sin(U1)*sin(U2) + cos(U1)*cos(U2)*cos(lambda_old)
sigma = arctan2(sin_sigma, cos_sigma)
sin_alpha = cos(U1)*cos(U2)*sin(lambda_old) / sin_sigma
cos_sq_alpha = 1 - sin_alpha**2
cos_2sigma_m = cos_sigma - 2*sin(U1)*sin(U2)/cos_sq_alpha
C = f*cos_sq_alpha*(4 + f*(4-3*cos_sq_alpha))/16
t = sigma + C*sin_sigma*(cos_2sigma_m + C*cos_sigma*(-1 + 2*cos_2sigma_m**2))
lambda_new = L + (1 - C)*f*sin_alpha*t
if abs(lambda_new - lambda_old) <= tolerance:
break
else:
lambda_old = lambda_new
u2 = cos_sq_alpha*((a**2 - b**2)/b**2)
A = 1 + (u2/16384)*(4096 + u2*(-768+u2*(320 - 175*u2)))
B = (u2/1024)*(256 + u2*(-128 + u2*(74 - 47*u2)))
t = cos_2sigma_m + 0.25*B*(cos_sigma*(-1 + 2*cos_2sigma_m**2))
t -= (B/6)*cos_2sigma_m*(-3 + 4*sin_sigma**2)*(-3 + 4*cos_2sigma_m**2)
delta_sigma = B * sin_sigma * t
s = b*A*(sigma - delta_sigma)
return s
# ---- End Source
# ================================================================================
p1Lat=pi/6
p1Lon=pi/18
p2Lat=(pi/6) + pi/180
p2Lon=(pi/18) + pi/180
k = 180/pi
print ("\nP1 Lat: ", p1Lat*k, " Lon: ", p1Lon*k)
print ("P2 Lat: ", p2Lat*k, " Lon: ", p2Lon*k)
print ("Spherical Distance : ", spherical_distance(p1Lat, p1Lon, p2Lat, p2Lon), "m")
print ("Ellipsoïdal Distance : ", ellipsoidal_distance(p1Lat, p1Lon, p2Lat, p2Lon), "m")
