#!/usr/bin/env python3 """ Receives latitude, longitude, heading, and distance. Returns new_latitude and new_longitude assuming the displacement along a spherical orthodrome (great circle) or a loxodrome (rhumb line). Latitude is positive if North, negative if South, in degrees Longitude is positive if East, negative if West, in degrees distances are in nautical miles (1 minute of arc on the sphere) heading is measured clockwise, in degrees, with North = 0 Copyright 2020 - J.J. Jacq - https://madinstro.net This is published under the 'copyleft' GPL 2.0 requirements. Version 1.1 change v1.1 - raise exception ExcessLengthError instead of sys.exit(1) """ import math import sys class ExcessLengthError (Exception): pass class LatitudeError (Exception): pass def Sign(x): ''' Receives a number and returns 1 if positive or zero, -1 if negative''' if x < 0: return -1 return 1 def Normalise(angle): '''Receives an angle in degrees but replaces it by the same angle but in the range -180 to + 180 ''' # First we do a modulo 360 so we know angle is positive angle = angle % 360 # Check if more than 180 if angle > 180: angle = angle - 360 return angle def CheckLat(lat): if lat > 90 : raise LatitudeError (f"The latitude {lat:.2f} cannot exceed 90 degrees on planet Earth!") if lat < -90 : raise LatitudeError (f"The latitude {lat:.2f} cannot be less than -90 degrees on planet Earth!") return def OrthoDist(lat1, lon1, lat2, lon2): '''Calculates the distance between 2 points by the great circle route. It returns the distance in nautical miles (minutes of arc).''' PI = math.pi dlon = lon2 - lon1 # Difference in longitude dlon = Normalise(dlon) # Normalise it so it is between -180 to +180 CheckLat(lat1) CheckLat(lat2) # Convert to radians dlon = math.radians(dlon) lat1 = math.radians(lat1) lat2 = math.radians(lat2) # Get rid of special cases first if lat1 == PI /2 or lat1 == -PI /2 or lat2 == PI /2 or lat2 == -PI /2: distance = math.degrees(abs(lat2 -lat1)) * 60 return distance # General case distance = math.sin(lat1)*math.sin(lat2) distance += math.cos(lat1)*math.cos(lat2)*math.cos(dlon) distance = math.degrees(math.acos(distance)) * 60 return distance def OrthoStep(latitude, longitude, heading, distance): ''' Calculates the destination Latitude and longitude given a starting position, start heading and distance to travel on great circle. It returns latitude and longitude of destination in degrees.''' # Reduce distance to less than 1 turn around the planet distance = distance % 21600 # If distance more than 1/2 turn take the short side and move opposite bearing if distance > 10800 : distance = distance % 10800 heading -= 180 CheckLat(latitude) bear = math.radians(heading % 360) lat1 = math.radians(latitude) lon1 = math.radians(longitude) # Special cases first # Case when we leave from a pole, in which case we move on a meridian given by the bearing if math.cos(lat1) == 0: lon2 = bear lat2 = lat1 - distance / 10800 if lat1 < 0: # Case of south pole lon2 = 2*math.pi - bear lat2 = lat1 + distance / 10800 return math.degrees(lat2), math.degrees(lon2) #Cases when heading = 0 or 180 if heading == 0: lat2 = latitude + distance / 60 lon2 = longitude if lat2 > 90 : # We went over the North Pole lat2 = 180 - lat2 lon2 = Normalise(longitude +180) return lat2, lon2 if heading == 180: lat2 = latitude -distance / 60 lon2 = longitude if lat2 < -90 : # We went over the South Pole lat2 = -180 - lat2 lon2 = Normalise(longitude +180) return lat2, lon2 dlon = math.pi / 2 if bear > math.pi: dlon = -dlon gap = dlon / 2 # Binary search for correct final latitude. 32 loops yield a worse error # of about 5 millimetres for i in range(32): # Based on assumed dlon we calculate the latitude on the correct # Great circle lat2 = math.sin(lat1) * math.cos(dlon) lat2 += math.sin(dlon) / math.tan(bear) lat2 /= math.cos(lat1) lat2 = math.atan(lat2) # with this derived latitude we calculate the distance length = OrthoDist(latitude, longitude, math.degrees(lat2), (longitude + math.degrees(dlon))) if length > distance: dlon -= gap else: dlon +=gap gap /= 2 lat2 = math.degrees(lat2) lon2 = Normalise(math.degrees(dlon) + longitude) return lat2, lon2 def OrthoHead(lat1, lon1, lat2, lon2): '''Calculates the heading on the orthodrome (great circle) at point1 (lat1, lon1) toward point2 (lat2, lon2). It returns the compass heading (0 = N, 90 = E, 180 = S, 270 = W) ''' PI = math.pi dlon = lon2 - lon1 # Difference in longitude dlon = Normalise(dlon) # Normalise from -180 to +180 CheckLat(lat1) CheckLat(lat2) # Convert to radians dlon = math.radians(dlon) lat1 = math.radians(lat1) lat2 = math.radians(lat2) # Get rid of special cases first if lat1 == PI /2 or lat2 == -PI /2: head = 180 return head if lat1 == - PI/2 or lat2 == PI / 2: head = 0 return 0 try: head = math.tan(lat2)*math.cos(lat1) head -= math.sin(lat1)*math.cos(dlon) head = math.atan( math.sin(dlon) / head) except ZeroDivisionError : # Start heading is along a parallel if dlon < 0 : return 270 return 90 # Quadrant resolving if dlon < 0 and head > 0 : head += PI if dlon > 0 and head < 0 : head += PI # Convert to degrees and set in range 0 to 360 return math.degrees(head) % 360 def LoxoHead(lat1, lon1, lat2, lon2): ''' Given the coordinates of two points, find the heading of the shortest loxodrome possible from the first point to the second point. The heading is returned in degrees, 0 being North and measured clockwise as per standard compass markings. ''' CheckLat(lat1) CheckLat(lat2) # Special case first # One of the points is at the pole, shortest path is either 0 or 180 if abs(lat1) == 90 or abs(lat2) == 90 : if lat1 == 90 or lat2 == -90: # Go South return 180 return 0 # Else go North young man! # General case # Convert to radians lat1 = math.radians(lat1) lat2 = math.radians(lat2) dlon = math.radians(Normalise(lon2 -lon1)) # Now the heading try: heading = Sign(lat2) * math.acosh(1 / math.cos(lat2)) heading -= Sign(lat1) * math.acosh(1 / math.cos(lat1)) heading = math.atan(dlon / heading) if lat2 < lat1 : heading += math.pi except ZeroDivisionError: # this means we're following a parallel if dlon < 0: return 270 # 2nd point is to the West of 1st point return 90 # 2nd point is to the East of 1st point return math.degrees(heading) % 360 def LoxoDist(lat1, lon1, lat2, lon2): ''' Calculates the length in nautical miles of the shortest loxodrome segment joining two points ''' # Special case first if lat1 == lat2: # Loxodrome is a parallel distance = 60 * abs(lon2 - lon1) * math.cos(math.radians(lat1)) return distance #General case heading = LoxoHead(lat1, lon1, lat2, lon2) distance = 60 * abs((lat2 -lat1) / math.cos(math.radians(heading))) return distance def LoxoStep(latitude, longitude, heading, distance): ''' Given a latitude, longitude, heading and distance it calculates the latitude and longitude of the final point. Units are degrees for latitude, longitude and heading, nautical miles for distance''' heading = heading % 360 if heading == 90 : # Special case, moving on parallel eastward end_lon = longitude + distance / 60 / math.cos(math.radians(latitude)) return latitude, Normalise(end_lon) elif heading == 270: # Special case moving westward end_lon = longitude - distance / 60 / math.cos(math.radians(latitude)) return latitude, Normalise(end_lon) # General case is next : # final latitude in degrees end_lat = latitude + distance / 60 * math.cos(math.radians(heading)) # Reality Check: Latitude cannot be more than 90 or less than -90 if abs(end_lat) > 90: maximum = int(distance * (1 - ((abs(end_lat) - 90) / abs(end_lat - latitude)))) raise ExcessLengthError (f"The loxodrome length {distance:.2f} nm exceeds the maximum possible: {maximum}.") # if either point is at the pole (either North or South) then we can # pick any longitude for the end point as they are all possible. So we # pick, arbitrarily, the same as the start longitude if not (abs(end_lat) == 90 or abs(latitude) == 90): end_lon = Sign(end_lat) * math.acosh(1/math.cos(math.radians(end_lat))) end_lon -= Sign(latitude) * math.acosh(1/math.cos(math.radians(latitude))) end_lon *= math.tan(math.radians(heading)) end_lon = longitude + math.degrees(end_lon) else: end_lon = longitude return end_lat, Normalise(end_lon) def main(): #Test 1 Orthodrome while True: try: answer=input("Orthodrome test. Enter lat1, lon1, lat2, lon2: ") if not answer: break data = answer.split() lat1 = float(data[0]) lon1 = float(data[1]) lat2 = float(data[2]) lon2 = float(data[3]) distance = OrthoDist(lat1, lon1, lat2, lon2) bearing = OrthoHead(lat1, lon1, lat2, lon2) print(f"Distance is: {distance:.6f}, bearing is: {bearing:.6f}") print() except Exception as err: print (err) print () continue #Test 2 Orthodrome while True: try: answer=input("Orthodrome test. Enter lat, lon, cap, distance: ") if not answer: break data = answer.split() lat = float(data[0]) lon = float(data[1]) cap = float(data[2]) stp = float(data[3]) CheckLat(lat) endlat, endlon = OrthoStep(lat,lon,cap,stp) print(f"Final latitude is {endlat:.6f} , final longitude is: {endlon:.6f}") print() except Exception as err: print (err) print () continue # Test1 Loxodrome while True: try: answer=input("Loxodrome test. Enter lat1, lon1, lat2, lon2: ") if not answer: break data = answer.split() lat1 = float(data[0]) lon1 = float(data[1]) lat2 = float(data[2]) lon2 = float(data[3]) CheckLat(lat1) CheckLat(lat2) bearing = LoxoHead(lat1, lon1, lat2, lon2) distance =LoxoDist(lat1, lon1, lat2, lon2) print(f"The bearing to point 2 is: {bearing:.6f}, the distance is {distance:.6f}") print() except Exception as err: print (err) print () continue # Test2 Loxodrome while True: try: answer=input("Loxodrome test. Enter latitude, longitude, heading, distance: ") if not answer: break data = answer.split() lat = float(data[0]) lon = float(data[1]) cap = float(data[2]) stp = float(data[3]) CheckLat(lat) endlat, endlon = LoxoStep(lat, lon, cap, stp) print(f"Final latitude is {endlat:.6f} , final longitude is: {endlon:.6f}") print() except Exception as err: print (err) print() continue if __name__ == '__main__': main()