Numerous improvments to the engine & physics system (thrust arrown, freezing, gimbal, etc...)

Assets/Scripts/RocketPhysics.cs

@@ -5,6 +5,7 @@ public class RocketPhysics : MonoBehaviour
 {
     [Header("References")]
     public MainEngine mainEngine;
+    public GimbalSystem gimbalSystem;
     public Rigidbody rb;
     public Altimeter altimeter;
 
@@ -16,6 +17,12 @@ public class RocketPhysics : MonoBehaviour
     [Header("Physics Mode Settings")]
     public float physicsTransitionAltitude = 1000f; // meters - switch to Unity physics below this
     public bool useUnityPhysics = true; // current physics mode
+    public bool isFrozen = false; // when true, freezes rocket physics (thrust arrow still works)
+    
+    [Header("Visualization")]
+    public bool showThrustArrow = true;
+    public float thrustArrowScale = 0.0001f; // scales thrust magnitude for arrow length
+    public float thrustArrowMaxLength = 100f; // prevents arrow from being too long
     
     private Vector3 lastPhysicsPosition;
 
@@ -34,12 +41,13 @@ public class RocketPhysics : MonoBehaviour
         // Determine which physics mode to use
         bool shouldUseUnityPhysics = altimeter.altitude < physicsTransitionAltitude;
         
-        // Handle mode transition
-        if (shouldUseUnityPhysics != useUnityPhysics)
+        // Handle mode transition (only if not frozen)
+        if (!isFrozen && shouldUseUnityPhysics != useUnityPhysics)
         {
             TransitionPhysicsMode(shouldUseUnityPhysics);
         }
         
+        // Always calculate physics, but only apply if not frozen
         if (useUnityPhysics)
         {
             UpdateWithUnityPhysics();
@@ -48,6 +56,12 @@ public class RocketPhysics : MonoBehaviour
         {
             UpdateWithDoublePrecision();
         }
+        
+        // Draw thrust visualization (works even when frozen)
+        if (showThrustArrow)
+        {
+            DrawThrustArrow();
+        }
     }
 
     void TransitionPhysicsMode(bool toUnityPhysics)
@@ -78,11 +92,34 @@ public class RocketPhysics : MonoBehaviour
         double gravityAcceleration = altimeter.gh;
         Vector3 gravityForce = new Vector3(0, (float)(-gravityAcceleration * rb.mass), 0);
         
+        // Thrust force (converted from kN to N)
         Vector3 thrustDirection = rb.rotation * Vector3.up;
-        Vector3 thrustForce = thrustDirection * mainEngine.thrust;
+        Vector3 thrustForce = thrustDirection * mainEngine.thrust * 1000f;
         
-        rb.AddForce(gravityForce, ForceMode.Force);
-        rb.AddForce(thrustForce, ForceMode.Force);
+        if (!isFrozen)
+        {
+            // Apply forces only when not frozen
+            rb.AddForce(gravityForce, ForceMode.Force);
+            rb.AddForce(thrustForce, ForceMode.Force);
+            
+            // Apply gimbal torque if available
+            if (gimbalSystem != null)
+            {
+                rb.AddTorque(gimbalSystem.appliedTorque, ForceMode.Force);
+            }
+        }
+        else
+        {
+            // When frozen, manually calculate what velocity would be
+            Vector3 totalForce = gravityForce + thrustForce;
+            Vector3 acceleration = totalForce / rb.mass;
+            Vector3 newVelocity = rb.linearVelocity + acceleration * Time.fixedDeltaTime;
+            velocity = new DoublePrecisionVector3(newVelocity.x, newVelocity.y, newVelocity.z);
+            // Position stays the same when frozen
+            position = new DoublePrecisionVector3(rb.position.x, rb.position.y, rb.position.z);
+            rotation = new DoublePrecisionVector3(rb.rotation.eulerAngles.x, rb.rotation.eulerAngles.y, rb.rotation.eulerAngles.z);
+            return;
+        }
         
         // Update double precision tracking from Unity physics
         position = new DoublePrecisionVector3(rb.position.x, rb.position.y, rb.position.z);
@@ -94,7 +131,7 @@ public class RocketPhysics : MonoBehaviour
     {
         // Manual double-precision integration - no ground interaction
         double gravityAcceleration = altimeter.gh;
-        double thrustAcceleration = mainEngine.thrust / rb.mass;
+        double thrustAcceleration = (mainEngine.thrust * 1000.0) / rb.mass; // Convert kN to N
 
         Vector3 thrustDirection = rb.rotation * Vector3.up;
         DoublePrecisionVector3 gravityAccelerationVector = new DoublePrecisionVector3(0, -gravityAcceleration, 0);
@@ -105,15 +142,110 @@ public class RocketPhysics : MonoBehaviour
         );
         DoublePrecisionVector3 totalAcceleration = gravityAccelerationVector + thrustAccelerationVector;
 
+        // Always calculate new velocity and position
         velocity = velocity + totalAcceleration * Time.fixedDeltaTime;
         position = position + velocity * Time.fixedDeltaTime;
 
-        // Update Unity transform (kinematic mode)
-        Vector3 newPosition = new Vector3((float)position.x, (float)position.y, (float)position.z);
-        rb.position = newPosition;
-        // Do not set linearVelocity for kinematic bodies
-        // rb.linearVelocity = new Vector3((float)velocity.x, (float)velocity.y, (float)velocity.z);
-        rb.rotation = Quaternion.Euler((float)rotation.x, (float)rotation.y, (float)rotation.z);
+        // Only apply to rigidbody if not frozen
+        if (!isFrozen)
+        {
+            // Apply gimbal torque if available
+            if (gimbalSystem != null && gimbalSystem.appliedTorque.magnitude > 0f)
+            {
+                ApplyManualTorque(gimbalSystem.appliedTorque);
+            }
+
+            // Update Unity transform (kinematic mode)
+            Vector3 newPosition = new Vector3((float)position.x, (float)position.y, (float)position.z);
+            rb.position = newPosition;
+            // Do not set linearVelocity for kinematic bodies
+            // rb.linearVelocity = new Vector3((float)velocity.x, (float)velocity.y, (float)velocity.z);
+            rb.rotation = Quaternion.Euler((float)rotation.x, (float)rotation.y, (float)rotation.z);
+        }
+    }
+
+    /// <summary>
+    /// Manually applies torque for double-precision physics mode
+    /// </summary>
+    private void ApplyManualTorque(Vector3 torque)
+    {
+        // Get moment of inertia tensor
+        Vector3 inertiaTensor = rb.inertiaTensor;
+        if (inertiaTensor == Vector3.zero)
+        {
+            // Fallback if inertia tensor not set - use a simplified calculation
+            float I = rb.mass * 25f; // Simplified inertia estimate
+            inertiaTensor = new Vector3(I, I, I);
+        }
+
+        // Calculate angular acceleration: α = τ / I (in local space)
+        Vector3 localTorque = Quaternion.Inverse(rb.rotation) * torque;
+        Vector3 angularAcceleration = new Vector3(
+            inertiaTensor.x > 0 ? localTorque.x / inertiaTensor.x : 0,
+            inertiaTensor.y > 0 ? localTorque.y / inertiaTensor.y : 0,
+            inertiaTensor.z > 0 ? localTorque.z / inertiaTensor.z : 0
+        );
+
+        // Integrate angular velocity (simplified Euler integration)
+        Vector3 angularDisplacement = angularAcceleration * Time.fixedDeltaTime * Time.fixedDeltaTime * Mathf.Rad2Deg;
+        
+        // Update rotation
+        Quaternion deltaRotation = Quaternion.Euler(angularDisplacement);
+        rb.rotation = rb.rotation * deltaRotation;
+
+        // Update rotation tracking
+        rotation.x = rb.rotation.eulerAngles.x;
+        rotation.y = rb.rotation.eulerAngles.y;
+        rotation.z = rb.rotation.eulerAngles.z;
+    }
+
+    /// <summary>
+    /// Draws a debug arrow showing the direction and magnitude of thrust
+    /// </summary>
+    private void DrawThrustArrow()
+    {
+        // Base thrust direction
+        Vector3 thrustDirection = Vector3.up;
+        
+        // Apply gimbal deflection if gimbal system is available
+        if (gimbalSystem != null)
+        {
+            // Apply pitch rotation around local X axis (right)
+            Quaternion pitchRotation = Quaternion.AngleAxis(gimbalSystem.currentPitchAngle, Vector3.right);
+            thrustDirection = pitchRotation * thrustDirection;
+            
+            // Apply yaw rotation around local Z axis (forward)
+            Quaternion yawRotation = Quaternion.AngleAxis(-gimbalSystem.currentYawAngle, Vector3.forward);
+            thrustDirection = yawRotation * thrustDirection;
+        }
+        
+        // Transform to world space
+        thrustDirection = rb.rotation * thrustDirection;
+        
+        float thrustMagnitude = mainEngine.thrust * 1000f; // Convert kN to N
+        
+        // Scale the arrow length proportionally but cap it
+        float arrowLength = Mathf.Min(thrustMagnitude * thrustArrowScale, thrustArrowMaxLength);
+        
+        if (arrowLength > 0.1f) // Only draw if there's meaningful thrust
+        {
+            Vector3 arrowEnd = rb.position + thrustDirection * arrowLength;
+            
+            // Draw main arrow line
+            Debug.DrawLine(rb.position, arrowEnd, Color.green, 0f);
+            
+            // Draw arrowhead (two lines forming a V)
+            Vector3 arrowHeadSize = thrustDirection * (arrowLength * 0.15f);
+            Vector3 perpendicular1 = Vector3.Cross(thrustDirection, Vector3.up).normalized * (arrowLength * 0.1f);
+            if (perpendicular1.magnitude < 0.01f) // Handle case where thrust is vertical
+            {
+                perpendicular1 = Vector3.Cross(thrustDirection, Vector3.right).normalized * (arrowLength * 0.1f);
+            }
+            Vector3 perpendicular2 = Vector3.Cross(thrustDirection, perpendicular1).normalized * (arrowLength * 0.1f);
+            
+            Debug.DrawLine(arrowEnd, arrowEnd - arrowHeadSize + perpendicular1, Color.green, 0f);
+            Debug.DrawLine(arrowEnd, arrowEnd - arrowHeadSize - perpendicular1, Color.green, 0f);
+        }
     }
 }