navigation.py

#!/usr/bin/env python3
import math

class Navigation:
		
	def findDistanceAndBearing(_, lat, lon):
		# TODO: Make location configurable
		x1 = float(40.579640)
		y1 = float(-73.839530)
		x2 = float(lat)
		y2 = float(lon)
		
		# The following formulas assume that angles are expressed in radians.
		# So convert to radians.
		
		x1 = math.radians(x1)
		y1 = math.radians(y1)
		x2 = math.radians(x2)
		y2 = math.radians(y2)
		
		# Compute using the Haversine formula.
		a = math.sin((x2-x1)/2.0) ** 2.0 \
			+ (math.cos(x1) * math.cos(x2) * (math.sin((y2-y1)/2.0) ** 2.0))
		
		# Great circle distance in radians
		angle2 = 2.0 * math.asin(min(1.0, math.sqrt(a)))
		
		# Convert back to degrees.
		angle2 = math.degrees(angle2)
		
		# Each degree on a great circle of Earth is 60 nautical miles.
		distance2 = 60.0 * angle2
		
		# Now, angle
		x = math.sin(y1 - y2) * math.cos(x2)
		y = math.cos(x1) * math.sin(x2) - (math.sin(x1) * math.cos(x2) * math.cos(y1 - y2))
		
		initial_bearing = math.atan2(x, y)
		initial_bearing = math.degrees(initial_bearing)
		
		# This results in right = -90°
		#                    top = 0°
		#                   left = 90°
		# Flip and normalize
		compass_bearing = ((initial_bearing * -1) + 360) % 360
		
		return (distance2, compass_bearing)
	
	def compassDirection(_, heading):
		compassDirections = {}
		direction = 0
		directionInterval = 45
		
		compassDirections["N"] = direction
		direction += directionInterval
		compassDirections["NE"] = direction
		direction += directionInterval
		compassDirections["E"] = direction
		direction += directionInterval
		compassDirections["SE"] = direction
		direction += directionInterval
		compassDirections["S"] = direction
		direction += directionInterval
		compassDirections["SW"] = direction
		direction += directionInterval
		compassDirections["W"] = direction
		direction += directionInterval
		compassDirections["NW"] = direction
		direction += directionInterval
		compassDirections["N_"] = direction
		
		directionOffsets = {}
		for direction in compassDirections.keys():
			directionHeading = compassDirections[direction]
			headingInt = int(heading.translate({ord('°'): None}))
			directionOffset = abs(directionHeading - headingInt)
			directionOffsets[direction] = directionOffset
			
		smallestOffset = 45 # largest possible
		smallestDirection = ""
		for direction in directionOffsets.keys():
			offset = directionOffsets[direction]
			if offset < 45 and offset < smallestOffset:
					smallestDirection = direction
				
		if smallestDirection == "N_":
			smallestDirection = "N"
			
		return smallestDirection