Skip to content

Instantly share code, notes, and snippets.

@gregology
Last active March 27, 2017 23:24
Show Gist options
  • Save gregology/0eeb05fc4145b7314585219ec03f1b62 to your computer and use it in GitHub Desktop.
Save gregology/0eeb05fc4145b7314585219ec03f1b62 to your computer and use it in GitHub Desktop.
Collision detection module for Python
from datetime import datetime, timedelta
from math import asin, sin, acos, cos, atan2, tan, radians, degrees, sqrt
import operator
# lots of help from http://www.movable-type.co.uk/scripts/latlong.html
EARTHS_RADIUS = 6371 * 1000.0 # meters
class Coordinates:
def __init__(self, latitude, longitude):
self.latitude = latitude
self.longitude = longitude
latitude = property(operator.attrgetter('_latitude'))
@latitude.setter
def latitude(self, lat):
if not isinstance(lat, (int, float, complex)): raise Exception('latitude is not a number')
if not -90 <= lat <= 90: raise Exception('latitude must be between -90 and 90')
self._latitude = lat
longitude = property(operator.attrgetter('_longitude'))
@longitude.setter
def longitude(self, lon):
if not isinstance(lon, (int, float, complex)): raise Exception('longitude is not a number')
if not -180 <= lon <= 180: raise Exception('latitude must be between -180 and 180')
self._longitude = lon
class Course:
'''
Speed: m/s
Bearing: clockwise degrees from north
'''
def __init__(self, bearing, speed, coordinates, timestamp):
self.bearing = bearing
self.speed = speed
self.coordinates = coordinates
self.timestamp = timestamp
bearing = property(operator.attrgetter('_bearing'))
@bearing.setter
def bearing(self, b):
if not isinstance(b, (int, float, complex)): raise Exception('bearing is not a number')
if not 0 <= b < 360: raise Exception('bearing must be greater or equal to 0 and less than 360')
self._bearing = b
speed = property(operator.attrgetter('_speed'))
@speed.setter
def speed(self, s):
if not isinstance(s, (int, float, complex)): raise Exception('bearing is not a number')
self._speed = s
coordinates = property(operator.attrgetter('_coordinates'))
@coordinates.setter
def coordinates(self, c):
if not isinstance(c, Coordinates): raise Exception('coordinates must be Coordinates class')
self._coordinates = c
timestamp = property(operator.attrgetter('_timestamp'))
@timestamp.setter
def timestamp(self, t):
if not isinstance(t, datetime): raise Exception('timestamp must be datetime')
self._timestamp = t
def predicted_coordinates(self, timestamp=None):
if not timestamp: timestamp = datetime.now()
distance = self.speed * (timestamp - self.timestamp).total_seconds() # meters
angular_distance = distance/EARTHS_RADIUS
brng = radians(self.bearing)
lat1 = radians(self.coordinates.latitude)
lon1 = radians(self.coordinates.longitude)
lat2 = asin(sin(lat1) * cos(angular_distance) + cos(lat1) * sin(angular_distance) * cos(brng))
lon2 = lon1 + atan2(sin(brng) * sin(angular_distance) * cos(lat1), cos(angular_distance) - sin(lat1) * sin(lat2))
predicted_latitude = round(degrees(lat2), 8)
predicted_longitude = round(degrees(lon2), 8)
return Coordinates(predicted_latitude, predicted_longitude)
def calculate_distance(coordinates1, coordinates2):
lat1 = radians(coordinates1.latitude)
lon1 = radians(coordinates1.longitude)
lat2 = radians(coordinates2.latitude)
lon2 = radians(coordinates2.longitude)
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * atan2(sqrt(a), sqrt(1 - a))
return round(EARTHS_RADIUS * c, 1)
def calculate_min_distance(course1, course2, timeframe, start_time=None):
# I will rewrite this function once I work out the math
if not start_time: start_time = datetime.now()
minimum_distance = calculate_distance(course1.predicted_coordinates(start_time), course2.predicted_coordinates(start_time))
for i in range(0, timeframe):
timestamp = start_time + timedelta(seconds=i)
distance = calculate_distance(course1.predicted_coordinates(timestamp), course2.predicted_coordinates(timestamp))
minimum_distance = min(minimum_distance, distance)
return minimum_distance
course1 = Course(bearing=90, speed=2.57, coordinates=Coordinates(0, -0.005), timestamp=datetime(2017, 1, 1, 0, 0, 0))
course2 = Course(bearing=270, speed=2.57, coordinates=Coordinates(0, 0.005), timestamp=datetime(2017, 1, 1, 0, 0, 0))
starting_distance = calculate_distance(course1.predicted_coordinates(datetime(2017, 1, 1, 0, 0, 0)), course2.predicted_coordinates(datetime(2017, 1, 1, 0, 0, 0)))
min_distance_next_600_seconds = calculate_min_distance(course1, course2, 600, datetime(2017, 1, 1, 0, 0, 0))
print("Initial distance:", starting_distance)
print("Minimum distance next 10 minutes:", min_distance_next_600_seconds)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment