diff --git a/CHANGELOG.md b/CHANGELOG.md index 597258823..64a586da8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -59,6 +59,8 @@ Attention: The newest changes should be on top --> ### Changed +- ENH: Refactor flight.py latitude/longitude to use inverted_haversine [#1055](https://github.com/RocketPy-Team/RocketPy/pull/1055) + ### Deprecated - MNT: Rename `radius` to `radius_function` in `CylindricalTank` and `SphericalTank`; old `radius=` keyword argument now raises `DeprecationWarning` [#957](https://github.com/RocketPy-Team/RocketPy/pull/957) diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py index 167364477..ede0009b6 100644 --- a/rocketpy/simulation/flight.py +++ b/rocketpy/simulation/flight.py @@ -21,6 +21,7 @@ find_closest, find_root_linear_interpolation, find_roots_cubic_function, + inverted_haversine, quaternions_to_nutation, quaternions_to_precession, quaternions_to_spin, @@ -3813,40 +3814,29 @@ def latitude(self): """Rocket latitude coordinate, in degrees, as a Function of time. """ - lat1 = np.deg2rad(self.env.latitude) # Launch lat point converted to radians - # Applies the haversine equation to find final lat/lon coordinates - latitude = np.rad2deg( - np.arcsin( - np.sin(lat1) * np.cos(self.drift[:, 1] / self.env.earth_radius) - + np.cos(lat1) - * np.sin(self.drift[:, 1] / self.env.earth_radius) - * np.cos(np.deg2rad(self.bearing[:, 1])) - ) + latitude, _ = inverted_haversine( + self.env.latitude, + self.env.longitude, + self.drift[:, 1], + self.bearing[:, 1], + self.env.earth_radius, ) return np.column_stack((self.time, latitude)) - # TODO: haversine should be defined in tools.py so we just invoke it in here. @funcify_method("Time (s)", "Longitude (°)", "linear", "constant") def longitude(self): """Rocket longitude coordinate, in degrees, as a Function of time. """ - lat1 = np.deg2rad(self.env.latitude) # Launch lat point converted to radians - lon1 = np.deg2rad(self.env.longitude) # Launch lon point converted to radians - # Applies the haversine equation to find final lat/lon coordinates - longitude = np.rad2deg( - lon1 - + np.arctan2( - np.sin(np.deg2rad(self.bearing[:, 1])) - * np.sin(self.drift[:, 1] / self.env.earth_radius) - * np.cos(lat1), - np.cos(self.drift[:, 1] / self.env.earth_radius) - - np.sin(lat1) * np.sin(np.deg2rad(self.latitude[:, 1])), - ) + _, longitude = inverted_haversine( + self.env.latitude, + self.env.longitude, + self.drift[:, 1], + self.bearing[:, 1], + self.env.earth_radius, ) - return np.column_stack((self.time, longitude)) def get_controller_observed_variables(self): diff --git a/rocketpy/tools.py b/rocketpy/tools.py index a7e3582f6..0d7f1a74e 100644 --- a/rocketpy/tools.py +++ b/rocketpy/tools.py @@ -425,18 +425,18 @@ def inverted_haversine(lat0, lon0, distance, bearing, earth_radius=6.3781e6): lon0_rad = np.deg2rad(lon0) # Apply inverted Haversine formula - lat1_rad = math.asin( - math.sin(lat0_rad) * math.cos(distance / earth_radius) - + math.cos(lat0_rad) - * math.sin(distance / earth_radius) - * math.cos(math.radians(bearing)) + lat1_rad = np.arcsin( + np.sin(lat0_rad) * np.cos(distance / earth_radius) + + np.cos(lat0_rad) + * np.sin(distance / earth_radius) + * np.cos(np.radians(bearing)) ) - lon1_rad = lon0_rad + math.atan2( - math.sin(math.radians(bearing)) - * math.sin(distance / earth_radius) - * math.cos(lat0_rad), - math.cos(distance / earth_radius) - math.sin(lat0_rad) * math.sin(lat1_rad), + lon1_rad = lon0_rad + np.arctan2( + np.sin(np.radians(bearing)) + * np.sin(distance / earth_radius) + * np.cos(lat0_rad), + np.cos(distance / earth_radius) - np.sin(lat0_rad) * np.sin(lat1_rad), ) # Convert back to degrees and then return diff --git a/tests/unit/test_tools.py b/tests/unit/test_tools.py index a1e96eb9e..ad79940e0 100644 --- a/tests/unit/test_tools.py +++ b/tests/unit/test_tools.py @@ -7,6 +7,7 @@ euler313_to_quaternions, find_roots_cubic_function, haversine, + inverted_haversine, tuple_handler, ) @@ -137,3 +138,50 @@ def test_invalid_pressure_conversion_factor(pressure_conversion_factor): dictionary="ECMWF", pressure_conversion_factor=pressure_conversion_factor, ) + + +def test_inverted_haversine_scalar(): + """Test inverted_haversine with scalar arguments matches haversine distance.""" + # Arrange + lat0, lon0 = -23.508958, -46.720080 + lat1, lon1 = -23.522939, -46.558253 + earth_radius = 6378100.0 + distance = haversine(lat0, lon0, lat1, lon1, earth_radius) + bearing = 90.0 + + # Act + lat_result, lon_result = inverted_haversine( + lat0, lon0, distance, bearing, earth_radius + ) + + # Assert + recalculated_distance = haversine(lat0, lon0, lat_result, lon_result, earth_radius) + assert recalculated_distance == pytest.approx(distance, abs=1e-2) + + +def test_inverted_haversine_array(): + """Test inverted_haversine with NumPy arrays returns correct array results.""" + # Arrange + lat0, lon0 = -23.508958, -46.720080 + distances = np.array([0.0, 5000.0, 16591.438]) + bearings = np.array([0.0, 45.0, 90.0]) + earth_radius = 6378100.0 + + # Act + lat_results, lon_results = inverted_haversine( + lat0, lon0, distances, bearings, earth_radius + ) + + # Assert + assert isinstance(lat_results, np.ndarray) + assert isinstance(lon_results, np.ndarray) + assert len(lat_results) == 3 + assert len(lon_results) == 3 + + # Check scalar consistency for each element + for i, distance in enumerate(distances): + lat_scalar, lon_scalar = inverted_haversine( + lat0, lon0, distance, bearings[i], earth_radius + ) + assert lat_results[i] == pytest.approx(lat_scalar) + assert lon_results[i] == pytest.approx(lon_scalar)