def test_calculate_turn_initiation_distance_20(self):
# A shallow turn at the Equator
LATS = np.array([0.0, 0.0, 0.2])
LONS = np.array([-1.0, 0.0, 1.0])
ecef_points = global_Point3d(LATS, LONS)
prev_arc = Arc3d(ecef_points[0], ecef_points[1])
arc = Arc3d(ecef_points[1], ecef_points[2])
TEN_NM = TWENTY_NM / 2
turn_0 = SphereTurnArc(prev_arc, arc, TEN_NM)
turn_angle_0 = turn_0.angle
distance_start = calculate_turn_initiation_distance(
prev_arc, arc, turn_0.start, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_start, TEN_NM)
distance_finish = calculate_turn_initiation_distance(
prev_arc, arc, turn_0.finish, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_finish, TEN_NM)
point_1 = turn_0.position(turn_angle_0 / 2)
distance_1 = calculate_turn_initiation_distance(
prev_arc, arc, point_1, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_1, TEN_NM, decimal=6)
point_2 = turn_0.position(turn_angle_0 / 4)
distance_2 = calculate_turn_initiation_distance(
prev_arc, arc, point_2, TWENTY_NM, ACROSS_TRACK_TOLERANCE / 4)
assert_almost_equal(distance_2, TEN_NM, decimal=6)
point_3 = turn_0.position(3 * turn_angle_0 / 4)
distance_3 = calculate_turn_initiation_distance(
prev_arc, arc, point_3, TWENTY_NM, ACROSS_TRACK_TOLERANCE / 4)
assert_almost_equal(distance_3, TEN_NM, decimal=5)
def test_fit_arc_to_points(self):
LATS_0 = np.zeros(4, dtype=np.float)
LONS_0 = np.array([0.0, 1.0, 2.0, 3.0])
# Test points along arc
ecef_points_0 = global_Point3d(LATS_0, LONS_0)
ecef_arc_0 = Arc3d(ecef_points_0[0], ecef_points_0[-1])
new_arc_0 = fit_arc_to_points(ecef_points_0, ecef_arc_0)
assert_almost_equal(distance_radians(new_arc_0.a(), ecef_arc_0.a()),
0.0)
assert_almost_equal(distance_radians(new_arc_0.b(), ecef_arc_0.b()),
0.0)
# Test slope away from start of arc
ecef_points_1 = global_Point3d(LONS_0, LONS_0)
ecef_arc_1 = Arc3d(ecef_points_1[0], ecef_points_1[-1])
new_arc_1 = fit_arc_to_points(ecef_points_1, ecef_arc_0)
assert_almost_equal(distance_radians(new_arc_1.a(), ecef_arc_0.a()),
0.0)
assert_almost_equal(
distance_radians(new_arc_1.pole(), ecef_arc_1.pole()), 0.0)
# Test slope towards end of arc
LATS_2 = np.array([3.0, 2.0, 1.0, 0.0])
ecef_points_2 = global_Point3d(LATS_2, LONS_0)
ecef_arc_2 = Arc3d(ecef_points_2[0], ecef_points_2[-1])
new_arc_2 = fit_arc_to_points(ecef_points_2, ecef_arc_0)
assert_almost_equal(
distance_radians(new_arc_2.pole(), ecef_arc_2.pole()), 0.0)
assert_almost_equal(distance_radians(new_arc_2.b(), ecef_arc_0.b()),
0.0)
def calculate_path_distance(self, point, index, across_track_tolerance):
"""
Calculate the distance of a point along the path by the leg at index.
It determines whether the point is closer to the previous or next leg
and calculates the path distance accordingly.
Note: index must be within the points, i.e.: index < (len(self) - 1):
Parameters
----------
point: Point3d
The point to measure.
index: integer
The index of the point at the start of the path leg.
across_track_tolerance: float
The maximum across track distance [radians]
Returns
-------
The distance [radians] of the point along the path at index.
"""
# calculate the closest distance between the point and the leg
arc = Arc3d(self.points[index], self.points[index + 1])
closest_distance = arc.closest_distance(point)
prev_distance = closest_distance + 1.0
if index > 0: # not first leg
# calculate the closest distance between the point and the previous leg
arc = Arc3d(self.points[index - 1], self.points[index])
prev_distance = arc.closest_distance(point)
next_distance = closest_distance + 1.0
if index < (len(self) - 2): # not last leg
# calculate the closest distance between the point and the next leg
arc = Arc3d(self.points[index + 1], self.points[index + 2])
next_distance = arc.closest_distance(point)
min_distance = min(closest_distance, min(prev_distance, next_distance))
if min_distance < across_track_tolerance:
# Get the index of the closest leg
if (prev_distance < closest_distance) \
or (next_distance < closest_distance):
index = index - 1 if (
prev_distance < next_distance) else index + 1
else: # None of the legs are within across_track_tolerance
index, _ = find_index_and_ratio(self.points, point)
index = min(index, len(self) - 2)
# Calculate the path distance of the closest leg
path_length = self.path_lengths[index + 1]
distance = np.clip(self.calculate_path_leg_distance(point, index), 0.0,
path_length)
# Add the cumulative path lengths
return distance + np.sum(self.path_lengths[:index + 1])
def calculate_path_cross_track_distance(self, point, index):
"""
Calculate the cross track distance of a point from a path leg at index.
It calculates the cross track distance from the leg starting at index.
However, if the leg has turns at the ends and the point is within one
of the turns, it calculates the cross track distance from the turn centre.
Note: index must be within the points, i.e.: index < (len(self) - 1):
Parameters
----------
point: Point3d
The point to measure.
index: integer
The index of the point at the start of the path leg.
Returns
-------
The cross track distance [radians] of the point from the path leg at index.
"""
# calculate the route leg arc and the point's distance from it
arc = Arc3d(self.points[index], self.points[index + 1])
xtd = arc.cross_track_distance(point)
prev_turn_initiation_distance = self.turn_initiation_distances[index] \
if (index > 0) else 0.0
next_turn_initiation_distance = self.turn_initiation_distances[index + 1] \
if (index < (len(self) - 2)) else 0.0
# if there is a turn at either end
if (prev_turn_initiation_distance > 0.0) or \
(next_turn_initiation_distance > 0.0):
distance = arc.along_track_distance(point)
inside_prev_turn = (prev_turn_initiation_distance > 0.0) and \
(distance < prev_turn_initiation_distance)
if inside_prev_turn:
inbound_leg = Arc3d(self.points[index - 1], self.points[index])
turn_arc = SphereTurnArc(inbound_leg, arc,
prev_turn_initiation_distance)
xtd = turn_arc.cross_track_distance(point)
else:
# calculate the distance to the turn by the next point
next_turn_distance = arc.length(
) - next_turn_initiation_distance
inside_next_turn = (next_turn_initiation_distance > 0.0) and \
(distance > next_turn_distance)
if inside_next_turn:
outbound_leg = Arc3d(self.points[index + 1],
self.points[index + 2])
turn_arc = SphereTurnArc(arc, outbound_leg,
next_turn_initiation_distance)
xtd = turn_arc.cross_track_distance(point)
return xtd
def turn_points(self, *, number_of_points=3):
"""
Calculate the path flown along route legs and around turns.
Parameters
----------
number_of_points: integer
The number of turn points to add between the start and end of each
turn, default 3.
Returns
----------
An ordered array of points [Point3ds] containing the path flown
along route legs and around turns.
"""
# Add the path start point
points = [self.points[0]]
for i in range(1, len(self) - 1):
turn_distance = self.turn_initiation_distances[i]
if turn_distance: # if there is a turn
# Calculate the SphereTurnArc for the turn
inbound_leg = Arc3d(self.points[i - 1], self.points[i])
outbound_leg = Arc3d(self.points[i], self.points[i + 1])
turn_arc = SphereTurnArc(inbound_leg, outbound_leg,
turn_distance)
if turn_arc:
# Add the turn start point
points.append(turn_arc.start)
if number_of_points: # any indermediate points?
# calculate the angle between each point
delta_angle = turn_arc.angle / (1.0 + number_of_points)
angle = delta_angle
for j in range(number_of_points): # create turn points
points.append(turn_arc.position(angle))
angle += delta_angle
# Add the turn finish point
points.append(turn_arc.finish)
else: # invalid turn_arc
points.append(self.points[i])
else: # no turn
points.append(self.points[i])
# Add the path finish point
points.append(self.points[-1])
return to_array(points)
def fit_arc_to_points(points, arc):
"""
Calculate a closest arc through points.
Note: there must be at least 2 points.
Parameters
----------
points: numpy array of Point3ds
The points in spherical vector coordinates.
arc: Arc3d
An initial arc to match the ecef_points.
Returns
-------
A closest arc through points, minimising their across track distances.
"""
atds = calculate_atds(arc, points)
xtds = calculate_xtds(arc, points)
# calculate the slope and intercept of the closest line through the points
slope, intercept, r_value, p_value, std_err = scipy.stats.linregress(
atds, xtds)
a = arc.perp_position(arc.a(), intercept)
b = arc.perp_position(arc.b(), intercept + arc.length() * slope)
return Arc3d(a, b)
def test_find_extreme_point_along_track_index(self):
# A line of Lat Longs around the Equator
LATITUDES = np.zeros(10, dtype=float)
LONGITUDES = np.array(range(-5, 5), dtype=float)
ecef_points_0 = global_Point3d(LATITUDES, LONGITUDES)
# All points within arc
arc = Arc3d(ecef_points_0[0], ecef_points_0[-1])
index_0 = find_extreme_point_along_track_index(arc, ecef_points_0,
1.0 * NM)
self.assertEqual(index_0, 0)
# Point 2 before start of arc
LONGITUDES[2] = -6
ecef_points_1 = global_Point3d(LATITUDES, LONGITUDES)
index_1 = find_extreme_point_along_track_index(arc, ecef_points_1,
1.0 * NM)
self.assertEqual(index_1, 2)
# Point 8 past end of arc
LONGITUDES[8] = 8
ecef_points_2 = global_Point3d(LATITUDES, LONGITUDES)
index_2 = find_extreme_point_along_track_index(arc, ecef_points_2,
1.0 * NM)
self.assertEqual(index_2, 8)
def calculate_intersection(prev_arc, arc):
"""
Calculate the intersection point between a pair of arcs.
If the arcs are on (or very close to) the same Great Circle, it returns
the start point of the second arc.
Parameters
----------
prev_arc, arc: Arc3ds
The arcs before and after the intersection.
Returns
-------
The intersection point of the arcs.
"""
intersection = Arc3d(prev_arc.pole(), arc.pole())
if MIN_LENGTH < intersection.length() < MAX_LENGTH:
intersection_point = intersection.pole()
# swap sign if intersection_point is antipodal point
return -intersection_point \
if distance_radians(arc.a(), intersection_point) > HALF_PI else \
intersection_point
else:
return arc.a()
def calculate_position(points, index, ratio=0.0):
"""
Calculate the position of a point at index and ratio along a list of Point3ds.
Parameters
----------
points: Point3ds array
An array of Point3ds.
index: integer
The index of the point, or the point before if ratio > 0.0
ratio: ratio
The ratio of the position after the point at index, default 0.0.
0.0 <= ratio < 1.0
Returns
-------
The Point3d at index and ratio along the Point3ds array.
"""
point = points[index]
if ratio > 0.0:
if index < len(points) - 1:
arc = Arc3d(point, points[index + 1])
point = arc.position(ratio * arc.length())
else:
point = points[-1]
return point
def test_SphereTurnArc_invalid_init(self):
"""Test initialisation of SphereTurnArc class."""
ecef_point_0 = Point3d(ECEF_ICOSAHEDRON[1][0], ECEF_ICOSAHEDRON[1][1],
ECEF_ICOSAHEDRON[1][2])
ecef_point_1 = Point3d(ECEF_ICOSAHEDRON[2][0], ECEF_ICOSAHEDRON[2][1],
ECEF_ICOSAHEDRON[2][2])
arc_0 = Arc3d(ecef_point_0, ecef_point_1)
arc_1 = Arc3d(ecef_point_1, ecef_point_0)
turn_0 = SphereTurnArc(arc_0, arc_1, TWENTY_NM)
self.assertFalse(turn_0)
# Ensure Turn start, centre and end points are at waypoint
assert_almost_equal(distance_radians(ecef_point_1, turn_0.start), 0.0)
assert_almost_equal(distance_radians(ecef_point_1, turn_0.centre), 0.0)
assert_almost_equal(distance_radians(ecef_point_1, turn_0.finish), 0.0)
def find_index_and_ratio(points, point):
"""
Calculate the index and ratio of the closest point along points to point.
Parameters
----------
points: numpy array of Point3ds
The trajectory points in spherical vector coordinates.
point: Point3d
The point to find the closest point to.
Returns
-------
The index and ratio of the closest point along the Point3ds array.
"""
# Calculate the closest distances of the point to the legs between points
arcs = calculate_Arc3ds(points)
distances = calculate_closest_distances(arcs, point)
# The index of the closest leg
index = distances.argmin()
# Calculate the ratio along the closest leg
arc = Arc3d(points[index], points[index + 1])
atd = arc.along_track_distance(point)
ratio = atd / arc.length()
# if the closest point is at the end of the leg, use start of next leg
if ratio >= 1.0:
ratio = 0.0
index += 1
return index, ratio
def test_calculate_turn_initiation_distance_90(self):
# A 90 degree turn at the Equator
LATS = np.array([0.0, 0.0, 1.0])
LONS = np.array([1.0, 0.0, 0.0])
ecef_points = global_Point3d(LATS, LONS)
prev_arc = Arc3d(ecef_points[0], ecef_points[1])
arc = Arc3d(ecef_points[1], ecef_points[2])
TEN_NM = TWENTY_NM / 2
turn_0 = SphereTurnArc(prev_arc, arc, TEN_NM)
turn_angle_0 = turn_0.angle
distance_start = calculate_turn_initiation_distance(
prev_arc, arc, turn_0.start, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_start, TEN_NM)
distance_finish = calculate_turn_initiation_distance(
prev_arc, arc, turn_0.finish, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_finish, TEN_NM)
point_1 = turn_0.position(turn_angle_0 / 2)
distance_1 = calculate_turn_initiation_distance(
prev_arc, arc, point_1, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_1, TEN_NM)
point_2 = turn_0.position(turn_angle_0 / 4)
distance_2 = calculate_turn_initiation_distance(
prev_arc, arc, point_2, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_2, TEN_NM)
point_3 = turn_0.position(3 * turn_angle_0 / 4)
distance_3 = calculate_turn_initiation_distance(
prev_arc, arc, point_3, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
assert_almost_equal(distance_3, TEN_NM)
# Test with a point further than TWENTY_NM from the intersection
turn_30 = SphereTurnArc(prev_arc, arc, TWENTY_NM + TEN_NM)
distance_4 = calculate_turn_initiation_distance(
prev_arc, arc, turn_30.start, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
self.assertEqual(distance_4, TWENTY_NM)
point_30 = turn_30.position(3 * turn_angle_0 / 4)
distance_30 = calculate_turn_initiation_distance(
prev_arc, arc, point_30, TWENTY_NM, ACROSS_TRACK_TOLERANCE)
self.assertEqual(distance_30, TWENTY_NM)
def test_eq(self):
ecef_point_0 = Point3d(ECEF_ICOSAHEDRON[1][0], ECEF_ICOSAHEDRON[1][1],
ECEF_ICOSAHEDRON[1][2])
ecef_point_1 = Point3d(ECEF_ICOSAHEDRON[2][0], ECEF_ICOSAHEDRON[2][1],
ECEF_ICOSAHEDRON[2][2])
ecef_point_2 = Point3d(ECEF_ICOSAHEDRON[3][0], ECEF_ICOSAHEDRON[3][1],
ECEF_ICOSAHEDRON[3][2])
arc_0 = Arc3d(ecef_point_0, ecef_point_1)
arc_1 = Arc3d(ecef_point_1, ecef_point_2)
turn_0 = SphereTurnArc(arc_0, arc_1, TWENTY_NM)
self.assertTrue(turn_0 == turn_0)
arc_2 = Arc3d(ecef_point_2, ecef_point_1)
arc_3 = Arc3d(ecef_point_1, ecef_point_0)
turn_1 = SphereTurnArc(arc_2, arc_3, TWENTY_NM)
self.assertFalse(turn_0 == turn_1)
def test_calculate_intersection(self):
ecef_points = global_Point3d(ROUTE_LATS, ROUTE_LONS)
# intersection point between arcs on different Great Circles
prev_arc = Arc3d(ecef_points[0], ecef_points[1])
arc = Arc3d(ecef_points[1], ecef_points[2])
point_1 = calculate_intersection(prev_arc, arc)
assert_almost_equal(distance_radians(point_1, ecef_points[1]), 0.0)
# intersection point between arcs on same Great Circles
next_point = arc.position(2 * arc.length())
next_arc = Arc3d(ecef_points[2], next_point)
point_2 = calculate_intersection(arc, next_arc)
assert_almost_equal(distance_radians(point_2, ecef_points[2]), 0.0)
# intersection point between arcs on different Great Circles,
# opposite direction turn
prev_arc = Arc3d(ecef_points[5], ecef_points[6])
arc = Arc3d(ecef_points[6], ecef_points[7])
point_3 = calculate_intersection(prev_arc, arc)
assert_almost_equal(distance_radians(point_3, ecef_points[6]), 0.0)
def subsection_positions(self, start_distance, finish_distance):
"""
Return the points between start_distance and finish_distance.
Parameters
----------
start_distance: float
The start distance along the EcefPath in Nautical Miles].
finish_distance: float
The finish distance along the EcefPath in Nautical Miles].
Returns
-------
points: Point3ds points array.
The array of Point3ds between start_distance and finish_distance
including points at start_distance and finish_distance.
"""
distances_nm = rad2nm(self.path_distances())
start_index, start_ratio = calculate_value_reference(
distances_nm, start_distance)
finish_index, finish_ratio = calculate_value_reference(
distances_nm, finish_distance)
arc = Arc3d(self.points[start_index], self.points[start_index + 1])
start_position = arc.position(start_ratio * arc.length())
positions = [start_position]
for i in range(start_index + 1, finish_index + 1):
positions.append(self.points[i])
if finish_ratio > 0.0:
arc = Arc3d(self.points[finish_index],
self.points[finish_index + 1])
finish_position = arc.position(finish_ratio * arc.length())
positions.append(finish_position)
return to_array(positions)
def point_angle(self, point):
"""
Calculate the angle of a point from the start of the turn arc.
Parameters
----------
point: Point3d
The point to measure.
Returns
-------
angle: float
The angle between point and the start of the turn [radians].
"""
start_arc = Arc3d(self.centre, self.start)
return start_arc.start_angle(point)
def position(self, angle):
"""
Calcuate the position of a point along the turn at angle from the start point.
Parameters
----------
angle: float
The angle between point and the start of the turn [radians].
Returns
-------
point: Point3d
The point at angle from the start along the turn arc.
"""
start_arc = Arc3d(self.centre, self.start)
return start_arc.angle_position(angle)
def calculate_ground_track(self, index, ratio):
"""
Calculate the ground track of a point along the path at index and ratio.
It calculates whether the point is on a route leg or in a turn at either
end of the route leg and then calculates the ground track accordingly.
Parameters
----------
index: integer
The index of the point at the start of a route leg.
ratio: ratio
The ratio of the position as its distance along the path divided
bu the path length. Note: 0.0 <= ratio < 1.0
Returns
-------
The ground track at index and ratio along the path in [radians].
"""
# ensure index is within the points
if index < len(self) - 1:
# calculate the route leg arc
arc = Arc3d(self.points[index], self.points[index + 1])
# calculate the distance from the point at index
path_length = self.path_lengths[index + 1]
distance = ratio * path_length
# calcuate the distance to the turn by the next point
next_turn_distance = path_length - self.turn_half_lengths[index +
1]
# if point is in a Turn
inside_start_turn = (self.turn_half_lengths[index] > 0.0) and \
(distance < self.turn_half_lengths[index])
inside_finish_turn = (self.turn_half_lengths[index + 1] > 0.0) and \
(distance > next_turn_distance)
if (inside_start_turn and (index > 0)) or \
(inside_finish_turn and (index < len(self) - 2)):
inbound_leg = arc
outbound_leg = arc
turn_initiation_distance = self.turn_initiation_distances[
index]
if inside_finish_turn:
turn_initiation_distance = self.turn_initiation_distances[
index + 1]
outbound_leg = Arc3d(self.points[index + 1],
self.points[index + 2])
distance -= next_turn_distance
ratio = 0.5 * distance / self.turn_half_lengths[index + 1]
else: # inside_start_turn
inbound_leg = Arc3d(self.points[index - 1],
self.points[index])
distance += self.turn_half_lengths[index]
ratio = 0.5 * distance / self.turn_half_lengths[index]
turn_arc = SphereTurnArc(inbound_leg, outbound_leg,
turn_initiation_distance)
return inbound_leg.calculate_azimuth(turn_arc.start) \
+ ratio * turn_arc.angle
else: # point is along straight section
# if the leg starts with a turn
if self.turn_initiation_distances[index]:
distance += self.turn_initiation_distances[index] - \
self.turn_half_lengths[index]
ratio = (distance / self.leg_lengths[index + 1])
point = arc.position(ratio * arc.length())
return arc.calculate_azimuth(point)
else:
arc = Arc3d(self.points[-2], self.points[-1])
return arc.calculate_azimuth(self.points[-1])
def calculate_path_leg_distance(self, point, index):
"""
Calculate the distance of a point along a path leg starting at index.
It calculates the along track distance along the leg starting at index.
If the leg starts with a turn and the point is within it,
the distance is the turn distance.
If the leg finishes with a turn and the point is within it,
the distance is the distance in the turn.
Otherwise, if the leg starts with a turn, the distance is adjusted by
the path length through the turn.
Note: index must be within the points, i.e.: index < (len(self) - 1):
Parameters
----------
point: Point3d
The point to measure.
index: integer
The index of the point at the start of the path leg.
Returns
-------
The distance [radians] of the point along the path leg at index.
"""
distance = 0.0
# calculate the route leg arc and the point's distance along it
arc = Arc3d(self.points[index], self.points[index + 1])
distance = arc.along_track_distance(point)
# if there is a start turn and the point is within it
prev_turn_initiation_distance = self.turn_initiation_distances[index] \
if (index > 0) else 0.0
inside_prev_turn = (prev_turn_initiation_distance > 0.0) and \
(distance < prev_turn_initiation_distance)
if inside_prev_turn:
inbound_leg = Arc3d(self.points[index - 1], self.points[index])
turn_arc = SphereTurnArc(inbound_leg, arc,
prev_turn_initiation_distance)
distance = turn_arc.along_track_distance(point) \
- self.turn_half_lengths[index]
else:
# calculate the distance to the turn by the next point
next_turn_initiation_distance = self.turn_initiation_distances[index + 1] \
if (index < (len(self) - 2)) else 0.0
next_turn_distance = arc.length() - next_turn_initiation_distance
inside_next_turn = (next_turn_initiation_distance > 0.0) and \
(distance > next_turn_distance)
if inside_next_turn:
outbound_leg = Arc3d(self.points[index + 1],
self.points[index + 2])
turn_arc = SphereTurnArc(arc, outbound_leg,
next_turn_initiation_distance)
distance = turn_arc.along_track_distance(point)
distance += self.path_lengths[index + 1] \
- self.turn_half_lengths[index + 1]
elif prev_turn_initiation_distance > 0.0:
# point is along straight section that starts with a turn
distance += self.turn_half_lengths[
index] - prev_turn_initiation_distance
return distance
def test_SphereTurnArc_init(self):
"""Test initialisation of SphereTurnArc class."""
ecef_point_0 = Point3d(ECEF_ICOSAHEDRON[1][0], ECEF_ICOSAHEDRON[1][1],
ECEF_ICOSAHEDRON[1][2])
ecef_point_1 = Point3d(ECEF_ICOSAHEDRON[2][0], ECEF_ICOSAHEDRON[2][1],
ECEF_ICOSAHEDRON[2][2])
ecef_point_2 = Point3d(ECEF_ICOSAHEDRON[3][0], ECEF_ICOSAHEDRON[3][1],
ECEF_ICOSAHEDRON[3][2])
arc_0 = Arc3d(ecef_point_0, ecef_point_1)
arc_1 = Arc3d(ecef_point_1, ecef_point_2)
turn_0 = SphereTurnArc(arc_0, arc_1, TWENTY_NM)
self.assertTrue(turn_0)
# Ensure Turn start and end points are along inbound and outbound arcs
assert_almost_equal(distance_radians(ecef_point_1, turn_0.start),
TWENTY_NM)
assert_almost_equal(distance_radians(ecef_point_1, turn_0.finish),
TWENTY_NM)
assert_almost_equal(arc_0.cross_track_distance(turn_0.start), 0.0)
assert_almost_equal(arc_1.cross_track_distance(turn_0.finish), 0.0)
ANGLE = 0.2 * np.pi
RADIUS = 20.0 / np.tan(ANGLE / 2.0) # 61.553670693462841 NM
assert_almost_equal(turn_0.angle, -ANGLE)
assert_almost_equal(rad2nm(turn_0.radius), RADIUS)
assert_almost_equal(rad2nm(turn_0.length()), RADIUS * ANGLE)
assert_almost_equal(turn_0.radial_distance(turn_0.start),
turn_0.radius,
decimal=6)
assert_almost_equal(turn_0.radial_distance(turn_0.finish),
turn_0.radius,
decimal=6)
DISTANCE = TWENTY_NM / np.sin(ANGLE / 2.0) # 64.721359549995796 NM
assert_almost_equal(turn_0.radial_distance(ecef_point_1), DISTANCE)
assert_almost_equal(turn_0.cross_track_distance(turn_0.start),
0.0,
decimal=6)
assert_almost_equal(turn_0.cross_track_distance(turn_0.finish),
0.0,
decimal=6)
assert_almost_equal(turn_0.cross_track_distance(ecef_point_1),
DISTANCE - turn_0.radius)
self.assertEqual(turn_0.point_angle(turn_0.centre), 0.0)
assert_almost_equal(turn_0.point_angle(turn_0.start), 0.0)
assert_almost_equal(turn_0.point_angle(turn_0.finish),
turn_0.angle,
decimal=4)
assert_almost_equal(turn_0.point_angle(ecef_point_1),
turn_0.angle / 2.0,
decimal=4)
assert_almost_equal(turn_0.along_track_distance(turn_0.start), 0.0)
assert_almost_equal(turn_0.along_track_distance(turn_0.finish),
turn_0.length(),
decimal=6)
assert_almost_equal(turn_0.along_track_distance(ecef_point_1),
turn_0.length() / 2.0,
decimal=6)
pos_1 = turn_0.position(turn_0.angle)
assert_almost_equal(distance_radians(pos_1, turn_0.centre),
turn_0.radius)
assert_almost_equal(distance_radians(pos_1, turn_0.finish),
0.0,
decimal=6)
pos_2 = turn_0.position(turn_0.angle / 2.0)
assert_almost_equal(distance_radians(pos_2, turn_0.centre),
turn_0.radius)
assert_almost_equal(turn_0.point_angle(pos_2), turn_0.angle / 2.0)
def find_extreme_point_index(points, first_index, last_index, threshold,
xtd_ratio, calc_along_track):
"""
Find the index of the point in points furthest from the Arc.
The points are searched between first_index and last_index.
If the Arc is longer than MINIMUM_ARC_LENGTH, it finds the point with the
largest across track distance. If the distance is larger than threshold
or xtd_ratio time the arc length, it returns the index of the point.
Otherwise, it calculates whether any points are beyond the end of the arc,
if so it returns the index of the furthest point.
If the Arc is not longer than MINIMUM_ARC_LENGTH, if finds the furthest point
the start. If the distance from the point to the start and end points is
greater than MINIMUM_ARC_LENGTH, it returns the index of the point.
Parameters
----------
points: numpy array of Point3ds
The trajectory points in spherical vector coordinates.
first_index, last_index: integers
Indicies of the first and last points to use.
threshold: float
The across track distance threshold.
Returns
-------
The index of the point furthest from the Arc joining the points at
first_index and last_index.
"""
max_xtd_index = last_index
# If there is at least a point between first_index and last_index
if ((last_index - first_index) > 1):
arc = Arc3d(points[first_index], points[last_index])
# first point is after arc start
if arc.length() > MINIMUM_ARC_LENGTH:
# calculate cross track distances relative to the base arc
xtds = calculate_xtds(arc, points[first_index + 1:last_index])
max_xtd, xtd_index = find_most_extreme_value(xtds)
xtd_index += 1
# set the threshold to the minimum of the threshold or a
# fraction of the arc length
xtd_threshold = min(threshold, xtd_ratio * arc.length())
xtd_threshold = max(xtd_threshold, MINIMUM_ARC_LENGTH)
if (np.abs(max_xtd) > xtd_threshold):
# if the point is further than the threshold, return it
max_xtd_index = first_index + xtd_index
elif calc_along_track: # points are in-line
# test whether a point is past the start or end of the arc
atd_index = find_extreme_point_along_track_index(
arc, points[first_index:last_index], MINIMUM_ARC_LENGTH)
if atd_index:
max_xtd_index = first_index + atd_index
else: # short arc
# calculate the furthest point from the start point
distance, xtd_index = \
find_furthest_distance(points[first_index: last_index])
if (distance > MINIMUM_ARC_LENGTH):
# ensure that the point is far enough from the end point
xtd_index += first_index
end_distance = distance_radians(points[xtd_index],
points[last_index])
if end_distance > MINIMUM_ARC_LENGTH:
max_xtd_index = xtd_index
return max_xtd_index
def derive_horizontal_path(points, threshold, calc_along_track=False):
"""
Derive horizontal path waypoints and turn anticipation distances from points.
Parameters
----------
points: numpy array of Point3ds
The trajectory points in spherical vector coordinates.
threshold: float
The across track distance threshold [radians].
calc_along_track : Boolean
Calculate the along track distance, default False.
Returns
-------
The SpherePath between the points.
"""
# Find extreme points and their indicies in the ecef_points array
indicies = find_extreme_point_indicies(points,
threshold,
calc_along_track=calc_along_track)
extreme_points = points[[indicies]]
# Calculate the Great Circle arc along the first route leg
prev_index = 0
index = indicies[1]
prev_arc = Arc3d(extreme_points[0], extreme_points[1])
prev_arc = fit_arc_to_points(points[prev_index:index + 1], prev_arc)
# Create lists for the waypoints and turn initiation distances
# Starting with the first point
path_waypoints = [prev_arc.a()]
turn_distances = [0.0]
prev_length = prev_arc.length()
for i in range(1, len(extreme_points) - 1):
# Calculate the Great Circle arc along the next route leg
prev_index = index
index = indicies[i + 1]
arc = Arc3d(extreme_points[i], extreme_points[i + 1])
arc = fit_arc_to_points(points[prev_index:index + 1], arc)
# Calculate the turn parameters at the waypoint
turn_angle = prev_arc.turn_angle(arc.b())
max_turn_distance = calculate_max_turn_initiation_distance(
prev_length, arc.length())
waypoint = arc.a()
turn_distance = 0.0
# Determine whether the turn is valid, calculate waypoint and
# turn initiation distance accordingly
is_valid_turn = (MIN_TURN_ANGLE < abs(turn_angle) <= MAX_TURN_ANGLE) and \
(max_turn_distance > TWO_NM)
if is_valid_turn:
waypoint = calculate_intersection(prev_arc, arc)
turn_distance = calculate_turn_initiation_distance(
prev_arc, arc, points[prev_index + 1], max_turn_distance,
threshold / 4.0)
path_waypoints.append(waypoint)
turn_distances.append(turn_distance)
prev_arc = arc
prev_length = arc.length()
# Add the last point
path_waypoints.append(prev_arc.b())
turn_distances.append(0.0)
return SpherePath(to_array(path_waypoints), np.array(turn_distances))
