ENH: Refactor flight.py latitude/longitude to use inverted_haversine (#1055)

Author: Gui-FernandesBR

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)

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

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

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)