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))