From 2714f01acdbe70ceb95c6691350cf53556b0055d Mon Sep 17 00:00:00 2001
From: Ting-Hong Shieh <32212900+ting-hong-shieh@users.noreply.github.com>
Date: Sat, 15 Aug 2026 08:54:39 +0800
Subject: [PATCH] Apply gimbaled thrust to translational dynamics

---
 rocketpy/simulation/flight.py        | 142 +++++++++++++--------------
 tests/unit/simulation/test_flight.py |  82 ++++++++++++++++
 2 files changed, 153 insertions(+), 71 deletions(-)

diff --git a/rocketpy/simulation/flight.py b/rocketpy/simulation/flight.py
index 470e0bf8c..db33ddfd9 100644
--- a/rocketpy/simulation/flight.py
+++ b/rocketpy/simulation/flight.py
@@ -1820,6 +1820,31 @@ def udot_rail2(self, t, u, post_processing=False):  # pragma: no cover
         # Hey! We will finish this function later, now we just can use u_dot
         return self.u_dot_generalized(t, u, post_processing=post_processing)
 
+    @staticmethod
+    def _calculate_thrust_vector(
+        effective_thrust, gimbal_angle_x=0.0, gimbal_angle_y=0.0
+    ):
+        """Calculate the motor thrust vector in the rocket body frame.
+
+        Positive ``gimbal_angle_x`` rotates the nominal +e3 thrust toward
+        -e2, while positive ``gimbal_angle_y`` rotates it toward +e1. Angles
+        are expressed in degrees and thrust components in newtons.
+
+        The axial component follows the two-axis decomposition already used
+        by the TVC moment model, so adding the lateral components preserves
+        the effective thrust magnitude.
+        """
+        angle_x = np.deg2rad(gimbal_angle_x)
+        angle_y = np.deg2rad(gimbal_angle_y)
+        thrust1 = effective_thrust * np.sin(angle_y)
+        thrust2 = -effective_thrust * np.sin(angle_x)
+        axial_thrust_squared = max(
+            effective_thrust**2 - thrust1**2 - thrust2**2,
+            0.0,
+        )
+        thrust3 = np.sqrt(axial_thrust_squared)
+        return Vector([thrust1, thrust2, thrust3])
+
     def u_dot(self, t, u, post_processing=False):  # pylint: disable=too-many-locals,too-many-statements
         """Calculates derivative of u state vector with respect to time
         when rocket is flying in 6 DOF motion during ascent out of rail
@@ -1879,40 +1904,26 @@ def u_dot(self, t, u, post_processing=False):  # pylint: disable=too-many-locals
             )
 
             # Thrust Vector Control (TVC)
-            if hasattr(self.rocket, "thrust_vector_control"):
-                # TVC Fz thrust: F = T * sqrt(1 - sin(gimbal_angle_x)**2 - sin(gimbal_angle_y)**2)
-                thrust3 = effective_thrust * np.sqrt(
-                    1
-                    - np.sin(
-                        self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180)
-                    )
-                    ** 2
-                    - np.sin(
-                        self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180)
-                    )
-                    ** 2
-                )
-                tvc_lever = self.rocket.nozzle_to_cdm
-                # TVC Mx My moments: M = T * sin(x) * r
-                M1 += (
-                    np.sin(
-                        self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180)
-                    )
-                    * effective_thrust
-                    * tvc_lever
-                )
-                M2 += (
-                    np.sin(
-                        self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180)
-                    )
-                    * effective_thrust
-                    * tvc_lever
-                )
-            else:
-                thrust3 = effective_thrust
-            # Off center moment
-            M1 += self.rocket.thrust_eccentricity_y * thrust3
-            M2 -= self.rocket.thrust_eccentricity_x * thrust3
+            tvc = getattr(self.rocket, "thrust_vector_control", None)
+            thrust_vector = self._calculate_thrust_vector(
+                effective_thrust,
+                getattr(tvc, "gimbal_angle_x", 0.0),
+                getattr(tvc, "gimbal_angle_y", 0.0),
+            )
+            thrust1, thrust2, thrust3 = thrust_vector
+
+            # Moment from applying the complete thrust vector at the nozzle.
+            thrust_position = Vector(
+                [
+                    self.rocket.thrust_eccentricity_x,
+                    self.rocket.thrust_eccentricity_y,
+                    self.rocket.nozzle_to_cdm,
+                ]
+            )
+            thrust_moment = thrust_position ^ thrust_vector
+            M1 += thrust_moment.x
+            M2 += thrust_moment.y
+            M3 += thrust_moment.z
 
         else:
             # Motor stopped
@@ -1926,7 +1937,7 @@ def u_dot(self, t, u, post_processing=False):  # pylint: disable=too-many-locals
             # Mass
             mass_flow_rate_at_t, propellant_mass_at_t = 0, 0
             # thrust
-            thrust3 = 0
+            thrust1, thrust2, thrust3 = 0, 0, 0
             net_thrust = 0
 
         # Retrieve important quantities
@@ -2149,12 +2160,14 @@ def u_dot(self, t, u, post_processing=False):  # pylint: disable=too-many-locals
         L = [
             (
                 R1
+                + thrust1
                 - b * propellant_mass_at_t * (omega2**2 + omega3**2)
                 - 2 * c * mass_flow_rate_at_t * omega2
             )
             / total_mass_at_t,
             (
                 R2
+                + thrust2
                 + b * propellant_mass_at_t * (alpha3 + omega1 * omega2)
                 + 2 * c * mass_flow_rate_at_t * omega1
             )
@@ -2606,43 +2619,30 @@ def u_dot_generalized(self, t, u, post_processing=False):  # pylint: disable=too
         )
 
         # Thrust Vector Control (TVC)
-        if hasattr(self.rocket, "thrust_vector_control"):
-            tvc_lever = self.rocket.nozzle_to_cdm
-            # TVC Mx My moments: M = T * sin(x) * r
-            M1 += (
-                np.sin(self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180))
-                * effective_thrust
-                * tvc_lever
-            )
-            M2 += (
-                np.sin(self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180))
-                * effective_thrust
-                * tvc_lever
-            )
-            # TVC Fz thrust: F = T * sqrt(1 - sin^2(x) - sin^2(y))
-            thrust3 = effective_thrust * np.sqrt(
-                1
-                - np.sin(
-                    self.rocket.thrust_vector_control.gimbal_angle_x * (np.pi / 180)
-                )
-                ** 2
-                - np.sin(
-                    self.rocket.thrust_vector_control.gimbal_angle_y * (np.pi / 180)
-                )
-                ** 2
-            )
-        else:
-            thrust3 = effective_thrust
-
-        # Off center moment
-        M1 += (
-            self.rocket.cp_eccentricity_y * R3
-            + self.rocket.thrust_eccentricity_y * thrust3
+        tvc = getattr(self.rocket, "thrust_vector_control", None)
+        thrust_vector = self._calculate_thrust_vector(
+            effective_thrust,
+            getattr(tvc, "gimbal_angle_x", 0.0),
+            getattr(tvc, "gimbal_angle_y", 0.0),
         )
-        M2 -= (
-            self.rocket.cp_eccentricity_x * R3
-            + self.rocket.thrust_eccentricity_x * thrust3
+        thrust3 = thrust_vector.z
+
+        # Moment from applying the complete thrust vector at the nozzle.
+        thrust_position = Vector(
+            [
+                self.rocket.thrust_eccentricity_x,
+                self.rocket.thrust_eccentricity_y,
+                self.rocket.nozzle_to_cdm,
+            ]
         )
+        thrust_moment = thrust_position ^ thrust_vector
+        M1 += thrust_moment.x
+        M2 += thrust_moment.y
+        M3 += thrust_moment.z
+
+        # Off center moment
+        M1 += self.rocket.cp_eccentricity_y * R3
+        M2 -= self.rocket.cp_eccentricity_x * R3
         M3 += self.rocket.cp_eccentricity_x * R2 - self.rocket.cp_eccentricity_y * R1
 
         # Roll control moment
@@ -2656,7 +2656,7 @@ def u_dot_generalized(self, t, u, post_processing=False):  # pylint: disable=too
         T00 = total_mass * r_CM
         T03 = 2 * total_mass_dot * (r_NOZ - r_CM) - 2 * total_mass * r_CM_dot
         T04 = (
-            Vector([0, 0, thrust3])
+            thrust_vector
             - total_mass * r_CM_ddot
             - 2 * total_mass_dot * r_CM_dot
             + total_mass_ddot * (r_NOZ - r_CM)
diff --git a/tests/unit/simulation/test_flight.py b/tests/unit/simulation/test_flight.py
index 9af1c5fd5..7fc248776 100644
--- a/tests/unit/simulation/test_flight.py
+++ b/tests/unit/simulation/test_flight.py
@@ -8,6 +8,7 @@
 from scipy import optimize
 
 from rocketpy import Components, Flight, Function, Rocket
+from rocketpy.mathutils.vector_matrix import Vector
 
 plt.rcParams.update({"figure.max_open_warning": 0})
 
@@ -83,6 +84,87 @@ def compute_static_margin_error_given_distance(position, static_margin, rocket):
 # Tests
 
 
+def test_calculate_thrust_vector_zero_gimbal():
+    """A centered nozzle must keep all effective thrust on the body e3 axis."""
+    # Arrange
+    effective_thrust = 100.0
+
+    # Act
+    thrust_vector = Flight._calculate_thrust_vector(effective_thrust)
+
+    # Assert
+    assert tuple(thrust_vector) == pytest.approx((0.0, 0.0, effective_thrust))
+
+
+@pytest.mark.parametrize(
+    "gimbal_angle_x, gimbal_angle_y, expected_direction",
+    [
+        (15.0, 0.0, (0.0, -1.0, 1.0)),
+        (0.0, 15.0, (1.0, 0.0, 1.0)),
+        (-15.0, 0.0, (0.0, 1.0, 1.0)),
+        (0.0, -15.0, (-1.0, 0.0, 1.0)),
+    ],
+)
+def test_calculate_thrust_vector_single_axis_gimbal(
+    gimbal_angle_x, gimbal_angle_y, expected_direction
+):
+    """A single-axis gimbal must create the expected signed lateral force."""
+    # Arrange
+    effective_thrust = 100.0
+    lateral = effective_thrust * np.sin(np.deg2rad(15.0))
+    axial = effective_thrust * np.cos(np.deg2rad(15.0))
+    scale = (lateral, lateral, axial)
+    expected = tuple(
+        direction * component
+        for direction, component in zip(expected_direction, scale)
+    )
+
+    # Act
+    thrust_vector = Flight._calculate_thrust_vector(
+        effective_thrust, gimbal_angle_x, gimbal_angle_y
+    )
+
+    # Assert
+    assert tuple(thrust_vector) == pytest.approx(expected)
+    assert abs(thrust_vector) == pytest.approx(effective_thrust)
+
+
+def test_calculate_thrust_vector_two_axis_gimbal_preserves_magnitude():
+    """Two-axis gimbaling must redirect, not create or destroy, thrust."""
+    # Arrange
+    effective_thrust = 100.0
+
+    # Act
+    thrust_vector = Flight._calculate_thrust_vector(
+        effective_thrust, gimbal_angle_x=6.0, gimbal_angle_y=8.0
+    )
+
+    # Assert
+    assert thrust_vector.x > 0.0
+    assert thrust_vector.y < 0.0
+    assert abs(thrust_vector) == pytest.approx(effective_thrust)
+
+
+def test_gimbaled_thrust_moment_matches_force_at_nozzle():
+    """The TVC moment must be the nozzle lever arm crossed with thrust."""
+    # Arrange
+    effective_thrust = 100.0
+    nozzle_to_cdm = 2.0
+    thrust_vector = Flight._calculate_thrust_vector(
+        effective_thrust, gimbal_angle_x=15.0
+    )
+    thrust_position = Vector([0.0, 0.0, nozzle_to_cdm])
+
+    # Act
+    thrust_moment = thrust_position ^ thrust_vector
+
+    # Assert
+    expected_moment_x = (
+        nozzle_to_cdm * effective_thrust * np.sin(np.deg2rad(15.0))
+    )
+    assert tuple(thrust_moment) == pytest.approx((expected_moment_x, 0.0, 0.0))
+
+
 def test_get_solution_at_time(flight_calisto):
     """Test the get_solution_at_time method of the Flight class. This test
     simply calls the method at the initial and final time and checks if the