Merge branch 'master' of https://github.com/PisterLab/micromissiles-u…

…nity

Assets/Scripts/Agent.cs

@@ -274,17 +274,6 @@ protected float CalculateMaxNormalAcceleration() {
     return Mathf.Pow((float)GetSpeed() / referenceSpeed, 2) * maxReferenceNormalAcceleration;
   }
 
-  protected Vector3 CalculateGravityProjectionOnPitchAndYaw() {
-    Vector3 gravity = Physics.gravity;
-
-    // Project the gravity onto the pitch and yaw axes
-    Vector3 gravityProjectedOnPitch = Vector3.Project(gravity, transform.right);
-    Vector3 gravityProjectedOnYaw = Vector3.Project(gravity, transform.up);
-
-    // Return the sum of the projections
-    return gravityProjectedOnPitch + gravityProjectedOnYaw;
-  }
-
   private float CalculateDrag() {
     float dragCoefficient = _staticAgentConfig.liftDragConfig.dragCoefficient;
     float crossSectionalArea = _staticAgentConfig.bodyConfig.crossSectionalArea;

Assets/Scripts/Interceptor.cs

@@ -54,13 +54,14 @@ protected override void UpdateBoost(double deltaTime) {
     }
     UpdateMissileTrailEffect();
 
-    // The interceptor only accelerates along its roll axis (forward in Unity)
-    Vector3 rollAxis = transform.forward;
-
     // Calculate boost acceleration
     float boostAcceleration =
         (float)(_staticAgentConfig.boostConfig.boostAcceleration * Constants.kGravity);
-    Vector3 accelerationInput = boostAcceleration * rollAxis;
+    Vector3 boostAccelerationVector = boostAcceleration * transform.forward;
+
+    // Add PN acceleration to boost acceleration
+    Vector3 pnAcceleration = CalculateProportionalNavigationAcceleration(deltaTime);
+    Vector3 accelerationInput = boostAccelerationVector + pnAcceleration;
 
     // Calculate the total acceleration
     Vector3 acceleration = CalculateAcceleration(accelerationInput);
@@ -73,34 +74,43 @@ protected override void UpdateMidCourse(double deltaTime) {
     UpdateMissileTrailEffect();
 
     _elapsedTime += deltaTime;
-    Vector3 accelerationInput = Vector3.zero;
-    if (HasAssignedTarget()) {
-      // Correct the state of the threat model at the sensor frequency
-      float sensorUpdatePeriod = 1f / _dynamicAgentConfig.dynamic_config.sensor_config.frequency;
-      if (_elapsedTime >= sensorUpdatePeriod) {
-        // TODO: Implement guidance filter to estimate state from sensor output
-        // For now, we'll use the threat's actual state
-        _targetModel.SetPosition(_target.GetPosition());
-        _targetModel.SetVelocity(_target.GetVelocity());
-        _elapsedTime = 0;
-      }
-
-      // Check whether the threat should be considered a miss
-      SensorOutput sensorOutput = GetComponent<Sensor>().Sense(_target);
-      if (sensorOutput.velocity.range > 1000f) {
-        this.HandleInterceptMiss();
-      }
-
-      // Calculate the acceleration input
-      _sensorOutput = GetComponent<Sensor>().Sense(_targetModel);
-      accelerationInput = CalculateAccelerationCommand(_sensorOutput);
-    }
+    Vector3 accelerationInput = CalculateProportionalNavigationAcceleration(deltaTime);
 
     // Calculate and set the total acceleration
     Vector3 acceleration = CalculateAcceleration(accelerationInput);
     GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
   }
 
+  private Vector3 CalculateProportionalNavigationAcceleration(double deltaTime) {
+    if (!HasAssignedTarget()) {
+      return Vector3.zero;
+    }
+
+    UpdateTargetModel(deltaTime);
+
+    // Check whether the threat should be considered a miss
+    SensorOutput sensorOutput = GetComponent<Sensor>().Sense(_target);
+    if (sensorOutput.velocity.range > 1000f) {
+      this.HandleInterceptMiss();
+      return Vector3.zero;
+    }
+
+    _sensorOutput = GetComponent<Sensor>().Sense(_targetModel);
+    return CalculateAccelerationCommand(_sensorOutput);
+  }
+
+  private void UpdateTargetModel(double deltaTime) {
+    _elapsedTime += deltaTime;
+    float sensorUpdatePeriod = 1f / _dynamicAgentConfig.dynamic_config.sensor_config.frequency;
+    if (_elapsedTime >= sensorUpdatePeriod) {
+      // TODO: Implement guidance filter to estimate state from sensor output
+      // For now, we'll use the threat's actual state
+      _targetModel.SetPosition(_target.GetPosition());
+      _targetModel.SetVelocity(_target.GetVelocity());
+      _elapsedTime = 0;
+    }
+  }
+
   private Vector3 CalculateAccelerationCommand(SensorOutput sensorOutput) {
     // Implement Proportional Navigation guidance law
     Vector3 accelerationCommand = Vector3.zero;
@@ -186,10 +196,17 @@ private void AttachMissileTrailEffect() {
         _missileTrailEffect.transform.parent = transform;
         _missileTrailEffect.transform.localPosition = Vector3.zero;
         _missileTrailEffectAttached = true;
+        ParticleSystem particleSystem = _missileTrailEffect.GetComponent<ParticleSystem>();
+        float duration = particleSystem.main.duration;
+
+        // Extend the duration of the missile trail effect to be the same as the boost time
+        if (duration < _staticAgentConfig.boostConfig.boostTime) {
+          ParticleSystem.MainModule mainModule = particleSystem.main;
+          mainModule.duration = _staticAgentConfig.boostConfig.boostTime;
+        }
 
-        float duration = _missileTrailEffect.GetComponent<ParticleSystem>().main.duration;
         _returnParticleToManagerCoroutine = StartCoroutine(ReturnParticleToManager(duration * 2f));
-        _missileTrailEffect.GetComponent<ParticleSystem>().Play();
+        particleSystem.Play();
       }
     }
   }

Assets/StreamingAssets/Configs/1_salvo_4_sfrj_10_brahmos.json

@@ -0,0 +1,82 @@
+{
+  "timeScale": 2,
+  "interceptor_swarm_configs": [
+    {
+      "num_agents": 4,
+      "dynamic_agent_config": {
+        "agent_model": "sfrj.json",
+        "initial_state": {
+          "position": { "x": 0, "y": 20, "z": 0 },
+          "rotation": { "x": -85, "y": 0, "z": 0 },
+          "velocity": { "x": 0, "y": 50, "z": 10 }
+        },
+        "standard_deviation": {
+          "position": { "x": 5, "y": 0, "z": 5 },
+          "velocity": { "x": 5, "y": 0, "z": 1 }
+        },
+        "dynamic_config": {
+          "launch_config": { "launch_time": 0 },
+          "sensor_config": {
+            "type": "IDEAL",
+            "frequency": 100
+          }
+        },
+        "submunitions_config": {
+          "num_submunitions": 7,
+          "launch_config": { "launch_time": 45 },
+          "dynamic_agent_config": {
+            "agent_model": "micromissile.json",
+            "initial_state": {
+              "position": { "x": 0, "y": 0, "z": 0 },
+              "rotation": { "x": 0, "y": 0, "z": 0 },
+              "velocity": { "x": 0, "y": 0, "z": 0 }
+            },
+            "standard_deviation": {
+              "position": { "x": 5, "y": 5, "z": 5 },
+              "velocity": { "x": 0, "y": 0, "z": 0 }
+            },
+            "dynamic_config": {
+              "launch_config": { "launch_time": 0 },
+              "sensor_config": {
+                "type": "IDEAL",
+                "frequency": 100
+              }
+            }
+          }
+        }
+      }
+    }
+  ],
+  "threat_swarm_configs": [
+    {
+      "num_agents": 10,
+      "dynamic_agent_config": {
+        "agent_model": "brahmos.json",
+        "attack_behavior": "brahmos_direct_attack.json",
+        "initial_state": {
+          "position": { "x": 0, "y": 5000, "z": 100000 },
+          "rotation": { "x": 90, "y": 0, "z": 0 },
+          "velocity": { "x": 0, "y": 0, "z": -800 }
+        },
+        "standard_deviation": {
+          "position": { "x": 1000, "y": 200, "z": 100 },
+          "velocity": { "x": 0, "y": 0, "z": 25 }
+        },
+        "dynamic_config": {
+          "launch_config": { "launch_time": 0 },
+          "sensor_config": {
+            "type": "IDEAL",
+            "frequency": 100
+          },
+          "flight_config": {
+            "evasionEnabled": true,
+            "evasionRangeThreshold": 1000
+          }
+        },
+        "submunitions_config": {
+          "num_submunitions": 0
+        }
+      }
+    }
+  ]
+}

Assets/StreamingAssets/Configs/Behaviors/Attack/brahmos_direct_attack.json

@@ -0,0 +1,44 @@
+{
+    "name": "DefaultDirectAttack",
+    "attackBehaviorType": "DIRECT_ATTACK",
+    "targetPosition": {
+        "x": 0.01,
+        "y": 0.0,
+        "z": 0.0
+    },
+    "targetVelocity": {
+        "x": 0.0001,
+        "y": 0.0,
+        "z": 0.0
+    },
+    "targetColliderSize": {
+        "x": 20.0,
+        "y": 20.0,
+        "z": 20.0
+    },
+    "flightPlan": {
+        "type": "DistanceToTarget",
+        "waypoints": [
+            {
+                "distance": 100000.0,
+                "altitude": 10000.0,
+                "power": "MIL"
+            },
+            {
+                "distance": 10000.0,
+                "altitude": 10000.0,
+                "power": "MIL"
+            },
+            {
+                "distance": 4000.0,
+                "altitude": 2000.0,
+                "power": "MIL"
+            },
+            {
+                "distance": 2000.0,
+                "altitude": 0.0,
+                "power": "MAX"
+            }
+        ]
+    }
+}

Assets/StreamingAssets/Configs/Models/Threats/brahmos.json

@@ -0,0 +1,34 @@
+{
+    "name": "BrahmosCruiseMissile",
+    "agentClass": "FixedWingThreat",
+    "accelerationConfig": {
+        "maxReferenceNormalAcceleration": 70,
+        "referenceSpeed": 960.4,
+        "maxForwardAcceleration": 30
+    },
+    "boostConfig": {
+        "boostTime": 0
+    },
+    "liftDragConfig": {
+        "liftCoefficient": 0.6,
+        "dragCoefficient": 0.3,
+        "liftDragRatio": 2
+    },
+    "bodyConfig": {
+        "mass": 3000,
+        "crossSectionalArea": 0.352,
+        "finArea": 1.2,
+        "bodyArea": 17.7
+    },
+    "hitConfig": {
+        "hitRadius": 4,
+        "killProbability": 0.9
+    },
+    "powerTable": {
+        "IDLE": 0,
+        "LOW": 480,
+        "CRUISE": 960.4,
+        "MIL": 990,
+        "MAX": 1029
+    }
+}

Assets/Tests/EditMode/BehaviorTests.cs

@@ -0,0 +1,75 @@
+using NUnit.Framework;
+using UnityEngine;
+using System.Collections.Generic;
+
+public class BehaviorTests : TestBase {
+  /*
+  [Test]
+  public void TestDirectAttackBehaviorWaypoints()
+  {
+      // Create a sample DirectAttackBehavior
+      DirectAttackBehavior behavior = new DirectAttackBehavior();
+      behavior.flightPlan = new DTTFlightPlan
+      {
+          waypoints = new List<DTTWaypoint>
+          {
+              new DTTWaypoint { distance = 1000, altitude = 100, power = PowerSetting.CRUISE },
+              new DTTWaypoint { distance = 500, altitude = 50, power = PowerSetting.MIL },
+              new DTTWaypoint { distance = 100, altitude = 25, power = PowerSetting.MAX }
+          }
+      };
+
+      // Waypoints are already sorted in descending order in the production code
+
+      Vector3 targetPosition = new Vector3(1000, 0, 0);
+      const float epsilon = 0.001f;
+
+      // Test waypoint selection based on distance
+      Vector3 currentPosition = new Vector3(0, 0, 0);
+      var result = behavior.GetNextWaypoint(currentPosition, targetPosition);
+      Assert.AreEqual(0, result.waypointPosition.x, epsilon);
+      Assert.AreEqual(100, result.waypointPosition.y, epsilon);
+      Assert.AreEqual(0, result.waypointPosition.z, epsilon);
+      Assert.AreEqual(PowerSetting.CRUISE, result.power);
+
+      currentPosition = new Vector3(600, 0, 0);
+      result = behavior.GetNextWaypoint(currentPosition, targetPosition);
+      Assert.AreEqual(500, result.waypointPosition.x, epsilon);
+      Assert.AreEqual(50, result.waypointPosition.y, epsilon);
+      Assert.AreEqual(0, result.waypointPosition.z, epsilon);
+      Assert.AreEqual(PowerSetting.MIL, result.power);
+
+      currentPosition = new Vector3(920, 0, 0);
+      result = behavior.GetNextWaypoint(currentPosition, targetPosition);
+      Assert.AreEqual(900, result.waypointPosition.x, epsilon);
+      Assert.AreEqual(25, result.waypointPosition.y, epsilon);
+      Assert.AreEqual(0, result.waypointPosition.z, epsilon);
+      Assert.AreEqual(PowerSetting.MAX, result.power);
+
+      // Test behavior within final distance
+      currentPosition = new Vector3(950, 0, 0);
+      result = behavior.GetNextWaypoint(currentPosition, targetPosition);
+      Assert.AreEqual(targetPosition.x, result.waypointPosition.x, epsilon);
+      Assert.AreEqual(targetPosition.y, result.waypointPosition.y, epsilon);
+      Assert.AreEqual(targetPosition.z, result.waypointPosition.z, epsilon);
+      Assert.AreEqual(PowerSetting.MAX, result.power); // Should use the power of the closest
+  waypoint
+
+      // Test with non-zero Z coordinate
+      targetPosition = new Vector3(800, 0, 600);
+      currentPosition = new Vector3(0, 0, 0);
+      result = behavior.GetNextWaypoint(currentPosition, targetPosition);
+      Assert.AreEqual(0, result.waypointPosition.x, epsilon);
+      Assert.AreEqual(100, result.waypointPosition.y, epsilon);
+      Assert.AreEqual(0, result.waypointPosition.z, epsilon);
+      Assert.AreEqual(PowerSetting.CRUISE, result.power);
+
+      currentPosition = new Vector3(400, 0, 300);
+      result = behavior.GetNextWaypoint(currentPosition, targetPosition);
+      Assert.AreEqual(400, result.waypointPosition.x, epsilon);
+      Assert.AreEqual(50, result.waypointPosition.y, epsilon);
+      Assert.AreEqual(300, result.waypointPosition.z, epsilon);
+      Assert.AreEqual(PowerSetting.MIL, result.power);
+  }
+  */
+}