From 4293bfec945cb6e2649e446fb2066e6b78507296 Mon Sep 17 00:00:00 2001
From: Titan Yuan <tryuan99@gmail.com>
Date: Tue, 24 Sep 2024 18:27:26 -0700
Subject: [PATCH] Add serialized fields for instantaneous velocity and
 acceleration

---
 Assets/Scripts/Agent.cs                | 15 +++++++++++++++
 Assets/Scripts/Interceptors/Hydra70.cs |  2 --
 Assets/Scripts/Missile.cs              |  7 +++----
 Assets/Scripts/SimManager.cs           | 25 +++++++++++--------------
 4 files changed, 29 insertions(+), 20 deletions(-)

diff --git a/Assets/Scripts/Agent.cs b/Assets/Scripts/Agent.cs
index 52b7a96..9a315c0 100644
--- a/Assets/Scripts/Agent.cs
+++ b/Assets/Scripts/Agent.cs
@@ -8,6 +8,15 @@ public abstract class Agent : MonoBehaviour {
   [SerializeField]
   private FlightPhase _flightPhase = FlightPhase.INITIALIZED;
 
+  [SerializeField]
+  protected Vector3 _velocity;
+
+  [SerializeField]
+  protected Vector3 _acceleration;
+
+  [SerializeField]
+  protected Vector3 _dragAcceleration;
+
   [SerializeField]
   protected Agent _target;
   protected bool _isHit = false;
@@ -131,6 +140,7 @@ public abstract class Agent : MonoBehaviour {
 
   protected virtual void Awake() {
     _staticConfig = ConfigLoader.LoadStaticConfig(staticConfigFile);
+    GetComponent<Rigidbody>().mass = _staticConfig.bodyConfig.mass;
   }
 
   // Start is called before the first frame update
@@ -174,7 +184,12 @@ public abstract class Agent : MonoBehaviour {
       case FlightPhase.TERMINATED:
         break;
     }
+
+    _velocity = GetComponent<Rigidbody>().velocity;
+    _acceleration =
+        GetComponent<Rigidbody>().GetAccumulatedForce() / GetComponent<Rigidbody>().mass;
   }
+
   protected virtual void AlignWithVelocity() {
     Vector3 velocity = GetVelocity();
     if (velocity.magnitude > 0.1f)  // Only align if we have significant velocity
diff --git a/Assets/Scripts/Interceptors/Hydra70.cs b/Assets/Scripts/Interceptors/Hydra70.cs
index 5107c74..55ef014 100644
--- a/Assets/Scripts/Interceptors/Hydra70.cs
+++ b/Assets/Scripts/Interceptors/Hydra70.cs
@@ -4,7 +4,6 @@ using JetBrains.Annotations;
 using UnityEngine;
 
 public class Hydra70 : Missile {
-  private Vector3 _acceleration;
   private bool _submunitionsLaunched = false;
 
   protected override void FixedUpdate() {
@@ -25,7 +24,6 @@ public class Hydra70 : Missile {
     // Calculate and set the total acceleration
     Vector3 acceleration = CalculateAcceleration(accelerationInput);
     GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
-    _acceleration = acceleration;
   }
 
   protected override void DrawDebugVectors() {
diff --git a/Assets/Scripts/Missile.cs b/Assets/Scripts/Missile.cs
index e1c4221..2579bfb 100644
--- a/Assets/Scripts/Missile.cs
+++ b/Assets/Scripts/Missile.cs
@@ -6,8 +6,6 @@ public class Missile : Agent {
   [SerializeField]
   protected bool _showDebugVectors = true;
 
-  [SerializeField]
-  private Vector3 _boostAcceleration;
   // Return whether a target can be assigned to the missile.
   public override bool IsAssignable() {
     bool assignable = !HasLaunched() && !HasAssignedTarget();
@@ -22,7 +20,7 @@ public class Missile : Agent {
   // Unassign the target from the missile.
   public override void UnassignTarget() {
     base.UnassignTarget();
-  }  
+  }
 
   protected override void UpdateReady(double deltaTime) {
     Vector3 accelerationInput = Vector3.zero;
@@ -51,8 +49,8 @@ public class Missile : Agent {
 
     // Apply the acceleration force
     GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
-    _boostAcceleration = acceleration;
   }
+
   protected override void UpdateMidCourse(double deltaTime) {}
 
   protected Vector3 CalculateAcceleration(Vector3 accelerationInput,
@@ -69,6 +67,7 @@ public class Missile : Agent {
 
     // Project the drag acceleration onto the forward direction
     Vector3 dragAccelerationAlongRoll = dragAcceleration * transform.forward;
+    _dragAcceleration = dragAccelerationAlongRoll;
 
     return accelerationInput + gravity + dragAccelerationAlongRoll;
   }
diff --git a/Assets/Scripts/SimManager.cs b/Assets/Scripts/SimManager.cs
index 9851126..e8ca321 100644
--- a/Assets/Scripts/SimManager.cs
+++ b/Assets/Scripts/SimManager.cs
@@ -51,7 +51,9 @@ public class SimManager : MonoBehaviour {
   }
 
   public List<Agent> GetActiveAgents() {
-    return _activeMissiles.ConvertAll(missile => missile as Agent).Concat(_activeTargets.ConvertAll(target => target as Agent)).ToList();
+    return _activeMissiles.ConvertAll(missile => missile as Agent)
+        .Concat(_activeTargets.ConvertAll(target => target as Agent))
+        .ToList();
   }
 
   void Awake() {
@@ -123,7 +125,7 @@ public class SimManager : MonoBehaviour {
     _assignmentScheme = new ThreatAssignment();
     OnSimulationStarted?.Invoke();
   }
-  
+
   public void AssignMissilesToTargets() {
     AssignMissilesToTargets(_missiles);
   }
@@ -266,21 +268,16 @@ public class SimManager : MonoBehaviour {
     return agentObject;
   }
 
-  public void LoadNewConfig(string configFileName)
-  {
-      simulationConfig = ConfigLoader.LoadSimulationConfig(configFileName);
-      if (simulationConfig != null)
-      {
-          Debug.Log($"Loaded new configuration: {configFileName}");
-          RestartSimulation();
-      }
-      else
-      {
-          Debug.LogError($"Failed to load configuration: {configFileName}");
+  public void LoadNewConfig(string configFileName) {
+    simulationConfig = ConfigLoader.LoadSimulationConfig(configFileName);
+    if (simulationConfig != null) {
+      Debug.Log($"Loaded new configuration: {configFileName}");
+      RestartSimulation();
+    } else {
+      Debug.LogError($"Failed to load configuration: {configFileName}");
     }
   }
 
-
   public void RestartSimulation() {
     OnSimulationEnded?.Invoke();
     Debug.Log("Simulation ended");