start on new threat types & strategy abstraction

Assets/Scripts/Threats/DroneThreat.cs

@@ -2,7 +2,7 @@ using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
 
-public class DroneTarget : Threat {
+public class DroneThreat : Threat {
   // Start is called before the first frame update
   protected override void Start() {
     base.Start();

Assets/Scripts/Threats/FixedWingMissileThreat.cs

@@ -0,0 +1,41 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class FixedWingMissileThreat : Threat {
+  protected override void UpdateBoost(double deltaTime) {
+    throw new System.NotImplementedException();
+  }
+
+  protected override void UpdateMidCourse(double deltaTime) {
+    throw new System.NotImplementedException();
+  }
+
+  /// <summary>
+  /// Strategy for moving the Threat in a straight path towards its target.
+  /// </summary>
+  public class DirectPathStrategy : NavigationStrategy {
+    public override void Execute(Threat threat, List<Threat> swarmMates, FlightPhase flightPhase,
+                                 List<Interceptor> interceptors, double deltaTime) {
+      throw new System.NotImplementedException();
+    }
+  }
+
+  /// <summary>
+  /// Strategy for moving the Threat in an S-curve towards a predefined target.
+  /// </summary>
+  public class SlalomStrategy : NavigationStrategy {
+    private float maxAmplitude;
+    private float periodDistance;
+
+    public SlalomStrategy(float maxAmplitude, float periodDistance) {
+      this.maxAmplitude = maxAmplitude;
+      this.periodDistance = periodDistance;
+    }
+
+    public override void Execute(Threat threat, List<Threat> swarmMates, FlightPhase flightPhase,
+                                 List<Interceptor> interceptors, double deltaTime) {
+      throw new System.NotImplementedException();
+    }
+  }
+}

Assets/Scripts/Threats/FixedWingMissileThreat.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 4e6c2fbd1e492be448760f5045b13b2e

Assets/Scripts/Threats/MissileThreat.cs

@@ -1,107 +0,0 @@
-using System.Collections;
-using System.Collections.Generic;
-using UnityEngine;
-
-/// <summary>
-/// Base class for missile targets. Uses same set of flight phases as base Hydra-70.
-/// </summary>
-public class MissileThreat : Threat
-{
-  protected float boostAcceleration = 20;
-  protected float midcourseAcceleration = 5;
-
-  protected override void UpdateReady(double deltaTime)
-  {
-    // if in ready phase, just set to boost phase immediately
-    SetFlightPhase(FlightPhase.BOOST);
-  }
-
-  protected override void UpdateBoost(double deltaTime)
-  {
-    // The interceptor only accelerates along its roll axis (forward in Unity)
-    Vector3 rollAxis = transform.forward;
-
-    // Calculate boost acceleration
-    float boostAcceleration =
-        (float)(_staticConfig.boostConfig.boostAcceleration * Constants.kGravity);
-    Vector3 accelerationInput = boostAcceleration * rollAxis;
-
-    // Calculate the total acceleration
-    Vector3 acceleration = CalculateAcceleration(accelerationInput);
-
-    // Apply the acceleration force
-    GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
-  }
-
-  protected override void UpdateMidCourse(double deltaTime)
-  {
-    Vector3 accelerationInput = Vector3.zero;
-    // Calculate and set the total acceleration
-    Vector3 acceleration = CalculateAcceleration(accelerationInput);
-    GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
-  }
-
-  protected Vector3 CalculateAcceleration(Vector3 accelerationInput,
-                                          bool compensateForGravity = false)
-  {
-    Vector3 gravity = Physics.gravity;
-    if (compensateForGravity)
-    {
-      Vector3 gravityProjection = CalculateGravityProjectionOnPitchAndYaw();
-      accelerationInput -= gravityProjection;
-    }
-
-    float airDrag = CalculateDrag();
-    float liftInducedDrag = CalculateLiftInducedDrag(accelerationInput);
-    float dragAcceleration = -(airDrag + liftInducedDrag);
-
-    // Project the drag acceleration onto the forward direction
-    Vector3 dragAccelerationAlongRoll = dragAcceleration * transform.forward;
-    _dragAcceleration = dragAccelerationAlongRoll;
-
-    return accelerationInput + gravity + dragAccelerationAlongRoll;
-  }
-
-  protected float CalculateMaxAcceleration()
-  {
-    float maxReferenceAcceleration =
-        (float)(_staticConfig.accelerationConfig.maxReferenceAcceleration * Constants.kGravity);
-    float referenceSpeed = _staticConfig.accelerationConfig.referenceSpeed;
-    return Mathf.Pow(GetComponent<Rigidbody>().linearVelocity.magnitude / referenceSpeed, 2) *
-           maxReferenceAcceleration;
-  }
-
-  protected Vector3 CalculateGravityProjectionOnPitchAndYaw()
-  {
-    Vector3 gravity = Physics.gravity;
-    Vector3 pitchAxis = transform.right;
-    Vector3 yawAxis = transform.up;
-
-    // Project the gravity onto the pitch and yaw axes
-    float gravityProjectionPitchCoefficient = Vector3.Dot(gravity, pitchAxis);
-    float gravityProjectionYawCoefficient = Vector3.Dot(gravity, yawAxis);
-
-    // Return the sum of the projections
-    return gravityProjectionPitchCoefficient * pitchAxis +
-           gravityProjectionYawCoefficient * yawAxis;
-  }
-
-  private float CalculateDrag()
-  {
-    float dragCoefficient = _staticConfig.liftDragConfig.dragCoefficient;
-    float crossSectionalArea = _staticConfig.bodyConfig.crossSectionalArea;
-    float mass = _staticConfig.bodyConfig.mass;
-    float dynamicPressure = (float)GetDynamicPressure();
-    float dragForce = dragCoefficient * dynamicPressure * crossSectionalArea;
-    return dragForce / mass;
-  }
-
-  private float CalculateLiftInducedDrag(Vector3 accelerationInput)
-  {
-    float liftAcceleration =
-        (accelerationInput - Vector3.Dot(accelerationInput, transform.up) * transform.up).magnitude;
-    float liftDragRatio = _staticConfig.liftDragConfig.liftDragRatio;
-    return Mathf.Abs(liftAcceleration / liftDragRatio);
-  }
-
-}

Assets/Scripts/Threats/RollStabilizedMissileThreat.cs

@@ -0,0 +1,204 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+/// <summary>
+/// Base class for missile targets. Uses same set of flight phases as base Hydra-70.
+/// </summary>
+public class RollStabilizedMissileThreat : Threat {
+  protected float boostAcceleration = 20;
+  protected float midcourseAcceleration = 0;
+  protected float terminalAcceleration = 22;
+  protected float maxAmplitude = 22;
+
+  public RollStabilizedMissileThreat() {
+    strategy = new DirectPathStrategy();
+  }
+
+  protected override void UpdateBoost(double deltaTime) {
+    // The interceptor only accelerates along its roll axis (forward in Unity)
+    Vector3 rollAxis = transform.forward;
+
+    // Calculate boost acceleration
+    float boostAcceleration =
+        (float)(_staticConfig.boostConfig.boostAcceleration * Constants.kGravity);
+    Vector3 accelerationInput = boostAcceleration * rollAxis;
+
+    // Calculate the total acceleration
+    Vector3 acceleration = CalculateAcceleration(accelerationInput);
+
+    // Apply the acceleration force
+    GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
+  }
+
+  protected override void UpdateMidCourse(double deltaTime) {
+    Vector3 accelerationInput = Vector3.zero;
+    // Calculate and set the total acceleration
+    Vector3 acceleration = CalculateAcceleration(accelerationInput);
+    GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
+  }
+
+  protected Vector3 CalculateAcceleration(Vector3 accelerationInput,
+                                          bool compensateForGravity = false) {
+    Vector3 gravity = Physics.gravity;
+    if (compensateForGravity) {
+      Vector3 gravityProjection = CalculateGravityProjectionOnPitchAndYaw();
+      accelerationInput -= gravityProjection;
+    }
+
+    float airDrag = CalculateDrag();
+    float liftInducedDrag = CalculateLiftInducedDrag(accelerationInput);
+    float dragAcceleration = -(airDrag + liftInducedDrag);
+
+    // Project the drag acceleration onto the forward direction
+    Vector3 dragAccelerationAlongRoll = dragAcceleration * transform.forward;
+    _dragAcceleration = dragAccelerationAlongRoll;
+
+    return accelerationInput + gravity + dragAccelerationAlongRoll;
+  }
+
+  protected float CalculateMaxAcceleration() {
+    float maxReferenceAcceleration =
+        (float)(_staticConfig.accelerationConfig.maxReferenceAcceleration * Constants.kGravity);
+    float referenceSpeed = _staticConfig.accelerationConfig.referenceSpeed;
+    return Mathf.Pow(GetComponent<Rigidbody>().linearVelocity.magnitude / referenceSpeed, 2) *
+           maxReferenceAcceleration;
+  }
+
+  protected Vector3 CalculateGravityProjectionOnPitchAndYaw() {
+    Vector3 gravity = Physics.gravity;
+    Vector3 pitchAxis = transform.right;
+    Vector3 yawAxis = transform.up;
+
+    // Project the gravity onto the pitch and yaw axes
+    float gravityProjectionPitchCoefficient = Vector3.Dot(gravity, pitchAxis);
+    float gravityProjectionYawCoefficient = Vector3.Dot(gravity, yawAxis);
+
+    // Return the sum of the projections
+    return gravityProjectionPitchCoefficient * pitchAxis +
+           gravityProjectionYawCoefficient * yawAxis;
+  }
+
+  private float CalculateDrag() {
+    float dragCoefficient = _staticConfig.liftDragConfig.dragCoefficient;
+    float crossSectionalArea = _staticConfig.bodyConfig.crossSectionalArea;
+    float mass = _staticConfig.bodyConfig.mass;
+    float dynamicPressure = (float)GetDynamicPressure();
+    float dragForce = dragCoefficient * dynamicPressure * crossSectionalArea;
+    return dragForce / mass;
+  }
+
+  private float CalculateLiftInducedDrag(Vector3 accelerationInput) {
+    float liftAcceleration =
+        (accelerationInput - Vector3.Dot(accelerationInput, transform.up) * transform.up).magnitude;
+    float liftDragRatio = _staticConfig.liftDragConfig.liftDragRatio;
+    return Mathf.Abs(liftAcceleration / liftDragRatio);
+  }
+
+  // ===========================================================
+  // STRATEGIES
+  // ===========================================================
+
+  /// <summary>
+  /// Strategy for moving the Threat in a straight path towards its target.
+  /// </summary>
+  public class DirectPathStrategy : NavigationStrategy {
+    DefendPoint target = new DefendPoint();
+
+    private float _navigationGain = 3f;  // Typically 3-5
+    private SensorOutput _sensorOutput;
+    private Vector3 _accelerationCommand;
+    private double _elapsedTime = 0;
+
+    public override void Execute(Threat threat, List<Threat> swarmMates, FlightPhase flightPhase,
+                                 List<Interceptor> interceptors, double deltaTime) {
+      RollStabilizedMissileThreat missileThreat = threat as RollStabilizedMissileThreat;
+      if (missileThreat == null) {
+        Debug.LogError("DirectPathStrategy can only be used with RollStabilizedMissileThreat");
+        return;
+      }
+
+      _elapsedTime += deltaTime;
+      Vector3 accelerationInput = Vector3.zero;
+
+      if (target != null) {
+        // Correct the state of the threat model at the sensor frequency
+        float sensorUpdatePeriod =
+            1f / missileThreat._agentConfig.dynamic_config.sensor_config.frequency;
+        if (_elapsedTime >= sensorUpdatePeriod) {
+          _sensorOutput = new SensorOutput();
+          missileThreat.GetComponent<Sensor>().Sense(target);
+          Debug.Log(_sensorOutput.velocity.range);
+          _elapsedTime = 0;
+        }
+
+        // Check whether the threat should be considered a miss
+        SensorOutput sensorOutput = missileThreat.GetComponent<Sensor>().Sense(target);
+        if (sensorOutput.velocity.range > 1000f) {
+          missileThreat.MarkAsMiss();
+        }
+
+        // Calculate the acceleration input
+        accelerationInput = CalculateAccelerationCommand(missileThreat, _sensorOutput);
+      } else {
+        Debug.LogError("DirectPathStrategy requires a target to be set");
+      }
+
+      // Calculate and set the total acceleration
+      Vector3 acceleration =
+          missileThreat.CalculateAcceleration(accelerationInput, compensateForGravity: true);
+      missileThreat.GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
+    }
+
+    private Vector3 CalculateAccelerationCommand(Threat threat, SensorOutput sensorOutput) {
+      RollStabilizedMissileThreat missileThreat = threat as RollStabilizedMissileThreat;
+
+      // Implement Proportional Navigation guidance law
+      Vector3 accelerationCommand;
+
+      // Extract relevant information from sensor output
+      float los_rate_az = sensorOutput.velocity.azimuth;
+      float los_rate_el = sensorOutput.velocity.elevation;
+      float closing_velocity =
+          -sensorOutput.velocity
+               .range;  // Negative because closing velocity is opposite to range rate
+
+      // Navigation gain (adjust as needed)
+      float N = _navigationGain;
+
+      // Calculate acceleration commands in azimuth and elevation planes
+      float acc_az = N * closing_velocity * los_rate_az;
+      float acc_el = N * closing_velocity * los_rate_el;
+
+      // Convert acceleration commands to craft body frame
+      accelerationCommand =
+          missileThreat.transform.right * acc_az + missileThreat.transform.up * acc_el;
+
+      // Clamp the acceleration command to the maximum acceleration
+      float maxAcceleration = missileThreat.CalculateMaxAcceleration();
+      accelerationCommand = Vector3.ClampMagnitude(accelerationCommand, maxAcceleration);
+
+      // Update the stored acceleration command for debugging
+      _accelerationCommand = accelerationCommand;
+      return accelerationCommand;
+    }
+  }
+
+  /// <summary>
+  /// Strategy for moving the Threat in a spiral towards a predefined target.
+  /// </summary>
+  public class SpiralStrategy : NavigationStrategy {
+    private float spiralRadius;
+    private float periodDistance;
+
+    public SpiralStrategy(float spiralRadius, float periodDistance) {
+      this.spiralRadius = spiralRadius;
+      this.periodDistance = periodDistance;
+    }
+
+    public override void Execute(Threat threat, List<Threat> swarmMates, FlightPhase flightPhase,
+                                 List<Interceptor> interceptors, double deltaTime) {
+      throw new System.NotImplementedException();
+    }
+  }
+}

Assets/Scripts/Threats/Strategies.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 0d798867791c1444b985d8807b144572
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Scripts/Threats/Threat.cs

@@ -3,6 +3,29 @@ using System.Collections.Generic;
 using UnityEngine;
 
 public abstract class Threat : Agent {
+  /// <summary>
+  /// Strategy for moving the Threat towards a predefined target.
+  /// </summary>
+  public abstract class NavigationStrategy {
+    /// <summary>
+    /// Execute one timestep of the strategy for the given threat and flight phase. Should only
+    /// apply normal forces to the threat. Can also change the threat's flight phase. Should NOT
+    /// manage thrust.
+    /// </summary>
+    /// <param name="threat">Parent Threat object</param>
+    /// <param name="swarmMates">List of other threats in the swarm</param>
+    /// <param name="flightPhase">Current flight phase</param>
+    /// <param name="interceptors">List of active interceptors</param>
+    /// <param name="deltaTime">Timestep</param>
+    public abstract void Execute(Threat threat, List<Threat> swarmMates, FlightPhase flightPhase,
+                                 List<Interceptor> interceptors, double deltaTime);
+  }
+
+  /// <summary>
+  /// Navigation strategy that this threat will use to move towards its target.
+  /// </summary>
+  public NavigationStrategy strategy;
+
   public override bool IsAssignable() {
     return false;
   }
@@ -13,5 +36,18 @@ public abstract class Threat : Agent {
 
   protected override void FixedUpdate() {
     base.FixedUpdate();
+    // NOTE: no swarm-mates for now
+    strategy.Execute(this, new List<Threat>(), GetFlightPhase(),
+                     SimManager.Instance.GetActiveInterceptors(), Time.fixedDeltaTime);
+  }
+
+  /// <summary>
+  /// OVERRIDE: THIS SHOULD NEVER BE CALLED; threats always start in midcourse or (at the very
+  /// earliest) boost phase.
+  /// </summary>
+  /// <param name="deltaTime"></param>
+  /// <exception cref="System.NotImplementedException"></exception>
+  protected override void UpdateReady(double deltaTime) {
+    throw new System.NotImplementedException();
   }
 }