def kml(self, kml):
"""
Appends kml nodes describing this flyzone.
Args:
kml: A simpleKML Container to which the fly zone will be added
"""
zone_name = 'Fly Zone {}'.format(self.pk)
pol = kml.newpolygon(name=zone_name)
fly_zone = []
flyzone_num = 1
for point in self.boundary_pts.all():
gps = point.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(point.position.altitude_msl))
fly_zone.append(coord)
# Add boundary marker
wp = kml.newpoint(name=str(flyzone_num), coords=[coord])
wp.description = str(point)
wp.visibility = False
flyzone_num += 1
fly_zone.append(fly_zone[0])
pol.outerboundaryis = fly_zone
# Search Area Style
pol.style.linestyle.color = Color.red
pol.style.linestyle.width = 3
pol.style.polystyle.color = Color.changealphaint(50, Color.green)
def test_kml_simple(self):
coordinates = [
(-76.0, 38.0, 0.0),
(-76.0, 38.0, 10.0),
(-76.0, 38.0, 20.0),
(-76.0, 38.0, 30.0),
(-76.0, 38.0, 100.0),
(-76.0, 38.0, 30.0),
(-76.0, 38.0, 60.0),
]
# Create Coordinates
start = TakeoffOrLandingEvent(user=self.user, uas_in_air=True)
start.save()
for coord in coordinates:
self.create_log_element(*coord)
end = TakeoffOrLandingEvent(user=self.user, uas_in_air=False)
end.save()
kml = Kml()
UasTelemetry.kml(user=self.user,
logs=UasTelemetry.by_user(self.user),
kml=kml,
kml_doc=kml)
for coord in coordinates:
tag = self.coord_format.format(coord[1], coord[0],
units.feet_to_meters(coord[2]))
self.assertTrue(tag in kml.kml())
def uas_telemetry_kml(user, flight_logs, kml, kml_doc):
"""
Appends kml nodes describing the given user's flight as described
by the log array given.
Args:
user: A Django User to get username from
flight_logs: A sequence of UasTelemetry logs per flight period.
kml: A simpleKML Container to which the flight data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
None
"""
# Lazily create folder iff there is data.
kml_folder = None
for i, logs in enumerate(flight_logs):
name = '%s Flight %d' % (user.username, i + 1)
logs = UasTelemetry.dedupe(UasTelemetry.filter_bad(logs))
coords = []
angles = []
when = []
for entry in logs:
# Spatial Coordinates
coord = (entry.longitude, entry.latitude,
units.feet_to_meters(entry.altitude_msl))
coords.append(coord)
# Time Elements
time = entry.timestamp.strftime(KML_DATETIME_FORMAT)
when.append(time)
# Degrees heading, tilt, and roll
angle = (entry.uas_heading, 0.0, 0.0)
angles.append(angle)
# Ignore tracks with no data.
if not coords or not angles or not when:
continue
# Start folder if not done so already.
if not kml_folder:
kml_folder = kml.newfolder(name=user.username)
# Create a new track in the folder
trk = kml_folder.newgxtrack(name=name)
trk.altitudemode = AltitudeMode.absolute
# Append flight data
trk.newwhen(when)
trk.newgxcoord(coords)
trk.newgxangle(angles)
# Set styling
trk.extrude = 1 # Extend path to ground
trk.style.linestyle.width = 2
trk.style.linestyle.color = Color.blue
trk.iconstyle.icon.href = KML_PLANE_ICON
def test_kml_simple(self):
coordinates = [
(0, -76.0, 38.0, 0.0, 0),
(1, -76.0, 38.0, 10.0, 0),
(2, -76.0, 38.0, 20.0, 0),
(3, -76.0, 38.0, 30.0, 0),
(4, -76.0, 38.0, 100.0, 0),
(5, -76.0, 38.0, 30.0, 0),
(6, -76.0, 38.0, 60.0, 0),
]
# Create Coordinates
start = TakeoffOrLandingEvent(user=self.user, uas_in_air=True)
start.save()
start.timestamp = self.now
start.save()
for coord in coordinates:
self.create_log_element(*coord)
end = TakeoffOrLandingEvent(user=self.user, uas_in_air=False)
end.save()
end.timestamp = self.now + datetime.timedelta(seconds=7)
end.save()
kml = Kml()
UasTelemetry.kml(
user=self.user,
logs=UasTelemetry.by_user(self.user),
kml=kml,
kml_doc=kml.document)
for coord in coordinates:
tag = self.coord_format.format(coord[2], coord[1],
units.feet_to_meters(coord[3]))
self.assertTrue(tag in kml.kml())
def live_kml(cls, kml, timespan, resolution=100):
"""
Appends kml nodes describing current paths of the obstacles
Args:
kml: A simpleKML Container to which the obstacle data will be added
timespan: A timedelta to look backwards in time
resolution: integer number of milliseconds between obstacle positions
Returns:
None
"""
def track(obstacle, span, dt):
curr = timezone.now()
last = curr - span
time = curr
while time >= last:
yield obstacle.get_position(time)
time -= dt
for obstacle in MovingObstacle.objects.all():
dt = timedelta(milliseconds=resolution)
linestring = kml.newlinestring(name="Obstacle")
coords = []
for pos in track(obstacle, timespan, dt):
# Longitude, Latitude, Altitude
coord = (pos[1], pos[0], units.feet_to_meters(pos[2]))
coords.append(coord)
linestring.coords = coords
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.red
linestring.style.polystyle.color = Color.changealphaint(100,
Color.red)
def kml(self, kml):
"""
Appends kml nodes describing this flyzone.
Args:
kml: A simpleKML Container to which the fly zone will be added
"""
zone_name = 'Fly Zone {}'.format(self.pk)
pol = kml.newpolygon(name=zone_name)
fly_zone = []
flyzone_num = 1
for point in self.boundary_pts.all():
gps = point.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(point.position.altitude_msl))
fly_zone.append(coord)
# Add boundary marker
wp = kml.newpoint(name=str(flyzone_num), coords=[coord])
wp.description = str(point)
wp.visibility = False
flyzone_num += 1
fly_zone.append(fly_zone[0])
pol.outerboundaryis = fly_zone
# Search Area Style
pol.style.linestyle.color = Color.red
pol.style.linestyle.width = 3
pol.style.polystyle.color = Color.changealphaint(50, Color.green)
def test_kml_simple(self):
coordinates = [
(-76.0, 38.0, 0.0),
(-76.0, 38.0, 10.0),
(-76.0, 38.0, 20.0),
(-76.0, 38.0, 30.0),
(-76.0, 38.0, 100.0),
(-76.0, 38.0, 30.0),
(-76.0, 38.0, 60.0),
]
# Create Coordinates
start = TakeoffOrLandingEvent(user=self.user, uas_in_air=True)
start.save()
for coord in coordinates:
self.create_log_element(*coord)
end = TakeoffOrLandingEvent(user=self.user, uas_in_air=False)
end.save()
kml = Kml()
UasTelemetry.kml(user=self.user,
logs=UasTelemetry.by_user(self.user),
kml=kml,
kml_doc=kml)
for coord in coordinates:
tag = self.coord_format.format(coord[1], coord[0],
units.feet_to_meters(coord[2]))
self.assertTrue(tag in kml.kml())
def live_kml(cls, kml, timespan, resolution=100):
"""
Appends kml nodes describing current paths of the obstacles
Args:
kml: A simpleKML Container to which the obstacle data will be added
timespan: A timedelta to look backwards in time
resolution: integer number of milliseconds between obstacle positions
Returns:
None
"""
def track(obstacle, span, dt):
curr = timezone.now()
last = curr - span
time = curr
while time >= last:
yield obstacle.get_position(time)
time -= dt
for obstacle in MovingObstacle.objects.all():
dt = timedelta(milliseconds=resolution)
linestring = kml.newlinestring(name="Obstacle")
coords = []
for pos in track(obstacle, timespan, dt):
# Longitude, Latitude, Altitude
coord = (pos[1], pos[0], units.feet_to_meters(pos[2]))
coords.append(coord)
linestring.coords = coords
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.red
linestring.style.polystyle.color = Color.changealphaint(
100, Color.red)
def kml(self, path, kml, kml_doc):
"""
Appends kml nodes describing the given user's flight as described
by the log array given. No nodes are added if less than two log
entries are given.
Args:
path: A list of UasTelemetry elements.
kml: A simpleKML Container to which the flight data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
None
"""
# KML Compliant Datetime Formatter
kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ"
icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png'
kml_output_resolution = 100 # milliseconds
coords = []
when = []
ranges = []
if len(path) < 2:
return
dt = timedelta(milliseconds=kml_output_resolution)
for pos, uav, time in self.times(path, dt):
# Spatial Coordinates (longitude, latitude, altitude)
coord = (pos[1], pos[0], units.feet_to_meters(pos[2]))
coords.append(coord)
# Time Elements
when.append(time.strftime(kml_datetime_format))
# Distance Elements
uas_range = distance.distance_to(*uav + pos)
ranges.append(uas_range)
# Create a new track in the folder
trk = kml.newgxtrack(name='Obstacle Path {}'.format(self.id))
trk.altitudemode = AltitudeMode.absolute
# Create a schema for extended data: proximity
schema = kml_doc.newschema()
schema.newgxsimplearrayfield(name='proximity',
type=Types.float,
displayname='UAS Proximity [ft]')
trk.extendeddata.schemadata.schemaurl = schema.id
# Append flight data
trk.newwhen(when)
trk.newgxcoord(coords)
trk.extendeddata.schemadata.newgxsimplearraydata('proximity', ranges)
# Set styling
trk.extrude = 1 # Extend path to ground
trk.style.linestyle.width = 2
trk.style.linestyle.color = Color.red
trk.iconstyle.icon.href = icon
def kml(self, path, kml, kml_doc):
"""
Appends kml nodes describing the given user's flight as described
by the log array given. No nodes are added if less than two log
entries are given.
Args:
path: A list of UasTelemetry elements.
kml: A simpleKML Container to which the flight data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
None
"""
# KML Compliant Datetime Formatter
kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ"
icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png'
kml_output_resolution = 100 # milliseconds
coords = []
when = []
ranges = []
if len(path) < 2:
return
dt = timedelta(milliseconds=kml_output_resolution)
for pos, uav, time in self.times(path, dt):
# Spatial Coordinates (longitude, latitude, altitude)
coord = (pos[1], pos[0], units.feet_to_meters(pos[2]))
coords.append(coord)
# Time Elements
when.append(time.strftime(kml_datetime_format))
# Distance Elements
uas_range = distance.distance_to(*uav + pos)
ranges.append(uas_range)
# Create a new track in the folder
trk = kml.newgxtrack(name='Obstacle Path {}'.format(self.id))
trk.altitudemode = AltitudeMode.absolute
# Create a schema for extended data: proximity
schema = kml_doc.newschema()
schema.newgxsimplearrayfield(name='proximity',
type=Types.float,
displayname='UAS Proximity [ft]')
trk.extendeddata.schemadata.schemaurl = schema.id
# Append flight data
trk.newwhen(when)
trk.newgxcoord(coords)
trk.extendeddata.schemadata.newgxsimplearraydata('proximity', ranges)
# Set styling
trk.extrude = 1 # Extend path to ground
trk.style.linestyle.width = 2
trk.style.linestyle.color = Color.red
trk.iconstyle.icon.href = icon
def kml(cls, user, logs, kml, kml_doc):
"""
Appends kml nodes describing the given user's flight as described
by the log array given.
Args:
user: A Django User to get username from
logs: A list of UasTelemetry elements
kml: A simpleKML Container to which the flight data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
None
"""
kml_folder = kml.newfolder(name=user.username)
flights = TakeoffOrLandingEvent.flights(user)
if len(flights) == 0:
return
logs = UasTelemetry.dedupe(UasTelemetry.filter_bad(logs))
for i, flight in enumerate(flights):
name = '%s Flight %d' % (user.username, i + 1)
flight_logs = filter(lambda x: flight.within(x.timestamp), logs)
coords = []
angles = []
when = []
for entry in logs:
pos = entry.uas_position.gps_position
# Spatial Coordinates
coord = (pos.longitude, pos.latitude,
units.feet_to_meters(entry.uas_position.altitude_msl))
coords.append(coord)
# Time Elements
time = entry.timestamp.strftime(KML_DATETIME_FORMAT)
when.append(time)
# Degrees heading, tilt, and roll
angle = (entry.uas_heading, 0.0, 0.0)
angles.append(angle)
# Create a new track in the folder
trk = kml_folder.newgxtrack(name=name)
trk.altitudemode = AltitudeMode.absolute
# Append flight data
trk.newwhen(when)
trk.newgxcoord(coords)
trk.newgxangle(angles)
# Set styling
trk.extrude = 1 # Extend path to ground
trk.style.linestyle.width = 2
trk.style.linestyle.color = Color.blue
trk.iconstyle.icon.href = KML_PLANE_ICON
def test_feet_to_meters(self):
"""Performs a data-driven test of the conversion."""
cases = [
# (feet, meters)
( 0, 0),
( 1, 0.3048),
( 10, 3.048),
(1000, 304.8),
] # yapf: disable
for (feet, meters_actual) in cases:
self.assertAlmostEqual(meters_actual,
units.feet_to_meters(feet),
delta=0.1)
def test_feet_to_meters(self):
"""Performs a data-driven test of the conversion."""
cases = [
# (feet, meters)
( 0, 0),
( 1, 0.3048),
( 10, 3.048),
(1000, 304.8),
] # yapf: disable
for (feet, meters_actual) in cases:
self.assertAlmostEqual(meters_actual,
units.feet_to_meters(feet),
delta=0.1)
def __init__(self, *args, **kwargs):
super(TestGenerateKMLWithFixture, self).__init__(*args, **kwargs)
self.folders = ['Teams', 'Missions']
self.users = ['testuser', 'user0', 'user1']
self.coordinates = [(lat, lon, units.feet_to_meters(alt))
for lat, lon, alt in [
(-76.0, 38.0, 0.0),
(-76.0, 38.0, 10.0),
(-76.0, 38.0, 20.0),
(-76.0, 38.0, 30.0),
(-76.0, 38.0, 100.0),
(-76.0, 38.0, 30.0),
(-77.0, 38.0, 60.0),
]]
def __init__(self, *args, **kwargs):
super(TestGenerateKMLWithFixture, self).__init__(*args, **kwargs)
self.folders = ['Teams', 'Missions']
self.users = ['testuser', 'user0', 'user1']
self.coordinates = [
(lat, lon, units.feet_to_meters(alt))
for lat, lon, alt in [
(-76.0, 38.0, 0.0),
(-76.0, 38.0, 10.0),
(-76.0, 38.0, 20.0),
(-76.0, 38.0, 30.0),
(-76.0, 38.0, 100.0),
(-76.0, 38.0, 30.0),
(-77.0, 38.0, 60.0),
]
]
def uas_telemetry_live_kml(kml, timespan):
users = User.objects.order_by('username').all()
for user in users:
try:
log = UasTelemetry.by_user(user).latest('timestamp')
except UasTelemetry.DoesNotExist:
continue
if log.timestamp < timezone.now() - timespan:
continue
point = kml.newpoint(name=user.username,
coords=[(log.longitude, log.latitude,
units.feet_to_meters(log.altitude_msl))])
point.iconstyle.icon.href = KML_PLANE_ICON
point.iconstyle.heading = log.uas_heading
point.extrude = 1 # Extend path to ground
point.altitudemode = AltitudeMode.absolute
def kml(self, time_periods, kml, kml_doc):
"""
Appends kml nodes describing the given user's flight as described
by the log array given. No nodes are added if less than two log
entries are given.
Args:
time_periods: The time period over which to generate positions.
kml: A simpleKML Container to which the flight data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
None
"""
# KML Compliant Datetime Formatter
kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ"
icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png'
# Generate track data for all time periods.
coords = []
when = []
ranges = []
for period in time_periods:
t = period.start
while t < period.end:
(lat, lon, alt) = self.get_position(t)
coords.append((lon, lat, units.feet_to_meters(alt)))
when.append(t.strftime(kml_datetime_format))
t += timedelta(milliseconds=100)
# Create a new track in the folder
trk = kml.newgxtrack(name='Obstacle Path {}'.format(self.id))
trk.altitudemode = AltitudeMode.absolute
# TODO: Add back proximity information lost when fixing #316.
# Append flight data
trk.newwhen(when)
trk.newgxcoord(coords)
# Set styling
trk.extrude = 1 # Extend path to ground
trk.style.linestyle.width = 2
trk.style.linestyle.color = Color.red
trk.iconstyle.icon.href = icon
def kml(self, kml):
"""
Appends kml nodes describing this flyzone.
Args:
kml: A simpleKML Container to which the fly zone will be added
"""
zone_name = 'Fly Zone {}'.format(self.pk)
pol = kml.newpolygon(name=zone_name)
fly_zone = []
for point in self.boundary_pts.order_by('order'):
gps = point.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(point.position.altitude_msl))
fly_zone.append(coord)
fly_zone.append(fly_zone[0])
pol.outerboundaryis = fly_zone
pol.style.linestyle.color = Color.red
pol.style.linestyle.width = 3
pol.style.polystyle.color = Color.changealphaint(50, Color.green)
def fly_zone_kml(fly_zone, kml):
"""
Appends kml nodes describing the flyzone.
Args:
fly_zone: The FlyZone for which to add KML
kml: A simpleKML Container to which the fly zone will be added
"""
zone_name = 'Fly Zone {}'.format(fly_zone.pk)
pol = kml.newpolygon(name=zone_name)
fly_zone_points = []
for point in fly_zone.boundary_pts.order_by('order'):
coord = (point.longitude, point.latitude,
units.feet_to_meters(point.altitude_msl))
fly_zone_points.append(coord)
fly_zone_points.append(fly_zone_points[0])
pol.outerboundaryis = fly_zone_points
pol.style.linestyle.color = Color.red
pol.style.linestyle.width = 3
pol.style.polystyle.color = Color.changealphaint(50, Color.green)
def live_kml(cls, kml, timespan):
users = User.objects.all()
for user in users:
period_logs = UasTelemetry.by_user(user)\
.filter(timestamp__gt=timezone.now() - timespan)
if len(period_logs) < 1:
continue
linestring = kml.newlinestring(name=user.username)
coords = []
for entry in period_logs:
pos = entry.uas_position.gps_position
# Spatial Coordinates
coord = (pos.longitude, pos.latitude,
units.feet_to_meters(entry.uas_position.altitude_msl))
coords.append(coord)
linestring.coords = coords
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.blue
linestring.style.polystyle.color = Color.changealphaint(
100, Color.blue)
def live_kml(cls, kml, timespan):
users = User.objects.all()
for user in users:
period_logs = UasTelemetry.by_user(user)\
.filter(timestamp__gt=timezone.now() - timespan)
if len(period_logs) < 1:
continue
linestring = kml.newlinestring(name=user.username)
coords = []
for entry in period_logs:
pos = entry.uas_position.gps_position
# Spatial Coordinates
coord = (pos.longitude, pos.latitude,
units.feet_to_meters(entry.uas_position.altitude_msl))
coords.append(coord)
linestring.coords = coords
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.blue
linestring.style.polystyle.color = Color.changealphaint(100,
Color.blue)
def kml(self, kml, kml_doc):
"""
Appends kml nodes describing this mission configurations.
Args:
kml: A simpleKML Container to which the mission data will be added
kml_doc: The simpleKML Document to which schemas will be added
"""
mission_name = 'Mission {}'.format(self.pk)
kml_folder = kml.newfolder(name=mission_name)
# Static Points
locations = {
'Home Position': self.home_pos,
'Emergent LKP': self.emergent_last_known_pos,
'Off Axis': self.off_axis_odlc_pos,
'Air Drop': self.air_drop_pos,
}
for key, point in locations.iteritems():
gps = (point.longitude, point.latitude)
wp = kml_folder.newpoint(name=key, coords=[gps])
wp.description = str(point)
# Waypoints
waypoints_folder = kml_folder.newfolder(name='Waypoints')
linestring = waypoints_folder.newlinestring(name='Waypoints')
waypoints = []
waypoint_num = 1
for waypoint in self.mission_waypoints.order_by('order'):
gps = waypoint.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(waypoint.position.altitude_msl))
waypoints.append(coord)
# Add waypoint marker
wp = waypoints_folder.newpoint(name=str(waypoint_num),
coords=[coord])
wp.description = str(waypoint)
wp.altitudemode = AltitudeMode.absolute
wp.extrude = 1
wp.visibility = False
waypoint_num += 1
linestring.coords = waypoints
# Waypoints Style
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.black
linestring.style.polystyle.color = Color.changealphaint(
100, Color.green)
# Search Area
search_area_folder = kml_folder.newfolder(name='Search Area')
search_area = []
search_area_num = 1
for point in self.search_grid_points.order_by('order'):
gps = point.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(point.position.altitude_msl))
search_area.append(coord)
# Add boundary marker
wp = search_area_folder.newpoint(name=str(search_area_num),
coords=[coord])
wp.description = str(point)
wp.visibility = False
search_area_num += 1
if search_area:
# Create search area polygon.
pol = search_area_folder.newpolygon(name='Search Area')
search_area.append(search_area[0])
pol.outerboundaryis = search_area
# Search Area Style.
pol.style.linestyle.color = Color.black
pol.style.linestyle.width = 2
pol.style.polystyle.color = Color.changealphaint(50, Color.blue)
# Stationary Obstacles.
stationary_obstacles_folder = kml_folder.newfolder(
name='Stationary Obstacles')
# TODO: Implement
# Moving Obstacles.
users = User.objects.all()
moving_obstacle_periods = []
for user in users:
moving_obstacle_periods.extend(TakeoffOrLandingEvent.flights(user))
moving_obstacles_folder = kml_folder.newfolder(name='Moving Obstacles')
for obstacle in self.moving_obstacles.all():
obstacle.kml(moving_obstacle_periods, moving_obstacles_folder,
kml_doc)
def distance_to_line(start, end, point, utm):
"""Compute the closest distance from point to a line segment.
Based on the point-line distance derived in:
http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
(l_1 - p) dot (l_2 - p)
t = - -----------------------
|l_2 - l_1|^2
We clamp t to 0 <= t <= 1 to ensure we calculate distance to a point on
the line segment.
The closest point can be determined using t:
p_c = l_1 + t * (l_2 - l_1)
And the distance is:
d = |p - p_c|
Arguments are points in the form (lat, lon, alt MSL (ft)).
Args:
start: Defines the start of the line.
end: Defines the end of the line.
point: Free point to compute distance from.
utm: The UTM Proj projection to project into. If start, end, or point
are well outside of this projection, this function returns
infinite.
Returns:
Closest distance in ft from point to the line.
"""
try:
# Convert points to UTM projection.
# We need a cartesian coordinate system to perform the calculation.
lat, lon, ftmsl = start
x, y = pyproj.transform(wgs84, utm, lon, lat)
l1 = np.array([x, y, units.feet_to_meters(ftmsl)])
lat, lon, ftmsl = end
x, y = pyproj.transform(wgs84, utm, lon, lat)
l2 = np.array([x, y, units.feet_to_meters(ftmsl)])
lat, lon, ftmsl = point
x, y = pyproj.transform(wgs84, utm, lon, lat)
p = np.array([x, y, units.feet_to_meters(ftmsl)])
except RuntimeError:
# pyproj throws RuntimeError if the coordinates are grossly beyond the
# bounds of the projection. We simplify this to "infinite" distance.
return float("inf")
d1 = l1 - p
d2 = l2 - l1
num = np.dot(d1, d2)
dem = np.linalg.norm(l2 - l1)**2
t = -num / dem
if t < 0:
t = 0
elif t > 1:
t = 1
p_c = l1 + t * (l2 - l1)
dist = np.linalg.norm(p - p_c)
return units.meters_to_feet(dist)
def kml(self, kml):
"""
Appends kml nodes describing this mission configurations.
Args:
kml: A simpleKML Container to which the mission data will be added
"""
mission_name = 'Mission {}'.format(self.pk)
kml_folder = kml.newfolder(name=mission_name)
# Static Points
locations = {
'Home Position': self.home_pos,
'Emergent LKP': self.emergent_last_known_pos,
'Off Axis': self.off_axis_target_pos,
'SRIC': self.sric_pos,
'Air Drop': self.air_drop_pos,
}
for key, point in locations.iteritems():
gps = (point.longitude, point.latitude)
wp = kml_folder.newpoint(name=key, coords=[gps])
wp.description = str(point)
# Waypoints
waypoints_folder = kml_folder.newfolder(name='Waypoints')
linestring = waypoints_folder.newlinestring(name='Waypoints')
waypoints = []
waypoint_num = 1
for waypoint in self.mission_waypoints.all():
gps = waypoint.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(waypoint.position.altitude_msl))
waypoints.append(coord)
# Add waypoint marker
wp = waypoints_folder.newpoint(name=str(waypoint_num),
coords=[coord])
wp.description = str(waypoint)
wp.altitudemode = AltitudeMode.absolute
wp.extrude = 1
wp.visibility = False
waypoint_num += 1
linestring.coords = waypoints
# Waypoints Style
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.black
linestring.style.polystyle.color = Color.changealphaint(100,
Color.green)
# Search Area
search_area_folder = kml_folder.newfolder(name='Search Area')
search_area = []
search_area_num = 1
for point in self.search_grid_points.all():
gps = point.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(point.position.altitude_msl))
search_area.append(coord)
# Add boundary marker
wp = search_area_folder.newpoint(name=str(search_area_num),
coords=[coord])
wp.description = str(point)
wp.visibility = False
search_area_num += 1
if search_area:
# Create search area polygon.
pol = search_area_folder.newpolygon(name='Search Area')
search_area.append(search_area[0])
pol.outerboundaryis = search_area
# Search Area Style.
pol.style.linestyle.color = Color.black
pol.style.linestyle.width = 2
pol.style.polystyle.color = Color.changealphaint(50, Color.blue)
# Stationary Obstacles
stationary_obstacles_folder = kml_folder.newfolder(
name='Stationary Obstacles')
def determine_interpolated_collision(self, start_log, end_log, utm):
"""Determines whether the UAS collided with the obstacle by
interpolating between start and end telemetry.
Args:
start_log: A UAS telemetry log.
end_log: A UAS telemetry log.
utm: The UTM Proj projection to project into.
Returns:
True if the UAS collided with the obstacle, False otherwise.
"""
start_lat = start_log.uas_position.gps_position.latitude
start_lon = start_log.uas_position.gps_position.longitude
start_alt = start_log.uas_position.altitude_msl
end_lat = end_log.uas_position.gps_position.latitude
end_lon = end_log.uas_position.gps_position.longitude
end_alt = end_log.uas_position.altitude_msl
latc = self.gps_position.latitude
lonc = self.gps_position.longitude
# First, check if altitude for both logs is above cylinder height.
if start_alt > self.cylinder_height and end_alt > self.cylinder_height:
return False
# We want to check if the line drawn between start_log and end_log
# ever crosses the obstacle.
# We do this by looking at only the x, y dimensions and checking if the
# 2d line intersects with the circle (cylindrical obstacle
# cross-section). We then use the line equation to determine the
# altitude at which the intersection occurs. If the altitude is between
# 0 and self.cylinder_height, a collision occured.
# Reference: https://math.stackexchange.com/questions/980089/how-to-check-if-a-3d-line-segment-intersects-a-cylinder
# Convert points to UTM projection.
# We need a cartesian coordinate system to perform the calculation.
try:
x1, y1 = pyproj.transform(distance.wgs84, utm, start_lon,
start_lat)
z1 = units.feet_to_meters(start_alt)
x2, y2 = pyproj.transform(distance.wgs84, utm, end_lon, end_lat)
z2 = units.feet_to_meters(end_alt)
cx, cy = pyproj.transform(distance.wgs84, utm, lonc, latc)
except RuntimeError:
# pyproj throws RuntimeError if the coordinates are grossly beyond
# the bounds of the projection. We do not count this as a collision.
return False
rm = units.feet_to_meters(self.cylinder_radius)
hm = units.feet_to_meters(self.cylinder_height)
# Calculate equation of line and substitute into circle equation.
# Equation of obstacle circle:
# (x - latc)^2 + (y - laty)^2 = self.cylinder_radius^2
if x2 - x1 == 0:
# If delta X is 0, substitute X as constant and solve for y.
p = [1, -2 * cy, cy**2 + (x1 - cx)**2 - rm**2]
roots = np.roots(p)
for root in roots:
# Solve for altitude and check if within bounds.
zcalc = ((root - y1) * (z2 - z1) / (y2 - y1)) + z1
if np.isreal(root) and zcalc <= hm:
return True
else:
# Calculate slope and intercept of line between start and end log.
m = (y2 - y1) / (x2 - x1)
b = y1 - m * x1
# Substitute in line equation and solve for x.
p = [
m**2 + 1, (2 * m * (b - cy)) - (2 * cx),
cx**2 + (b - cy)**2 - rm**2
]
roots = np.roots(p)
for root in roots:
# Solve for altitude and check if within bounds.
zcalc = ((root - x1) * (z2 - z1) / (x2 - x1)) + z1
if np.isreal(root) and zcalc <= hm:
return True
return False
def mission_kml(mission, kml, kml_doc):
"""
Appends kml nodes describing the mission.
Args:
mission: The mission to add to the KML.
kml: A simpleKML Container to which the mission data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
The KML folder for the mission data.
"""
mission_name = 'Mission {}'.format(mission.pk)
kml_folder = kml.newfolder(name=mission_name)
# Transform from WGS84 to UTM at home position.
wgs_to_utm = pyproj.transformer.Transformer.from_proj(
distance.proj_wgs84,
distance.proj_utm(mission.home_pos.latitude,
mission.home_pos.longitude))
# Transform from WGS84 to Web Mercator.
wgs_to_web_mercator = pyproj.transformer.Transformer.from_proj(
distance.proj_wgs84, distance.proj_web_mercator)
# Flight boundaries.
fly_zone_folder = kml_folder.newfolder(name='Fly Zones')
for flyzone in mission.fly_zones.all():
fly_zone_kml(flyzone, fly_zone_folder)
# Static points.
locations = [
('Home', mission.home_pos, KML_HOME_ICON),
('Emergent LKP', mission.emergent_last_known_pos, KML_ODLC_ICON),
('Off Axis', mission.off_axis_odlc_pos, KML_ODLC_ICON),
('Air Drop', mission.air_drop_pos, KML_DROP_ICON),
('Map Center', mission.map_center_pos, KML_MAP_CENTER_ICON),
]
for key, point, icon in locations:
gps = (point.longitude, point.latitude)
p = kml_folder.newpoint(name=key, coords=[gps])
p.iconstyle.icon.href = icon
p.description = str(point)
# ODLCs.
oldc_folder = kml_folder.newfolder(name='ODLCs')
for odlc in mission.odlcs.select_related().all():
name = 'ODLC %d' % odlc.pk
gps = (odlc.location.longitude, odlc.location.latitude)
p = oldc_folder.newpoint(name=name, coords=[gps])
p.iconstyle.icon.href = KML_ODLC_ICON
p.description = name
# Waypoints
waypoints_folder = kml_folder.newfolder(name='Waypoints')
linestring = waypoints_folder.newlinestring(name='Waypoints')
waypoints = []
for i, waypoint in enumerate(mission.mission_waypoints.order_by('order')):
coord = (waypoint.longitude, waypoint.latitude,
units.feet_to_meters(waypoint.altitude_msl))
waypoints.append(coord)
# Add waypoint marker
p = waypoints_folder.newpoint(name='Waypoint %d' % (i + 1),
coords=[coord])
p.iconstyle.icon.href = KML_WAYPOINT_ICON
p.description = str(waypoint)
p.altitudemode = AltitudeMode.absolute
p.extrude = 1
linestring.coords = waypoints
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.green
linestring.style.polystyle.color = Color.changealphaint(100, Color.green)
# Search Area
search_area = []
for point in mission.search_grid_points.order_by('order'):
coord = (point.longitude, point.latitude,
units.feet_to_meters(point.altitude_msl))
search_area.append(coord)
if search_area:
search_area.append(search_area[0])
pol = kml_folder.newpolygon(name='Search Area')
pol.outerboundaryis = search_area
pol.style.linestyle.color = Color.blue
pol.style.linestyle.width = 2
pol.style.polystyle.color = Color.changealphaint(50, Color.blue)
# Map Area
map_x, map_y = wgs_to_web_mercator.transform(
mission.map_center_pos.longitude, mission.map_center_pos.latitude)
map_height = units.feet_to_meters(mission.map_height_ft)
map_width = (map_height * 16) / 9
map_points = [
(map_x - map_width / 2, map_y - map_height / 2),
(map_x + map_width / 2, map_y - map_height / 2),
(map_x + map_width / 2, map_y + map_height / 2),
(map_x - map_width / 2, map_y + map_height / 2),
(map_x - map_width / 2, map_y - map_height / 2),
]
map_points = [
wgs_to_web_mercator.transform(
px, py, direction=pyproj.enums.TransformDirection.INVERSE)
for (px, py) in map_points
]
map_points = [(x, y, 0) for (x, y) in map_points]
map_pol = kml_folder.newpolygon(name='Map')
map_pol.outerboundaryis = map_points
map_pol.style.linestyle.color = Color.green
map_pol.style.linestyle.width = 2
map_pol.style.polystyle.color = Color.changealphaint(50, Color.green)
# Stationary Obstacles.
stationary_obstacles_folder = kml_folder.newfolder(
name='Stationary Obstacles')
for obst in mission.stationary_obstacles.all():
cx, cy = wgs_to_utm.transform(obst.longitude, obst.latitude)
rm = units.feet_to_meters(obst.cylinder_radius)
hm = units.feet_to_meters(obst.cylinder_height)
obst_points = []
for angle in np.linspace(0, 2 * math.pi, num=KML_OBST_NUM_POINTS):
px = cx + rm * math.cos(angle)
py = cy + rm * math.sin(angle)
lon, lat = wgs_to_utm.transform(
px, py, direction=pyproj.enums.TransformDirection.INVERSE)
obst_points.append((lon, lat, hm))
pol = stationary_obstacles_folder.newpolygon(name='Obstacle %d' %
obst.pk)
pol.outerboundaryis = obst_points
pol.altitudemode = AltitudeMode.absolute
pol.extrude = 1
pol.style.linestyle.color = Color.yellow
pol.style.linestyle.width = 2
pol.style.polystyle.color = Color.changealphaint(50, Color.yellow)
return kml_folder
def distance_to_line(start, end, point, utm):
"""Compute the closest distance from point to a line segment.
Based on the point-line distance derived in:
http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
(l_1 - p) dot (l_2 - p)
t = - -----------------------
|l_2 - l_1|^2
We clamp t to 0 <= t <= 1 to ensure we calculate distance to a point on
the line segment.
The closest point can be determined using t:
p_c = l_1 + t * (l_2 - l_1)
And the distance is:
d = |p - p_c|
Arguments are points in the form (lat, lon, alt MSL (ft)).
Args:
start: Defines the start of the line.
end: Defines the end of the line.
point: Free point to compute distance from.
utm: The UTM Proj projection to project into. If start, end, or point
are well outside of this projection, this function returns
infinite.
Returns:
Closest distance in ft from point to the line.
"""
try:
# Convert points to UTM projection.
# We need a cartesian coordinate system to perform the calculation.
lat, lon, ftmsl = start
x, y = pyproj.transform(wgs84, utm, lon, lat)
l1 = np.array([x, y, units.feet_to_meters(ftmsl)])
lat, lon, ftmsl = end
x, y = pyproj.transform(wgs84, utm, lon, lat)
l2 = np.array([x, y, units.feet_to_meters(ftmsl)])
lat, lon, ftmsl = point
x, y = pyproj.transform(wgs84, utm, lon, lat)
p = np.array([x, y, units.feet_to_meters(ftmsl)])
except RuntimeError:
# pyproj throws RuntimeError if the coordinates are grossly beyond the
# bounds of the projection. We simplify this to "infinite" distance.
return float("inf")
d1 = l1 - p
d2 = l2 - l1
num = np.dot(d1, d2)
dem = np.linalg.norm(l2 - l1)**2
t = -num / dem
if t < 0:
t = 0
elif t > 1:
t = 1
p_c = l1 + t * (l2 - l1)
dist = np.linalg.norm(p - p_c)
return units.meters_to_feet(dist)
def kml(cls, user, logs, kml, kml_doc):
"""
Appends kml nodes describing the given user's flight as described
by the log array given.
Args:
user: A Django User to get username from
logs: A list of UasTelemetry elements
kml: A simpleKML Container to which the flight data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
None
"""
# KML Compliant Datetime Formatter
kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ"
icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png'
threshold = 1 # Degrees
kml_folder = kml.newfolder(name=user.username)
flights = TakeoffOrLandingEvent.flights(user)
if len(flights) == 0:
return
logs = filter(lambda log: cls._is_bad_position(log, threshold), logs)
for i, flight in enumerate(flights):
label = 'Flight {}'.format(i + 1) # Flights are one-indexed
kml_flight = kml_folder.newfolder(name=label)
flight_logs = filter(lambda x: flight.within(x.timestamp), logs)
if len(flight_logs) < 2:
continue
coords = []
angles = []
when = []
for entry in flight_logs:
pos = entry.uas_position.gps_position
# Spatial Coordinates
coord = (pos.longitude, pos.latitude,
units.feet_to_meters(entry.uas_position.altitude_msl))
coords.append(coord)
# Time Elements
time = entry.timestamp.strftime(kml_datetime_format)
when.append(time)
# Degrees heading, tilt, and roll
angle = (entry.uas_heading, 0.0, 0.0)
angles.append(angle)
# Create a new track in the folder
trk = kml_flight.newgxtrack(name='Flight Path')
trk.altitudemode = AltitudeMode.absolute
# Append flight data
trk.newwhen(when)
trk.newgxcoord(coords)
trk.newgxangle(angles)
# Set styling
trk.extrude = 1 # Extend path to ground
trk.style.linestyle.width = 2
trk.style.linestyle.color = Color.blue
trk.iconstyle.icon.href = icon
for obstacle in MovingObstacle.objects.all():
obstacle.kml(path=flight_logs, kml=kml_flight, kml_doc=kml_doc)
def kml(cls, user, logs, kml, kml_doc):
"""
Appends kml nodes describing the given user's flight as described
by the log array given.
Args:
user: A Django User to get username from
logs: A list of UasTelemetry elements
kml: A simpleKML Container to which the flight data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
None
"""
# KML Compliant Datetime Formatter
kml_datetime_format = "%Y-%m-%dT%H:%M:%S.%fZ"
icon = 'http://maps.google.com/mapfiles/kml/shapes/airports.png'
kml_folder = kml.newfolder(name=user.username)
flights = TakeoffOrLandingEvent.flights(user)
if len(flights) == 0:
return
logs = UasTelemetry.dedupe(UasTelemetry.filter_bad(logs))
for i, flight in enumerate(flights):
label = 'Flight {}'.format(i + 1) # Flights are one-indexed
kml_flight = kml_folder.newfolder(name=label)
flight_logs = filter(lambda x: flight.within(x.timestamp), logs)
if len(flight_logs) < 2:
continue
coords = []
angles = []
when = []
for entry in flight_logs:
pos = entry.uas_position.gps_position
# Spatial Coordinates
coord = (pos.longitude, pos.latitude,
units.feet_to_meters(entry.uas_position.altitude_msl))
coords.append(coord)
# Time Elements
time = entry.timestamp.strftime(kml_datetime_format)
when.append(time)
# Degrees heading, tilt, and roll
angle = (entry.uas_heading, 0.0, 0.0)
angles.append(angle)
# Create a new track in the folder
trk = kml_flight.newgxtrack(name='Flight Path')
trk.altitudemode = AltitudeMode.absolute
# Append flight data
trk.newwhen(when)
trk.newgxcoord(coords)
trk.newgxangle(angles)
# Set styling
trk.extrude = 1 # Extend path to ground
trk.style.linestyle.width = 2
trk.style.linestyle.color = Color.blue
trk.iconstyle.icon.href = icon
def mission_kml(mission, kml, kml_doc):
"""
Appends kml nodes describing the mission.
Args:
mission: The mission to add to the KML.
kml: A simpleKML Container to which the mission data will be added
kml_doc: The simpleKML Document to which schemas will be added
Returns:
The KML folder for the mission data.
"""
mission_name = 'Mission {}'.format(mission.pk)
kml_folder = kml.newfolder(name=mission_name)
# Flight boundaries.
fly_zone_folder = kml_folder.newfolder(name='Fly Zones')
for flyzone in mission.fly_zones.all():
fly_zone_kml(flyzone, fly_zone_folder)
# Static points.
locations = [
('Home', mission.home_pos, KML_HOME_ICON),
('Emergent LKP', mission.emergent_last_known_pos, KML_ODLC_ICON),
('Off Axis', mission.off_axis_odlc_pos, KML_ODLC_ICON),
('Air Drop', mission.air_drop_pos, KML_DROP_ICON),
]
for key, point, icon in locations:
gps = (point.longitude, point.latitude)
p = kml_folder.newpoint(name=key, coords=[gps])
p.iconstyle.icon.href = icon
p.description = str(point)
# ODLCs.
oldc_folder = kml_folder.newfolder(name='ODLCs')
for odlc in mission.odlcs.all():
name = 'ODLC %d' % odlc.pk
gps = (odlc.location.longitude, odlc.location.latitude)
p = oldc_folder.newpoint(name=name, coords=[gps])
p.iconstyle.icon.href = KML_ODLC_ICON
p.description = name
# Waypoints
waypoints_folder = kml_folder.newfolder(name='Waypoints')
linestring = waypoints_folder.newlinestring(name='Waypoints')
waypoints = []
for i, waypoint in enumerate(mission.mission_waypoints.order_by('order')):
gps = waypoint.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(waypoint.position.altitude_msl))
waypoints.append(coord)
# Add waypoint marker
p = waypoints_folder.newpoint(name='Waypoint %d' % (i + 1),
coords=[coord])
p.iconstyle.icon.href = KML_WAYPOINT_ICON
p.description = str(waypoint)
p.altitudemode = AltitudeMode.absolute
p.extrude = 1
linestring.coords = waypoints
linestring.altitudemode = AltitudeMode.absolute
linestring.extrude = 1
linestring.style.linestyle.color = Color.green
linestring.style.polystyle.color = Color.changealphaint(100, Color.green)
# Search Area
search_area = []
for point in mission.search_grid_points.order_by('order'):
gps = point.position.gps_position
coord = (gps.longitude, gps.latitude,
units.feet_to_meters(point.position.altitude_msl))
search_area.append(coord)
if search_area:
search_area.append(search_area[0])
pol = kml_folder.newpolygon(name='Search Area')
pol.outerboundaryis = search_area
pol.style.linestyle.color = Color.blue
pol.style.linestyle.width = 2
pol.style.polystyle.color = Color.changealphaint(50, Color.blue)
# Stationary Obstacles.
stationary_obstacles_folder = kml_folder.newfolder(
name='Stationary Obstacles')
for obst in mission.stationary_obstacles.all():
gpos = obst.gps_position
zone, north = distance.utm_zone(gpos.latitude, gpos.longitude)
proj = distance.proj_utm(zone, north)
cx, cy = proj(gpos.longitude, gpos.latitude)
rm = units.feet_to_meters(obst.cylinder_radius)
hm = units.feet_to_meters(obst.cylinder_height)
obst_points = []
for angle in np.linspace(0, 2 * math.pi, num=KML_OBST_NUM_POINTS):
px = cx + rm * math.cos(angle)
py = cy + rm * math.sin(angle)
lon, lat = proj(px, py, inverse=True)
obst_points.append((lon, lat, hm))
pol = stationary_obstacles_folder.newpolygon(name='Obstacle %d' %
obst.pk)
pol.outerboundaryis = obst_points
pol.altitudemode = AltitudeMode.absolute
pol.extrude = 1
pol.style.linestyle.color = Color.yellow
pol.style.linestyle.width = 2
pol.style.polystyle.color = Color.changealphaint(50, Color.yellow)
return kml_folder
def determine_interpolated_collision(self, start_log, end_log, utm):
"""Determines whether the UAS collided with the obstacle by
interpolating between start and end telemetry.
Args:
start_log: A UAS telemetry log.
end_log: A UAS telemetry log.
utm: The UTM Proj projection to project into.
Returns:
True if the UAS collided with the obstacle, False otherwise.
"""
start_lat = start_log.uas_position.gps_position.latitude
start_lon = start_log.uas_position.gps_position.longitude
start_alt = start_log.uas_position.altitude_msl
end_lat = end_log.uas_position.gps_position.latitude
end_lon = end_log.uas_position.gps_position.longitude
end_alt = end_log.uas_position.altitude_msl
latc = self.gps_position.latitude
lonc = self.gps_position.longitude
# First, check if altitude for both logs is above cylinder height.
if start_alt > self.cylinder_height and end_alt > self.cylinder_height:
return False
# We want to check if the line drawn between start_log and end_log
# ever crosses the obstacle.
# We do this by looking at only the x, y dimensions and checking if the
# 2d line intersects with the circle (cylindrical obstacle
# cross-section). We then use the line equation to determine the
# altitude at which the intersection occurs. If the altitude is between
# 0 and self.cylinder_height, a collision occured.
# Reference: https://math.stackexchange.com/questions/980089/how-to-check-if-a-3d-line-segment-intersects-a-cylinder
# Convert points to UTM projection.
# We need a cartesian coordinate system to perform the calculation.
try:
x1, y1 = pyproj.transform(distance.wgs84, utm, start_lon,
start_lat)
z1 = units.feet_to_meters(start_alt)
x2, y2 = pyproj.transform(distance.wgs84, utm, end_lon, end_lat)
z2 = units.feet_to_meters(end_alt)
cx, cy = pyproj.transform(distance.wgs84, utm, lonc, latc)
except RuntimeError:
# pyproj throws RuntimeError if the coordinates are grossly beyond
# the bounds of the projection. We do not count this as a collision.
return False
rm = units.feet_to_meters(self.cylinder_radius)
hm = units.feet_to_meters(self.cylinder_height)
# Calculate equation of line and substitute into circle equation.
# Equation of obstacle circle:
# (x - latc)^2 + (y - laty)^2 = self.cylinder_radius^2
if x2 - x1 == 0:
# If delta X is 0, substitute X as constant and solve for y.
p = [1, -2 * cy, cy**2 + (x1 - cx)**2 - rm**2]
roots = np.roots(p)
for root in roots:
# Solve for altitude and check if within bounds.
zcalc = ((root - y1) * (z2 - z1) / (y2 - y1)) + z1
if np.isreal(root) and zcalc <= hm:
return True
else:
# Calculate slope and intercept of line between start and end log.
m = (y2 - y1) / (x2 - x1)
b = y1 - m * x1
# Substitute in line equation and solve for x.
p = [m**2 + 1, (2 * m * (b - cy)) - (2 * cx),
cx**2 + (b - cy)**2 - rm**2]
roots = np.roots(p)
for root in roots:
# Solve for altitude and check if within bounds.
zcalc = ((root - x1) * (z2 - z1) / (x2 - x1)) + z1
if np.isreal(root) and zcalc <= hm:
return True
return False
