diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index 1b3fb88d8..eed3a8be9 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -9,10 +9,15 @@ public class IADS : MonoBehaviour { public static IADS Instance { get; private set; } + private const float LaunchInterceptorsPeriod = 0.4f; + private const float CheckForEscapingThreatsPeriod = 5.0f; + private const float ClusterThreatsPeriod = 2.0f; + // TODO(titan): Choose the CSV file based on the interceptor type. private ILaunchAnglePlanner _launchAnglePlanner = new LaunchAngleCsvInterpolator(Path.Combine("Planning", "hydra70_launch_angle.csv")); private IAssignment _assignmentScheme = new MaxSpeedAssignment(); + private Coroutine _launchInterceptorsCoroutine; [SerializeField] private List _trackFiles = new List(); @@ -26,6 +31,10 @@ public class IADS : MonoBehaviour { new Dictionary(); private HashSet _assignableInterceptors = new HashSet(); + private HashSet _threatsToCluster = new HashSet(); + private Coroutine _checkForEscapingThreatsCoroutine; + private Coroutine _clusterThreatsCoroutine; + private int _trackFileIdTicker = 0; private void Awake() { @@ -36,6 +45,18 @@ private void Awake() { } } + private void OnDestroy() { + if (_launchInterceptorsCoroutine != null) { + StopCoroutine(_launchInterceptorsCoroutine); + } + if (_checkForEscapingThreatsCoroutine != null) { + StopCoroutine(_checkForEscapingThreatsCoroutine); + } + if (_clusterThreatsCoroutine != null) { + StopCoroutine(_clusterThreatsCoroutine); + } + } + public void Start() { SimManager.Instance.OnSimulationStarted += RegisterSimulationStarted; SimManager.Instance.OnSimulationEnded += RegisterSimulationEnded; @@ -46,6 +67,7 @@ public void Start() { public void LateUpdate() { // Update the cluster centroids. foreach (var cluster in _threatClusters) { + cluster.Recenter(); _threatClusterMap[cluster].UpdateCentroid(); } @@ -55,39 +77,40 @@ public void LateUpdate() { } private void RegisterSimulationStarted() { - // Cluster the threats. - ClusterThreats(SimManager.Instance.GetActiveThreats()); + _launchInterceptorsCoroutine = + StartCoroutine(LaunchInterceptorsManager(LaunchInterceptorsPeriod)); + _checkForEscapingThreatsCoroutine = + StartCoroutine(CheckForEscapingThreatsManager(CheckForEscapingThreatsPeriod)); + _clusterThreatsCoroutine = StartCoroutine(ClusterThreatsManager(ClusterThreatsPeriod)); } - // Cluster the threats. - public void ClusterThreats(List threats) { - // Maximum number of threats per cluster. - const int MaxSize = 7; - // Maximum cluster radius in meters. - const float MaxRadius = 500; - - // Cluster to threats. - IClusterer clusterer = new AgglomerativeClusterer(new List(threats), MaxSize, MaxRadius); - clusterer.Cluster(); - var clusters = clusterer.Clusters; - Debug.Log($"[IADS] Clustered {threats.Count} threats into {clusters.Count} clusters."); - UIManager.Instance.LogActionMessage( - $"[IADS] Clustered {threats.Count} threats into {clusters.Count} clusters."); - - _threatClusters = clusters.ToList(); - foreach (var cluster in clusters) { - _threatClusterMap.Add(cluster, new ThreatClusterData(cluster)); + private IEnumerator LaunchInterceptorsManager(float period) { + while (true) { + // Check whether an interceptor should be launched at a cluster and launch it. + CheckAndLaunchInterceptors(); + yield return new WaitForSeconds(period); } } - // Check whether an interceptor should be launched at a cluster and launch it. - public void CheckAndLaunchInterceptors() { + private void CheckAndLaunchInterceptors() { foreach (var cluster in _threatClusters) { // Check whether an interceptor has already been assigned to the cluster. if (_threatClusterMap[cluster].Status != ThreatClusterStatus.UNASSIGNED) { continue; } + // Check whether all threats in the cluster have terminated. + bool allTerminated = true; + foreach (var threat in cluster.Threats) { + if (!threat.IsTerminated()) { + allTerminated = false; + break; + } + } + if (allTerminated) { + continue; + } + // Create a predictor to track the cluster's centroid. IPredictor predictor = new LinearExtrapolator(_threatClusterMap[cluster].Centroid); @@ -195,16 +218,15 @@ public void RequestAssignInterceptorToThreat(Interceptor interceptor) { _assignableInterceptors.Add(interceptor); } - public void AssignInterceptorToThreat(in IReadOnlyList interceptors) { + private void AssignInterceptorToThreat(in IReadOnlyList interceptors) { if (interceptors.Count == 0) { return; } // The threat originally assigned to the interceptor has been terminated, so assign another // threat to the interceptor. - // TODO: We do not use clusters after the first assignment, we should consider re-clustering. - // This pulls from ALL available track files, not from our previously assigned cluster. + // This pulls from all available track files, not from our previously assigned cluster. List threats = _trackFiles.Where(trackFile => trackFile.Agent is Threat) .Select(trackFile => trackFile.Agent as Threat) .ToList(); @@ -222,18 +244,100 @@ public void AssignInterceptorToThreat(in IReadOnlyList interceptors } } + public void RequestClusterThreat(Threat threat) { + _threatsToCluster.Add(threat); + } + + private IEnumerator CheckForEscapingThreatsManager(float period) { + while (true) { + yield return new WaitForSeconds(period); + CheckForEscapingThreats(); + } + } + + private void CheckForEscapingThreats() { + List threats = _trackFiles + .Where(trackFile => trackFile.Status == TrackStatus.ASSIGNED && + trackFile.Agent is Threat) + .Select(trackFile => trackFile.Agent as Threat) + .ToList(); + if (threats.Count == 0) { + return; + } + + // Check whether the threats are escaping the pursuing interceptors. + foreach (var threat in threats) { + bool isEscaping = true; + foreach (var interceptor in threat.AssignedInterceptors) { + Vector3 interceptorPosition = interceptor.GetPosition(); + Vector3 threatPosition = threat.GetPosition(); + + float threatTimeToHit = (float)(threatPosition.magnitude / threat.GetSpeed()); + float interceptorTimeToHit = + (float)((threatPosition - interceptorPosition).magnitude / interceptor.GetSpeed()); + if (interceptorPosition.magnitude < threatPosition.magnitude && + threatTimeToHit > interceptorTimeToHit) { + isEscaping = false; + break; + } + } + if (isEscaping) { + RequestClusterThreat(threat); + } + } + } + + private IEnumerator ClusterThreatsManager(float period) { + while (true) { + ClusterThreats(); + yield return new WaitForSeconds(period); + } + } + + private void ClusterThreats() { + // Maximum number of threats per cluster. + const int MaxSize = 7; + // Maximum cluster radius in meters. + const float MaxRadius = 500; + + // Filter the threats. + List threats = + _threatsToCluster + .Where(threat => !threat.IsTerminated() && threat.AssignedInterceptors.Count == 0) + .ToList(); + if (threats.Count == 0) { + return; + } + + // Cluster threats. + IClusterer clusterer = new AgglomerativeClusterer(new List(threats), MaxSize, MaxRadius); + clusterer.Cluster(); + var clusters = clusterer.Clusters; + Debug.Log($"Clustered {threats.Count} threats into {clusters.Count} clusters."); + UIManager.Instance.LogActionMessage( + $"[IADS] Clustered {threats.Count} threats into {clusters.Count} clusters."); + + _threatClusters = clusters.ToList(); + foreach (var cluster in clusters) { + _threatClusterMap.Add(cluster, new ThreatClusterData(cluster)); + } + + _threatsToCluster.Clear(); + } + public void RegisterNewThreat(Threat threat) { - string trackID = $"T{1000 + _trackFileIdTicker++}"; + string trackID = $"T{1000 + ++_trackFileIdTicker}"; ThreatData trackFile = new ThreatData(threat, trackID); _trackFiles.Add(trackFile); _trackFileMap.Add(threat, trackFile); + RequestClusterThreat(threat); threat.OnThreatHit += RegisterThreatHit; threat.OnThreatMiss += RegisterThreatMiss; } public void RegisterNewInterceptor(Interceptor interceptor) { - string trackID = $"I{2000 + _trackFileIdTicker++}"; + string trackID = $"I{2000 + ++_trackFileIdTicker}"; InterceptorData trackFile = new InterceptorData(interceptor, trackID); _trackFiles.Add(trackFile); _trackFileMap.Add(interceptor, trackFile); @@ -271,6 +375,11 @@ private void RegisterInterceptorMiss(Interceptor interceptor, Threat threat) { if (threatTrack != null) { threatTrack.RemoveInterceptor(interceptor); + + // Check if the threat is being targeted by at least one interceptor. + if (threatTrack.AssignedInterceptorCount == 0) { + RequestClusterThreat(threat); + } } if (interceptorTrack != null) { @@ -310,12 +419,14 @@ public List GetInterceptorTracks() => _trackFiles.OfType().ToList(); private void RegisterSimulationEnded() { - _assignableInterceptors.Clear(); _trackFiles.Clear(); - _threatClusters.Clear(); - _threatClusterMap.Clear(); _trackFileMap.Clear(); _assignmentQueue.Clear(); + _threatClusters.Clear(); + _threatClusterMap.Clear(); + _interceptorClusterMap.Clear(); + _assignableInterceptors.Clear(); + _threatsToCluster.Clear(); _trackFileIdTicker = 0; } } diff --git a/Assets/Scripts/IADS/TrackFileData.cs b/Assets/Scripts/IADS/TrackFileData.cs index 35801382a..e2b5f259d 100644 --- a/Assets/Scripts/IADS/TrackFileData.cs +++ b/Assets/Scripts/IADS/TrackFileData.cs @@ -35,20 +35,23 @@ public virtual void MarkDestroyed() { [System.Serializable] public class ThreatData : TrackFileData { - public int assignedInterceptorCount = 0; [SerializeField] private List _assignedInterceptors = new List(); public ThreatData(Threat threat, string trackID) : base(threat, trackID) {} + public int AssignedInterceptorCount { + get { return _assignedInterceptors.Count; } + } + public void AssignInterceptor(Interceptor interceptor) { if (Status == TrackStatus.DESTROYED) { - Debug.LogError($"AssignInterceptor: Track {TrackID} is destroyed, cannot assign interceptor"); + Debug.LogError( + $"AssignInterceptor: Track {TrackID} is destroyed, cannot assign interceptor."); return; } _status = TrackStatus.ASSIGNED; _assignedInterceptors.Add(interceptor); - assignedInterceptorCount++; } public void RemoveInterceptor(Interceptor interceptor) { if (_assignedInterceptors.Contains(interceptor)) { @@ -56,30 +59,26 @@ public void RemoveInterceptor(Interceptor interceptor) { if (_assignedInterceptors.Count == 0) { _status = TrackStatus.UNASSIGNED; } - assignedInterceptorCount--; } } public override void MarkDestroyed() { base.MarkDestroyed(); _assignedInterceptors.Clear(); - assignedInterceptorCount = 0; } } [System.Serializable] public class InterceptorData : TrackFileData { [SerializeField] - private List _assignedThreats; + private List _assignedThreats = new List(); public InterceptorData(Interceptor interceptor, string interceptorID) - : base(interceptor, interceptorID) { - _assignedThreats = new List(); - } + : base(interceptor, interceptorID) {} public void AssignThreat(Threat threat) { if (_status == TrackStatus.DESTROYED) { - Debug.LogError($"AssignThreat: Interceptor {TrackID} is destroyed, cannot assign threat"); + Debug.LogError($"AssignThreat: Interceptor {TrackID} is destroyed, cannot assign threat."); return; } _status = TrackStatus.ASSIGNED; diff --git a/Assets/Scripts/SimManager.cs b/Assets/Scripts/SimManager.cs index 9866c0aaf..c65cf0582 100644 --- a/Assets/Scripts/SimManager.cs +++ b/Assets/Scripts/SimManager.cs @@ -589,9 +589,6 @@ void FixedUpdate() { RestartSimulation(); Debug.Log("Simulation completed."); } - - // Check whether to launch the interceptors. If so, create and launch the interceptor. - IADS.Instance.CheckAndLaunchInterceptors(); } public void QuitSimulation() { diff --git a/Assets/Tests/EditMode/CoordinatesTest.cs b/Assets/Tests/EditMode/CoordinatesTest.cs index 9e5facaa2..5602f8923 100644 --- a/Assets/Tests/EditMode/CoordinatesTest.cs +++ b/Assets/Tests/EditMode/CoordinatesTest.cs @@ -3,95 +3,119 @@ using UnityEngine.TestTools; public class Coordinates2Test { + private const float Epsilon = 0.0001f; + [Test] public void TestCartesianToPolarOrigin() { Vector2 cartesian = Vector2.zero; - Assert.AreEqual(0, Coordinates2.ConvertCartesianToPolar(cartesian).x); + Assert.That(Coordinates2.ConvertCartesianToPolar(cartesian).x, Is.EqualTo(0).Within(Epsilon)); } [Test] public void TestCartesianToPolarXAxis() { Vector2 cartesian = new Vector2(5, 0); Vector2 expectedPolar = new Vector2(5.0f, 0.0f); - Assert.AreEqual(expectedPolar, Coordinates2.ConvertCartesianToPolar(cartesian)); + Vector2 actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); + Assert.That(actualPolar.x, Is.EqualTo(expectedPolar.x).Within(Epsilon)); + Assert.That(actualPolar.y, Is.EqualTo(expectedPolar.y).Within(Epsilon)); } [Test] public void TestCartesianToPolarYAxis() { Vector2 cartesian = new Vector2(0, 5); Vector2 expectedPolar = new Vector2(5.0f, 90.0f); - Assert.AreEqual(expectedPolar, Coordinates2.ConvertCartesianToPolar(cartesian)); + Vector2 actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); + Assert.That(actualPolar.x, Is.EqualTo(expectedPolar.x).Within(Epsilon)); + Assert.That(actualPolar.y, Is.EqualTo(expectedPolar.y).Within(Epsilon)); } [Test] public void TestPolarToCartesianOrigin() { Vector2 expectedCartesian = Vector2.zero; - Vector2 polar1 = new Vector2(0, 0); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar1)); - Vector2 polar2 = new Vector2(0, 45); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar2)); - Vector2 polar3 = new Vector2(0, 90); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar3)); - Vector2 polar4 = new Vector2(0, 180); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar4)); - Vector2 polar5 = new Vector2(0, 360); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar5)); + Vector2[] polarInputs = new[] { new Vector2(0, 0), new Vector2(0, 45), new Vector2(0, 90), + new Vector2(0, 180), new Vector2(0, 360) }; + + foreach (var polar in polarInputs) { + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian.x, Is.EqualTo(expectedCartesian.x).Within(Epsilon)); + Assert.That(actualCartesian.y, Is.EqualTo(expectedCartesian.y).Within(Epsilon)); + } } [Test] public void TestPolarToCartesianXAxis() { Vector2 polar = new Vector2(10, 0); Vector2 expectedCartesian = new Vector2(10.0f, 0.0f); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar)); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian.x, Is.EqualTo(expectedCartesian.x).Within(Epsilon)); + Assert.That(actualCartesian.y, Is.EqualTo(expectedCartesian.y).Within(Epsilon)); } [Test] public void TestPolarToCartesianYAxis() { Vector2 polar = new Vector2(10, 90); Vector2 expectedCartesian = new Vector2(0.0f, 10.0f); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar)); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian.x, Is.EqualTo(expectedCartesian.x).Within(Epsilon)); + Assert.That(actualCartesian.y, Is.EqualTo(expectedCartesian.y).Within(Epsilon)); } [Test] public void TestPolarToCartesian30() { Vector2 polar = new Vector2(20, 30); Vector2 expectedCartesian = new Vector2(10 * Mathf.Sqrt(3), 10); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar)); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian.x, Is.EqualTo(expectedCartesian.x).Within(Epsilon)); + Assert.That(actualCartesian.y, Is.EqualTo(expectedCartesian.y).Within(Epsilon)); } [Test] public void TestPolarToCartesian45() { Vector2 polar = new Vector2(10 * Mathf.Sqrt(2), 45); Vector2 expectedCartesian = new Vector2(10.0f, 10.0f); - Assert.AreEqual(expectedCartesian, Coordinates2.ConvertPolarToCartesian(polar)); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian.x, Is.EqualTo(expectedCartesian.x).Within(Epsilon)); + Assert.That(actualCartesian.y, Is.EqualTo(expectedCartesian.y).Within(Epsilon)); } } public class Coordinates3Test { + private const float Epsilon = 0.0001f; + [Test] public void TestCartesianToSphericalOrigin() { Vector3 cartesian = Vector3.zero; - Assert.AreEqual(0, Coordinates3.ConvertCartesianToSpherical(cartesian).x); + Assert.That(Coordinates3.ConvertCartesianToSpherical(cartesian).x, + Is.EqualTo(0).Within(Epsilon)); } [Test] public void TestCartesianToSpherical() { Vector3 cartesian = new Vector3(2, -1, 3); Vector3 expectedSpherical = new Vector3(Mathf.Sqrt(14), 33.690068f, -15.501360f); - Assert.AreEqual(expectedSpherical, Coordinates3.ConvertCartesianToSpherical(cartesian)); + Vector3 actualSpherical = Coordinates3.ConvertCartesianToSpherical(cartesian); + Assert.That(actualSpherical.x, Is.EqualTo(expectedSpherical.x).Within(Epsilon)); + Assert.That(actualSpherical.y, Is.EqualTo(expectedSpherical.y).Within(Epsilon)); + Assert.That(actualSpherical.z, Is.EqualTo(expectedSpherical.z).Within(Epsilon)); } [Test] public void TestSphericalToCartesianOrigin() { Vector3 spherical = new Vector3(0, 45, 90); Vector3 expectedCartesian = Vector3.zero; - Assert.AreEqual(expectedCartesian, Coordinates3.ConvertSphericalToCartesian(spherical)); + Vector3 actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); + Assert.That(actualCartesian.x, Is.EqualTo(expectedCartesian.x).Within(Epsilon)); + Assert.That(actualCartesian.y, Is.EqualTo(expectedCartesian.y).Within(Epsilon)); + Assert.That(actualCartesian.z, Is.EqualTo(expectedCartesian.z).Within(Epsilon)); } [Test] public void TestSphericalToCartesian() { Vector3 spherical = new Vector3(10, 60, 30); Vector3 expectedCartesian = new Vector3(7.5f, 5.0f, 4.3301270189221945f); - Assert.True(expectedCartesian == Coordinates3.ConvertSphericalToCartesian(spherical)); + Vector3 actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); + Assert.That(actualCartesian.x, Is.EqualTo(expectedCartesian.x).Within(Epsilon)); + Assert.That(actualCartesian.y, Is.EqualTo(expectedCartesian.y).Within(Epsilon)); + Assert.That(actualCartesian.z, Is.EqualTo(expectedCartesian.z).Within(Epsilon)); } } diff --git a/Assets/Tests/PlayMode/ConfigTest.cs b/Assets/Tests/PlayMode/ConfigTest.cs index 652164dc6..395108546 100644 --- a/Assets/Tests/PlayMode/ConfigTest.cs +++ b/Assets/Tests/PlayMode/ConfigTest.cs @@ -40,10 +40,10 @@ public IEnumerator TestAllConfigFilesLoad() { "All INTERCEPTOR agents should be in the INITIALIZED flight phase after loading while paused"); } else if (agent is Threat) { - // All threats start in MIDCOURSE phase + // All threats start in INITIALIZED phase Assert.AreEqual( - Agent.FlightPhase.MIDCOURSE, agent.GetFlightPhase(), - "All THREAT agents should be in the MIDCOURSE flight phase after loading while paused"); + Agent.FlightPhase.INITIALIZED, agent.GetFlightPhase(), + "All THREAT agents should be in the INITIALIZED flight phase after loading while paused"); } } Assert.LessOrEqual(Mathf.Abs(Time.fixedDeltaTime), epsilon,