From 9aff2bdd9bb2633172941f8ad772962581a982e2 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 11 Oct 2025 23:07:19 -0700 Subject: [PATCH 01/74] Implement all agent-related interfaces and base implementations --- Assets/Scripts/Agents/AgentBase.cs | 195 ++++++++++++++++++ Assets/Scripts/Agents/AgentBase.cs.meta | 8 + Assets/Scripts/Agents/IAgent.cs | 25 +++ Assets/Scripts/Agents/IAgent.cs.meta | 8 + Assets/Scripts/Controller/Controller.cs | 4 +- Assets/Scripts/Controller/ControllerBase.cs | 20 ++ .../Scripts/Controller/ControllerBase.cs.meta | 2 + Assets/Scripts/Controller/IController.cs | 13 ++ Assets/Scripts/Controller/IController.cs.meta | 2 + Assets/Scripts/Controller/PnController.cs | 2 +- .../Scripts/Controller/WaypointController.cs | 2 +- .../Scripts/Hierarchical/HierarchicalAgent.cs | 23 +++ .../Hierarchical/HierarchicalAgent.cs.meta | 2 + .../Scripts/Hierarchical/HierarchicalBase.cs | 5 + Assets/Scripts/Hierarchical/IHierarchical.cs | 1 + Assets/Scripts/Interceptors/Interceptor.cs | 2 +- Assets/Scripts/Movements.meta | 8 + Assets/Scripts/Movements/IMovement.cs | 11 + Assets/Scripts/Movements/IMovement.cs.meta | 2 + Assets/Scripts/Movements/MovementBase.cs | 12 ++ Assets/Scripts/Movements/MovementBase.cs.meta | 2 + Assets/Scripts/Threats/FixedWingThreat.cs | 2 +- Assets/Scripts/Threats/RotaryWingThreat.cs | 2 +- .../Tests/EditMode/HierarchicalBaseTests.cs | 28 ++- 24 files changed, 373 insertions(+), 8 deletions(-) create mode 100644 Assets/Scripts/Agents/AgentBase.cs create mode 100644 Assets/Scripts/Agents/AgentBase.cs.meta create mode 100644 Assets/Scripts/Agents/IAgent.cs create mode 100644 Assets/Scripts/Agents/IAgent.cs.meta create mode 100644 Assets/Scripts/Controller/ControllerBase.cs create mode 100644 Assets/Scripts/Controller/ControllerBase.cs.meta create mode 100644 Assets/Scripts/Controller/IController.cs create mode 100644 Assets/Scripts/Controller/IController.cs.meta create mode 100644 Assets/Scripts/Hierarchical/HierarchicalAgent.cs create mode 100644 Assets/Scripts/Hierarchical/HierarchicalAgent.cs.meta create mode 100644 Assets/Scripts/Movements.meta create mode 100644 Assets/Scripts/Movements/IMovement.cs create mode 100644 Assets/Scripts/Movements/IMovement.cs.meta create mode 100644 Assets/Scripts/Movements/MovementBase.cs create mode 100644 Assets/Scripts/Movements/MovementBase.cs.meta diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs new file mode 100644 index 000000000..69c79449d --- /dev/null +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -0,0 +1,195 @@ +using UnityEngine; + +// Base implementation of an agent. +public abstract class AgentBase : MonoBehaviour, IAgent { + [SerializeField] + // The agent's position within the hierarchical strategy is given by the hierarchical agent. + private HierarchicalAgent _hierarchicalAgent; + + [SerializeField] + // Static configuration of the agent, including agent type, unit cost, acceleration configuration, + // aerodynamics parameters, power table, and visualization configuration. + private Configs.StaticConfig _staticConfig; + + [SerializeField] + // Agent configuration, including initial state, attack behavior configuration (for threats), + // dynamic configuration, and sub-agent configuration (for interceptors). + private Configs.AgentConfig _agentConfig; + + // Movement behavior of the agent. + private IMovement _movement; + + // The controller calculates the acceleration input, given the agent's current state and its + // target's current state. + private IController _controller; + + [SerializeField] + // The acceleration field is not part of the Rigidbody component, so it is tracked separately. The + // acceleration is applied as a force during each frame update. + private Vector3 _acceleration; + + [SerializeField] + // The acceleration input is calculated by the controller and provided to the movement behavior. + private Vector3 _accelerationInput; + + public HierarchicalAgent HierarchicalAgent { + get => _hierarchicalAgent; + set => _hierarchicalAgent = value; + } + public Configs.StaticConfig StaticConfig { + get => _staticConfig; + set => _staticConfig = value; + } + public Configs.AgentConfig AgentConfig { + get => _agentConfig; + set => _agentConfig = value; + } + public IMovement Movement { + get => _movement; + set => _movement = value; + } + public IController Controller { + get => _controller; + set => _controller = value; + } + public Vector3 Position { + get => transform.position; + set => transform.position = value; + } + public Vector3 Velocity { + get => GetComponent().linearVelocity; + set => GetComponent().linearVelocity = value; + } + public float Speed => Velocity.magnitude; + public Vector3 Acceleration { + get => _acceleration; + set => _acceleration = value; + } + public Vector3 AccelerationInput { + get => _accelerationInput; + set => _accelerationInput = value; + } + + // TODO(titan): Event handlers for hits and misses. + + // TODO(titan): Methods for updating the agent and terminating the agent. + + public Transformation GetRelativeTransformation(IAgent target) { + return GetRelativeTransformation(target.HierarchicalAgent); + } + + public Transformation GetRelativeTransformation(IHierarchical target) { + Transformation transformation = new Transformation(); + + // Get the relative position transformation. + transformation.position = GetRelativePositionTransformation(target.Position - Position); + + // Get the relative velocity transformation. + transformation.velocity = + GetRelativeVelocityTransformation(target.Position - Position, target.Velocity - Velocity); + + // Get the relative acceleration transformation. + transformation.acceleration = GetRelativeAccelerationTransformation(target.Acceleration); + return transformation; + } + + public Transformation GetRelativeTransformation(in Vector3 waypoint) { + Transformation transformation = new Transformation(); + + // Get the relative position transformation. + transformation.position = GetRelativePositionTransformation(waypoint - Position); + + // Get the relative velocity transformation. + transformation.velocity = GetRelativeVelocityTransformation(waypoint - Position, -Velocity); + return transformation; + } + + private PositionTransformation GetRelativePositionTransformation(in Vector3 relativePosition) { + PositionTransformation positionTransformation = new PositionTransformation(); + + // Set the relative position in Cartesian coordinates. + positionTransformation.cartesian = relativePosition; + + // Calculate the distance (range) to the target. + positionTransformation.range = relativePosition.magnitude; + + Vector3 flatRelativePosition = Vector3.ProjectOnPlane(relativePosition, transform.up); + Vector3 verticalRelativePosition = relativePosition - flatRelativePosition; + + // Calculate the elevation (vertical angle relative to forward). + positionTransformation.elevation = + Mathf.Atan(verticalRelativePosition.magnitude / flatRelativePosition.magnitude); + + // Calculate the azimuth (horizontal angle relative to forward). + if (flatRelativePosition.magnitude == 0) { + positionTransformation.azimuth = 0; + } else { + positionTransformation.azimuth = + Vector3.SignedAngle(transform.forward, flatRelativePosition, transform.up) * Mathf.PI / + 180; + } + + return positionTransformation; + } + + private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 relativePosition, + in Vector3 relativeVelocity) { + VelocityTransformation velocityTransformation = new VelocityTransformation(); + + // Set the relative velocity in Cartesian coordinates. + velocityTransformation.cartesian = relativeVelocity; + + // Calculate range rate (radial velocity). + velocityTransformation.range = Vector3.Dot(relativeVelocity, relativePosition.normalized); + + // Project relative velocity onto the sphere passing through the target. + Vector3 tangentialVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); + + // The target azimuth vector is orthogonal to the relative position vector and + // points to the starboard of the target along the azimuth-elevation sphere. + Vector3 targetAzimuth = Vector3.Cross(transform.up, relativePosition); + // The target elevation vector is orthogonal to the relative position vector + // and points upwards from the target along the azimuth-elevation sphere. + Vector3 targetElevation = Vector3.Cross(relativePosition, transform.right); + // If the relative position vector is parallel to the yaw or pitch axis, the + // target azimuth vector or the target elevation vector will be undefined. + if (targetAzimuth.magnitude == 0) { + targetAzimuth = Vector3.Cross(targetElevation, relativePosition); + } else if (targetElevation.magnitude == 0) { + targetElevation = Vector3.Cross(relativePosition, targetAzimuth); + } + + // Project the relative velocity vector on the azimuth-elevation sphere onto + // the target azimuth vector. + Vector3 tangentialVelocityOnAzimuth = Vector3.Project(tangentialVelocity, targetAzimuth); + + // Calculate the time derivative of the azimuth to the target. + velocityTransformation.azimuth = + tangentialVelocityOnAzimuth.magnitude / relativePosition.magnitude; + if (Vector3.Dot(tangentialVelocityOnAzimuth, targetAzimuth) < 0) { + velocityTransformation.azimuth *= -1; + } + + // Project the velocity vector on the azimuth-elevation sphere onto the target + // elevation vector. + Vector3 tangentialVelocityOnElevation = Vector3.Project(tangentialVelocity, targetElevation); + + // Calculate the time derivative of the elevation to the target. + velocityTransformation.elevation = + tangentialVelocityOnElevation.magnitude / relativePosition.magnitude; + if (Vector3.Dot(tangentialVelocityOnElevation, targetElevation) < 0) { + velocityTransformation.elevation *= -1; + } + + return velocityTransformation; + } + + private AccelerationTransformation GetRelativeAccelerationTransformation( + in Vector3 relativeAcceleration) { + // Since the agent's acceleration is an input, the relative acceleration is just the agent's + // acceleration. + AccelerationTransformation accelerationTransformation = new AccelerationTransformation(); + accelerationTransformation.cartesian = relativeAcceleration; + return accelerationTransformation; + } +} diff --git a/Assets/Scripts/Agents/AgentBase.cs.meta b/Assets/Scripts/Agents/AgentBase.cs.meta new file mode 100644 index 000000000..87330043c --- /dev/null +++ b/Assets/Scripts/Agents/AgentBase.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a2350314331ce4528b9f7184126794a0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Agents/IAgent.cs b/Assets/Scripts/Agents/IAgent.cs new file mode 100644 index 000000000..7d0613c04 --- /dev/null +++ b/Assets/Scripts/Agents/IAgent.cs @@ -0,0 +1,25 @@ +using UnityEngine; + +// Interface for an agent. +// +// An agent represents a physical entity, such as a ship, an interceptor, or a threat subject to the +// laws of physics. +public interface IAgent { + HierarchicalAgent HierarchicalAgent { get; set; } + + Configs.StaticConfig StaticConfig { get; set; } + Configs.AgentConfig AgentConfig { get; set; } + + IMovement Movement { get; set; } + IController Controller { get; set; } + + Vector3 Position { get; set; } + Vector3 Velocity { get; set; } + float Speed { get; } + Vector3 Acceleration { get; set; } + Vector3 AccelerationInput { get; set; } + + Transformation GetRelativeTransformation(IAgent target); + Transformation GetRelativeTransformation(IHierarchical target); + Transformation GetRelativeTransformation(in Vector3 waypoint); +} diff --git a/Assets/Scripts/Agents/IAgent.cs.meta b/Assets/Scripts/Agents/IAgent.cs.meta new file mode 100644 index 000000000..e39bc447b --- /dev/null +++ b/Assets/Scripts/Agents/IAgent.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 02cd27103e96448d585fa0de4baf1c0e +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Controller/Controller.cs b/Assets/Scripts/Controller/Controller.cs index 4bcdce3c5..3aa564cf9 100644 --- a/Assets/Scripts/Controller/Controller.cs +++ b/Assets/Scripts/Controller/Controller.cs @@ -2,11 +2,11 @@ using UnityEngine; // The controller class is an interface between the agent and its control law. -public class IController { +public class IControllerLegacy { // Agent that the controller is controlling. protected Agent _agent; - public IController(Agent agent) { + public IControllerLegacy(Agent agent) { _agent = agent; } diff --git a/Assets/Scripts/Controller/ControllerBase.cs b/Assets/Scripts/Controller/ControllerBase.cs new file mode 100644 index 000000000..d66b3e9a4 --- /dev/null +++ b/Assets/Scripts/Controller/ControllerBase.cs @@ -0,0 +1,20 @@ +using UnityEngine; + +// Base implementation of a controller. +public abstract class ControllerBase { + // Plan the next optimal control to intercept the target. + Vector3 PlanToTarget(IAgent agent) { + Transformation relativeTransformation = + agent.GetRelativeTransformation(agent.HierarchicalAgent.TargetModel); + return Plan(relativeTransformation); + } + + // Plan the next optimal control to the waypoint. + Vector3 PlanToWaypoint(IAgent agent, in Vector3 waypoint) { + Transformation relativeTransformation = agent.GetRelativeTransformation(waypoint); + return Plan(relativeTransformation); + } + + // Controller-dependent implementation of the control law. + protected abstract Vector3 Plan(in Transformation relativeTransformation); +} diff --git a/Assets/Scripts/Controller/ControllerBase.cs.meta b/Assets/Scripts/Controller/ControllerBase.cs.meta new file mode 100644 index 000000000..e64fbfb43 --- /dev/null +++ b/Assets/Scripts/Controller/ControllerBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e77ac74a2681a43cea169b763f9d3bd7 \ No newline at end of file diff --git a/Assets/Scripts/Controller/IController.cs b/Assets/Scripts/Controller/IController.cs new file mode 100644 index 000000000..6f9735a63 --- /dev/null +++ b/Assets/Scripts/Controller/IController.cs @@ -0,0 +1,13 @@ +using UnityEngine; + +// Interface for a controller. +// +// The controller determines the acceleration input given the agent's current state and its intended +// target or waypoint. +public interface IController { + // Plan the next optimal control to intercept the target. + Vector3 PlanToTarget(IAgent agent); + + // Plan the next optimal control to the waypoint. + Vector3 PlanToWaypoint(IAgent agent, in Vector3 waypoint); +} diff --git a/Assets/Scripts/Controller/IController.cs.meta b/Assets/Scripts/Controller/IController.cs.meta new file mode 100644 index 000000000..39c1e77d2 --- /dev/null +++ b/Assets/Scripts/Controller/IController.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 0613ea2faf02a49de9f72667c9e56182 \ No newline at end of file diff --git a/Assets/Scripts/Controller/PnController.cs b/Assets/Scripts/Controller/PnController.cs index 9a7a8b6c1..75853c97d 100644 --- a/Assets/Scripts/Controller/PnController.cs +++ b/Assets/Scripts/Controller/PnController.cs @@ -3,7 +3,7 @@ // The proportional navigation controller applies proportional navigation to steer the agent towards // its target. -public class PnController : IController { +public class PnController : IControllerLegacy { // Negative closing velocity turn factor. protected const float negativeClosingVelocityTurnFactor = 100f; diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointController.cs index 7818a7ade..334e629d0 100644 --- a/Assets/Scripts/Controller/WaypointController.cs +++ b/Assets/Scripts/Controller/WaypointController.cs @@ -2,7 +2,7 @@ using UnityEngine; // The waypoint controller steers the agent to the target using direct linear guidance. -public class WaypointController : IController { +public class WaypointController : IControllerLegacy { // Desired speed in m/s. float _desiredSpeed; diff --git a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs new file mode 100644 index 000000000..ae0caecef --- /dev/null +++ b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs @@ -0,0 +1,23 @@ +using UnityEngine; + +// Hierarchy node owned by an agent. +// +// Since agents cannot inherit from both HierarchicalBase and MonoBehaviour, agents use composition +// to know their position within the hierarchical strategy. +public class HierarchicalAgent : HierarchicalBase { + [SerializeField] + private AgentBase _agent; + + public AgentBase Agent { + get => _agent; + set => _agent = value; + } + + protected override Vector3 GetPosition() { + return Agent.Position; + } + + protected override Vector3 GetVelocity() { + return Agent.Velocity; + } +} diff --git a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs.meta b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs.meta new file mode 100644 index 000000000..845acc1c3 --- /dev/null +++ b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 5453ab57d0b83493d8e8ed068847dc09 \ No newline at end of file diff --git a/Assets/Scripts/Hierarchical/HierarchicalBase.cs b/Assets/Scripts/Hierarchical/HierarchicalBase.cs index 9a29ef7a3..4f53f8bae 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalBase.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalBase.cs @@ -28,6 +28,7 @@ public IHierarchical TargetModel { public Vector3 Position => GetPosition(); public Vector3 Velocity => GetVelocity(); public float Speed => Velocity.magnitude; + public Vector3 Acceleration => GetAcceleration(); public void AddSubHierarchical(IHierarchical subHierarchical) { if (!_subHierarchicals.Contains(subHierarchical)) { @@ -47,6 +48,10 @@ protected virtual Vector3 GetVelocity() { return GetMean(s => s.Velocity); } + protected virtual Vector3 GetAcceleration() { + return GetMean(s => s.Acceleration); + } + private Vector3 GetMean(System.Func selector) { if (_subHierarchicals.Count == 0) { return Vector3.zero; diff --git a/Assets/Scripts/Hierarchical/IHierarchical.cs b/Assets/Scripts/Hierarchical/IHierarchical.cs index d644fd1ca..2cf012d1d 100644 --- a/Assets/Scripts/Hierarchical/IHierarchical.cs +++ b/Assets/Scripts/Hierarchical/IHierarchical.cs @@ -16,6 +16,7 @@ public interface IHierarchical { Vector3 Position { get; } Vector3 Velocity { get; } float Speed { get; } + Vector3 Acceleration { get; } void AddSubHierarchical(IHierarchical subHierarchical); void RemoveSubHierarchical(IHierarchical subHierarchical); diff --git a/Assets/Scripts/Interceptors/Interceptor.cs b/Assets/Scripts/Interceptors/Interceptor.cs index 963980198..556502874 100644 --- a/Assets/Scripts/Interceptors/Interceptor.cs +++ b/Assets/Scripts/Interceptors/Interceptor.cs @@ -97,7 +97,7 @@ private Vector3 CalculateAccelerationInput(double deltaTime) { // return Vector3.zero; // } - IController controller; + IControllerLegacy controller; switch (agentConfig.DynamicConfig.FlightConfig.ControllerType) { case Configs.ControllerType.ProportionalNavigation: { controller = new PnController(this, _navigationGain); diff --git a/Assets/Scripts/Movements.meta b/Assets/Scripts/Movements.meta new file mode 100644 index 000000000..212d28b02 --- /dev/null +++ b/Assets/Scripts/Movements.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: ed65193565b2748a08fdcfb019575a9b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Movements/IMovement.cs b/Assets/Scripts/Movements/IMovement.cs new file mode 100644 index 000000000..246ce7913 --- /dev/null +++ b/Assets/Scripts/Movements/IMovement.cs @@ -0,0 +1,11 @@ +// Interface for a movement behavior. +// +// The movement behavior determines how the agent navigates through the environment, such as whether +// it is aerial or ground-based. The movement behavior also considers drag and gravity. +public interface IMovement { + IAgent Agent { get; set; } + + // Determine the next movement for the agent by using the agent's controller to calculate the + // acceleration input. + void Update(double deltaTime); +} diff --git a/Assets/Scripts/Movements/IMovement.cs.meta b/Assets/Scripts/Movements/IMovement.cs.meta new file mode 100644 index 000000000..92d56e597 --- /dev/null +++ b/Assets/Scripts/Movements/IMovement.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 76b9d40c359324501a3c3b0bb89b5b4e \ No newline at end of file diff --git a/Assets/Scripts/Movements/MovementBase.cs b/Assets/Scripts/Movements/MovementBase.cs new file mode 100644 index 000000000..8a4346986 --- /dev/null +++ b/Assets/Scripts/Movements/MovementBase.cs @@ -0,0 +1,12 @@ +// Base implementation of a movement behavior. +public abstract class MovementBase : IMovement { + // Agent to which the movement behavior is assigned. + private IAgent _agent; + + public IAgent Agent { + get => _agent; + set => _agent = value; + } + + public abstract void Update(double deltaTime); +} diff --git a/Assets/Scripts/Movements/MovementBase.cs.meta b/Assets/Scripts/Movements/MovementBase.cs.meta new file mode 100644 index 000000000..8280c82ad --- /dev/null +++ b/Assets/Scripts/Movements/MovementBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 7dc60f346ed1b40aba8814ca796a393d \ No newline at end of file diff --git a/Assets/Scripts/Threats/FixedWingThreat.cs b/Assets/Scripts/Threats/FixedWingThreat.cs index b383213e5..16026c5f7 100644 --- a/Assets/Scripts/Threats/FixedWingThreat.cs +++ b/Assets/Scripts/Threats/FixedWingThreat.cs @@ -70,7 +70,7 @@ private Vector3 CalculateAccelerationInput() { Vector3 velocity = GetVelocity(); float speed = velocity.magnitude; - IController controller = new PnController(this, _navigationGain); + IControllerLegacy controller = new PnController(this, _navigationGain); Vector3 accelerationInput = controller.PlanToWaypoint(_currentWaypoint); // Counter gravity as much as possible. diff --git a/Assets/Scripts/Threats/RotaryWingThreat.cs b/Assets/Scripts/Threats/RotaryWingThreat.cs index 3ae25c77c..6c69e2ba7 100644 --- a/Assets/Scripts/Threats/RotaryWingThreat.cs +++ b/Assets/Scripts/Threats/RotaryWingThreat.cs @@ -42,7 +42,7 @@ private void UpdateWaypointAndPower() { private Vector3 CalculateAccelerationToWaypoint() { float desiredSpeed = LookupPowerTable(_currentPower); - IController controller = new WaypointController(this, desiredSpeed); + IControllerLegacy controller = new WaypointController(this, desiredSpeed); Vector3 accelerationInput = controller.PlanToWaypoint(_currentWaypoint); Vector3 forwardAccelerationInput = Vector3.Project(accelerationInput, transform.forward); diff --git a/Assets/Tests/EditMode/HierarchicalBaseTests.cs b/Assets/Tests/EditMode/HierarchicalBaseTests.cs index fbefcafe4..fe90254a3 100644 --- a/Assets/Tests/EditMode/HierarchicalBaseTests.cs +++ b/Assets/Tests/EditMode/HierarchicalBaseTests.cs @@ -8,10 +8,14 @@ public class HierarchicalBaseTests { private class FixedHierarchical : HierarchicalBase { private Vector3 _position; private Vector3 _velocity; + private Vector3 _acceleration; - public FixedHierarchical(in Vector3 position, in Vector3 velocity) { + public FixedHierarchical(in Vector3 position, in Vector3 velocity) + : this(position, velocity, Vector3.zero) {} + public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { _position = position; _velocity = velocity; + _acceleration = acceleration; } protected override Vector3 GetPosition() { @@ -20,6 +24,9 @@ protected override Vector3 GetPosition() { protected override Vector3 GetVelocity() { return _velocity; } + protected override Vector3 GetAcceleration() { + return _acceleration; + } } [Test] @@ -81,6 +88,25 @@ public void Speed_ReturnsMagnitudeOfVelocity() { Assert.AreEqual(5f, parent.Speed, _epsilon); } + [Test] + public void Acceleration_ReturnsMeanOfSubHierarchicals() { + var parent = new HierarchicalBase(); + var child1 = + new FixedHierarchical(new Vector3(2, 0, 0), new Vector3(2, 1, 1), new Vector3(0, -1, 3)); + parent.AddSubHierarchical(child1); + var child2 = + new FixedHierarchical(new Vector3(-1, 2, 3), new Vector3(1, -2, 0), new Vector3(0, 2, -2)); + parent.AddSubHierarchical(child2); + + Assert.AreEqual(new Vector3(0f, 0.5f, 0.5f), parent.Acceleration); + } + + [Test] + public void Acceleration_WithNoSubHierarchicals_IsZero() { + var parent = new HierarchicalBase(); + Assert.AreEqual(Vector3.zero, parent.Velocity); + } + [Test] public void AddSubHierarchical_AddsCorrectly() { var parent = new HierarchicalBase(); From de1df58056f278bdfecf6f961a15ae8d53ba62dc Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 10:04:28 -0700 Subject: [PATCH 02/74] Add unit tests for relative transformation --- Assets/Scripts/Agents/AgentBase.cs | 30 ++++---- Assets/Tests/EditMode/AgentBaseTests.cs | 79 ++++++++++++++++++++ Assets/Tests/EditMode/AgentBaseTests.cs.meta | 2 + Assets/Tests/EditMode/AgentTestBase.cs | 3 +- 4 files changed, 96 insertions(+), 18 deletions(-) create mode 100644 Assets/Tests/EditMode/AgentBaseTests.cs create mode 100644 Assets/Tests/EditMode/AgentBaseTests.cs.meta diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 69c79449d..44da147e5 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -1,7 +1,7 @@ using UnityEngine; // Base implementation of an agent. -public abstract class AgentBase : MonoBehaviour, IAgent { +public class AgentBase : MonoBehaviour, IAgent { [SerializeField] // The agent's position within the hierarchical strategy is given by the hierarchical agent. private HierarchicalAgent _hierarchicalAgent; @@ -75,32 +75,30 @@ public Vector3 AccelerationInput { // TODO(titan): Methods for updating the agent and terminating the agent. public Transformation GetRelativeTransformation(IAgent target) { - return GetRelativeTransformation(target.HierarchicalAgent); + return GetRelativeTransformation(target.Position, target.Velocity, target.Acceleration); } public Transformation GetRelativeTransformation(IHierarchical target) { - Transformation transformation = new Transformation(); - - // Get the relative position transformation. - transformation.position = GetRelativePositionTransformation(target.Position - Position); - - // Get the relative velocity transformation. - transformation.velocity = - GetRelativeVelocityTransformation(target.Position - Position, target.Velocity - Velocity); - - // Get the relative acceleration transformation. - transformation.acceleration = GetRelativeAccelerationTransformation(target.Acceleration); - return transformation; + return GetRelativeTransformation(target.Position, target.Velocity, target.Acceleration); } public Transformation GetRelativeTransformation(in Vector3 waypoint) { + return GetRelativeTransformation(waypoint, Vector3.zero, Vector3.zero); + } + + private Transformation GetRelativeTransformation(in Vector3 position, in Vector3 velocity, + in Vector3 acceleration) { Transformation transformation = new Transformation(); // Get the relative position transformation. - transformation.position = GetRelativePositionTransformation(waypoint - Position); + transformation.position = GetRelativePositionTransformation(position - Position); // Get the relative velocity transformation. - transformation.velocity = GetRelativeVelocityTransformation(waypoint - Position, -Velocity); + transformation.velocity = + GetRelativeVelocityTransformation(position - Position, velocity - Velocity); + + // Get the relative acceleration transformation. + transformation.acceleration = GetRelativeAccelerationTransformation(acceleration); return transformation; } diff --git a/Assets/Tests/EditMode/AgentBaseTests.cs b/Assets/Tests/EditMode/AgentBaseTests.cs new file mode 100644 index 000000000..46fe60c62 --- /dev/null +++ b/Assets/Tests/EditMode/AgentBaseTests.cs @@ -0,0 +1,79 @@ +using NUnit.Framework; +using UnityEngine; + +public class AgentBaseTests { + [Test] + public void GetRelativeTransformation_TargetAtBoresight() { + AgentBase agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = agent.gameObject.AddComponent(); + agent.Position = new Vector3(0, 0, 0); + agent.Velocity = new Vector3(0, 0, 0); + + AgentBase target = new GameObject("Target").AddComponent(); + Rigidbody targetRb = target.gameObject.AddComponent(); + target.Position = new Vector3(0, 0, 20); + target.Velocity = new Vector3(0, 20, -1); + + Transformation relativeTransformation = agent.GetRelativeTransformation(target); + + Assert.AreEqual(relativeTransformation.position.cartesian, + target.transform.position - agent.transform.position); + Assert.AreEqual(relativeTransformation.position.range, 20); + Assert.AreEqual(relativeTransformation.position.azimuth, 0); + Assert.AreEqual(relativeTransformation.position.elevation, 0); + Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.velocity.range, -1); + Assert.AreEqual(relativeTransformation.velocity.azimuth, 0); + Assert.AreEqual(relativeTransformation.velocity.elevation, 1); + } + + [Test] + public void GetRelativeTransformation_TargetAtStarboard() { + AgentBase agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = agent.gameObject.AddComponent(); + agent.Position = new Vector3(0, 0, 0); + agent.Velocity = new Vector3(0, 0, 0); + + AgentBase target = new GameObject("Target").AddComponent(); + Rigidbody targetRb = target.gameObject.AddComponent(); + target.Position = new Vector3(20, 0, 0); + target.Velocity = new Vector3(0, 0, 20); + + Transformation relativeTransformation = agent.GetRelativeTransformation(target); + + Assert.AreEqual(relativeTransformation.position.cartesian, + target.transform.position - agent.transform.position); + Assert.AreEqual(relativeTransformation.position.range, 20); + Assert.AreEqual(relativeTransformation.position.azimuth, Mathf.PI / 2); + Assert.AreEqual(relativeTransformation.position.elevation, 0); + Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.velocity.range, 0); + Assert.AreEqual(relativeTransformation.velocity.azimuth, -1); + Assert.AreEqual(relativeTransformation.velocity.elevation, 0); + } + + [Test] + public void GetRelativeTransformation_TargetWithElevation() { + AgentBase agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = agent.gameObject.AddComponent(); + agent.Position = new Vector3(0, 0, 0); + agent.Velocity = new Vector3(0, 0, 0); + + AgentBase target = new GameObject("Target").AddComponent(); + Rigidbody targetRb = target.gameObject.AddComponent(); + target.Position = new Vector3(0, 20, 0); + target.Velocity = new Vector3(0, 0, 20); + + Transformation relativeTransformation = agent.GetRelativeTransformation(target); + + Assert.AreEqual(relativeTransformation.position.cartesian, + target.transform.position - agent.transform.position); + Assert.AreEqual(relativeTransformation.position.range, 20); + Assert.AreEqual(relativeTransformation.position.azimuth, 0); + Assert.AreEqual(relativeTransformation.position.elevation, Mathf.PI / 2); + Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.velocity.range, 0); + Assert.AreEqual(relativeTransformation.velocity.azimuth, 0); + Assert.AreEqual(relativeTransformation.velocity.elevation, -1); + } +} diff --git a/Assets/Tests/EditMode/AgentBaseTests.cs.meta b/Assets/Tests/EditMode/AgentBaseTests.cs.meta new file mode 100644 index 000000000..678d7d136 --- /dev/null +++ b/Assets/Tests/EditMode/AgentBaseTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 7a40187458fb14a149c64a1cd617aefa \ No newline at end of file diff --git a/Assets/Tests/EditMode/AgentTestBase.cs b/Assets/Tests/EditMode/AgentTestBase.cs index 215b84278..8b462b34d 100644 --- a/Assets/Tests/EditMode/AgentTestBase.cs +++ b/Assets/Tests/EditMode/AgentTestBase.cs @@ -1,6 +1,5 @@ -using System; -using System.Reflection; using NUnit.Framework; +using System.Reflection; using UnityEngine; public abstract class AgentTestBase : TestBase { From e53c687e8b3fb723a7f91f9bf162d7dfe2f48868 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 10:39:07 -0700 Subject: [PATCH 03/74] Address comments --- Assets/Scripts/Agents/AgentBase.cs | 65 ++++++++++++------- Assets/Scripts/Controller/ControllerBase.cs | 6 +- .../Scripts/Hierarchical/HierarchicalAgent.cs | 4 +- Assets/Tests/EditMode/AgentBaseTests.cs | 46 +++++++------ .../Tests/EditMode/HierarchicalBaseTests.cs | 2 +- 5 files changed, 75 insertions(+), 48 deletions(-) diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 44da147e5..7c5c85c49 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -2,6 +2,8 @@ // Base implementation of an agent. public class AgentBase : MonoBehaviour, IAgent { + private const float _epsilon = 1e-12f; + [SerializeField] // The agent's position within the hierarchical strategy is given by the hierarchical agent. private HierarchicalAgent _hierarchicalAgent; @@ -24,14 +26,17 @@ public class AgentBase : MonoBehaviour, IAgent { private IController _controller; [SerializeField] - // The acceleration field is not part of the Rigidbody component, so it is tracked separately. The - // acceleration is applied as a force during each frame update. + // The acceleration field is not part of the rigid body component, so it is tracked separately. + // The acceleration is applied as a force during each frame update. private Vector3 _acceleration; [SerializeField] // The acceleration input is calculated by the controller and provided to the movement behavior. private Vector3 _accelerationInput; + // Rigid body component. + private Rigidbody _rigidbody; + public HierarchicalAgent HierarchicalAgent { get => _hierarchicalAgent; set => _hierarchicalAgent = value; @@ -57,8 +62,8 @@ public Vector3 Position { set => transform.position = value; } public Vector3 Velocity { - get => GetComponent().linearVelocity; - set => GetComponent().linearVelocity = value; + get => _rigidbody.linearVelocity; + set => _rigidbody.linearVelocity = value; } public float Speed => Velocity.magnitude; public Vector3 Acceleration { @@ -70,6 +75,10 @@ public Vector3 AccelerationInput { set => _accelerationInput = value; } + private void Awake() { + _rigidbody = GetComponent(); + } + // TODO(titan): Event handlers for hits and misses. // TODO(titan): Methods for updating the agent and terminating the agent. @@ -88,16 +97,19 @@ public Transformation GetRelativeTransformation(in Vector3 waypoint) { private Transformation GetRelativeTransformation(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { - Transformation transformation = new Transformation(); + var transformation = new Transformation(); // Get the relative position transformation. - transformation.position = GetRelativePositionTransformation(position - Position); + var relativePosition = position - Position; + transformation.position = GetRelativePositionTransformation(relativePosition); // Get the relative velocity transformation. transformation.velocity = - GetRelativeVelocityTransformation(position - Position, velocity - Velocity); + GetRelativeVelocityTransformation(relativePosition, velocity - Velocity); // Get the relative acceleration transformation. + // Since the agent's acceleration is an input and can be set arbitrarily, the relative + // acceleration is just the other agent's acceleration. transformation.acceleration = GetRelativeAccelerationTransformation(acceleration); return transformation; } @@ -111,20 +123,20 @@ private PositionTransformation GetRelativePositionTransformation(in Vector3 rela // Calculate the distance (range) to the target. positionTransformation.range = relativePosition.magnitude; - Vector3 flatRelativePosition = Vector3.ProjectOnPlane(relativePosition, transform.up); - Vector3 verticalRelativePosition = relativePosition - flatRelativePosition; + var flatRelativePosition = Vector3.ProjectOnPlane(relativePosition, transform.up); + var verticalRelativePosition = relativePosition - flatRelativePosition; // Calculate the elevation (vertical angle relative to forward). positionTransformation.elevation = - Mathf.Atan(verticalRelativePosition.magnitude / flatRelativePosition.magnitude); + Mathf.Atan2(verticalRelativePosition.magnitude, flatRelativePosition.magnitude); // Calculate the azimuth (horizontal angle relative to forward). - if (flatRelativePosition.magnitude == 0) { + if (flatRelativePosition.sqrMagnitude < _epsilon) { positionTransformation.azimuth = 0; } else { positionTransformation.azimuth = - Vector3.SignedAngle(transform.forward, flatRelativePosition, transform.up) * Mathf.PI / - 180; + Vector3.SignedAngle(transform.forward, flatRelativePosition, transform.up) * + Mathf.Deg2Rad; } return positionTransformation; @@ -132,34 +144,41 @@ private PositionTransformation GetRelativePositionTransformation(in Vector3 rela private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 relativePosition, in Vector3 relativeVelocity) { - VelocityTransformation velocityTransformation = new VelocityTransformation(); + var velocityTransformation = new VelocityTransformation(); // Set the relative velocity in Cartesian coordinates. velocityTransformation.cartesian = relativeVelocity; + if (relativePosition.sqrMagnitude < _epsilon) { + velocityTransformation.range = relativeVelocity.magnitude; + velocityTransformation.azimuth = 0; + velocityTransformation.elevation = 0; + return velocityTransformation; + } + // Calculate range rate (radial velocity). velocityTransformation.range = Vector3.Dot(relativeVelocity, relativePosition.normalized); // Project relative velocity onto the sphere passing through the target. - Vector3 tangentialVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); + var tangentialVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); // The target azimuth vector is orthogonal to the relative position vector and // points to the starboard of the target along the azimuth-elevation sphere. - Vector3 targetAzimuth = Vector3.Cross(transform.up, relativePosition); + var targetAzimuth = Vector3.Cross(transform.up, relativePosition); // The target elevation vector is orthogonal to the relative position vector // and points upwards from the target along the azimuth-elevation sphere. - Vector3 targetElevation = Vector3.Cross(relativePosition, transform.right); + var targetElevation = Vector3.Cross(relativePosition, transform.right); // If the relative position vector is parallel to the yaw or pitch axis, the // target azimuth vector or the target elevation vector will be undefined. - if (targetAzimuth.magnitude == 0) { + if (targetAzimuth.sqrMagnitude < _epsilon) { targetAzimuth = Vector3.Cross(targetElevation, relativePosition); - } else if (targetElevation.magnitude == 0) { + } else if (targetElevation.sqrMagnitude < _epsilon) { targetElevation = Vector3.Cross(relativePosition, targetAzimuth); } // Project the relative velocity vector on the azimuth-elevation sphere onto // the target azimuth vector. - Vector3 tangentialVelocityOnAzimuth = Vector3.Project(tangentialVelocity, targetAzimuth); + var tangentialVelocityOnAzimuth = Vector3.Project(tangentialVelocity, targetAzimuth); // Calculate the time derivative of the azimuth to the target. velocityTransformation.azimuth = @@ -170,7 +189,7 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela // Project the velocity vector on the azimuth-elevation sphere onto the target // elevation vector. - Vector3 tangentialVelocityOnElevation = Vector3.Project(tangentialVelocity, targetElevation); + var tangentialVelocityOnElevation = Vector3.Project(tangentialVelocity, targetElevation); // Calculate the time derivative of the elevation to the target. velocityTransformation.elevation = @@ -184,9 +203,7 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela private AccelerationTransformation GetRelativeAccelerationTransformation( in Vector3 relativeAcceleration) { - // Since the agent's acceleration is an input, the relative acceleration is just the agent's - // acceleration. - AccelerationTransformation accelerationTransformation = new AccelerationTransformation(); + var accelerationTransformation = new AccelerationTransformation(); accelerationTransformation.cartesian = relativeAcceleration; return accelerationTransformation; } diff --git a/Assets/Scripts/Controller/ControllerBase.cs b/Assets/Scripts/Controller/ControllerBase.cs index d66b3e9a4..8a3bebb2a 100644 --- a/Assets/Scripts/Controller/ControllerBase.cs +++ b/Assets/Scripts/Controller/ControllerBase.cs @@ -1,16 +1,16 @@ using UnityEngine; // Base implementation of a controller. -public abstract class ControllerBase { +public abstract class ControllerBase : IController { // Plan the next optimal control to intercept the target. - Vector3 PlanToTarget(IAgent agent) { + public Vector3 PlanToTarget(IAgent agent) { Transformation relativeTransformation = agent.GetRelativeTransformation(agent.HierarchicalAgent.TargetModel); return Plan(relativeTransformation); } // Plan the next optimal control to the waypoint. - Vector3 PlanToWaypoint(IAgent agent, in Vector3 waypoint) { + public Vector3 PlanToWaypoint(IAgent agent, in Vector3 waypoint) { Transformation relativeTransformation = agent.GetRelativeTransformation(waypoint); return Plan(relativeTransformation); } diff --git a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs index ae0caecef..80a3c0603 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs @@ -16,8 +16,10 @@ public AgentBase Agent { protected override Vector3 GetPosition() { return Agent.Position; } - protected override Vector3 GetVelocity() { return Agent.Velocity; } + protected override Vector3 GetAcceleration() { + return Agent.Acceleration; + } } diff --git a/Assets/Tests/EditMode/AgentBaseTests.cs b/Assets/Tests/EditMode/AgentBaseTests.cs index 46fe60c62..da26b3ed8 100644 --- a/Assets/Tests/EditMode/AgentBaseTests.cs +++ b/Assets/Tests/EditMode/AgentBaseTests.cs @@ -1,16 +1,20 @@ using NUnit.Framework; using UnityEngine; -public class AgentBaseTests { +public class AgentBaseTests : TestBase { + private const float _epsilon = 1e-3f; + [Test] public void GetRelativeTransformation_TargetAtBoresight() { AgentBase agent = new GameObject("Agent").AddComponent(); Rigidbody agentRb = agent.gameObject.AddComponent(); + InvokePrivateMethod(agent, "Awake"); agent.Position = new Vector3(0, 0, 0); agent.Velocity = new Vector3(0, 0, 0); AgentBase target = new GameObject("Target").AddComponent(); Rigidbody targetRb = target.gameObject.AddComponent(); + InvokePrivateMethod(target, "Awake"); target.Position = new Vector3(0, 0, 20); target.Velocity = new Vector3(0, 20, -1); @@ -18,24 +22,26 @@ public void GetRelativeTransformation_TargetAtBoresight() { Assert.AreEqual(relativeTransformation.position.cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20); - Assert.AreEqual(relativeTransformation.position.azimuth, 0); - Assert.AreEqual(relativeTransformation.position.elevation, 0); + Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); + Assert.AreEqual(relativeTransformation.position.azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.position.elevation, 0f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); - Assert.AreEqual(relativeTransformation.velocity.range, -1); - Assert.AreEqual(relativeTransformation.velocity.azimuth, 0); - Assert.AreEqual(relativeTransformation.velocity.elevation, 1); + Assert.AreEqual(relativeTransformation.velocity.range, -1f, _epsilon); + Assert.AreEqual(relativeTransformation.velocity.azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.velocity.elevation, 1f, _epsilon); } [Test] public void GetRelativeTransformation_TargetAtStarboard() { AgentBase agent = new GameObject("Agent").AddComponent(); Rigidbody agentRb = agent.gameObject.AddComponent(); + InvokePrivateMethod(agent, "Awake"); agent.Position = new Vector3(0, 0, 0); agent.Velocity = new Vector3(0, 0, 0); AgentBase target = new GameObject("Target").AddComponent(); Rigidbody targetRb = target.gameObject.AddComponent(); + InvokePrivateMethod(target, "Awake"); target.Position = new Vector3(20, 0, 0); target.Velocity = new Vector3(0, 0, 20); @@ -43,24 +49,26 @@ public void GetRelativeTransformation_TargetAtStarboard() { Assert.AreEqual(relativeTransformation.position.cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20); - Assert.AreEqual(relativeTransformation.position.azimuth, Mathf.PI / 2); - Assert.AreEqual(relativeTransformation.position.elevation, 0); + Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); + Assert.AreEqual(relativeTransformation.position.azimuth, Mathf.PI / 2, _epsilon); + Assert.AreEqual(relativeTransformation.position.elevation, 0f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); - Assert.AreEqual(relativeTransformation.velocity.range, 0); - Assert.AreEqual(relativeTransformation.velocity.azimuth, -1); - Assert.AreEqual(relativeTransformation.velocity.elevation, 0); + Assert.AreEqual(relativeTransformation.velocity.range, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.velocity.azimuth, -1f, _epsilon); + Assert.AreEqual(relativeTransformation.velocity.elevation, 0f, _epsilon); } [Test] public void GetRelativeTransformation_TargetWithElevation() { AgentBase agent = new GameObject("Agent").AddComponent(); Rigidbody agentRb = agent.gameObject.AddComponent(); + InvokePrivateMethod(agent, "Awake"); agent.Position = new Vector3(0, 0, 0); agent.Velocity = new Vector3(0, 0, 0); AgentBase target = new GameObject("Target").AddComponent(); Rigidbody targetRb = target.gameObject.AddComponent(); + InvokePrivateMethod(target, "Awake"); target.Position = new Vector3(0, 20, 0); target.Velocity = new Vector3(0, 0, 20); @@ -68,12 +76,12 @@ public void GetRelativeTransformation_TargetWithElevation() { Assert.AreEqual(relativeTransformation.position.cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20); - Assert.AreEqual(relativeTransformation.position.azimuth, 0); - Assert.AreEqual(relativeTransformation.position.elevation, Mathf.PI / 2); + Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); + Assert.AreEqual(relativeTransformation.position.azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.position.elevation, Mathf.PI / 2, _epsilon); Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); - Assert.AreEqual(relativeTransformation.velocity.range, 0); - Assert.AreEqual(relativeTransformation.velocity.azimuth, 0); - Assert.AreEqual(relativeTransformation.velocity.elevation, -1); + Assert.AreEqual(relativeTransformation.velocity.range, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.velocity.azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.velocity.elevation, -1f, _epsilon); } } diff --git a/Assets/Tests/EditMode/HierarchicalBaseTests.cs b/Assets/Tests/EditMode/HierarchicalBaseTests.cs index fe90254a3..03c8ae67c 100644 --- a/Assets/Tests/EditMode/HierarchicalBaseTests.cs +++ b/Assets/Tests/EditMode/HierarchicalBaseTests.cs @@ -104,7 +104,7 @@ public void Acceleration_ReturnsMeanOfSubHierarchicals() { [Test] public void Acceleration_WithNoSubHierarchicals_IsZero() { var parent = new HierarchicalBase(); - Assert.AreEqual(Vector3.zero, parent.Velocity); + Assert.AreEqual(Vector3.zero, parent.Acceleration); } [Test] From b5ff98beab7303b6efccdbf02bdbab9d197269b0 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 10:16:45 -0700 Subject: [PATCH 04/74] Add interceptor and threat interfaces and base implementations --- Assets/Scripts/Behavior/AttackBehaviorBase.cs | 23 +++++++++++++++++++ .../Behavior/AttackBehaviorBase.cs.meta | 2 ++ Assets/Scripts/Behavior/IAttackBehavior.cs | 14 +++++++++++ .../Scripts/Behavior/IAttackBehavior.cs.meta | 2 ++ Assets/Scripts/Evasion.meta | 8 +++++++ Assets/Scripts/Evasion/EvasionBase.cs | 10 ++++++++ Assets/Scripts/Evasion/EvasionBase.cs.meta | 2 ++ Assets/Scripts/Evasion/IEvasion.cs | 13 +++++++++++ Assets/Scripts/Evasion/IEvasion.cs.meta | 2 ++ Assets/Scripts/Interceptors/CarrierBase.cs | 0 .../Scripts/Interceptors/CarrierBase.cs.meta | 2 ++ Assets/Scripts/Interceptors/IInterceptor.cs | 4 ++++ .../Scripts/Interceptors/IInterceptor.cs.meta | 2 ++ .../Scripts/Interceptors/InterceptorBase.cs | 2 ++ .../Interceptors/InterceptorBase.cs.meta | 2 ++ Assets/Scripts/Interceptors/LauncherBase.cs | 0 .../Scripts/Interceptors/LauncherBase.cs.meta | 2 ++ Assets/Scripts/Threats/IThreat.cs | 7 ++++++ Assets/Scripts/Threats/IThreat.cs.meta | 2 ++ Assets/Scripts/Threats/ThreatBase.cs | 17 ++++++++++++++ Assets/Scripts/Threats/ThreatBase.cs.meta | 2 ++ 21 files changed, 118 insertions(+) create mode 100644 Assets/Scripts/Behavior/AttackBehaviorBase.cs create mode 100644 Assets/Scripts/Behavior/AttackBehaviorBase.cs.meta create mode 100644 Assets/Scripts/Behavior/IAttackBehavior.cs create mode 100644 Assets/Scripts/Behavior/IAttackBehavior.cs.meta create mode 100644 Assets/Scripts/Evasion.meta create mode 100644 Assets/Scripts/Evasion/EvasionBase.cs create mode 100644 Assets/Scripts/Evasion/EvasionBase.cs.meta create mode 100644 Assets/Scripts/Evasion/IEvasion.cs create mode 100644 Assets/Scripts/Evasion/IEvasion.cs.meta create mode 100644 Assets/Scripts/Interceptors/CarrierBase.cs create mode 100644 Assets/Scripts/Interceptors/CarrierBase.cs.meta create mode 100644 Assets/Scripts/Interceptors/IInterceptor.cs create mode 100644 Assets/Scripts/Interceptors/IInterceptor.cs.meta create mode 100644 Assets/Scripts/Interceptors/InterceptorBase.cs create mode 100644 Assets/Scripts/Interceptors/InterceptorBase.cs.meta create mode 100644 Assets/Scripts/Interceptors/LauncherBase.cs create mode 100644 Assets/Scripts/Interceptors/LauncherBase.cs.meta create mode 100644 Assets/Scripts/Threats/IThreat.cs create mode 100644 Assets/Scripts/Threats/IThreat.cs.meta create mode 100644 Assets/Scripts/Threats/ThreatBase.cs create mode 100644 Assets/Scripts/Threats/ThreatBase.cs.meta diff --git a/Assets/Scripts/Behavior/AttackBehaviorBase.cs b/Assets/Scripts/Behavior/AttackBehaviorBase.cs new file mode 100644 index 000000000..db3c41d1a --- /dev/null +++ b/Assets/Scripts/Behavior/AttackBehaviorBase.cs @@ -0,0 +1,23 @@ +using UnityEngine; + +// Base implementation of an attack behavior. +public abstract class AttackBehaviorBase : IAttackBehavior { + // Attack behavior configuration. + [SerializeField] + private Configs.AttackBehaviorConfig _config; + + // Flight plan. + [SerializeField] + private FlightPlan _flightPlan; + + public Configs.AttackBehaviorConfig Config { + get => _config; + set => _config = value; + } + public FlightPlan FlightPlan => new(Config.FlightPlan); + + // Return the next waypoint for the agent to navigate to and the power setting to use towards the + // waypoint. + public abstract (Vector3 waypointPosition, Configs.Power power) + GetNextWaypoint(IAgent agent, in Vector3 targetPosition); +} diff --git a/Assets/Scripts/Behavior/AttackBehaviorBase.cs.meta b/Assets/Scripts/Behavior/AttackBehaviorBase.cs.meta new file mode 100644 index 000000000..387cd24be --- /dev/null +++ b/Assets/Scripts/Behavior/AttackBehaviorBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: ba1b23b63093945f2b858680fdd70484 \ No newline at end of file diff --git a/Assets/Scripts/Behavior/IAttackBehavior.cs b/Assets/Scripts/Behavior/IAttackBehavior.cs new file mode 100644 index 000000000..2fd8964e3 --- /dev/null +++ b/Assets/Scripts/Behavior/IAttackBehavior.cs @@ -0,0 +1,14 @@ +using UnityEngine; + +// Interface for an attack behavior. +// +// The attack behavior determines how the the threat navigates towards the asset. +public interface IAttackBehavior { + Configs.AttackBehaviorConfig Config { get; set; } + FlightPlan FlightPlan { get; } + + // Return the next waypoint for the agent to navigate to and the power setting to use towards the + // waypoint. + (Vector3 waypointPosition, Configs.Power power) + GetNextWaypoint(IAgent agent, in Vector3 targetPosition); +} diff --git a/Assets/Scripts/Behavior/IAttackBehavior.cs.meta b/Assets/Scripts/Behavior/IAttackBehavior.cs.meta new file mode 100644 index 000000000..17f14d407 --- /dev/null +++ b/Assets/Scripts/Behavior/IAttackBehavior.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: fe1e3a2ccca0b4f0c909ac3bc2a55712 \ No newline at end of file diff --git a/Assets/Scripts/Evasion.meta b/Assets/Scripts/Evasion.meta new file mode 100644 index 000000000..a17c8488c --- /dev/null +++ b/Assets/Scripts/Evasion.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 578b89e2cd80e4a958b7e5eac564297b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Evasion/EvasionBase.cs b/Assets/Scripts/Evasion/EvasionBase.cs new file mode 100644 index 000000000..5fa2816c7 --- /dev/null +++ b/Assets/Scripts/Evasion/EvasionBase.cs @@ -0,0 +1,10 @@ +using UnityEngine; + +// Base implementation of an evasion. +public abstract class EvasionBase : IEvasion { + // Determine whether to perform any evasive maneuvers. + public abstract bool ShouldEvade(IAgent agent, IAgent pursuer); + + // Calculate the acceleration input to evade the pursuer. + public abstract Vector3 Evade(IAgent agent, IAgent pursuer); +} diff --git a/Assets/Scripts/Evasion/EvasionBase.cs.meta b/Assets/Scripts/Evasion/EvasionBase.cs.meta new file mode 100644 index 000000000..9677d3749 --- /dev/null +++ b/Assets/Scripts/Evasion/EvasionBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 271e496a090424e89b3f1b68c6d39ffa \ No newline at end of file diff --git a/Assets/Scripts/Evasion/IEvasion.cs b/Assets/Scripts/Evasion/IEvasion.cs new file mode 100644 index 000000000..0d07e2ad6 --- /dev/null +++ b/Assets/Scripts/Evasion/IEvasion.cs @@ -0,0 +1,13 @@ +using UnityEngine; + +// Interface for an evasion. +// +// When a threat detects a pursuing interceptor in its vicinity, it may attempt to evade the +// interceptor before resuming its attack behavior. +public interface IEvasion { + // Determine whether to perform any evasive maneuvers. + bool ShouldEvade(IAgent agent, IAgent pursuer); + + // Calculate the acceleration input to evade the pursuer. + Vector3 Evade(IAgent agent, IAgent pursuer); +} diff --git a/Assets/Scripts/Evasion/IEvasion.cs.meta b/Assets/Scripts/Evasion/IEvasion.cs.meta new file mode 100644 index 000000000..98200a03a --- /dev/null +++ b/Assets/Scripts/Evasion/IEvasion.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 216a866edfd5f4f328c42022f568aae8 \ No newline at end of file diff --git a/Assets/Scripts/Interceptors/CarrierBase.cs b/Assets/Scripts/Interceptors/CarrierBase.cs new file mode 100644 index 000000000..e69de29bb diff --git a/Assets/Scripts/Interceptors/CarrierBase.cs.meta b/Assets/Scripts/Interceptors/CarrierBase.cs.meta new file mode 100644 index 000000000..b60d98d8c --- /dev/null +++ b/Assets/Scripts/Interceptors/CarrierBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: c2b80db88bdf44d2b8fc00dfa62a891d \ No newline at end of file diff --git a/Assets/Scripts/Interceptors/IInterceptor.cs b/Assets/Scripts/Interceptors/IInterceptor.cs new file mode 100644 index 000000000..2f26a2379 --- /dev/null +++ b/Assets/Scripts/Interceptors/IInterceptor.cs @@ -0,0 +1,4 @@ +// Interface for an interceptor. +// +// Interceptors defend the asset against incoming threats. +public interface IInterceptor {} diff --git a/Assets/Scripts/Interceptors/IInterceptor.cs.meta b/Assets/Scripts/Interceptors/IInterceptor.cs.meta new file mode 100644 index 000000000..946385e46 --- /dev/null +++ b/Assets/Scripts/Interceptors/IInterceptor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 783455bb4e0fe4a709c065b912a56925 \ No newline at end of file diff --git a/Assets/Scripts/Interceptors/InterceptorBase.cs b/Assets/Scripts/Interceptors/InterceptorBase.cs new file mode 100644 index 000000000..97441273e --- /dev/null +++ b/Assets/Scripts/Interceptors/InterceptorBase.cs @@ -0,0 +1,2 @@ +// Base implementation of an interceptor. +public abstract class InterceptorBase : AgentBase, IInterceptor {} diff --git a/Assets/Scripts/Interceptors/InterceptorBase.cs.meta b/Assets/Scripts/Interceptors/InterceptorBase.cs.meta new file mode 100644 index 000000000..2dcdc4a60 --- /dev/null +++ b/Assets/Scripts/Interceptors/InterceptorBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 3c76435e719ce4f8492812841e3289b9 \ No newline at end of file diff --git a/Assets/Scripts/Interceptors/LauncherBase.cs b/Assets/Scripts/Interceptors/LauncherBase.cs new file mode 100644 index 000000000..e69de29bb diff --git a/Assets/Scripts/Interceptors/LauncherBase.cs.meta b/Assets/Scripts/Interceptors/LauncherBase.cs.meta new file mode 100644 index 000000000..51ff3f954 --- /dev/null +++ b/Assets/Scripts/Interceptors/LauncherBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 602fc16beda3e428092e40ee95a48425 \ No newline at end of file diff --git a/Assets/Scripts/Threats/IThreat.cs b/Assets/Scripts/Threats/IThreat.cs new file mode 100644 index 000000000..f63155085 --- /dev/null +++ b/Assets/Scripts/Threats/IThreat.cs @@ -0,0 +1,7 @@ +// Interface for a threat. +// +// Threats attempt to reach the asset while avoiding the defending interceptors. +public interface IThreat { + IAttackBehavior AttackBehavior { get; set; } + IEvasion Evasion { get; set; } +} diff --git a/Assets/Scripts/Threats/IThreat.cs.meta b/Assets/Scripts/Threats/IThreat.cs.meta new file mode 100644 index 000000000..860bec993 --- /dev/null +++ b/Assets/Scripts/Threats/IThreat.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 897e26323ba3d40719966c06c8661fa6 \ No newline at end of file diff --git a/Assets/Scripts/Threats/ThreatBase.cs b/Assets/Scripts/Threats/ThreatBase.cs new file mode 100644 index 000000000..53cc11b36 --- /dev/null +++ b/Assets/Scripts/Threats/ThreatBase.cs @@ -0,0 +1,17 @@ +// Base implementation of a threat. +public abstract class ThreatBase : AgentBase, IThreat { + // The attack behavior determines how the threat navigates towards the asset. + private IAttackBehavior _attackBehavior; + + // The evasion handles how the threat behaves in the vicinity of a pursuing interceptor. + private IEvasion _evasion; + + public IAttackBehavior AttackBehavior { + get => _attackBehavior; + set => _attackBehavior = value; + } + public IEvasion Evasion { + get => _evasion; + set => _evasion = value; + } +} diff --git a/Assets/Scripts/Threats/ThreatBase.cs.meta b/Assets/Scripts/Threats/ThreatBase.cs.meta new file mode 100644 index 000000000..74fa97a34 --- /dev/null +++ b/Assets/Scripts/Threats/ThreatBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: fb3070050ee7d465b91899b1716e05e5 \ No newline at end of file From 221e3abf6b71ce38cec8b2a3a6c9954dd49af7b4 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 13:02:38 -0700 Subject: [PATCH 05/74] Maintain a list of pursuers in hierarchical object --- .../Scripts/Hierarchical/HierarchicalBase.cs | 15 +++++++ Assets/Scripts/Hierarchical/IHierarchical.cs | 4 ++ .../Tests/EditMode/HierarchicalBaseTests.cs | 45 +++++++++++++++++++ 3 files changed, 64 insertions(+) diff --git a/Assets/Scripts/Hierarchical/HierarchicalBase.cs b/Assets/Scripts/Hierarchical/HierarchicalBase.cs index 4f53f8bae..1aa6683d8 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalBase.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalBase.cs @@ -16,6 +16,9 @@ public class HierarchicalBase : IHierarchical { // be used by the controller to model imperfect knowledge of the engagement. private IHierarchical _targetModel; + // List of hierarchical objects pursuing the hierarchical object. + private List _pursuers = new List(); + public IReadOnlyList SubHierarchicals => _subHierarchicals.AsReadOnly(); public IHierarchical Target { get => _target; @@ -25,6 +28,8 @@ public IHierarchical TargetModel { get => _targetModel; set => _targetModel = value; } + public IReadOnlyList Pursuers => _pursuers.AsReadOnly(); + public Vector3 Position => GetPosition(); public Vector3 Velocity => GetVelocity(); public float Speed => Velocity.magnitude; @@ -40,6 +45,16 @@ public void RemoveSubHierarchical(IHierarchical subHierarchical) { _subHierarchicals.Remove(subHierarchical); } + public void AddPursuer(IHierarchical pursuer) { + if (!_pursuers.Contains(pursuer)) { + _pursuers.Add(pursuer); + } + } + + public void RemovePursuer(IHierarchical pursuer) { + _pursuers.Remove(pursuer); + } + protected virtual Vector3 GetPosition() { return GetMean(s => s.Position); } diff --git a/Assets/Scripts/Hierarchical/IHierarchical.cs b/Assets/Scripts/Hierarchical/IHierarchical.cs index 2cf012d1d..76271ff0b 100644 --- a/Assets/Scripts/Hierarchical/IHierarchical.cs +++ b/Assets/Scripts/Hierarchical/IHierarchical.cs @@ -12,6 +12,7 @@ public interface IHierarchical { IReadOnlyList SubHierarchicals { get; } IHierarchical Target { get; set; } IHierarchical TargetModel { get; set; } + IReadOnlyList Pursuers { get; } Vector3 Position { get; } Vector3 Velocity { get; } @@ -20,4 +21,7 @@ public interface IHierarchical { void AddSubHierarchical(IHierarchical subHierarchical); void RemoveSubHierarchical(IHierarchical subHierarchical); + + void AddPursuer(IHierarchical pursuer); + void RemovePursuer(IHierarchical pursuer); } diff --git a/Assets/Tests/EditMode/HierarchicalBaseTests.cs b/Assets/Tests/EditMode/HierarchicalBaseTests.cs index 03c8ae67c..997488140 100644 --- a/Assets/Tests/EditMode/HierarchicalBaseTests.cs +++ b/Assets/Tests/EditMode/HierarchicalBaseTests.cs @@ -151,4 +151,49 @@ public void RemoveSubHierarchical_DoesNotRemoveNonExisting() { Assert.AreEqual(1, parent.SubHierarchicals.Count); Assert.AreSame(child1, parent.SubHierarchicals[0]); } + + [Test] + public void AddPursuer_AddsCorrectly() { + var parent = new HierarchicalBase(); + var child = new HierarchicalBase(); + parent.AddPursuer(child); + + Assert.AreEqual(1, parent.Pursuers.Count); + Assert.AreSame(child, parent.Pursuers[0]); + } + + [Test] + public void AddPursuer_DoesNotAddDuplicates() { + var parent = new HierarchicalBase(); + var child = new HierarchicalBase(); + parent.AddPursuer(child); + parent.AddPursuer(child); + + Assert.AreEqual(1, parent.Pursuers.Count); + Assert.AreSame(child, parent.Pursuers[0]); + } + + [Test] + public void RemovePursuer_RemovesCorrectly() { + var parent = new HierarchicalBase(); + var child = new HierarchicalBase(); + + parent.AddPursuer(child); + parent.RemovePursuer(child); + + Assert.AreEqual(0, parent.Pursuers.Count); + } + + [Test] + public void RemovePursuer_DoesNotRemoveNonExisting() { + var parent = new HierarchicalBase(); + var child1 = new HierarchicalBase(); + var child2 = new HierarchicalBase(); + + parent.AddPursuer(child1); + parent.RemovePursuer(child2); + + Assert.AreEqual(1, parent.Pursuers.Count); + Assert.AreSame(child1, parent.Pursuers[0]); + } } From 072c1d34b002caf0176e77d00c6fa763e9b47378 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 13:19:57 -0700 Subject: [PATCH 06/74] Remove flight plan from attack behavior --- Assets/Scripts/Behavior/AttackBehaviorBase.cs | 1 - Assets/Scripts/Behavior/IAttackBehavior.cs | 1 - 2 files changed, 2 deletions(-) diff --git a/Assets/Scripts/Behavior/AttackBehaviorBase.cs b/Assets/Scripts/Behavior/AttackBehaviorBase.cs index db3c41d1a..76c097b2a 100644 --- a/Assets/Scripts/Behavior/AttackBehaviorBase.cs +++ b/Assets/Scripts/Behavior/AttackBehaviorBase.cs @@ -14,7 +14,6 @@ public Configs.AttackBehaviorConfig Config { get => _config; set => _config = value; } - public FlightPlan FlightPlan => new(Config.FlightPlan); // Return the next waypoint for the agent to navigate to and the power setting to use towards the // waypoint. diff --git a/Assets/Scripts/Behavior/IAttackBehavior.cs b/Assets/Scripts/Behavior/IAttackBehavior.cs index 2fd8964e3..c5024a002 100644 --- a/Assets/Scripts/Behavior/IAttackBehavior.cs +++ b/Assets/Scripts/Behavior/IAttackBehavior.cs @@ -5,7 +5,6 @@ // The attack behavior determines how the the threat navigates towards the asset. public interface IAttackBehavior { Configs.AttackBehaviorConfig Config { get; set; } - FlightPlan FlightPlan { get; } // Return the next waypoint for the agent to navigate to and the power setting to use towards the // waypoint. From c58ad6bcb5317bf70c43aeec0bd60b06bbaca575 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 13:17:23 -0700 Subject: [PATCH 07/74] Rename existing Cluster to ClusterLegacy --- .../Clustering/AgglomerativeClusterer.cs | 2 +- .../{Cluster.cs => ClusterLegacy.cs} | 14 +-- ...{Cluster.cs.meta => ClusterLegacy.cs.meta} | 0 .../Algorithms/Clustering/Clusterer.cs | 12 +- .../Algorithms/Clustering/KMeansClusterer.cs | 12 +- Assets/Scripts/IADS/IADS.cs | 21 ++-- Assets/Scripts/IADS/ThreatClusterData.cs | 6 +- .../EditMode/AgglomerativeClustererTests.cs | 5 +- ...{ClusterTests.cs => ClusterLegacyTests.cs} | 22 ++-- ...sts.cs.meta => ClusterLegacyTests.cs.meta} | 0 .../Tests/EditMode/IADSClusterCleanupTests.cs | 110 +++++++++--------- Assets/Tests/EditMode/KMeansClustererTests.cs | 8 +- 12 files changed, 106 insertions(+), 106 deletions(-) rename Assets/Scripts/Algorithms/Clustering/{Cluster.cs => ClusterLegacy.cs} (91%) rename Assets/Scripts/Algorithms/Clustering/{Cluster.cs.meta => ClusterLegacy.cs.meta} (100%) rename Assets/Tests/EditMode/{ClusterTests.cs => ClusterLegacyTests.cs} (80%) rename Assets/Tests/EditMode/{ClusterTests.cs.meta => ClusterLegacyTests.cs.meta} (100%) diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs index 549ad6e62..cc70295c4 100644 --- a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs @@ -14,7 +14,7 @@ public AgglomerativeClusterer(List agents, int maxSize, float maxRadius) public override void Cluster() { // Add a cluster for every game object. foreach (var obj in _objects) { - Cluster cluster = new Cluster(obj); + ClusterLegacy cluster = new ClusterLegacy(obj); cluster.AddObject(obj); _clusters.Add(cluster); } diff --git a/Assets/Scripts/Algorithms/Clustering/Cluster.cs b/Assets/Scripts/Algorithms/Clustering/ClusterLegacy.cs similarity index 91% rename from Assets/Scripts/Algorithms/Clustering/Cluster.cs rename to Assets/Scripts/Algorithms/Clustering/ClusterLegacy.cs index 3606fb211..0cc7644cf 100644 --- a/Assets/Scripts/Algorithms/Clustering/Cluster.cs +++ b/Assets/Scripts/Algorithms/Clustering/ClusterLegacy.cs @@ -4,18 +4,18 @@ using UnityEngine; // The cluster class represents a collection of game objects. -public class Cluster { +public class ClusterLegacy { // Coordinates of the cluster. private Vector3 _coordinates = Vector3.zero; // List of game objects in the cluster. private List _objects = new List(); - public Cluster() {} - public Cluster(in Vector3 coordinates) { + public ClusterLegacy() {} + public ClusterLegacy(in Vector3 coordinates) { _coordinates = coordinates; } - public Cluster(in GameObject obj) { + public ClusterLegacy(GameObject obj) { _coordinates = obj.transform.position; } @@ -104,19 +104,19 @@ public Vector3 Velocity() { // Add a game object to the cluster. // This function does not update the centroid of the cluster. - public void AddObject(in GameObject obj) { + public void AddObject(GameObject obj) { _objects.Add(obj); } // Add multiple game objects to the cluster. // This function does not update the centroid of the cluster. - public void AddObjects(in IReadOnlyList objects) { + public void AddObjects(IReadOnlyList objects) { _objects.AddRange(objects); } // Merge another cluster into this one. // This function does not update the centroid of the cluster. - public void Merge(in Cluster cluster) { + public void Merge(ClusterLegacy cluster) { AddObjects(cluster.Objects); } diff --git a/Assets/Scripts/Algorithms/Clustering/Cluster.cs.meta b/Assets/Scripts/Algorithms/Clustering/ClusterLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Algorithms/Clustering/Cluster.cs.meta rename to Assets/Scripts/Algorithms/Clustering/ClusterLegacy.cs.meta diff --git a/Assets/Scripts/Algorithms/Clustering/Clusterer.cs b/Assets/Scripts/Algorithms/Clustering/Clusterer.cs index ad101e601..b36216355 100644 --- a/Assets/Scripts/Algorithms/Clustering/Clusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/Clusterer.cs @@ -4,17 +4,17 @@ using UnityEngine; // The clusterer class is an interface for clustering algorithms. -public abstract class IClusterer { +public abstract class IClustererLegacy { // List of game objects to cluster. protected List _objects = new List(); // List of clusters. - protected List _clusters = new List(); + protected List _clusters = new List(); - public IClusterer(List objects) { + public IClustererLegacy(List objects) { _objects = objects; } - public IClusterer(List agents) { + public IClustererLegacy(List agents) { _objects = agents.ConvertAll(agent => agent.gameObject).ToList(); } @@ -24,7 +24,7 @@ public IReadOnlyList Objects { } // Get the list of clusters. - public IReadOnlyList Clusters { + public IReadOnlyList Clusters { get { return _clusters; } } @@ -36,7 +36,7 @@ public IReadOnlyList Clusters { // size and radius constraints. The size is defined as the maximum number of game objects within a // cluster, and the radius denotes the maximum distance from the cluster's centroid to any of its // assigned game objects. -public abstract class ISizeAndRadiusConstrainedClusterer : IClusterer { +public abstract class ISizeAndRadiusConstrainedClusterer : IClustererLegacy { // Maximum cluster size. protected readonly int _maxSize = 0; diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs index 6471f5691..9f6ad0ff6 100644 --- a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs @@ -4,7 +4,7 @@ using UnityEngine; // The k-means clusterer class performs k-means clustering. -public class KMeansClusterer : IClusterer { +public class KMeansClusterer : IClustererLegacy { public const float Epsilon = 1e-3f; // Number of clusters. @@ -32,7 +32,7 @@ public override void Cluster() { (_objects[i], _objects[j]) = (_objects[j], _objects[i]); } for (int i = _objects.Count - 1; i >= _objects.Count - _k; --i) { - _clusters.Add(new Cluster(_objects[i])); + _clusters.Add(new ClusterLegacy(_objects[i])); } bool converged = false; @@ -43,12 +43,12 @@ public override void Cluster() { // Calculate the new clusters as the mean of all assigned game objects. converged = true; for (int clusterIndex = 0; clusterIndex < _clusters.Count; ++clusterIndex) { - Cluster newCluster; + ClusterLegacy newCluster; if (_clusters[clusterIndex].IsEmpty()) { int objectIndex = random.Next(_objects.Count); - newCluster = new Cluster(_objects[objectIndex]); + newCluster = new ClusterLegacy(_objects[objectIndex]); } else { - newCluster = new Cluster(_clusters[clusterIndex].Centroid()); + newCluster = new ClusterLegacy(_clusters[clusterIndex].Centroid()); } // Check whether the algorithm has converged by checking whether the cluster has moved. @@ -118,6 +118,6 @@ public override void Cluster() { numClusters += (int)Mathf.Ceil(Mathf.Max(numOverPopulatedClusters, numOverSizedClusters) / 2.0f); } - _clusters = new List(clusterer.Clusters); + _clusters = new List(clusterer.Clusters); } } diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index d3427d92c..fa82b749e 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -24,11 +24,11 @@ public class IADS : MonoBehaviour { private Dictionary _trackFileMap = new Dictionary(); private List _assignmentQueue = new List(); - private List _threatClusters = new List(); - private Dictionary _threatClusterMap = - new Dictionary(); - private Dictionary _interceptorClusterMap = - new Dictionary(); + private List _threatClusters = new List(); + private Dictionary _threatClusterMap = + new Dictionary(); + private Dictionary _interceptorClusterMap = + new Dictionary(); private HashSet _assignableInterceptors = new HashSet(); private HashSet _threatsToCluster = new HashSet(); @@ -161,7 +161,7 @@ public bool ShouldLaunchSubmunitions(Interceptor carrier) { // as the boost time. const float SubmunitionSpawnPredictionTime = 0.6f; - Cluster cluster = _interceptorClusterMap[carrier]; + ClusterLegacy cluster = _interceptorClusterMap[carrier]; List threats = cluster.Threats.ToList(); Vector3 carrierPosition = carrier.GetPosition(); Vector3 carrierVelocity = carrier.GetVelocity(); @@ -196,7 +196,7 @@ private bool HasClusterAssignment(Interceptor interceptor) { // Releases interceptors from clusters that only contain destroyed threats. private void CleanupDestroyedClusters() { - foreach (Cluster cluster in _threatClusters) { + foreach (var cluster in _threatClusters) { if (!cluster.IsFullyTerminated()) { continue; } @@ -215,7 +215,7 @@ private void CleanupDestroyedClusters() { // Detach a single interceptor from a cluster and decide its next action. // All interceptors are queued for reassignment in the hierarchical architecture, // including carrier interceptors. - private void ReleaseInterceptorFromCluster(Cluster cluster, Interceptor interceptor) { + private void ReleaseInterceptorFromCluster(ClusterLegacy cluster, Interceptor interceptor) { _threatClusterMap[cluster].RemoveInterceptor(interceptor); _interceptorClusterMap.Remove(interceptor); // TODO(titan): During the hierarchical architecture overhaul/refactor, @@ -228,7 +228,7 @@ private void ReleaseInterceptorFromCluster(Cluster cluster, Interceptor intercep public void AssignSubmunitionsToThreats(Interceptor carrier, List interceptors) { // Assign threats to the submunitions. - Cluster cluster = _interceptorClusterMap[carrier]; + ClusterLegacy cluster = _interceptorClusterMap[carrier]; List threats = cluster.Threats.ToList(); IEnumerable assignments = _assignmentScheme.Assign(interceptors, threats); @@ -342,7 +342,8 @@ private void ClusterThreats() { } // Cluster threats. - IClusterer clusterer = new AgglomerativeClusterer(new List(threats), MaxSize, MaxRadius); + IClustererLegacy clusterer = + new AgglomerativeClusterer(new List(threats), MaxSize, MaxRadius); clusterer.Cluster(); var clusters = clusterer.Clusters; Debug.Log($"Clustered {threats.Count} threats into {clusters.Count} clusters."); diff --git a/Assets/Scripts/IADS/ThreatClusterData.cs b/Assets/Scripts/IADS/ThreatClusterData.cs index be031db2a..0eefa5904 100644 --- a/Assets/Scripts/IADS/ThreatClusterData.cs +++ b/Assets/Scripts/IADS/ThreatClusterData.cs @@ -7,7 +7,7 @@ public enum ThreatClusterStatus { UNASSIGNED, ASSIGNED, DELEGATED } [System.Serializable] public class ThreatClusterData { - private Cluster _cluster; + private ClusterLegacy _cluster; // The agent tracks the centroid of the cluster. private Agent _agent; @@ -18,12 +18,12 @@ public class ThreatClusterData { [SerializeField] private List _assignedInterceptors = new List(); - public ThreatClusterData(Cluster cluster) { + public ThreatClusterData(ClusterLegacy cluster) { _cluster = cluster; _agent = SimManager.Instance.CreateDummyAgent(cluster.Centroid(), cluster.Velocity()); } - public Cluster Cluster { + public ClusterLegacy Cluster { get { return _cluster; } } diff --git a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs index e94aeec21..0608e2e57 100644 --- a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs +++ b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs @@ -25,7 +25,7 @@ public void TestSingleCluster() { new AgglomerativeClusterer(Objects, maxSize: Objects.Count, maxRadius: Mathf.Infinity); clusterer.Cluster(); Assert.AreEqual(1, clusterer.Clusters.Count); - Cluster cluster = clusterer.Clusters[0]; + ClusterLegacy cluster = clusterer.Clusters[0]; Assert.AreEqual(Objects.Count, cluster.Size()); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid()); } @@ -58,7 +58,8 @@ public void TestSmallRadius() { new AgglomerativeClusterer(Objects, maxSize: Objects.Count, maxRadius: 1); clusterer.Cluster(); Assert.AreEqual(3, clusterer.Clusters.Count); - List clusters = clusterer.Clusters.OrderBy(cluster => cluster.Coordinates[1]).ToList(); + List clusters = + clusterer.Clusters.OrderBy(cluster => cluster.Coordinates[1]).ToList(); Assert.AreEqual(1, clusters[0].Size()); Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Coordinates); Assert.AreEqual(2, clusters[1].Size()); diff --git a/Assets/Tests/EditMode/ClusterTests.cs b/Assets/Tests/EditMode/ClusterLegacyTests.cs similarity index 80% rename from Assets/Tests/EditMode/ClusterTests.cs rename to Assets/Tests/EditMode/ClusterLegacyTests.cs index b8ba1b8a9..31bba948c 100644 --- a/Assets/Tests/EditMode/ClusterTests.cs +++ b/Assets/Tests/EditMode/ClusterLegacyTests.cs @@ -4,15 +4,15 @@ using UnityEngine; using UnityEngine.TestTools; -public class ClusterTests { +public class ClusterLegacyTests { public static GameObject GenerateObject(in Vector3 position) { GameObject obj = new GameObject(); obj.transform.position = position; return obj; } - public static Cluster GenerateCluster(in IReadOnlyList objects) { - Cluster cluster = new Cluster(); + public static ClusterLegacy GenerateCluster(in IReadOnlyList objects) { + ClusterLegacy cluster = new ClusterLegacy(); cluster.AddObjects(objects); cluster.Recenter(); return cluster; @@ -25,16 +25,16 @@ public void TestSize() { for (int i = 0; i < size; ++i) { objects.Add(GenerateObject(new Vector3(0, i, 0))); } - Cluster cluster = GenerateCluster(objects); + ClusterLegacy cluster = GenerateCluster(objects); Assert.AreEqual(size, cluster.Size()); } [Test] public void TestIsEmpty() { - Cluster emptyCluster = new Cluster(); + ClusterLegacy emptyCluster = new ClusterLegacy(); Assert.IsTrue(emptyCluster.IsEmpty()); - Cluster cluster = new Cluster(); + ClusterLegacy cluster = new ClusterLegacy(); cluster.AddObject(new GameObject()); Assert.IsFalse(cluster.IsEmpty()); } @@ -45,7 +45,7 @@ public void TestRadius() { List objects = new List(); objects.Add(GenerateObject(new Vector3(0, radius, 0))); objects.Add(GenerateObject(new Vector3(0, -radius, 0))); - Cluster cluster = GenerateCluster(objects); + ClusterLegacy cluster = GenerateCluster(objects); Assert.AreEqual(radius, cluster.Radius()); } @@ -57,7 +57,7 @@ public void TestCentroid() { objects.Add(GenerateObject(new Vector3(i, j, 0))); } } - Cluster cluster = GenerateCluster(objects); + ClusterLegacy cluster = GenerateCluster(objects); Assert.AreEqual(Vector3.zero, cluster.Centroid()); } @@ -69,7 +69,7 @@ public void TestRecenter() { objects.Add(GenerateObject(new Vector3(i, j, 0))); } } - Cluster cluster = GenerateCluster(objects); + ClusterLegacy cluster = GenerateCluster(objects); cluster.AddObject(GenerateObject(new Vector3(10, -10, 0))); Assert.AreNotEqual(new Vector3(1, -1, 0), cluster.Coordinates); cluster.Recenter(); @@ -85,8 +85,8 @@ public void TestMerge() { objects1.Add(GenerateObject(new Vector3(0, i, 0))); objects2.Add(GenerateObject(new Vector3(i, 0, 0))); } - Cluster cluster1 = GenerateCluster(objects1); - Cluster cluster2 = GenerateCluster(objects2); + ClusterLegacy cluster1 = GenerateCluster(objects1); + ClusterLegacy cluster2 = GenerateCluster(objects2); int size1 = cluster1.Size(); int size2 = cluster2.Size(); Vector3 centroid1 = cluster1.Centroid(); diff --git a/Assets/Tests/EditMode/ClusterTests.cs.meta b/Assets/Tests/EditMode/ClusterLegacyTests.cs.meta similarity index 100% rename from Assets/Tests/EditMode/ClusterTests.cs.meta rename to Assets/Tests/EditMode/ClusterLegacyTests.cs.meta diff --git a/Assets/Tests/EditMode/IADSClusterCleanupTests.cs b/Assets/Tests/EditMode/IADSClusterCleanupTests.cs index 85c83baee..ff1d1f481 100644 --- a/Assets/Tests/EditMode/IADSClusterCleanupTests.cs +++ b/Assets/Tests/EditMode/IADSClusterCleanupTests.cs @@ -23,75 +23,72 @@ public override void Teardown() { } private AgentConfig CreateThreatConfig() => new AgentConfig { - ConfigFile = "brahmos.pbtxt", - AttackBehaviorConfigFile = "brahmos_direct_attack.pbtxt", - InitialState = new Simulation.State { - Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, - Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } - }, - StandardDeviation = new Simulation.State { - Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, - Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } - }, - DynamicConfig = new DynamicConfig { - SensorConfig = new Simulation.SensorConfig { - Type = Simulation.SensorType.Ideal, - Frequency = 10 - } - } + ConfigFile = "brahmos.pbtxt", AttackBehaviorConfigFile = "brahmos_direct_attack.pbtxt", + InitialState = + new Simulation.State { + Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, + Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } + }, + StandardDeviation = + new Simulation.State { + Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, + Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } + }, + DynamicConfig = new DynamicConfig { SensorConfig = + new Simulation.SensorConfig { + Type = Simulation.SensorType.Ideal, Frequency = 10 + } } }; private AgentConfig CreateCarrierConfig() => new AgentConfig { ConfigFile = "hydra70.pbtxt", - InitialState = new Simulation.State { - Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, - Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 50 } - }, - StandardDeviation = new Simulation.State { - Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, - Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } - }, - DynamicConfig = new DynamicConfig { - SensorConfig = new Simulation.SensorConfig { - Type = Simulation.SensorType.Ideal, - Frequency = 10 - }, - FlightConfig = new FlightConfig { - ControllerType = ControllerType.ProportionalNavigation - } - } + InitialState = + new Simulation.State { + Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, + Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 50 } + }, + StandardDeviation = + new Simulation.State { + Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, + Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } + }, + DynamicConfig = + new DynamicConfig { + SensorConfig = new Simulation.SensorConfig { Type = Simulation.SensorType.Ideal, + Frequency = 10 }, + FlightConfig = new FlightConfig { ControllerType = ControllerType.ProportionalNavigation } + } }; private AgentConfig CreateMissileConfig() => new AgentConfig { ConfigFile = "micromissile.pbtxt", - InitialState = new Simulation.State { - Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, - Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 50 } - }, - StandardDeviation = new Simulation.State { - Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, - Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } - }, - DynamicConfig = new DynamicConfig { - SensorConfig = new Simulation.SensorConfig { - Type = Simulation.SensorType.Ideal, - Frequency = 10 - }, - FlightConfig = new FlightConfig { - ControllerType = ControllerType.ProportionalNavigation - } - } + InitialState = + new Simulation.State { + Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, + Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 50 } + }, + StandardDeviation = + new Simulation.State { + Position = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 }, + Velocity = new Simulation.CartesianCoordinates { X = 0, Y = 0, Z = 0 } + }, + DynamicConfig = + new DynamicConfig { + SensorConfig = new Simulation.SensorConfig { Type = Simulation.SensorType.Ideal, + Frequency = 10 }, + FlightConfig = new FlightConfig { ControllerType = ControllerType.ProportionalNavigation } + } }; - private (Cluster, ThreatClusterData) MakeClusterWithThreats(params Threat[] threats) { - var cluster = new Cluster(); + private (ClusterLegacy, ThreatClusterData) MakeClusterWithThreats(params Threat[] threats) { + var cluster = new ClusterLegacy(); foreach (var threat in threats) { cluster.AddObject(threat.gameObject); } var threatClusterData = new ThreatClusterData(cluster); - var clusters = new List { cluster }; + var clusters = new List { cluster }; SetPrivateField(_iads, "_threatClusters", clusters); - var map = new Dictionary { [cluster] = threatClusterData }; + var map = new Dictionary { [cluster] = threatClusterData }; SetPrivateField(_iads, "_threatClusterMap", map); return (cluster, threatClusterData); } @@ -102,7 +99,7 @@ public void Carrier_Unassigned_WhenClusterFullyDestroyed() { var (cluster, threatClusterData) = MakeClusterWithThreats(threat); var carrier = (CarrierInterceptor)CreateTestInterceptor(CreateCarrierConfig()); - var interceptorClusterMap = new Dictionary { [carrier] = cluster }; + var interceptorClusterMap = new Dictionary { [carrier] = cluster }; SetPrivateField(_iads, "_interceptorClusterMap", interceptorClusterMap); carrier.AssignTarget(threatClusterData.Centroid); @@ -113,7 +110,8 @@ public void Carrier_Unassigned_WhenClusterFullyDestroyed() { Assert.IsFalse(carrier.HasAssignedTarget(), "Carrier should be unassigned (ballistic)."); - var map = GetPrivateField>(_iads, "_interceptorClusterMap"); + var map = + GetPrivateField>(_iads, "_interceptorClusterMap"); Assert.IsFalse(map.ContainsKey(carrier), "Carrier should be removed from cluster mapping."); Assert.IsFalse(_iads.ShouldLaunchSubmunitions(carrier), diff --git a/Assets/Tests/EditMode/KMeansClustererTests.cs b/Assets/Tests/EditMode/KMeansClustererTests.cs index 077149146..7cf68b41f 100644 --- a/Assets/Tests/EditMode/KMeansClustererTests.cs +++ b/Assets/Tests/EditMode/KMeansClustererTests.cs @@ -23,7 +23,7 @@ public static GameObject GenerateObject(in Vector3 position) { public void TestSingleCluster() { KMeansClusterer clusterer = new KMeansClusterer(Objects, k: 1); clusterer.Cluster(); - Cluster cluster = clusterer.Clusters[0]; + ClusterLegacy cluster = clusterer.Clusters[0]; Assert.AreEqual(Objects.Count, cluster.Size()); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Coordinates); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid()); @@ -61,8 +61,8 @@ public void TestTwoDistinctClustersWithResetNeeded() { Assert.AreEqual(2, clusterer.Clusters.Count); // Retrieve the clusters. - Cluster c0 = clusterer.Clusters[0]; - Cluster c1 = clusterer.Clusters[1]; + ClusterLegacy c0 = clusterer.Clusters[0]; + ClusterLegacy c1 = clusterer.Clusters[1]; // Because the clusters are well-separated, each cluster should contain all points from one // group or the other, not a mixture. Check via centroids. @@ -113,7 +113,7 @@ public void TestSingleCluster() { new ConstrainedKMeansClusterer(Objects, maxSize: Objects.Count, maxRadius: Mathf.Infinity); clusterer.Cluster(); Assert.AreEqual(1, clusterer.Clusters.Count); - Cluster cluster = clusterer.Clusters[0]; + ClusterLegacy cluster = clusterer.Clusters[0]; Assert.AreEqual(Objects.Count, cluster.Size()); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid()); } From 6639b6fefdea0928e24c8e5ea21bdde7b1c9121c Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 14:45:31 -0700 Subject: [PATCH 08/74] Add clustering algorithm interface and base implementations --- .../Scripts/Algorithms/Clustering/Cluster.cs | 35 ++++++ .../Algorithms/Clustering/Cluster.cs.meta | 2 + .../Algorithms/Clustering/ClustererBase.cs | 21 ++++ .../Clustering/ClustererBase.cs.meta | 2 + .../Algorithms/Clustering/IClusterer.cs | 12 ++ .../Algorithms/Clustering/IClusterer.cs.meta | 2 + .../SizeAndRadiusConstrainedClustererBase.cs | 21 ++++ ...eAndRadiusConstrainedClustererBase.cs.meta | 2 + .../Scripts/Hierarchical/FixedHierarchical.cs | 27 ++++ .../Hierarchical/FixedHierarchical.cs.meta | 2 + Assets/Tests/EditMode/ClusterTests.cs | 117 ++++++++++++++++++ Assets/Tests/EditMode/ClusterTests.cs.meta | 2 + .../Tests/EditMode/HierarchicalBaseTests.cs | 24 ---- 13 files changed, 245 insertions(+), 24 deletions(-) create mode 100644 Assets/Scripts/Algorithms/Clustering/Cluster.cs create mode 100644 Assets/Scripts/Algorithms/Clustering/Cluster.cs.meta create mode 100644 Assets/Scripts/Algorithms/Clustering/ClustererBase.cs create mode 100644 Assets/Scripts/Algorithms/Clustering/ClustererBase.cs.meta create mode 100644 Assets/Scripts/Algorithms/Clustering/IClusterer.cs create mode 100644 Assets/Scripts/Algorithms/Clustering/IClusterer.cs.meta create mode 100644 Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs create mode 100644 Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs.meta create mode 100644 Assets/Scripts/Hierarchical/FixedHierarchical.cs create mode 100644 Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta create mode 100644 Assets/Tests/EditMode/ClusterTests.cs create mode 100644 Assets/Tests/EditMode/ClusterTests.cs.meta diff --git a/Assets/Scripts/Algorithms/Clustering/Cluster.cs b/Assets/Scripts/Algorithms/Clustering/Cluster.cs new file mode 100644 index 000000000..5bd81d416 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/Cluster.cs @@ -0,0 +1,35 @@ +using System.Linq; +using UnityEngine; + +// The cluster represents a collection of hierarchical objects. +public class Cluster : HierarchicalBase { + [SerializeField] + private Vector3 _centroid; + + public Vector3 Centroid { + get => _centroid; + set => _centroid = value; + } + + public int Size => SubHierarchicals.Count; + public bool IsEmpty => Size == 0; + + public float Radius() { + if (IsEmpty) { + return 0; + } + return SubHierarchicals.Max(subHierarchical => + Vector3.Distance(Centroid, subHierarchical.Position)); + } + + public void Recenter() { + Centroid = Position; + } + + // Merging with another cluster does not update the centroid of the current cluster. + public void Merge(Cluster cluster) { + foreach (var subHierarchical in cluster.SubHierarchicals) { + AddSubHierarchical(subHierarchical); + } + } +} diff --git a/Assets/Scripts/Algorithms/Clustering/Cluster.cs.meta b/Assets/Scripts/Algorithms/Clustering/Cluster.cs.meta new file mode 100644 index 000000000..d4b364e15 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/Cluster.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 5725982843f30498691b635e7c2a9136 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs new file mode 100644 index 000000000..70a829584 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs @@ -0,0 +1,21 @@ +using System.Collections.Generic; +using System.Linq; + +// Base implementation of a clustering algorithm. +public abstract class ClustererBase : IClusterer { + // list of hierarchical objects to cluster. + protected List _objects = new List(); + + // list of generated clusters. + protected List _clusters = new List(); + + public ClustererBase(IEnumerable objects) { + _objects = objects.ToList(); + } + + public IReadOnlyList Objects => _objects.AsReadOnly(); + public IReadOnlyList Clusters => _clusters.AsReadOnly(); + + // Generate the clusters from the list of hierarchical objects. + public abstract void Cluster(); +} diff --git a/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs.meta b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs.meta new file mode 100644 index 000000000..cef409fb4 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e6dd3130883f8412599006d1f8db3bce \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Clustering/IClusterer.cs b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs new file mode 100644 index 000000000..0cf9e7b29 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs @@ -0,0 +1,12 @@ +using System.Collections.Generic; + +// Interface for a clustering algorithm. +// +// The clustering algorithm generaters a list of clusters given a list of hierarchical objects. +public interface IClusterer { + IReadOnlyList Objects { get; } + IReadOnlyList Clusters { get; } + + // Generate the clusters from the list of hierarchical objects. + void Cluster(); +} diff --git a/Assets/Scripts/Algorithms/Clustering/IClusterer.cs.meta b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs.meta new file mode 100644 index 000000000..839efd962 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 6cd89f9e8ec1249cc8b3739e8547ec69 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs new file mode 100644 index 000000000..ff9f82e11 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs @@ -0,0 +1,21 @@ +using System.Collections.Generic; + +// Base implementation of a size and radius-constrained clustering algorithm. +// +// The size is defined as the maximum number of hierarchical objects within a cluster, and the +// radius denotes the maximum distance from the cluster's centroid to any of its assigned +// hierarchical objects. +public abstract class SizeAndRadiusConstrainedClustererBase : ClustererBase { + // Maximum cluster size. + protected readonly int _maxSize = 0; + + // Maximum cluster radius. + protected readonly float _maxRadius = 0; + + public SizeAndRadiusConstrainedClustererBase(IEnumerable objects, int maxSize, + float maxRadius) + : base(objects) { + _maxSize = maxSize; + _maxRadius = maxRadius; + } +} diff --git a/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs.meta b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs.meta new file mode 100644 index 000000000..c6753530d --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: c4b6ea391730d4b23a218e9ce991ba79 \ No newline at end of file diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs b/Assets/Scripts/Hierarchical/FixedHierarchical.cs new file mode 100644 index 000000000..3e29fd3d7 --- /dev/null +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs @@ -0,0 +1,27 @@ +using UnityEngine; + +// The fixed hierarchical object represents a hierarchical object with a fixed position, velocity, +// and acceleration and should primarily be used for testing. +public class FixedHierarchical : HierarchicalBase { + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _acceleration; + + public FixedHierarchical(in Vector3 position, in Vector3 velocity) + : this(position, velocity, Vector3.zero) {} + public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { + _position = position; + _velocity = velocity; + _acceleration = acceleration; + } + + protected override Vector3 GetPosition() { + return _position; + } + protected override Vector3 GetVelocity() { + return _velocity; + } + protected override Vector3 GetAcceleration() { + return _acceleration; + } +} diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta b/Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta new file mode 100644 index 000000000..dea21cd2e --- /dev/null +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 76ccd61f72bad430a896ca4a5b9490b9 \ No newline at end of file diff --git a/Assets/Tests/EditMode/ClusterTests.cs b/Assets/Tests/EditMode/ClusterTests.cs new file mode 100644 index 000000000..1c200e7c0 --- /dev/null +++ b/Assets/Tests/EditMode/ClusterTests.cs @@ -0,0 +1,117 @@ +using NUnit.Framework; +using System.Collections.Generic; +using UnityEngine; + +public class ClusterTests { + private const float _epsilon = 1e-3f; + + private static FixedHierarchical GenerateObject(in Vector3 position) { + return new FixedHierarchical(position, Vector3.zero, Vector3.zero); + } + + private static Cluster GenerateCluster(IReadOnlyList objects) { + Cluster cluster = new Cluster(); + foreach (var obj in objects) { + cluster.AddSubHierarchical(obj); + } + cluster.Recenter(); + return cluster; + } + + [Test] + public void Centroid_SetAndGets_WorksCorrectly() { + var cluster = new Cluster(); + var centroid = new Vector3(5, 1, -3); + cluster.Centroid = centroid; + Assert.AreEqual(centroid, cluster.Centroid); + } + + [Test] + public void Size_ReturnsCorrectly() { + const int size = 10; + var objects = new List(); + for (int i = 0; i < size; ++i) { + objects.Add(GenerateObject(new Vector3(0, i, 0))); + } + Cluster cluster = GenerateCluster(objects); + Assert.AreEqual(size, cluster.Size); + } + + [Test] + public void IsEmpty_ReturnsCorrectly() { + var cluster = new Cluster(); + Assert.IsTrue(cluster.IsEmpty); + cluster.AddSubHierarchical(GenerateObject(Vector3.zero)); + Assert.IsFalse(cluster.IsEmpty); + } + + [Test] + public void Radius_ReturnsDistanceFromCentroidToFarthest() { + const int size = 10; + var objects = new List(); + for (int i = 0; i < size; ++i) { + objects.Add(GenerateObject(new Vector3(0, i, 0))); + } + Cluster cluster = GenerateCluster(objects); + var centroid = size / 2 - 2; + cluster.Centroid = new Vector3(0, centroid, 0); + Assert.AreEqual(size - 1 - centroid, cluster.Radius(), _epsilon); + } + + [Test] + public void Radius_With_NoSubHierarchicals_ReturnsZero() { + var cluster = new Cluster(); + Assert.AreEqual(0f, cluster.Radius(), _epsilon); + } + + [Test] + public void Recenter_SetsCentroidToMeanOfSubHierarchicalPositions() { + var objects = new List(); + for (int i = -1; i <= 1; ++i) { + for (int j = -1; j <= 1; ++j) { + objects.Add(GenerateObject(new Vector3(i, j, 0))); + } + } + var cluster = GenerateCluster(objects); + cluster.AddSubHierarchical(GenerateObject(new Vector3(10, -10, 0))); + Assert.AreNotEqual(new Vector3(1, -1, 0), cluster.Centroid); + cluster.Recenter(); + Assert.AreEqual(new Vector3(1, -1, 0), cluster.Centroid); + } + + [Test] + public void Merge_CombinesAllSubHierarchicals() { + const int size = 10; + var objects1 = new List(); + var objects2 = new List(); + for (int i = 0; i < size; ++i) { + objects1.Add(GenerateObject(new Vector3(0, i, 0))); + objects2.Add(GenerateObject(new Vector3(i, 0, 0))); + } + var cluster1 = GenerateCluster(objects1); + var cluster2 = GenerateCluster(objects2); + int size1 = cluster1.Size; + int size2 = cluster2.Size; + cluster1.Merge(cluster2); + Assert.AreEqual(size1 + size2, cluster1.Size); + } + + [Test] + public void Merge_DoesNotUpdateCentroid() { + const int size = 10; + var objects1 = new List(); + var objects2 = new List(); + for (int i = 0; i < size; ++i) { + objects1.Add(GenerateObject(new Vector3(0, i, 0))); + objects2.Add(GenerateObject(new Vector3(i, 0, 0))); + } + var cluster1 = GenerateCluster(objects1); + var cluster2 = GenerateCluster(objects2); + var centroid1 = cluster1.Centroid; + var centroid2 = cluster2.Centroid; + cluster1.Merge(cluster2); + Assert.AreNotEqual((centroid1 + centroid2) / 2, cluster1.Centroid); + cluster1.Recenter(); + Assert.AreEqual((centroid1 + centroid2) / 2, cluster1.Centroid); + } +} diff --git a/Assets/Tests/EditMode/ClusterTests.cs.meta b/Assets/Tests/EditMode/ClusterTests.cs.meta new file mode 100644 index 000000000..1176156ae --- /dev/null +++ b/Assets/Tests/EditMode/ClusterTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 1bbfb12efbebc4cf19750d7d969fbc54 \ No newline at end of file diff --git a/Assets/Tests/EditMode/HierarchicalBaseTests.cs b/Assets/Tests/EditMode/HierarchicalBaseTests.cs index 997488140..2bfa00299 100644 --- a/Assets/Tests/EditMode/HierarchicalBaseTests.cs +++ b/Assets/Tests/EditMode/HierarchicalBaseTests.cs @@ -5,30 +5,6 @@ public class HierarchicalBaseTests { private const float _epsilon = 1e-3f; - private class FixedHierarchical : HierarchicalBase { - private Vector3 _position; - private Vector3 _velocity; - private Vector3 _acceleration; - - public FixedHierarchical(in Vector3 position, in Vector3 velocity) - : this(position, velocity, Vector3.zero) {} - public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { - _position = position; - _velocity = velocity; - _acceleration = acceleration; - } - - protected override Vector3 GetPosition() { - return _position; - } - protected override Vector3 GetVelocity() { - return _velocity; - } - protected override Vector3 GetAcceleration() { - return _acceleration; - } - } - [Test] public void Target_SetAndGet_WorksCorrectly() { var parent = new HierarchicalBase(); From 8d011737e69dcd52a7b07482076fe37719760406 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 14:51:23 -0700 Subject: [PATCH 09/74] Add legacy to PredictorState and IPredictor --- .../Planning/IterativeLaunchPlanner.cs | 6 +++--- .../Algorithms/Planning/LaunchPlanner.cs | 4 ++-- .../Prediction/LinearExtrapolator.cs | 6 +++--- .../Scripts/Algorithms/Prediction/Predictor.cs | 18 +++++++++--------- Assets/Scripts/IADS/IADS.cs | 6 +++--- .../Tests/EditMode/LinearExtrapolatorTests.cs | 6 +++--- 6 files changed, 23 insertions(+), 23 deletions(-) diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs index ed5659a14..066668910 100644 --- a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs +++ b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs @@ -23,12 +23,12 @@ public class IterativeLaunchPlanner : ILaunchPlanner { // granularity of the possible intercept positions. private const float InterceptPositionThreshold = 1000f; - public IterativeLaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictor predictor) + public IterativeLaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictorLegacy predictor) : base(launchAnglePlanner, predictor) {} // Plan the launch. public override LaunchPlan Plan() { - PredictorState initialState = _predictor.Predict(time: 0); + var initialState = _predictor.Predict(time: 0); Vector3 targetPosition = initialState.Position; LaunchAngleOutput launchAngleOutput = new LaunchAngleOutput(); @@ -39,7 +39,7 @@ public override LaunchPlan Plan() { float timeToIntercept = launchAngleOutput.TimeToPosition; // Estimate the target position. - PredictorState predictedState = _predictor.Predict(timeToIntercept); + var predictedState = _predictor.Predict(timeToIntercept); targetPosition = predictedState.Position; // Check whether the intercept direction has changed, in which case the algorithm has diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs index 189af3114..c47bf4034 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs @@ -38,9 +38,9 @@ public abstract class ILaunchPlanner { protected ILaunchAnglePlanner _launchAnglePlanner; // Agent trajectory predictor. - protected IPredictor _predictor; + protected IPredictorLegacy _predictor; - public ILaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictor predictor) { + public ILaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictorLegacy predictor) { _launchAnglePlanner = launchAnglePlanner; _predictor = predictor; } diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs index 9e51dc49a..047dde9ef 100644 --- a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs +++ b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs @@ -3,12 +3,12 @@ // The linear extrapolator class predicts the trajectory of an agent by linearly extrapolating its // current position and velocity. -public class LinearExtrapolator : IPredictor { +public class LinearExtrapolator : IPredictorLegacy { public LinearExtrapolator(in Agent agent) : base(agent) {} // Predict the state at the given time. - public override PredictorState Predict(float time) { + public override PredictorStateLegacy Predict(float time) { Vector3 position = _state.Position + _state.Velocity * time; - return new PredictorState(position, _state.Velocity, _state.Acceleration); + return new PredictorStateLegacy(position, _state.Velocity, _state.Acceleration); } } diff --git a/Assets/Scripts/Algorithms/Prediction/Predictor.cs b/Assets/Scripts/Algorithms/Prediction/Predictor.cs index 6c24a59be..accb3069d 100644 --- a/Assets/Scripts/Algorithms/Prediction/Predictor.cs +++ b/Assets/Scripts/Algorithms/Prediction/Predictor.cs @@ -1,6 +1,6 @@ using UnityEngine; -public class PredictorState { +public class PredictorStateLegacy { // Position. public Vector3 Position { get; } @@ -10,13 +10,13 @@ public class PredictorState { // Acceleration. public Vector3 Acceleration { get; } - public PredictorState() {} - public PredictorState(Agent agent) { + public PredictorStateLegacy() {} + public PredictorStateLegacy(Agent agent) { Position = agent.GetPosition(); Velocity = agent.GetVelocity(); Acceleration = agent.GetAcceleration(); } - public PredictorState(Vector3 position, Vector3 velocity, Vector3 acceleration) { + public PredictorStateLegacy(Vector3 position, Vector3 velocity, Vector3 acceleration) { Position = position; Velocity = velocity; Acceleration = acceleration; @@ -24,14 +24,14 @@ public PredictorState(Vector3 position, Vector3 velocity, Vector3 acceleration) } // The predictor class is an interface for predicting the trajectories of agents. -public abstract class IPredictor { +public abstract class IPredictorLegacy { // Agent state. - protected PredictorState _state; + protected PredictorStateLegacy _state; - public IPredictor(in Agent agent) { - _state = new PredictorState(agent); + public IPredictorLegacy(Agent agent) { + _state = new PredictorStateLegacy(agent); } // Predict the state at the given time. - public abstract PredictorState Predict(float time); + public abstract PredictorStateLegacy Predict(float time); } diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index fa82b749e..753510902 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -109,7 +109,7 @@ private void CheckAndLaunchInterceptors() { } // Create a predictor to track the cluster's centroid. - IPredictor predictor = new LinearExtrapolator(_threatClusterMap[cluster].Centroid); + var predictor = new LinearExtrapolator(_threatClusterMap[cluster].Centroid); // Create a launch planner. ILaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); @@ -166,8 +166,8 @@ public bool ShouldLaunchSubmunitions(Interceptor carrier) { Vector3 carrierPosition = carrier.GetPosition(); Vector3 carrierVelocity = carrier.GetVelocity(); foreach (var threat in threats) { - IPredictor predictor = new LinearExtrapolator(threat); - PredictorState predictedState = predictor.Predict(SubmunitionSpawnPredictionTime); + var predictor = new LinearExtrapolator(threat); + var predictedState = predictor.Predict(SubmunitionSpawnPredictionTime); Vector3 positionToPredictedThreat = predictedState.Position - carrierPosition; float predictedDistanceToThreat = positionToPredictedThreat.magnitude; diff --git a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs index b9b058cf4..526b6ca78 100644 --- a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs +++ b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs @@ -17,7 +17,7 @@ public static Agent GenerateAgent() { public void TestPresent() { Agent agent = GenerateAgent(); LinearExtrapolator predictor = new LinearExtrapolator(agent); - PredictorState predictedState = predictor.Predict(time: 0f); + var predictedState = predictor.Predict(time: 0f); Assert.AreEqual(agent.GetPosition(), predictedState.Position); Assert.AreEqual(agent.GetVelocity(), predictedState.Velocity); } @@ -26,7 +26,7 @@ public void TestPresent() { public void TestPast() { Agent agent = GenerateAgent(); LinearExtrapolator predictor = new LinearExtrapolator(agent); - PredictorState predictedState = predictor.Predict(time: -5f); + var predictedState = predictor.Predict(time: -5f); Assert.AreEqual(new Vector3(5, 10, -5), predictedState.Position); Assert.AreEqual(agent.GetVelocity(), predictedState.Velocity); } @@ -35,7 +35,7 @@ public void TestPast() { public void TestFuture() { Agent agent = GenerateAgent(); LinearExtrapolator predictor = new LinearExtrapolator(agent); - PredictorState predictedState = predictor.Predict(time: 10f); + var predictedState = predictor.Predict(time: 10f); Assert.AreEqual(new Vector3(20, -20, 25), predictedState.Position); Assert.AreEqual(agent.GetVelocity(), predictedState.Velocity); } From cb8fc2b39d6d2f0546ff1fa6e8db26bed438a547 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 14:53:25 -0700 Subject: [PATCH 10/74] Add argument annotations for FixedHierarchical --- Assets/Tests/EditMode/ClusterTests.cs | 2 +- .../Tests/EditMode/HierarchicalBaseTests.cs | 21 ++++++++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/Assets/Tests/EditMode/ClusterTests.cs b/Assets/Tests/EditMode/ClusterTests.cs index 1c200e7c0..cd814d0fc 100644 --- a/Assets/Tests/EditMode/ClusterTests.cs +++ b/Assets/Tests/EditMode/ClusterTests.cs @@ -6,7 +6,7 @@ public class ClusterTests { private const float _epsilon = 1e-3f; private static FixedHierarchical GenerateObject(in Vector3 position) { - return new FixedHierarchical(position, Vector3.zero, Vector3.zero); + return new FixedHierarchical(position, velocity: Vector3.zero, acceleration: Vector3.zero); } private static Cluster GenerateCluster(IReadOnlyList objects) { diff --git a/Assets/Tests/EditMode/HierarchicalBaseTests.cs b/Assets/Tests/EditMode/HierarchicalBaseTests.cs index 2bfa00299..ee881abda 100644 --- a/Assets/Tests/EditMode/HierarchicalBaseTests.cs +++ b/Assets/Tests/EditMode/HierarchicalBaseTests.cs @@ -24,9 +24,11 @@ public void TargetModel_SetAndGet_WorksCorrectly() { [Test] public void Position_ReturnsMeanOfSubHierarchicals() { var parent = new HierarchicalBase(); - var child1 = new FixedHierarchical(new Vector3(2, 0, 0), new Vector3(2, 1, 1)); + var child1 = + new FixedHierarchical(position: new Vector3(2, 0, 0), velocity: new Vector3(2, 1, 1)); parent.AddSubHierarchical(child1); - var child2 = new FixedHierarchical(new Vector3(-1, 2, 3), new Vector3(1, -2, 0)); + var child2 = + new FixedHierarchical(position: new Vector3(-1, 2, 3), velocity: new Vector3(1, -2, 0)); parent.AddSubHierarchical(child2); Assert.AreEqual(new Vector3(0.5f, 1f, 1.5f), parent.Position); @@ -41,9 +43,11 @@ public void Position_WithNoSubHierarchicals_IsZero() { [Test] public void Velocity_ReturnsMeanOfSubHierarchicals() { var parent = new HierarchicalBase(); - var child1 = new FixedHierarchical(new Vector3(2, 0, 0), new Vector3(2, 1, 1)); + var child1 = + new FixedHierarchical(position: new Vector3(2, 0, 0), velocity: new Vector3(2, 1, 1)); parent.AddSubHierarchical(child1); - var child2 = new FixedHierarchical(new Vector3(-1, 2, 3), new Vector3(1, -2, 0)); + var child2 = + new FixedHierarchical(position: new Vector3(-1, 2, 3), velocity: new Vector3(1, -2, 0)); parent.AddSubHierarchical(child2); Assert.AreEqual(new Vector3(1.5f, -0.5f, 0.5f), parent.Velocity); @@ -58,7 +62,8 @@ public void Velocity_WithNoSubHierarchicals_IsZero() { [Test] public void Speed_ReturnsMagnitudeOfVelocity() { var parent = new HierarchicalBase(); - var child = new FixedHierarchical(new Vector3(2, 0, 0), new Vector3(0, -4, 3)); + var child = + new FixedHierarchical(position: new Vector3(2, 0, 0), velocity: new Vector3(0, -4, 3)); parent.AddSubHierarchical(child); Assert.AreEqual(5f, parent.Speed, _epsilon); @@ -68,10 +73,12 @@ public void Speed_ReturnsMagnitudeOfVelocity() { public void Acceleration_ReturnsMeanOfSubHierarchicals() { var parent = new HierarchicalBase(); var child1 = - new FixedHierarchical(new Vector3(2, 0, 0), new Vector3(2, 1, 1), new Vector3(0, -1, 3)); + new FixedHierarchical(position: new Vector3(2, 0, 0), velocity: new Vector3(2, 1, 1), + acceleration: new Vector3(0, -1, 3)); parent.AddSubHierarchical(child1); var child2 = - new FixedHierarchical(new Vector3(-1, 2, 3), new Vector3(1, -2, 0), new Vector3(0, 2, -2)); + new FixedHierarchical(position: new Vector3(-1, 2, 3), velocity: new Vector3(1, -2, 0), + acceleration: new Vector3(0, 2, -2)); parent.AddSubHierarchical(child2); Assert.AreEqual(new Vector3(0f, 0.5f, 0.5f), parent.Acceleration); From b12ba0936b47a4ba44a50e79176e230e7e162c4a Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 15:01:43 -0700 Subject: [PATCH 11/74] Add predictor interface and base implementation --- .../Algorithms/Prediction/IPredictor.cs | 9 ++++++++ .../Algorithms/Prediction/IPredictor.cs.meta | 2 ++ .../Algorithms/Prediction/PredictorBase.cs | 18 ++++++++++++++++ .../Prediction/PredictorBase.cs.meta | 2 ++ .../Algorithms/Prediction/PredictorState.cs | 21 +++++++++++++++++++ .../Prediction/PredictorState.cs.meta | 2 ++ 6 files changed, 54 insertions(+) create mode 100644 Assets/Scripts/Algorithms/Prediction/IPredictor.cs create mode 100644 Assets/Scripts/Algorithms/Prediction/IPredictor.cs.meta create mode 100644 Assets/Scripts/Algorithms/Prediction/PredictorBase.cs create mode 100644 Assets/Scripts/Algorithms/Prediction/PredictorBase.cs.meta create mode 100644 Assets/Scripts/Algorithms/Prediction/PredictorState.cs create mode 100644 Assets/Scripts/Algorithms/Prediction/PredictorState.cs.meta diff --git a/Assets/Scripts/Algorithms/Prediction/IPredictor.cs b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs new file mode 100644 index 000000000..7b700d5ea --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs @@ -0,0 +1,9 @@ +// Interface for a predictor. +// +// The predictor predicts the trajectories of hierarchical objects. +public interface IPredictor { + PredictorState CurrentState { get; } + PredictorState PredictedState { get; } + + void Predict(float time); +} diff --git a/Assets/Scripts/Algorithms/Prediction/IPredictor.cs.meta b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs.meta new file mode 100644 index 000000000..7e5269380 --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: cfe69c3e4826149d39642707c4103686 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs new file mode 100644 index 000000000..4aeaf858c --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs @@ -0,0 +1,18 @@ +// Base implementation of a predictor. +public abstract class PredictorBase : IPredictor { + // Current state. + protected PredictorState _currentState; + + // Predicted state. + protected PredictorState _predictedState; + + public PredictorState CurrentState => _currentState; + public PredictorState PredictedState => _predictedState; + + public PredictorBase(IHierarchical hierarchical) { + _currentState = new PredictorState(hierarchical); + } + + // Predict the state at the given time. + public abstract void Predict(float time); +} diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs.meta b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs.meta new file mode 100644 index 000000000..aac6e0645 --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 50f9738e914464dd6bb87ddb1a9d5140 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorState.cs b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs new file mode 100644 index 000000000..7a186bbe6 --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs @@ -0,0 +1,21 @@ +using UnityEngine; + +// The predictor state defines the input and the output of a predictor. +public class PredictorState { + // Position. + public Vector3 Position { get; } + + // Velocity. + public Vector3 Velocity { get; } + + // Acceleration. + public Vector3 Acceleration { get; } + + public PredictorState(IHierarchical hierarchical) + : this(hierarchical.Position, hierarchical.Velocity, hierarchical.Acceleration) {} + public PredictorState(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { + Position = position; + Velocity = velocity; + Acceleration = acceleration; + } +} diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorState.cs.meta b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs.meta new file mode 100644 index 000000000..610b3dd14 --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 075f823a6c4df4a97b3f03c6c428fa6d \ No newline at end of file From 66825e0ee0c974f69c588b6e44642e6cfcda4abf Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 15:04:45 -0700 Subject: [PATCH 12/74] Add legacy to IAssignment and AssignmentItem --- Assets/Scripts/Assignment/Assignment.cs | 10 +++++----- .../Scripts/Assignment/MaxSpeedAssignment.cs | 18 +++++++++-------- .../Assignment/RoundRobinAssignment.cs | 16 ++++++++------- Assets/Scripts/Assignment/ThreatAssignment.cs | 18 +++++++++-------- Assets/Scripts/IADS/IADS.cs | 6 +++--- .../Tests/EditMode/MaxSpeedAssignmentTests.cs | 20 +++++++++++-------- .../Tests/EditMode/ThreatAssignmentTests.cs | 16 +++++++-------- 7 files changed, 57 insertions(+), 47 deletions(-) diff --git a/Assets/Scripts/Assignment/Assignment.cs b/Assets/Scripts/Assignment/Assignment.cs index a72871d89..17e6cb31a 100644 --- a/Assets/Scripts/Assignment/Assignment.cs +++ b/Assets/Scripts/Assignment/Assignment.cs @@ -6,15 +6,15 @@ using UnityEngine; // The assignment class is an interface for assigning a threat to each interceptor. -public interface IAssignment { +public interface IAssignmentLegacy { // Assignment item type. // The first element corresponds to the interceptor index, and the second element // corresponds to the threat index. - public struct AssignmentItem { + public struct AssignmentItemLegacy { public Interceptor Interceptor; public Threat Threat; - public AssignmentItem(Interceptor interceptor, Threat threat) { + public AssignmentItemLegacy(Interceptor interceptor, Threat threat) { Interceptor = interceptor; Threat = threat; } @@ -22,8 +22,8 @@ public AssignmentItem(Interceptor interceptor, Threat threat) { // Assign a target to each interceptor that has not been assigned a target yet. [Pure] - public abstract IEnumerable Assign(in IReadOnlyList interceptors, - in IReadOnlyList threats); + public abstract IEnumerable Assign( + in IReadOnlyList interceptors, in IReadOnlyList threats); // Get the list of assignable interceptors. [Pure] diff --git a/Assets/Scripts/Assignment/MaxSpeedAssignment.cs b/Assets/Scripts/Assignment/MaxSpeedAssignment.cs index fcf653e93..e53398688 100644 --- a/Assets/Scripts/Assignment/MaxSpeedAssignment.cs +++ b/Assets/Scripts/Assignment/MaxSpeedAssignment.cs @@ -4,19 +4,21 @@ // The maximum speed assignment class assigns interceptors to the threats to maximize the intercept // speed by defining a cost of the assignment equal to the speed lost for the maneuver. -public class MaxSpeedAssignment : IAssignment { +public class MaxSpeedAssignment : IAssignmentLegacy { // Assign a threat to each interceptor that has not been assigned a threat yet. - public IEnumerable Assign(in IReadOnlyList interceptors, - in IReadOnlyList threats) { - List assignments = new List(); + public IEnumerable Assign( + in IReadOnlyList interceptors, in IReadOnlyList threats) { + List assignments = + new List(); - List assignableInterceptors = IAssignment.GetAssignableInterceptors(interceptors); + List assignableInterceptors = + IAssignmentLegacy.GetAssignableInterceptors(interceptors); if (assignableInterceptors.Count == 0) { Debug.LogWarning("No assignable interceptors found."); return assignments; } - List activeThreats = IAssignment.GetActiveThreats(threats); + List activeThreats = IAssignmentLegacy.GetActiveThreats(threats); if (activeThreats.Count == 0) { Debug.LogWarning("No active threats found."); return assignments; @@ -70,8 +72,8 @@ public class MaxSpeedAssignment : IAssignment { for (int i = 0; i < numAssignments; ++i) { int interceptorIndex = assignedInterceptorIndices[i]; int threatIndex = assignedThreatIndices[i]; - assignments.Add(new IAssignment.AssignmentItem(assignableInterceptors[interceptorIndex], - activeThreats[threatIndex])); + assignments.Add(new IAssignmentLegacy.AssignmentItemLegacy( + assignableInterceptors[interceptorIndex], activeThreats[threatIndex])); } return assignments; } diff --git a/Assets/Scripts/Assignment/RoundRobinAssignment.cs b/Assets/Scripts/Assignment/RoundRobinAssignment.cs index c0143d1a0..9f0b17830 100644 --- a/Assets/Scripts/Assignment/RoundRobinAssignment.cs +++ b/Assets/Scripts/Assignment/RoundRobinAssignment.cs @@ -5,24 +5,26 @@ // The round-robin assignment class assigns interceptors to the targets in a // round-robin order using the new paradigm. -public class RoundRobinAssignment : IAssignment { +public class RoundRobinAssignment : IAssignmentLegacy { // Previous target index that was assigned. private int prevTargetIndex = -1; // Assign a target to each interceptor that has not been assigned a target yet. [Pure] - public IEnumerable Assign(in IReadOnlyList interceptors, - in IReadOnlyList threats) { - List assignments = new List(); + public IEnumerable Assign( + in IReadOnlyList interceptors, in IReadOnlyList threats) { + List assignments = + new List(); // Get the list of interceptors that are available for assignment. - List assignableInterceptors = IAssignment.GetAssignableInterceptors(interceptors); + List assignableInterceptors = + IAssignmentLegacy.GetAssignableInterceptors(interceptors); if (assignableInterceptors.Count == 0) { return assignments; } // Get the list of active threats that need to be addressed. - List activeThreats = IAssignment.GetActiveThreats(threats); + List activeThreats = IAssignmentLegacy.GetActiveThreats(threats); if (activeThreats.Count == 0) { Debug.LogWarning("No active threats found."); return assignments; @@ -35,7 +37,7 @@ public class RoundRobinAssignment : IAssignment { Threat threat = activeThreats[nextTargetIndex]; // Assign the interceptor to the selected threat. - assignments.Add(new IAssignment.AssignmentItem(interceptor, threat)); + assignments.Add(new IAssignmentLegacy.AssignmentItemLegacy(interceptor, threat)); // Update the previous target index. prevTargetIndex = nextTargetIndex; diff --git a/Assets/Scripts/Assignment/ThreatAssignment.cs b/Assets/Scripts/Assignment/ThreatAssignment.cs index e823a6d55..5dfee21f6 100644 --- a/Assets/Scripts/Assignment/ThreatAssignment.cs +++ b/Assets/Scripts/Assignment/ThreatAssignment.cs @@ -6,20 +6,22 @@ // The threat assignment class assigns interceptors to the targets based // on the threat level of the targets. -public class ThreatAssignment : IAssignment { +public class ThreatAssignment : IAssignmentLegacy { // Assign a target to each interceptor that has not been assigned a target yet. [Pure] - public IEnumerable Assign(in IReadOnlyList interceptors, - in IReadOnlyList threats) { - List assignments = new List(); + public IEnumerable Assign( + in IReadOnlyList interceptors, in IReadOnlyList threats) { + List assignments = + new List(); - List assignableInterceptors = IAssignment.GetAssignableInterceptors(interceptors); + List assignableInterceptors = + IAssignmentLegacy.GetAssignableInterceptors(interceptors); if (assignableInterceptors.Count == 0) { Debug.LogWarning("No assignable interceptors found."); return assignments; } - List activeThreats = IAssignment.GetActiveThreats(threats); + List activeThreats = IAssignmentLegacy.GetActiveThreats(threats); if (activeThreats.Count == 0) { Debug.LogWarning("No active threats found."); return assignments; @@ -37,8 +39,8 @@ public class ThreatAssignment : IAssignment { var assignableInterceptorsEnumerator = assignableInterceptors.GetEnumerator(); int threatIndex = 0; while (assignableInterceptorsEnumerator.MoveNext()) { - assignments.Add(new IAssignment.AssignmentItem(assignableInterceptorsEnumerator.Current, - threatInfos[threatIndex].Threat)); + assignments.Add(new IAssignmentLegacy.AssignmentItemLegacy( + assignableInterceptorsEnumerator.Current, threatInfos[threatIndex].Threat)); threatIndex = (threatIndex + 1) % threatInfos.Count; } return assignments; diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index 753510902..7c636b406 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -16,7 +16,7 @@ public class IADS : MonoBehaviour { // 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 IAssignmentLegacy _assignmentScheme = new MaxSpeedAssignment(); private Coroutine _launchInterceptorsCoroutine; [SerializeField] @@ -230,7 +230,7 @@ public void AssignSubmunitionsToThreats(Interceptor carrier, List i // Assign threats to the submunitions. ClusterLegacy cluster = _interceptorClusterMap[carrier]; List threats = cluster.Threats.ToList(); - IEnumerable assignments = + IEnumerable assignments = _assignmentScheme.Assign(interceptors, threats); // Mark the cluster as delegated to submunitions. @@ -270,7 +270,7 @@ private void AssignInterceptorToThreat(in IReadOnlyList interceptor return; } - IEnumerable assignments = + IEnumerable assignments = _assignmentScheme.Assign(interceptors, threats); // Apply the assignments to the submunitions. diff --git a/Assets/Tests/EditMode/MaxSpeedAssignmentTests.cs b/Assets/Tests/EditMode/MaxSpeedAssignmentTests.cs index 495f9eaf7..e982831db 100644 --- a/Assets/Tests/EditMode/MaxSpeedAssignmentTests.cs +++ b/Assets/Tests/EditMode/MaxSpeedAssignmentTests.cs @@ -12,7 +12,7 @@ public static Configs.StaticConfig LoadStaticConfig() { [Test] public void AssignNoInterceptors() { // Define the assignment. - IAssignment assignment = new MaxSpeedAssignment(); + IAssignmentLegacy assignment = new MaxSpeedAssignment(); // Create interceptors. List interceptors = new List(); @@ -23,14 +23,15 @@ public void AssignNoInterceptors() { // Assign the interceptors to the threats. LogAssert.Expect(LogType.Warning, "No assignable interceptors found."); - IEnumerable assignments = assignment.Assign(interceptors, threats); + IEnumerable assignments = + assignment.Assign(interceptors, threats); Assert.AreEqual(0, assignments.Count(), "There should be no assignments."); } [Test] public void AssignNoThreats() { // Define the assignment. - IAssignment assignment = new MaxSpeedAssignment(); + IAssignmentLegacy assignment = new MaxSpeedAssignment(); // Create interceptors. List interceptors = @@ -41,14 +42,15 @@ public void AssignNoThreats() { // Assign the interceptors to the threats. LogAssert.Expect(LogType.Warning, "No active threats found."); - IEnumerable assignments = assignment.Assign(interceptors, threats); + IEnumerable assignments = + assignment.Assign(interceptors, threats); Assert.AreEqual(0, assignments.Count(), "There should be no assignments."); } [Test] public void AssignShouldAssignAllInterceptorsAndThreats() { // Define the assignment. - IAssignment assignment = new MaxSpeedAssignment(); + IAssignmentLegacy assignment = new MaxSpeedAssignment(); // Create the interceptors. Interceptor interceptor1 = new GameObject("Interceptor 1").AddComponent(); @@ -87,7 +89,8 @@ public void AssignShouldAssignAllInterceptorsAndThreats() { List threats = new List { threat1, threat2, threat3 }; // Assign the interceptors to the threats. - IEnumerable assignments = assignment.Assign(interceptors, threats); + IEnumerable assignments = + assignment.Assign(interceptors, threats); Assert.AreEqual(3, assignments.Count(), "All interceptors should be assigned."); HashSet assignedInterceptors = new HashSet(); @@ -114,7 +117,7 @@ public void AssignShouldAssignAllInterceptorsAndThreats() { [Test] public void AssignShouldHandleMoreInterceptorsThanThreats() { // Define the assignment. - IAssignment assignment = new MaxSpeedAssignment(); + IAssignmentLegacy assignment = new MaxSpeedAssignment(); // Create the interceptors. Interceptor interceptor1 = new GameObject("Interceptor 1").AddComponent(); @@ -151,7 +154,8 @@ public void AssignShouldHandleMoreInterceptorsThanThreats() { List threats = new List { threat1, threat2 }; // Assign the interceptors to the threats. - IEnumerable assignments = assignment.Assign(interceptors, threats); + IEnumerable assignments = + assignment.Assign(interceptors, threats); Assert.AreEqual(3, assignments.Count(), "All interceptors should be assigned."); HashSet assignedInterceptors = new HashSet(); diff --git a/Assets/Tests/EditMode/ThreatAssignmentTests.cs b/Assets/Tests/EditMode/ThreatAssignmentTests.cs index ca6ed400e..20c254dfe 100644 --- a/Assets/Tests/EditMode/ThreatAssignmentTests.cs +++ b/Assets/Tests/EditMode/ThreatAssignmentTests.cs @@ -8,7 +8,7 @@ public class ThreatAssignmentTests { [Test] public void AssignNoInterceptors() { // Define the threat assignment. - IAssignment threatAssignment = new ThreatAssignment(); + IAssignmentLegacy threatAssignment = new ThreatAssignment(); // Create interceptors. List interceptors = new List(); @@ -19,7 +19,7 @@ public void AssignNoInterceptors() { // Assign the interceptors to the threats. LogAssert.Expect(LogType.Warning, "No assignable interceptors found."); - IEnumerable assignments = + IEnumerable assignments = threatAssignment.Assign(interceptors, threats); Assert.AreEqual(0, assignments.Count(), "There should be no assignments."); } @@ -27,7 +27,7 @@ public void AssignNoInterceptors() { [Test] public void AssignNoThreats() { // Define the threat assignment. - IAssignment threatAssignment = new ThreatAssignment(); + IAssignmentLegacy threatAssignment = new ThreatAssignment(); // Create interceptors. List interceptors = @@ -38,7 +38,7 @@ public void AssignNoThreats() { // Assign the interceptors to the threats. LogAssert.Expect(LogType.Warning, "No active threats found."); - IEnumerable assignments = + IEnumerable assignments = threatAssignment.Assign(interceptors, threats); Assert.AreEqual(0, assignments.Count(), "There should be no assignments."); } @@ -46,7 +46,7 @@ public void AssignNoThreats() { [Test] public void AssignShouldAssignAllInterceptorsAndThreats() { // Define the threat assignment. - IAssignment threatAssignment = new ThreatAssignment(); + IAssignmentLegacy threatAssignment = new ThreatAssignment(); // Create interceptors. List interceptors = new List { @@ -78,7 +78,7 @@ public void AssignShouldAssignAllInterceptorsAndThreats() { List threats = new List { threat1, threat2, threat3 }; // Assign the interceptors to the threats. - IEnumerable assignments = + IEnumerable assignments = threatAssignment.Assign(interceptors, threats); Assert.AreEqual(3, assignments.Count(), "All interceptors should be assigned."); @@ -114,7 +114,7 @@ public void AssignShouldAssignAllInterceptorsAndThreats() { [Test] public void AssignShouldHandleMoreInterceptorsThanThreats() { // Define the threat assignment. - IAssignment threatAssignment = new ThreatAssignment(); + IAssignmentLegacy threatAssignment = new ThreatAssignment(); // Create interceptors. List interceptors = new List { @@ -142,7 +142,7 @@ public void AssignShouldHandleMoreInterceptorsThanThreats() { List threats = new List { threat1, threat2 }; // Assign the interceptors to the threats. - IEnumerable assignments = + IEnumerable assignments = threatAssignment.Assign(interceptors, threats); Assert.AreEqual(3, assignments.Count(), "All interceptors should be assigned."); From b7769df7013b80ae8d04c5d6e786606afddd780e Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 15:09:30 -0700 Subject: [PATCH 13/74] Add assignment interface and base implementation --- Assets/Scripts/Assignment/AssignmentBase.cs | 26 +++++++++++++++++++ .../Scripts/Assignment/AssignmentBase.cs.meta | 2 ++ Assets/Scripts/Assignment/AssignmentItem.cs | 10 +++++++ .../Scripts/Assignment/AssignmentItem.cs.meta | 2 ++ Assets/Scripts/Assignment/IAssignment.cs | 13 ++++++++++ Assets/Scripts/Assignment/IAssignment.cs.meta | 2 ++ 6 files changed, 55 insertions(+) create mode 100644 Assets/Scripts/Assignment/AssignmentBase.cs create mode 100644 Assets/Scripts/Assignment/AssignmentBase.cs.meta create mode 100644 Assets/Scripts/Assignment/AssignmentItem.cs create mode 100644 Assets/Scripts/Assignment/AssignmentItem.cs.meta create mode 100644 Assets/Scripts/Assignment/IAssignment.cs create mode 100644 Assets/Scripts/Assignment/IAssignment.cs.meta diff --git a/Assets/Scripts/Assignment/AssignmentBase.cs b/Assets/Scripts/Assignment/AssignmentBase.cs new file mode 100644 index 000000000..1c9fe79cf --- /dev/null +++ b/Assets/Scripts/Assignment/AssignmentBase.cs @@ -0,0 +1,26 @@ +using System.Collections.Generic; +using System.Linq; + +// Base implementation of an assignment algorithm. +public abstract class AssignmentBase : IAssignment { + // List of hierarchical objects to assign. + protected List _first = new List(); + + // List of hierarchical objects to assign to. + protected List _second = new List(); + + // List of assignments. + protected List _assignments = new List(); + + public IReadOnlyList First => _first.AsReadOnly(); + public IReadOnlyList Second => _second.AsReadOnly(); + public IReadOnlyList Assignments => _assignments.AsReadOnly(); + + public AssignmentBase(IEnumerable first, IEnumerable second) { + _first = first.ToList(); + _second = second.ToList(); + } + + // Run the assignment algorithm and assign the hierarchical objects. + public abstract void Assign(); +} diff --git a/Assets/Scripts/Assignment/AssignmentBase.cs.meta b/Assets/Scripts/Assignment/AssignmentBase.cs.meta new file mode 100644 index 000000000..d0eecdd95 --- /dev/null +++ b/Assets/Scripts/Assignment/AssignmentBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 8b649471f51f840659cd2bfc39dc9be5 \ No newline at end of file diff --git a/Assets/Scripts/Assignment/AssignmentItem.cs b/Assets/Scripts/Assignment/AssignmentItem.cs new file mode 100644 index 000000000..f909392e6 --- /dev/null +++ b/Assets/Scripts/Assignment/AssignmentItem.cs @@ -0,0 +1,10 @@ +// The assignment item is the output of an assignment algorithm. +public struct AssignmentItem { + public IHierarchical First; + public IHierarchical Second; + + public AssignmentItem(IHierarchical first, IHierarchical second) { + First = first; + Second = second; + } +} diff --git a/Assets/Scripts/Assignment/AssignmentItem.cs.meta b/Assets/Scripts/Assignment/AssignmentItem.cs.meta new file mode 100644 index 000000000..613c36acb --- /dev/null +++ b/Assets/Scripts/Assignment/AssignmentItem.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: be9d1a68bf30540bd873ed928624ddd1 \ No newline at end of file diff --git a/Assets/Scripts/Assignment/IAssignment.cs b/Assets/Scripts/Assignment/IAssignment.cs new file mode 100644 index 000000000..68a063a10 --- /dev/null +++ b/Assets/Scripts/Assignment/IAssignment.cs @@ -0,0 +1,13 @@ +using System.Collections.Generic; + +// Interface for an assignment algorithm. +// +// The assignment algorithm assigns hierarchical objects to each other in a bipartite manner. +public interface IAssignment { + IReadOnlyList First { get; } + IReadOnlyList Second { get; } + IReadOnlyList Assignments { get; } + + // Run the assignment algorithm and assign the hierarchical objects. + void Assign(); +} diff --git a/Assets/Scripts/Assignment/IAssignment.cs.meta b/Assets/Scripts/Assignment/IAssignment.cs.meta new file mode 100644 index 000000000..a3dfd04ee --- /dev/null +++ b/Assets/Scripts/Assignment/IAssignment.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 3a48f7ac1d2804a0f89681653de3bc6e \ No newline at end of file From fde8285fb2c389ec76f59439a9b3f050b2c5e549 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 20:17:29 -0700 Subject: [PATCH 14/74] Make algorithms state-less --- .../Algorithms/Clustering/ClustererBase.cs | 16 +------------- .../Algorithms/Clustering/IClusterer.cs | 5 +---- .../SizeAndRadiusConstrainedClustererBase.cs | 6 +---- .../Algorithms/Prediction/IPredictor.cs | 6 ++--- .../Algorithms/Prediction/PredictorBase.cs | 17 ++------------ .../Algorithms/Prediction/PredictorState.cs | 16 ++++---------- Assets/Scripts/Assignment/AssignmentBase.cs | 22 ++----------------- Assets/Scripts/Assignment/IAssignment.cs | 7 ++---- 8 files changed, 15 insertions(+), 80 deletions(-) diff --git a/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs index 70a829584..4debbf9f7 100644 --- a/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs +++ b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs @@ -1,21 +1,7 @@ using System.Collections.Generic; -using System.Linq; // Base implementation of a clustering algorithm. public abstract class ClustererBase : IClusterer { - // list of hierarchical objects to cluster. - protected List _objects = new List(); - - // list of generated clusters. - protected List _clusters = new List(); - - public ClustererBase(IEnumerable objects) { - _objects = objects.ToList(); - } - - public IReadOnlyList Objects => _objects.AsReadOnly(); - public IReadOnlyList Clusters => _clusters.AsReadOnly(); - // Generate the clusters from the list of hierarchical objects. - public abstract void Cluster(); + public abstract IEnumerable Cluster(IEnumerable hierarchicals); } diff --git a/Assets/Scripts/Algorithms/Clustering/IClusterer.cs b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs index 0cf9e7b29..21c398e3e 100644 --- a/Assets/Scripts/Algorithms/Clustering/IClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs @@ -4,9 +4,6 @@ // // The clustering algorithm generaters a list of clusters given a list of hierarchical objects. public interface IClusterer { - IReadOnlyList Objects { get; } - IReadOnlyList Clusters { get; } - // Generate the clusters from the list of hierarchical objects. - void Cluster(); + IEnumerable Cluster(IEnumerable hierarchicals); } diff --git a/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs index ff9f82e11..a24119e92 100644 --- a/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs +++ b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs @@ -1,5 +1,3 @@ -using System.Collections.Generic; - // Base implementation of a size and radius-constrained clustering algorithm. // // The size is defined as the maximum number of hierarchical objects within a cluster, and the @@ -12,9 +10,7 @@ public abstract class SizeAndRadiusConstrainedClustererBase : ClustererBase { // Maximum cluster radius. protected readonly float _maxRadius = 0; - public SizeAndRadiusConstrainedClustererBase(IEnumerable objects, int maxSize, - float maxRadius) - : base(objects) { + public SizeAndRadiusConstrainedClustererBase(int maxSize, float maxRadius) { _maxSize = maxSize; _maxRadius = maxRadius; } diff --git a/Assets/Scripts/Algorithms/Prediction/IPredictor.cs b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs index 7b700d5ea..7050b79a5 100644 --- a/Assets/Scripts/Algorithms/Prediction/IPredictor.cs +++ b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs @@ -2,8 +2,6 @@ // // The predictor predicts the trajectories of hierarchical objects. public interface IPredictor { - PredictorState CurrentState { get; } - PredictorState PredictedState { get; } - - void Predict(float time); + // Predict the future state of the hierarchical object. + PredictorState Predict(IHierarchical hierarchical, float time); } diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs index 4aeaf858c..2e7e64530 100644 --- a/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs +++ b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs @@ -1,18 +1,5 @@ // Base implementation of a predictor. public abstract class PredictorBase : IPredictor { - // Current state. - protected PredictorState _currentState; - - // Predicted state. - protected PredictorState _predictedState; - - public PredictorState CurrentState => _currentState; - public PredictorState PredictedState => _predictedState; - - public PredictorBase(IHierarchical hierarchical) { - _currentState = new PredictorState(hierarchical); - } - - // Predict the state at the given time. - public abstract void Predict(float time); + // Predict the future state of the hierarchical object. + public abstract PredictorState Predict(IHierarchical hierarchical, float time); } diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorState.cs b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs index 7a186bbe6..09f4e80f7 100644 --- a/Assets/Scripts/Algorithms/Prediction/PredictorState.cs +++ b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs @@ -1,21 +1,13 @@ using UnityEngine; -// The predictor state defines the input and the output of a predictor. +// The predictor state defines the output of a predictor. public class PredictorState { // Position. - public Vector3 Position { get; } + public Vector3 Position { get; set; } // Velocity. - public Vector3 Velocity { get; } + public Vector3 Velocity { get; set; } // Acceleration. - public Vector3 Acceleration { get; } - - public PredictorState(IHierarchical hierarchical) - : this(hierarchical.Position, hierarchical.Velocity, hierarchical.Acceleration) {} - public PredictorState(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { - Position = position; - Velocity = velocity; - Acceleration = acceleration; - } + public Vector3 Acceleration { get; set; } } diff --git a/Assets/Scripts/Assignment/AssignmentBase.cs b/Assets/Scripts/Assignment/AssignmentBase.cs index 1c9fe79cf..e0315b399 100644 --- a/Assets/Scripts/Assignment/AssignmentBase.cs +++ b/Assets/Scripts/Assignment/AssignmentBase.cs @@ -1,26 +1,8 @@ using System.Collections.Generic; -using System.Linq; // Base implementation of an assignment algorithm. public abstract class AssignmentBase : IAssignment { - // List of hierarchical objects to assign. - protected List _first = new List(); - - // List of hierarchical objects to assign to. - protected List _second = new List(); - - // List of assignments. - protected List _assignments = new List(); - - public IReadOnlyList First => _first.AsReadOnly(); - public IReadOnlyList Second => _second.AsReadOnly(); - public IReadOnlyList Assignments => _assignments.AsReadOnly(); - - public AssignmentBase(IEnumerable first, IEnumerable second) { - _first = first.ToList(); - _second = second.ToList(); - } - // Run the assignment algorithm and assign the hierarchical objects. - public abstract void Assign(); + public abstract IEnumerable Assign(IEnumerable first, + IEnumerable second); } diff --git a/Assets/Scripts/Assignment/IAssignment.cs b/Assets/Scripts/Assignment/IAssignment.cs index 68a063a10..9c8804880 100644 --- a/Assets/Scripts/Assignment/IAssignment.cs +++ b/Assets/Scripts/Assignment/IAssignment.cs @@ -4,10 +4,7 @@ // // The assignment algorithm assigns hierarchical objects to each other in a bipartite manner. public interface IAssignment { - IReadOnlyList First { get; } - IReadOnlyList Second { get; } - IReadOnlyList Assignments { get; } - // Run the assignment algorithm and assign the hierarchical objects. - void Assign(); + IEnumerable Assign(IEnumerable first, + IEnumerable second); } From cc574ba2ad5c11d702d1edca8690e5dc6d48bacd Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sun, 12 Oct 2025 23:07:36 -0700 Subject: [PATCH 15/74] Define all movement behaviors --- Assets/Scripts/Agents/AgentBase.cs | 12 +++++++ Assets/Scripts/Movements/AerialMovement.cs | 35 +++++++++++++++++++ .../Scripts/Movements/AerialMovement.cs.meta | 2 ++ Assets/Scripts/Movements/GroundMovement.cs | 15 ++++++++ .../Scripts/Movements/GroundMovement.cs.meta | 2 ++ Assets/Scripts/Movements/IdealMovement.cs | 18 ++++++++++ .../Scripts/Movements/IdealMovement.cs.meta | 2 ++ Assets/Scripts/Movements/MovementBase.cs | 6 ++++ Assets/Scripts/Movements/RocketMovement.cs | 22 ++++++++++++ .../Scripts/Movements/RocketMovement.cs.meta | 2 ++ 10 files changed, 116 insertions(+) create mode 100644 Assets/Scripts/Movements/AerialMovement.cs create mode 100644 Assets/Scripts/Movements/AerialMovement.cs.meta create mode 100644 Assets/Scripts/Movements/GroundMovement.cs create mode 100644 Assets/Scripts/Movements/GroundMovement.cs.meta create mode 100644 Assets/Scripts/Movements/IdealMovement.cs create mode 100644 Assets/Scripts/Movements/IdealMovement.cs.meta create mode 100644 Assets/Scripts/Movements/RocketMovement.cs create mode 100644 Assets/Scripts/Movements/RocketMovement.cs.meta diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 7c5c85c49..59ee5c072 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -83,6 +83,18 @@ private void Awake() { // TODO(titan): Methods for updating the agent and terminating the agent. + public float CalculateMaxForwardAcceleration() { + return StaticConfig.AccelerationConfig?.MaxForwardAcceleration ?? 0; + } + + public float CalculateMaxNormalAcceleration() { + float maxReferenceNormalAcceleration = + (float)((StaticConfig.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * + Constants.kGravity); + float referenceSpeed = StaticConfig.AccelerationConfig?.ReferenceSpeed ?? 1; + return Mathf.Pow(Speed / referenceSpeed, 2) * maxReferenceNormalAcceleration; + } + public Transformation GetRelativeTransformation(IAgent target) { return GetRelativeTransformation(target.Position, target.Velocity, target.Acceleration); } diff --git a/Assets/Scripts/Movements/AerialMovement.cs b/Assets/Scripts/Movements/AerialMovement.cs new file mode 100644 index 000000000..ee04ef521 --- /dev/null +++ b/Assets/Scripts/Movements/AerialMovement.cs @@ -0,0 +1,35 @@ +using UnityEngine; + +// Aerial movement. +// +// Interceptors and threats have different movements since threats are not affected by drag and +// gravity and have specified attack behaviors. +public abstract class AerialMovement : MovementBase { + public AerialMovement(IAgent agent) : base(agent) {} + + // TODO(titan): Methods to calculate gravity and drag based on the agent as well as the total + // acceleration. + + protected float GetDynamicPressure() { + var airDensity = (float)Constants.CalculateAirDensityAtAltitude(Agent.Position.y); + var flowSpeed = Agent.Speed; + return 0.5f * airDensity * (flowSpeed * flowSpeed); + } + + protected float CalculateDrag() { + var staticConfig = Agent.StaticConfig; + var dragCoefficient = staticConfig.LiftDragConfig?.DragCoefficient ?? 0; + var crossSectionalArea = staticConfig.BodyConfig?.CrossSectionalArea ?? 0; + var mass = staticConfig.BodyConfig?.Mass ?? 1; + var dynamicPressure = GetDynamicPressure(); + var dragForce = dragCoefficient * dynamicPressure * crossSectionalArea; + return dragForce / mass; + } + + protected float CalculateLiftInducedDrag(in Vector3 accelerationInput) { + var staticConfig = Agent.StaticConfig; + var liftAcceleration = Vector3.ProjectOnPlane(accelerationInput, transform.forward).magnitude; + var liftDragRatio = staticConfig.LiftDragConfig?.LiftDragRatio ?? 1; + return Mathf.Abs(liftAcceleration / liftDragRatio); + } +} diff --git a/Assets/Scripts/Movements/AerialMovement.cs.meta b/Assets/Scripts/Movements/AerialMovement.cs.meta new file mode 100644 index 000000000..1732a5d17 --- /dev/null +++ b/Assets/Scripts/Movements/AerialMovement.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4b914bef23aba4f938ee9f790f79aa1c \ No newline at end of file diff --git a/Assets/Scripts/Movements/GroundMovement.cs b/Assets/Scripts/Movements/GroundMovement.cs new file mode 100644 index 000000000..3d5e47b80 --- /dev/null +++ b/Assets/Scripts/Movements/GroundMovement.cs @@ -0,0 +1,15 @@ +using UnityEngine; + +// Ground-based movement. +// +// The agent is confined to only move along the y=0 plane. +public class GroundMovement : MovementBase { + public GroundMovement(IAgent agent) : base(agent) {} + + public override void Update(double deltaTime) { + // Determine the agent's acceleration input using its controller. + var accelerationInput = Vector3.zero; + // There is drag along the ground and ignore gravity. + Agent.AccelerationInput = accelerationInput; + } +} diff --git a/Assets/Scripts/Movements/GroundMovement.cs.meta b/Assets/Scripts/Movements/GroundMovement.cs.meta new file mode 100644 index 000000000..5558e9895 --- /dev/null +++ b/Assets/Scripts/Movements/GroundMovement.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: abe50cb8db4aa432788e7e086f207742 \ No newline at end of file diff --git a/Assets/Scripts/Movements/IdealMovement.cs b/Assets/Scripts/Movements/IdealMovement.cs new file mode 100644 index 000000000..0e68e7583 --- /dev/null +++ b/Assets/Scripts/Movements/IdealMovement.cs @@ -0,0 +1,18 @@ +using UnityEngine; + +// Ideal movement. +// +// Threats can be modeled as moving ideally, i.e., without the influence of drag or gravity. +public class IdealMovement : AerialMovement { + public IdealMovement(IAgent agent) : base(agent) {} + + // Determine the next movement for the agent by using the agent's controller to calculate the + // acceleration input. + // Ideal movement implies no drag or gravity. + public override void Update(double deltaTime) { + // Determine the agent's acceleration input using its controller. + var accelerationInput = Vector3.zero; + // Ignore drag and gravity. + Agent.AccelerationInput = accelerationInput; + } +} diff --git a/Assets/Scripts/Movements/IdealMovement.cs.meta b/Assets/Scripts/Movements/IdealMovement.cs.meta new file mode 100644 index 000000000..4e911909e --- /dev/null +++ b/Assets/Scripts/Movements/IdealMovement.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 9406177aa998e45bb9d8152a32f189d3 \ No newline at end of file diff --git a/Assets/Scripts/Movements/MovementBase.cs b/Assets/Scripts/Movements/MovementBase.cs index 8a4346986..ece627bd6 100644 --- a/Assets/Scripts/Movements/MovementBase.cs +++ b/Assets/Scripts/Movements/MovementBase.cs @@ -8,5 +8,11 @@ public IAgent Agent { set => _agent = value; } + public MovementBase(IAgent agent) { + _agent = agent; + } + + // Determine the next movement for the agent by using the agent's controller to calculate the + // acceleration input. public abstract void Update(double deltaTime); } diff --git a/Assets/Scripts/Movements/RocketMovement.cs b/Assets/Scripts/Movements/RocketMovement.cs new file mode 100644 index 000000000..a940756f3 --- /dev/null +++ b/Assets/Scripts/Movements/RocketMovement.cs @@ -0,0 +1,22 @@ +using UnityEngine; + +// Rocket movement. +// +// A rocket is defined by having three modes during its flight: ready mode (before it is launched), +// boost mode (when its burner is on), and midcourse mode (after the burner has completed burning). +public class RocketMovement : AerialMovement { + public RocketMovement(IAgent agent) : base(agent) {} + + // Determine the next movement for the agent by using the agent's controller to calculate the + // acceleration input. + public override void Update(double deltaTime) { + // Step through the READY, BOOST, and MIDCOURSE phases. + // Determine the agent's acceleration input using its controller and consider drag and gravity. + var accelerationInput = Vector3.zero; + Agent.AccelerationInput = accelerationInput; + } + + private void UpdateReady(double deltaTime) {} + private void UpdateBoost(double deltaTime) {} + private void UpdateMidCourse(double deltaTime) {} +} diff --git a/Assets/Scripts/Movements/RocketMovement.cs.meta b/Assets/Scripts/Movements/RocketMovement.cs.meta new file mode 100644 index 000000000..a41c82d21 --- /dev/null +++ b/Assets/Scripts/Movements/RocketMovement.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 52ce240f0e22c4c5682b5991f0f29e42 \ No newline at end of file From 3a23a6699f49edce78c0fc7328f3d122a32ad8c5 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 12:29:44 -0700 Subject: [PATCH 16/74] Implement all movement behaviors --- Assets/Scripts/Agents/Agent.cs | 17 ++-- Assets/Scripts/Agents/AgentBase.cs | 28 ++++--- Assets/Scripts/Agents/IAgent.cs | 2 + .../Scripts/Assignment/MaxSpeedAssignment.cs | 8 +- Assets/Scripts/IADS/IADS.cs | 4 +- Assets/Scripts/Interceptors/Interceptor.cs | 2 +- Assets/Scripts/Managers/SimManager.cs | 10 +-- Assets/Scripts/Monitors/SimMonitor.cs | 6 +- Assets/Scripts/Movements/AerialMovement.cs | 12 +-- Assets/Scripts/Movements/GroundMovement.cs | 15 ++-- Assets/Scripts/Movements/IMovement.cs | 8 +- Assets/Scripts/Movements/IdealMovement.cs | 13 ++- Assets/Scripts/Movements/MovementBase.cs | 36 ++++++++- Assets/Scripts/Movements/RocketMovement.cs | 81 ++++++++++++++++--- Assets/Scripts/Threats/FixedWingThreat.cs | 3 +- Assets/Scripts/Utils/Constants.cs | 18 ++--- docs/Simulator_Overview.md | 2 +- 17 files changed, 179 insertions(+), 86 deletions(-) diff --git a/Assets/Scripts/Agents/Agent.cs b/Assets/Scripts/Agents/Agent.cs index 73c4cbf8d..003400f6f 100644 --- a/Assets/Scripts/Agents/Agent.cs +++ b/Assets/Scripts/Agents/Agent.cs @@ -189,7 +189,7 @@ public void SetPosition(Vector3 position) { transform.position = position; } - public double GetSpeed() { + public float GetSpeed() { return GetComponent().linearVelocity.magnitude; } @@ -326,10 +326,10 @@ private AccelerationTransformation GetRelativeAccelerationTransformation(Agent a return accelerationTransformation; } - public double GetDynamicPressure() { + public float GetDynamicPressure() { var airDensity = Constants.CalculateAirDensityAtAltitude(transform.position.y); var flowSpeed = GetSpeed(); - return 0.5 * airDensity * (flowSpeed * flowSpeed); + return 0.5f * airDensity * (flowSpeed * flowSpeed); } protected abstract void UpdateReady(double deltaTime); @@ -341,15 +341,13 @@ protected virtual void Awake() {} protected virtual void Start() {} protected virtual void FixedUpdate() { - _speed = (float)GetSpeed(); + _speed = GetSpeed(); if (_flightPhase != FlightPhase.INITIALIZED && _flightPhase != FlightPhase.READY) { _timeSinceBoost += Time.fixedDeltaTime; } _timeInPhase += Time.fixedDeltaTime; var boostTime = staticConfig.BoostConfig?.BoostTime ?? 0; - double elapsedSimulationTime = SimManager.Instance.GetElapsedSimulationTime(); - if (_flightPhase == FlightPhase.TERMINATED) { return; } @@ -415,17 +413,16 @@ public float CalculateMaxForwardAcceleration() { public float CalculateMaxNormalAcceleration() { float maxReferenceNormalAcceleration = - (float)((staticConfig.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * - Constants.kGravity); + (staticConfig.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * Constants.kGravity; float referenceSpeed = staticConfig.AccelerationConfig?.ReferenceSpeed ?? 1; - return Mathf.Pow((float)GetSpeed() / referenceSpeed, 2) * maxReferenceNormalAcceleration; + return Mathf.Pow(GetSpeed() / referenceSpeed, 2) * maxReferenceNormalAcceleration; } private float CalculateDrag() { float dragCoefficient = staticConfig.LiftDragConfig?.DragCoefficient ?? 0; float crossSectionalArea = staticConfig.BodyConfig?.CrossSectionalArea ?? 0; float mass = staticConfig.BodyConfig?.Mass ?? 1; - float dynamicPressure = (float)GetDynamicPressure(); + float dynamicPressure = GetDynamicPressure(); float dragForce = dragCoefficient * dynamicPressure * crossSectionalArea; return dragForce / mass; } diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 59ee5c072..dfb5dffab 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -83,18 +83,6 @@ private void Awake() { // TODO(titan): Methods for updating the agent and terminating the agent. - public float CalculateMaxForwardAcceleration() { - return StaticConfig.AccelerationConfig?.MaxForwardAcceleration ?? 0; - } - - public float CalculateMaxNormalAcceleration() { - float maxReferenceNormalAcceleration = - (float)((StaticConfig.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * - Constants.kGravity); - float referenceSpeed = StaticConfig.AccelerationConfig?.ReferenceSpeed ?? 1; - return Mathf.Pow(Speed / referenceSpeed, 2) * maxReferenceNormalAcceleration; - } - public Transformation GetRelativeTransformation(IAgent target) { return GetRelativeTransformation(target.Position, target.Velocity, target.Acceleration); } @@ -107,6 +95,22 @@ public Transformation GetRelativeTransformation(in Vector3 waypoint) { return GetRelativeTransformation(waypoint, Vector3.zero, Vector3.zero); } + private void AlignWithVelocity() { + const float SpeedThreshold = 0.1f; + const float MaxDegreesDeltaTimeFactor = 10000f; + + // Only align if the velocity is significant. + if (Speed > SpeedThreshold) { + // Create a rotation with forward along velocity and up along world up. + Quaternion targetRotation = Quaternion.LookRotation(Velocity, Vector3.up); + + // Smoothly rotate towards the target rotation. + transform.rotation = Quaternion.RotateTowards( + transform.rotation, targetRotation, + maxDegreesDelta: MaxDegreesDeltaTimeFactor * Time.fixedDeltaTime); + } + } + private Transformation GetRelativeTransformation(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { var transformation = new Transformation(); diff --git a/Assets/Scripts/Agents/IAgent.cs b/Assets/Scripts/Agents/IAgent.cs index 7d0613c04..fdbe582f0 100644 --- a/Assets/Scripts/Agents/IAgent.cs +++ b/Assets/Scripts/Agents/IAgent.cs @@ -19,6 +19,8 @@ public interface IAgent { Vector3 Acceleration { get; set; } Vector3 AccelerationInput { get; set; } + Transform transform { get; } + Transformation GetRelativeTransformation(IAgent target); Transformation GetRelativeTransformation(IHierarchical target); Transformation GetRelativeTransformation(in Vector3 waypoint); diff --git a/Assets/Scripts/Assignment/MaxSpeedAssignment.cs b/Assets/Scripts/Assignment/MaxSpeedAssignment.cs index e53398688..ec14c08ff 100644 --- a/Assets/Scripts/Assignment/MaxSpeedAssignment.cs +++ b/Assets/Scripts/Assignment/MaxSpeedAssignment.cs @@ -33,13 +33,13 @@ public class MaxSpeedAssignment : IAssignmentLegacy { // The speed decays exponentially with the travelled distance and with the bearing change. float distanceTimeConstant = 2 * (interceptor.staticConfig.BodyConfig?.Mass ?? 0) / - ((float)Constants.CalculateAirDensityAtAltitude(interceptor.GetPosition().y) * + (Constants.CalculateAirDensityAtAltitude(interceptor.GetPosition().y) * (interceptor.staticConfig.LiftDragConfig?.DragCoefficient ?? 0) * (interceptor.staticConfig.BodyConfig?.CrossSectionalArea ?? 0)); float angleTimeConstant = interceptor.staticConfig.LiftDragConfig?.LiftDragRatio ?? 1; // During the turn, the minimum radius dictates the minimum distance needed to make the turn. - float minTurningRadius = (float)(interceptor.GetVelocity().sqrMagnitude / - interceptor.CalculateMaxNormalAcceleration()); + float minTurningRadius = + interceptor.GetVelocity().sqrMagnitude / interceptor.CalculateMaxNormalAcceleration(); for (int threatIndex = 0; threatIndex < activeThreats.Count; ++threatIndex) { Threat threat = activeThreats[threatIndex]; @@ -53,7 +53,7 @@ public class MaxSpeedAssignment : IAssignmentLegacy { float fractionalSpeed = Mathf.Exp( -((distanceToThreat + angleToThreat * minTurningRadius) / distanceTimeConstant + angleToThreat / angleTimeConstant)); - float cost = (float)interceptor.GetSpeed() / fractionalSpeed; + float cost = interceptor.GetSpeed() / fractionalSpeed; assignmentCosts[interceptorIndex * activeThreats.Count + threatIndex] = cost; } } diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index 7c636b406..7077f6ae1 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -307,9 +307,9 @@ private void CheckForEscapingThreats() { Vector3 interceptorPosition = interceptor.GetPosition(); Vector3 threatPosition = threat.GetPosition(); - float threatTimeToHit = (float)(threatPosition.magnitude / threat.GetSpeed()); + float threatTimeToHit = (threatPosition.magnitude / threat.GetSpeed()); float interceptorTimeToHit = - (float)((threatPosition - interceptorPosition).magnitude / interceptor.GetSpeed()); + (threatPosition - interceptorPosition).magnitude / interceptor.GetSpeed(); return interceptorPosition.magnitude > threatPosition.magnitude || threatTimeToHit < interceptorTimeToHit; }); diff --git a/Assets/Scripts/Interceptors/Interceptor.cs b/Assets/Scripts/Interceptors/Interceptor.cs index 556502874..72e16ef78 100644 --- a/Assets/Scripts/Interceptors/Interceptor.cs +++ b/Assets/Scripts/Interceptors/Interceptor.cs @@ -49,7 +49,7 @@ protected override void UpdateBoost(double deltaTime) { // Calculate the boost acceleration. float boostAcceleration = - (float)((staticConfig.BoostConfig?.BoostAcceleration ?? 0) * Constants.kGravity); + (staticConfig.BoostConfig?.BoostAcceleration ?? 0) * Constants.kGravity; Vector3 boostAccelerationVector = boostAcceleration * transform.forward; // Add the PN acceleration to the boost acceleration. diff --git a/Assets/Scripts/Managers/SimManager.cs b/Assets/Scripts/Managers/SimManager.cs index 19526010e..c3a6946c7 100644 --- a/Assets/Scripts/Managers/SimManager.cs +++ b/Assets/Scripts/Managers/SimManager.cs @@ -76,17 +76,17 @@ public class SimManager : MonoBehaviour { public event NewInterceptorEventHandler OnNewInterceptor; // Returns the elapsed simulation time in seconds. - public double GetElapsedSimulationTime() { + public float GetElapsedSimulationTime() { return _elapsedSimulationTime; } // Returns the total cost of launched interceptors. - public double GetCostLaunchedInterceptors() { + public float GetCostLaunchedInterceptors() { return _costLaunchedInterceptors; } // Returns the total cost of destroyed threats. - public double GetCostDestroyedThreats() { + public float GetCostDestroyedThreats() { return _costDestroyedThreats; } @@ -167,7 +167,7 @@ public void PauseSimulation() { } public void ResumeSimulation() { - Time.fixedDeltaTime = (float)(1.0f / SimulatorConfig.PhysicsUpdateRate); + Time.fixedDeltaTime = 1.0f / SimulatorConfig.PhysicsUpdateRate; SetTimeScale(SimulationConfig.TimeScale); _isSimulationPaused = false; } @@ -180,7 +180,7 @@ private void InitializeSimulation() { if (!IsSimulationPaused()) { // If the simulation was not paused, we need to update the time scale. SetTimeScale(SimulationConfig.TimeScale); - Time.fixedDeltaTime = (float)(1.0f / SimulatorConfig.PhysicsUpdateRate); + Time.fixedDeltaTime = 1.0f / SimulatorConfig.PhysicsUpdateRate; // If the simulation was paused, then ResumeSimulation will handle updating the time scale and // fixed delta time from the newly loaded configuration. } diff --git a/Assets/Scripts/Monitors/SimMonitor.cs b/Assets/Scripts/Monitors/SimMonitor.cs index f55f998c2..5a668e9a1 100644 --- a/Assets/Scripts/Monitors/SimMonitor.cs +++ b/Assets/Scripts/Monitors/SimMonitor.cs @@ -91,7 +91,7 @@ private IEnumerator MonitorRoutine() { } private void RecordTelemetry() { - float time = (float)SimManager.Instance.GetElapsedSimulationTime(); + float time = SimManager.Instance.GetElapsedSimulationTime(); var agents = SimManager.Instance.GetActiveAgents(); if (_telemetryBinaryWriter == null) { Debug.LogWarning("Telemetry binary writer is null"); @@ -218,7 +218,7 @@ public void RegisterNewInterceptor(Interceptor interceptor) { } private void RegisterNewAgent(Agent agent, string eventType) { - float time = (float)SimManager.Instance.GetElapsedSimulationTime(); + float time = SimManager.Instance.GetElapsedSimulationTime(); Vector3 pos = agent.transform.position; var record = new EventRecord { Time = time, PositionX = pos.x, PositionY = pos.y, PositionZ = pos.z, EventType = eventType, Details = agent.name }; @@ -238,7 +238,7 @@ public void RegisterInterceptorMiss(Interceptor interceptor, Threat threat) { } public void RegisterInterceptorEvent(Interceptor interceptor, Threat threat, bool hit) { - float time = (float)SimManager.Instance.GetElapsedSimulationTime(); + float time = SimManager.Instance.GetElapsedSimulationTime(); Vector3 pos = interceptor.transform.position; string eventType = hit ? "INTERCEPTOR_HIT" : "INTERCEPTOR_MISS"; var record = new EventRecord { diff --git a/Assets/Scripts/Movements/AerialMovement.cs b/Assets/Scripts/Movements/AerialMovement.cs index ee04ef521..69f860d41 100644 --- a/Assets/Scripts/Movements/AerialMovement.cs +++ b/Assets/Scripts/Movements/AerialMovement.cs @@ -7,15 +7,14 @@ public abstract class AerialMovement : MovementBase { public AerialMovement(IAgent agent) : base(agent) {} - // TODO(titan): Methods to calculate gravity and drag based on the agent as well as the total - // acceleration. - + // Calculate the dynamic pressure on the agent. protected float GetDynamicPressure() { - var airDensity = (float)Constants.CalculateAirDensityAtAltitude(Agent.Position.y); + var airDensity = Constants.CalculateAirDensityAtAltitude(Agent.Position.y); var flowSpeed = Agent.Speed; return 0.5f * airDensity * (flowSpeed * flowSpeed); } + // Calculate the air drag acting on the agent. protected float CalculateDrag() { var staticConfig = Agent.StaticConfig; var dragCoefficient = staticConfig.LiftDragConfig?.DragCoefficient ?? 0; @@ -26,9 +25,12 @@ protected float CalculateDrag() { return dragForce / mass; } + // Calculate the lift-induced drag acting on the agent. Since the agent is flying, any + // acceleration normal to the velocity vector is considered "lift". protected float CalculateLiftInducedDrag(in Vector3 accelerationInput) { var staticConfig = Agent.StaticConfig; - var liftAcceleration = Vector3.ProjectOnPlane(accelerationInput, transform.forward).magnitude; + var liftAcceleration = + Vector3.ProjectOnPlane(accelerationInput, Agent.transform.forward).magnitude; var liftDragRatio = staticConfig.LiftDragConfig?.LiftDragRatio ?? 1; return Mathf.Abs(liftAcceleration / liftDragRatio); } diff --git a/Assets/Scripts/Movements/GroundMovement.cs b/Assets/Scripts/Movements/GroundMovement.cs index 3d5e47b80..4fb86b805 100644 --- a/Assets/Scripts/Movements/GroundMovement.cs +++ b/Assets/Scripts/Movements/GroundMovement.cs @@ -2,14 +2,17 @@ // Ground-based movement. // -// The agent is confined to only move along the y=0 plane. +// The agent is confined to only move along the x-z plane. public class GroundMovement : MovementBase { public GroundMovement(IAgent agent) : base(agent) {} - public override void Update(double deltaTime) { - // Determine the agent's acceleration input using its controller. - var accelerationInput = Vector3.zero; - // There is drag along the ground and ignore gravity. - Agent.AccelerationInput = accelerationInput; + // Determine the agent's actual acceleration input given its intended acceleration input by + // applying physics and other constraints. Ensure that the agent moves along the x-z plane and + // ignore drag and gravity along the ground. + public override Vector3 Act(in Vector3 accelerationInput) { + // Ensure that there is no acceleration out of the x-z plane. + var constrainedAccelerationInput = accelerationInput; + constrainedAccelerationInput.y = 0; + return LimitAccelerationInput(constrainedAccelerationInput); } } diff --git a/Assets/Scripts/Movements/IMovement.cs b/Assets/Scripts/Movements/IMovement.cs index 246ce7913..42657494b 100644 --- a/Assets/Scripts/Movements/IMovement.cs +++ b/Assets/Scripts/Movements/IMovement.cs @@ -1,3 +1,5 @@ +using UnityEngine; + // Interface for a movement behavior. // // The movement behavior determines how the agent navigates through the environment, such as whether @@ -5,7 +7,7 @@ public interface IMovement { IAgent Agent { get; set; } - // Determine the next movement for the agent by using the agent's controller to calculate the - // acceleration input. - void Update(double deltaTime); + // Determine the agent's actual acceleration input given its intended acceleration input by + // applying physics and other constraints. + Vector3 Act(in Vector3 accelerationInput); } diff --git a/Assets/Scripts/Movements/IdealMovement.cs b/Assets/Scripts/Movements/IdealMovement.cs index 0e68e7583..4f4bdedc5 100644 --- a/Assets/Scripts/Movements/IdealMovement.cs +++ b/Assets/Scripts/Movements/IdealMovement.cs @@ -6,13 +6,10 @@ public class IdealMovement : AerialMovement { public IdealMovement(IAgent agent) : base(agent) {} - // Determine the next movement for the agent by using the agent's controller to calculate the - // acceleration input. - // Ideal movement implies no drag or gravity. - public override void Update(double deltaTime) { - // Determine the agent's acceleration input using its controller. - var accelerationInput = Vector3.zero; - // Ignore drag and gravity. - Agent.AccelerationInput = accelerationInput; + // Determine the agent's actual acceleration input given its intended acceleration input by + // applying physics and other constraints. Ideal movement implies no drag or gravity, but the + // agent may be limited by its own maximum forward and normal accelerations. + public override Vector3 Act(in Vector3 accelerationInput) { + return LimitAccelerationInput(accelerationInput); } } diff --git a/Assets/Scripts/Movements/MovementBase.cs b/Assets/Scripts/Movements/MovementBase.cs index ece627bd6..67d9dc1c1 100644 --- a/Assets/Scripts/Movements/MovementBase.cs +++ b/Assets/Scripts/Movements/MovementBase.cs @@ -1,3 +1,6 @@ +using System; +using UnityEngine; + // Base implementation of a movement behavior. public abstract class MovementBase : IMovement { // Agent to which the movement behavior is assigned. @@ -12,7 +15,34 @@ public MovementBase(IAgent agent) { _agent = agent; } - // Determine the next movement for the agent by using the agent's controller to calculate the - // acceleration input. - public abstract void Update(double deltaTime); + // Determine the agent's actual acceleration input given its intended acceleration input by + // applying physics and other constraints. + public abstract Vector3 Act(in Vector3 accelerationInput); + + // Get the maximum forward acceleration from the agent's static configuration. + public float MaxForwardAcceleration() { + return Agent.StaticConfig?.AccelerationConfig?.MaxForwardAcceleration ?? 0; + } + + // Get the maximum normal acceleration from the agent's static configuration. + public float MaxNormalAcceleration() { + var maxReferenceNormalAcceleration = + (Agent.StaticConfig?.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * + Constants.kGravity; + var referenceSpeed = Agent.StaticConfig?.AccelerationConfig?.ReferenceSpeed ?? 1; + return Mathf.Pow(Agent.Speed / referenceSpeed, 2) * maxReferenceNormalAcceleration; + } + + // Limit acceleration input to the agent's maximum forward and normal accelerations. + protected Vector3 LimitAccelerationInput(in Vector3 accelerationInput) { + var forwardAccelerationInput = Vector3.Project(accelerationInput, Agent.transform.forward); + var normalAccelerationInput = accelerationInput - forwardAccelerationInput; + + // Limit the forward and the normal acceleration magnitude. + forwardAccelerationInput = + Vector3.ClampMagnitude(forwardAccelerationInput, MaxForwardAcceleration()); + normalAccelerationInput = + Vector3.ClampMagnitude(normalAccelerationInput, MaxNormalAcceleration()); + return forwardAccelerationInput + normalAccelerationInput; + } } diff --git a/Assets/Scripts/Movements/RocketMovement.cs b/Assets/Scripts/Movements/RocketMovement.cs index a940756f3..da5b2af3c 100644 --- a/Assets/Scripts/Movements/RocketMovement.cs +++ b/Assets/Scripts/Movements/RocketMovement.cs @@ -5,18 +5,75 @@ // A rocket is defined by having three modes during its flight: ready mode (before it is launched), // boost mode (when its burner is on), and midcourse mode (after the burner has completed burning). public class RocketMovement : AerialMovement { - public RocketMovement(IAgent agent) : base(agent) {} - - // Determine the next movement for the agent by using the agent's controller to calculate the - // acceleration input. - public override void Update(double deltaTime) { - // Step through the READY, BOOST, and MIDCOURSE phases. - // Determine the agent's acceleration input using its controller and consider drag and gravity. - var accelerationInput = Vector3.zero; - Agent.AccelerationInput = accelerationInput; + // Flight phase of the agent. + public Simulation.FlightPhase FlightPhase { get; set; } + + public RocketMovement(IAgent agent) : base(agent) { + FlightPhase = Simulation.FlightPhase.Initialized; + } + + // Determine the agent's actual acceleration input given its intended acceleration input by + // applying physics and other constraints. + public override Vector3 Act(in Vector3 accelerationInput) { + switch (FlightPhase) { + case Simulation.FlightPhase.Initialized: { + // In the initialized phase, the agent is not subject to any acceleration. + return Vector3.zero; + } + case Simulation.FlightPhase.Ready: { + return ActReady(accelerationInput); + } + case Simulation.FlightPhase.Boost: { + return ActBoost(accelerationInput); + } + case Simulation.FlightPhase.Midcourse: { + return ActMidCourse(accelerationInput); + } + case Simulation.FlightPhase.Terminal: { + // + return ActMidCourse(accelerationInput); + } + case Simulation.FlightPhase.Terminated: { + // In the terminated phase, the agent is not subject to any acceleration. + return Vector3.zero; + } + default: { + return Vector3.zero; + } + } } - private void UpdateReady(double deltaTime) {} - private void UpdateBoost(double deltaTime) {} - private void UpdateMidCourse(double deltaTime) {} + // In the ready phase, the agent is subject to drag and gravity but has not boosted yet. + private Vector3 ActReady(in Vector3 accelerationInput) { + var limitedAccelerationInput = LimitAccelerationInput(accelerationInput); + return CalculateNetAccelerationInput(limitedAccelerationInput); + } + + // In the boost phase, the boost acceleration is added to the control acceleration input. + private Vector3 ActBoost(in Vector3 accelerationInput) { + var limitedAccelerationInput = LimitAccelerationInput(accelerationInput); + + // Determine the boost acceleration. + var boostAcceleration = + (Agent.StaticConfig?.BoostConfig?.BoostAcceleration ?? 0) * Constants.kGravity; + var totalAccelerationInput = + boostAcceleration * Agent.transform.forward + limitedAccelerationInput; + return CalculateNetAccelerationInput(totalAccelerationInput); + } + + // In the midcourse phase, the agent accelerates according to the control acceleration input but + // is subject to drag and gravity. + private Vector3 ActMidCourse(in Vector3 accelerationInput) { + var limitedAccelerationInput = LimitAccelerationInput(accelerationInput); + return CalculateNetAccelerationInput(limitedAccelerationInput); + } + + // Calculate the acceleration input with drag and gravity. + private Vector3 CalculateNetAccelerationInput(in Vector3 accelerationInput) { + var gravity = Physics.gravity; + var airDrag = CalculateDrag(); + var liftInducedDrag = CalculateLiftInducedDrag(accelerationInput + Physics.gravity); + var dragAcceleration = -(airDrag + liftInducedDrag); + return accelerationInput + gravity + dragAcceleration * Agent.transform.forward; + } } diff --git a/Assets/Scripts/Threats/FixedWingThreat.cs b/Assets/Scripts/Threats/FixedWingThreat.cs index 16026c5f7..ff1e51fac 100644 --- a/Assets/Scripts/Threats/FixedWingThreat.cs +++ b/Assets/Scripts/Threats/FixedWingThreat.cs @@ -74,8 +74,7 @@ private Vector3 CalculateAccelerationInput() { Vector3 accelerationInput = controller.PlanToWaypoint(_currentWaypoint); // Counter gravity as much as possible. - accelerationInput += - (float)Constants.kGravity / Vector3.Dot(transform.up, Vector3.up) * transform.up; + accelerationInput += Constants.kGravity / Vector3.Dot(transform.up, Vector3.up) * transform.up; // Clamp the normal acceleration input to the maximum normal acceleration. float maxNormalAcceleration = CalculateMaxNormalAcceleration(); diff --git a/Assets/Scripts/Utils/Constants.cs b/Assets/Scripts/Utils/Constants.cs index 70d75ed4c..2e5a5564e 100644 --- a/Assets/Scripts/Utils/Constants.cs +++ b/Assets/Scripts/Utils/Constants.cs @@ -1,17 +1,17 @@ using System; public static class Constants { - // Constants (these should be defined with appropriate values) - public const double kAirDensity = 1.204; // Sea level air density in kg/m^3 - public const double kAirDensityScaleHeight = 10.4; // Scale height in km - public const double kGravity = 9.80665; // Standard gravity in m/s^2 - public const double kEarthMeanRadius = 6378137; // Earth's mean radius in meters + // Constants (these should be defined with appropriate values). + public const float kAirDensity = 1.204f; // Sea level air density in kg/m^3. + public const float kAirDensityScaleHeight = 10.4f; // Scale height in km. + public const float kGravity = 9.80665f; // Standard gravity in m/s^2. + public const float kEarthMeanRadius = 6378137f; // Earth's mean radius in meters. - public static double CalculateAirDensityAtAltitude(double altitude) { - return kAirDensity * Math.Exp(-altitude / (kAirDensityScaleHeight * 1000)); + public static float CalculateAirDensityAtAltitude(float altitude) { + return kAirDensity * MathF.Exp(-altitude / (kAirDensityScaleHeight * 1000)); } - public static double CalculateGravityAtAltitude(double altitude) { - return kGravity * Math.Pow(kEarthMeanRadius / (kEarthMeanRadius + altitude), 2); + public static float CalculateGravityAtAltitude(float altitude) { + return kGravity * MathF.Pow(kEarthMeanRadius / (kEarthMeanRadius + altitude), 2); } } diff --git a/docs/Simulator_Overview.md b/docs/Simulator_Overview.md index 8dc65c7d7..d0137eb8e 100644 --- a/docs/Simulator_Overview.md +++ b/docs/Simulator_Overview.md @@ -275,7 +275,7 @@ float angleToThreat = float fractionalSpeed = Mathf.Exp( -((distanceToThreat + angleToThreat * minTurningRadius) / distanceTimeConstant + angleToThreat / angleTimeConstant)); -float cost = (float)interceptor.GetSpeed() / fractionalSpeed; +float cost = interceptor.GetSpeed() / fractionalSpeed; ``` Cost-based assignment schemes can be formulated as integer linear programming (ILP) problems that optimize over boolean variables $x_{mt}$ representing whether missile $m$ is assigned to threat $t$. From 4d177bc7729f92d0e4fd27de177df4c75be368db Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 13:59:53 -0700 Subject: [PATCH 17/74] Add unit tests for movement behaviors --- Assets/Tests/EditMode/GroundMovementTests.cs | 36 ++++++ .../EditMode/GroundMovementTests.cs.meta | 2 + Assets/Tests/EditMode/IdealMovementTests.cs | 54 +++++++++ .../Tests/EditMode/IdealMovementTests.cs.meta | 2 + Assets/Tests/EditMode/RocketMovementTests.cs | 107 ++++++++++++++++++ .../EditMode/RocketMovementTests.cs.meta | 2 + 6 files changed, 203 insertions(+) create mode 100644 Assets/Tests/EditMode/GroundMovementTests.cs create mode 100644 Assets/Tests/EditMode/GroundMovementTests.cs.meta create mode 100644 Assets/Tests/EditMode/IdealMovementTests.cs create mode 100644 Assets/Tests/EditMode/IdealMovementTests.cs.meta create mode 100644 Assets/Tests/EditMode/RocketMovementTests.cs create mode 100644 Assets/Tests/EditMode/RocketMovementTests.cs.meta diff --git a/Assets/Tests/EditMode/GroundMovementTests.cs b/Assets/Tests/EditMode/GroundMovementTests.cs new file mode 100644 index 000000000..0d781311a --- /dev/null +++ b/Assets/Tests/EditMode/GroundMovementTests.cs @@ -0,0 +1,36 @@ +using NUnit.Framework; +using UnityEngine; + +public class GroundMovementTests : AgentTestBase { + private AgentBase _agent; + private GroundMovement _movement; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + _agent.StaticConfig = new Configs.StaticConfig() { + AccelerationConfig = + new Configs.AccelerationConfig() { + MaxForwardAcceleration = 10, + MaxReferenceNormalAcceleration = 5 / Constants.kGravity, + ReferenceSpeed = 100, + }, + BoostConfig = + new Configs.BoostConfig() { + BoostTime = 1, + BoostAcceleration = 100, + }, + }; + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _agent.Velocity = new Vector3(x: 0, y: 0, z: 200); + _movement = new GroundMovement(_agent); + } + + [Test] + public void Act_KeepsAgentWithinXZPlane() { + var accelerationInput = new Vector3(1, -2, 3); + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(new Vector3(1, 0, 3), appliedAccelerationInput); + } +} diff --git a/Assets/Tests/EditMode/GroundMovementTests.cs.meta b/Assets/Tests/EditMode/GroundMovementTests.cs.meta new file mode 100644 index 000000000..f4f122f31 --- /dev/null +++ b/Assets/Tests/EditMode/GroundMovementTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 1e83c8793bd9b40478062bfbafb77b6c \ No newline at end of file diff --git a/Assets/Tests/EditMode/IdealMovementTests.cs b/Assets/Tests/EditMode/IdealMovementTests.cs new file mode 100644 index 000000000..8d3ddf899 --- /dev/null +++ b/Assets/Tests/EditMode/IdealMovementTests.cs @@ -0,0 +1,54 @@ +using NUnit.Framework; +using UnityEngine; +using UnityEngine.TestTools.Utils; + +public class IdealMovementTests : AgentTestBase { + private const float _epsilon = 1e-3f; + + private AgentBase _agent; + private IdealMovement _movement; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + _agent.StaticConfig = new Configs.StaticConfig() { + AccelerationConfig = + new Configs.AccelerationConfig() { + MaxForwardAcceleration = 10, + MaxReferenceNormalAcceleration = 5 / Constants.kGravity, + ReferenceSpeed = 100, + }, + BoostConfig = + new Configs.BoostConfig() { + BoostTime = 1, + BoostAcceleration = 100, + }, + }; + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _agent.Velocity = new Vector3(x: 0, y: 0, z: 200); + _movement = new IdealMovement(_agent); + } + + [Test] + public void Act_LimitsForwardAccelerationInput() { + var accelerationInput = new Vector3(0, 0, 100); + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(new Vector3(0, 0, 10), appliedAccelerationInput); + } + + [Test] + public void Act_LimitsForwardAccelerationInput_NegativeAcceleration() { + var accelerationInput = new Vector3(0, 0, -20); + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(new Vector3(0, 0, -10), appliedAccelerationInput); + } + + [Test] + public void Act_LimitsNormalAccelerationInput() { + var accelerationInput = new Vector3(0, 50, 0); + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.That(appliedAccelerationInput, + Is.EqualTo(new Vector3(0, 20f, 0)).Using(Vector3EqualityComparer.Instance)); + } +} diff --git a/Assets/Tests/EditMode/IdealMovementTests.cs.meta b/Assets/Tests/EditMode/IdealMovementTests.cs.meta new file mode 100644 index 000000000..8b74e399e --- /dev/null +++ b/Assets/Tests/EditMode/IdealMovementTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 3d4b1a77a1aa34f9e9f5a1a6e764d3e6 \ No newline at end of file diff --git a/Assets/Tests/EditMode/RocketMovementTests.cs b/Assets/Tests/EditMode/RocketMovementTests.cs new file mode 100644 index 000000000..5db954459 --- /dev/null +++ b/Assets/Tests/EditMode/RocketMovementTests.cs @@ -0,0 +1,107 @@ +using NUnit.Framework; +using UnityEngine; + +public class RocketMovementTests : AgentTestBase { + private const float _epsilon = 1e-3f; + + private AgentBase _agent; + private RocketMovement _movement; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + _agent.StaticConfig = new Configs.StaticConfig() { + AccelerationConfig = + new Configs.AccelerationConfig() { + MaxForwardAcceleration = 10, + MaxReferenceNormalAcceleration = 5 / Constants.kGravity, + ReferenceSpeed = 100, + }, + BoostConfig = + new Configs.BoostConfig() { + BoostTime = 1, + BoostAcceleration = 100, + }, + LiftDragConfig = + new Configs.LiftDragConfig() { + DragCoefficient = 1, + LiftDragRatio = 1, + }, + BodyConfig = + new Configs.BodyConfig() { + CrossSectionalArea = 1, + Mass = 1, + }, + }; + Rigidbody agentRb = _agent.gameObject.AddComponent(); + _agent.transform.rotation = Quaternion.LookRotation(new Vector3(0, 0, 1)); + InvokePrivateMethod(_agent, "Awake"); + _movement = new RocketMovement(_agent); + } + + [Test] + public void Act_Initialized_ReturnsZero() { + _movement.FlightPhase = Simulation.FlightPhase.Initialized; + var accelerationInput = new Vector3(1, -2, 3); + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Vector3.zero, appliedAccelerationInput); + } + + [Test] + public void Act_Ready_AddsAirDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Ready; + _agent.Velocity = new Vector3(0, 0, 100); + var accelerationInput = Vector3.zero; + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Physics.gravity.y - 6020, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Ready_AddsLiftInducedDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Ready; + var accelerationInput = Vector3.zero; + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Ready_AddsGravity() { + _movement.FlightPhase = Simulation.FlightPhase.Ready; + var accelerationInput = Vector3.zero; + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.y, _epsilon); + } + + [Test] + public void Act_Midcourse_AddsAirDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Midcourse; + _agent.Velocity = new Vector3(0, 0, 100); + var accelerationInput = Vector3.zero; + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Physics.gravity.y - 6020, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Midcourse_AddsLiftInducedDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Midcourse; + var accelerationInput = Vector3.zero; + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_MidCourse_AddsGravity() { + _movement.FlightPhase = Simulation.FlightPhase.Midcourse; + var accelerationInput = Vector3.zero; + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.y, _epsilon); + } + + [Test] + public void Act_Terminated_ReturnsZero() { + _movement.FlightPhase = Simulation.FlightPhase.Terminated; + var accelerationInput = new Vector3(1, -2, 3); + var appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Vector3.zero, appliedAccelerationInput); + } +} diff --git a/Assets/Tests/EditMode/RocketMovementTests.cs.meta b/Assets/Tests/EditMode/RocketMovementTests.cs.meta new file mode 100644 index 000000000..09a32b643 --- /dev/null +++ b/Assets/Tests/EditMode/RocketMovementTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 563e865a0f6864f878a9dece8c19fa5a \ No newline at end of file From 8d2dea3353e80acc7e549736956c3dfd168e71ae Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 14:39:38 -0700 Subject: [PATCH 18/74] Address comments --- Assets/Scripts/Agents/AgentBase.cs | 7 ++++--- Assets/Scripts/Movements/RocketMovement.cs | 10 ++++++++-- Assets/Scripts/Utils/Constants.cs | 9 +++++---- Assets/Tests/EditMode/RocketMovementTests.cs | 12 ++++++------ 4 files changed, 23 insertions(+), 15 deletions(-) diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index dfb5dffab..9375df6c2 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -97,17 +97,18 @@ public Transformation GetRelativeTransformation(in Vector3 waypoint) { private void AlignWithVelocity() { const float SpeedThreshold = 0.1f; - const float MaxDegreesDeltaTimeFactor = 10000f; + const float RotationSpeedDegreesPerSecond = 10000f; // Only align if the velocity is significant. if (Speed > SpeedThreshold) { - // Create a rotation with forward along velocity and up along world up. + // Create a rotation with the forward direction along the velocity vector and the up direction + // along world up. Quaternion targetRotation = Quaternion.LookRotation(Velocity, Vector3.up); // Smoothly rotate towards the target rotation. transform.rotation = Quaternion.RotateTowards( transform.rotation, targetRotation, - maxDegreesDelta: MaxDegreesDeltaTimeFactor * Time.fixedDeltaTime); + maxDegreesDelta: RotationSpeedDegreesPerSecond * Time.fixedDeltaTime); } } diff --git a/Assets/Scripts/Movements/RocketMovement.cs b/Assets/Scripts/Movements/RocketMovement.cs index da5b2af3c..8133db38e 100644 --- a/Assets/Scripts/Movements/RocketMovement.cs +++ b/Assets/Scripts/Movements/RocketMovement.cs @@ -30,8 +30,7 @@ public override Vector3 Act(in Vector3 accelerationInput) { return ActMidCourse(accelerationInput); } case Simulation.FlightPhase.Terminal: { - // - return ActMidCourse(accelerationInput); + return ActTerminal(accelerationInput); } case Simulation.FlightPhase.Terminated: { // In the terminated phase, the agent is not subject to any acceleration. @@ -68,6 +67,13 @@ private Vector3 ActMidCourse(in Vector3 accelerationInput) { return CalculateNetAccelerationInput(limitedAccelerationInput); } + // In the terminal phase, the agent is homing in on the target and is still subject to drag and + // gravity. + private Vector3 ActTerminal(in Vector3 accelerationInput) { + // Currently, the agent acts the same in the terminal phase as in the midcourse phase. + return ActMidCourse(accelerationInput); + } + // Calculate the acceleration input with drag and gravity. private Vector3 CalculateNetAccelerationInput(in Vector3 accelerationInput) { var gravity = Physics.gravity; diff --git a/Assets/Scripts/Utils/Constants.cs b/Assets/Scripts/Utils/Constants.cs index 2e5a5564e..8df717117 100644 --- a/Assets/Scripts/Utils/Constants.cs +++ b/Assets/Scripts/Utils/Constants.cs @@ -1,11 +1,12 @@ using System; +using UnityEngine; public static class Constants { // Constants (these should be defined with appropriate values). - public const float kAirDensity = 1.204f; // Sea level air density in kg/m^3. - public const float kAirDensityScaleHeight = 10.4f; // Scale height in km. - public const float kGravity = 9.80665f; // Standard gravity in m/s^2. - public const float kEarthMeanRadius = 6378137f; // Earth's mean radius in meters. + public const float kAirDensity = 1.204f; // Sea level air density in kg/m^3. + public const float kAirDensityScaleHeight = 10.4f; // Scale height in km. + public static readonly float kGravity = -Physics.gravity.y; // Standard gravity in m/s^2. + public const float kEarthMeanRadius = 6378137f; // Earth's mean radius in meters. public static float CalculateAirDensityAtAltitude(float altitude) { return kAirDensity * MathF.Exp(-altitude / (kAirDensityScaleHeight * 1000)); diff --git a/Assets/Tests/EditMode/RocketMovementTests.cs b/Assets/Tests/EditMode/RocketMovementTests.cs index 5db954459..15609cfff 100644 --- a/Assets/Tests/EditMode/RocketMovementTests.cs +++ b/Assets/Tests/EditMode/RocketMovementTests.cs @@ -53,7 +53,7 @@ public void Act_Ready_AddsAirDrag() { _agent.Velocity = new Vector3(0, 0, 100); var accelerationInput = Vector3.zero; var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Physics.gravity.y - 6020, appliedAccelerationInput.z, _epsilon); + Assert.AreEqual(-6020f - Constants.kGravity, appliedAccelerationInput.z, _epsilon); } [Test] @@ -61,7 +61,7 @@ public void Act_Ready_AddsLiftInducedDrag() { _movement.FlightPhase = Simulation.FlightPhase.Ready; var accelerationInput = Vector3.zero; var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.z, _epsilon); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.z, _epsilon); } [Test] @@ -69,7 +69,7 @@ public void Act_Ready_AddsGravity() { _movement.FlightPhase = Simulation.FlightPhase.Ready; var accelerationInput = Vector3.zero; var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.y, _epsilon); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.y, _epsilon); } [Test] @@ -78,7 +78,7 @@ public void Act_Midcourse_AddsAirDrag() { _agent.Velocity = new Vector3(0, 0, 100); var accelerationInput = Vector3.zero; var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Physics.gravity.y - 6020, appliedAccelerationInput.z, _epsilon); + Assert.AreEqual(-6020f - Constants.kGravity, appliedAccelerationInput.z, _epsilon); } [Test] @@ -86,7 +86,7 @@ public void Act_Midcourse_AddsLiftInducedDrag() { _movement.FlightPhase = Simulation.FlightPhase.Midcourse; var accelerationInput = Vector3.zero; var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.z, _epsilon); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.z, _epsilon); } [Test] @@ -94,7 +94,7 @@ public void Act_MidCourse_AddsGravity() { _movement.FlightPhase = Simulation.FlightPhase.Midcourse; var accelerationInput = Vector3.zero; var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Physics.gravity.y, appliedAccelerationInput.y, _epsilon); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.y, _epsilon); } [Test] From 37ce45743df9b283fe0ebf84caed87181ba9089b Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 15:13:47 -0700 Subject: [PATCH 19/74] Fix style of local constant variable --- Assets/Scripts/Agents/AgentBase.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 9375df6c2..2be5e70e7 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -96,11 +96,11 @@ public Transformation GetRelativeTransformation(in Vector3 waypoint) { } private void AlignWithVelocity() { - const float SpeedThreshold = 0.1f; - const float RotationSpeedDegreesPerSecond = 10000f; + const float speedThreshold = 0.1f; + const float rotationSpeedDegreesPerSecond = 10000f; // Only align if the velocity is significant. - if (Speed > SpeedThreshold) { + if (Speed > speedThreshold) { // Create a rotation with the forward direction along the velocity vector and the up direction // along world up. Quaternion targetRotation = Quaternion.LookRotation(Velocity, Vector3.up); @@ -108,7 +108,7 @@ private void AlignWithVelocity() { // Smoothly rotate towards the target rotation. transform.rotation = Quaternion.RotateTowards( transform.rotation, targetRotation, - maxDegreesDelta: RotationSpeedDegreesPerSecond * Time.fixedDeltaTime); + maxDegreesDelta: rotationSpeedDegreesPerSecond * Time.fixedDeltaTime); } } From 5b241222bb855df6c0fe2513173b575e1ee837cf Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 16:36:28 -0700 Subject: [PATCH 20/74] Inherit from TestBase instead of AgentTestBase in movement tests --- Assets/Tests/EditMode/GroundMovementTests.cs | 2 +- Assets/Tests/EditMode/IdealMovementTests.cs | 2 +- Assets/Tests/EditMode/RocketMovementTests.cs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Assets/Tests/EditMode/GroundMovementTests.cs b/Assets/Tests/EditMode/GroundMovementTests.cs index 0d781311a..423bccf1d 100644 --- a/Assets/Tests/EditMode/GroundMovementTests.cs +++ b/Assets/Tests/EditMode/GroundMovementTests.cs @@ -1,7 +1,7 @@ using NUnit.Framework; using UnityEngine; -public class GroundMovementTests : AgentTestBase { +public class GroundMovementTests : TestBase { private AgentBase _agent; private GroundMovement _movement; diff --git a/Assets/Tests/EditMode/IdealMovementTests.cs b/Assets/Tests/EditMode/IdealMovementTests.cs index 8d3ddf899..b17a2b829 100644 --- a/Assets/Tests/EditMode/IdealMovementTests.cs +++ b/Assets/Tests/EditMode/IdealMovementTests.cs @@ -2,7 +2,7 @@ using UnityEngine; using UnityEngine.TestTools.Utils; -public class IdealMovementTests : AgentTestBase { +public class IdealMovementTests : TestBase { private const float _epsilon = 1e-3f; private AgentBase _agent; diff --git a/Assets/Tests/EditMode/RocketMovementTests.cs b/Assets/Tests/EditMode/RocketMovementTests.cs index 15609cfff..50a05ad12 100644 --- a/Assets/Tests/EditMode/RocketMovementTests.cs +++ b/Assets/Tests/EditMode/RocketMovementTests.cs @@ -1,7 +1,7 @@ using NUnit.Framework; using UnityEngine; -public class RocketMovementTests : AgentTestBase { +public class RocketMovementTests : TestBase { private const float _epsilon = 1e-3f; private AgentBase _agent; From 3055f7233336c245cb2e64ea25d2ec9205a57a7b Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 16:08:41 -0700 Subject: [PATCH 21/74] Add legacy suffix to sensor classes --- Assets/Scripts/Interceptors/Interceptor.cs | 2 +- Assets/Scripts/Managers/SimManager.cs | 2 +- .../Sensors/{IdealSensor.cs => IdealSensorLegacy.cs} | 2 +- .../{IdealSensor.cs.meta => IdealSensorLegacy.cs.meta} | 0 Assets/Scripts/Sensors/Sensor.cs | 7 +------ Assets/Scripts/Sensors/SensorOutput.cs | 9 +++++++++ Assets/Scripts/Sensors/SensorOutput.cs.meta | 2 ++ Assets/Scripts/Threats/Threat.cs | 4 ++-- Assets/Tests/EditMode/AgentTestBase.cs | 4 ++-- 9 files changed, 19 insertions(+), 13 deletions(-) rename Assets/Scripts/Sensors/{IdealSensor.cs => IdealSensorLegacy.cs} (95%) rename Assets/Scripts/Sensors/{IdealSensor.cs.meta => IdealSensorLegacy.cs.meta} (100%) create mode 100644 Assets/Scripts/Sensors/SensorOutput.cs create mode 100644 Assets/Scripts/Sensors/SensorOutput.cs.meta diff --git a/Assets/Scripts/Interceptors/Interceptor.cs b/Assets/Scripts/Interceptors/Interceptor.cs index 72e16ef78..7af164616 100644 --- a/Assets/Scripts/Interceptors/Interceptor.cs +++ b/Assets/Scripts/Interceptors/Interceptor.cs @@ -90,7 +90,7 @@ private Vector3 CalculateAccelerationInput(double deltaTime) { UpdateTargetModel(deltaTime); // Check whether the threat should be considered a miss. - SensorOutput sensorOutput = GetComponent().Sense(_target); + SensorOutput sensorOutput = GetComponent().Sense(_target); // TODO(dlovell): This causes trouble with the Fateh 110B (high-speed threats). // if (sensorOutput.velocity.range > 1000f) { // HandleInterceptMiss(); diff --git a/Assets/Scripts/Managers/SimManager.cs b/Assets/Scripts/Managers/SimManager.cs index c3a6946c7..8f60725f1 100644 --- a/Assets/Scripts/Managers/SimManager.cs +++ b/Assets/Scripts/Managers/SimManager.cs @@ -394,7 +394,7 @@ public Interceptor CreateInterceptor(Configs.AgentConfig config, Simulation.Stat // Create a sensor for the interceptor. switch (config.DynamicConfig.SensorConfig.Type) { case Simulation.SensorType.Ideal: { - interceptorObject.AddComponent(); + interceptorObject.AddComponent(); break; } default: { diff --git a/Assets/Scripts/Sensors/IdealSensor.cs b/Assets/Scripts/Sensors/IdealSensorLegacy.cs similarity index 95% rename from Assets/Scripts/Sensors/IdealSensor.cs rename to Assets/Scripts/Sensors/IdealSensorLegacy.cs index 13c465e0d..287c3d448 100644 --- a/Assets/Scripts/Sensors/IdealSensor.cs +++ b/Assets/Scripts/Sensors/IdealSensorLegacy.cs @@ -1,6 +1,6 @@ using UnityEngine; -public class IdealSensor : Sensor { +public class IdealSensorLegacy : SensorLegacy { public override SensorOutput Sense(Agent agent) { SensorOutput agentSensorOutput = new SensorOutput(); diff --git a/Assets/Scripts/Sensors/IdealSensor.cs.meta b/Assets/Scripts/Sensors/IdealSensorLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Sensors/IdealSensor.cs.meta rename to Assets/Scripts/Sensors/IdealSensorLegacy.cs.meta diff --git a/Assets/Scripts/Sensors/Sensor.cs b/Assets/Scripts/Sensors/Sensor.cs index ca028ebea..05bac3cce 100644 --- a/Assets/Scripts/Sensors/Sensor.cs +++ b/Assets/Scripts/Sensors/Sensor.cs @@ -1,7 +1,7 @@ using System; using UnityEngine; -public abstract class Sensor : MonoBehaviour { +public abstract class SensorLegacy : MonoBehaviour { protected Agent _agent; protected virtual void Start() { @@ -16,8 +16,3 @@ protected virtual void Start() { /// Returns the sensor output containing the sensed position and velocity. public abstract SensorOutput SenseWaypoint(Vector3 waypoint); } - -public struct SensorOutput { - public PositionTransformation position; - public VelocityTransformation velocity; -} diff --git a/Assets/Scripts/Sensors/SensorOutput.cs b/Assets/Scripts/Sensors/SensorOutput.cs new file mode 100644 index 000000000..f56a197df --- /dev/null +++ b/Assets/Scripts/Sensors/SensorOutput.cs @@ -0,0 +1,9 @@ +// The sensor output defines the output of a sensor. The transformations are relative to the sensing +// agent's state. +public struct SensorOutput { + // Relative position transformation. + public PositionTransformation position; + + // Relative velocity transformation. + public VelocityTransformation velocity; +} diff --git a/Assets/Scripts/Sensors/SensorOutput.cs.meta b/Assets/Scripts/Sensors/SensorOutput.cs.meta new file mode 100644 index 000000000..a9026aeb3 --- /dev/null +++ b/Assets/Scripts/Sensors/SensorOutput.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 00d11f5e410504b9a99805f351e5d99b \ No newline at end of file diff --git a/Assets/Scripts/Threats/Threat.cs b/Assets/Scripts/Threats/Threat.cs index da06e02b8..ca919ac14 100644 --- a/Assets/Scripts/Threats/Threat.cs +++ b/Assets/Scripts/Threats/Threat.cs @@ -10,7 +10,7 @@ public abstract class Threat : Agent { protected Configs.Power _currentPower; protected SensorOutput _sensorOutput; - protected Sensor _sensor; + protected SensorLegacy _sensor; protected override void Awake() { base.Awake(); @@ -39,7 +39,7 @@ public override bool IsAssignable() { protected override void Start() { base.Start(); - _sensor = GetComponent(); + _sensor = GetComponent(); } protected override void FixedUpdate() { diff --git a/Assets/Tests/EditMode/AgentTestBase.cs b/Assets/Tests/EditMode/AgentTestBase.cs index 8b462b34d..c7161a20e 100644 --- a/Assets/Tests/EditMode/AgentTestBase.cs +++ b/Assets/Tests/EditMode/AgentTestBase.cs @@ -27,14 +27,14 @@ protected Interceptor CreateTestInterceptor(Configs.AgentConfig config) { config, new Simulation.State() { Position = Coordinates3.ToProto(Vector3.zero), Velocity = Coordinates3.ToProto(Vector3.zero) }); InvokePrivateMethod(interceptor, "Start"); - InvokePrivateMethod(interceptor.gameObject.GetComponent(), "Start"); + InvokePrivateMethod(interceptor.gameObject.GetComponent(), "Start"); return interceptor; } protected Threat CreateTestThreat(Configs.AgentConfig config) { Threat threat = SimManager.Instance.CreateThreat(config); InvokePrivateMethod(threat, "Start"); - InvokePrivateMethod(threat.gameObject.GetComponent(), "Start"); + InvokePrivateMethod(threat.gameObject.GetComponent(), "Start"); return threat; } } From ccf32b0c80cbec4661626cb4ba871a0aec9df8ac Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 16:28:02 -0700 Subject: [PATCH 22/74] Capitalize public struct attributes --- Assets/Scripts/Agents/Agent.cs | 34 ++++++------- Assets/Scripts/Agents/AgentBase.cs | 36 +++++++------- Assets/Scripts/Controller/ApnController.cs | 2 +- Assets/Scripts/Controller/PnController.cs | 10 ++-- .../Scripts/Controller/WaypointController.cs | 2 +- Assets/Scripts/Sensors/IdealSensorLegacy.cs | 8 ++-- Assets/Scripts/Sensors/SensorOutput.cs | 4 +- Assets/Scripts/Threats/Threat.cs | 6 +-- Assets/Scripts/Utils/Transformation.cs | 24 +++++----- Assets/Tests/EditMode/AgentBaseTests.cs | 48 +++++++++---------- Assets/Tests/EditMode/TransformationTests.cs | 48 +++++++++---------- 11 files changed, 111 insertions(+), 111 deletions(-) diff --git a/Assets/Scripts/Agents/Agent.cs b/Assets/Scripts/Agents/Agent.cs index 003400f6f..ca606784b 100644 --- a/Assets/Scripts/Agents/Agent.cs +++ b/Assets/Scripts/Agents/Agent.cs @@ -213,15 +213,15 @@ public Transformation GetRelativeTransformation(Agent target) { Transformation transformation = new Transformation(); // Get the relative position transformation. - transformation.position = + transformation.Position = GetRelativePositionTransformation(target.GetPosition() - GetPosition()); // Get the relative velocity transformation. - transformation.velocity = GetRelativeVelocityTransformation( + transformation.Velocity = GetRelativeVelocityTransformation( target.GetPosition() - GetPosition(), target.GetVelocity() - GetVelocity()); // Get the relative acceleration transformation. - transformation.acceleration = GetRelativeAccelerationTransformation(target); + transformation.Acceleration = GetRelativeAccelerationTransformation(target); return transformation; } @@ -229,10 +229,10 @@ public Transformation GetRelativeTransformationToWaypoint(Vector3 waypoint) { Transformation transformation = new Transformation(); // Get the relative position transformation. - transformation.position = GetRelativePositionTransformation(waypoint - GetPosition()); + transformation.Position = GetRelativePositionTransformation(waypoint - GetPosition()); // Get the relative velocity transformation. - transformation.velocity = + transformation.Velocity = GetRelativeVelocityTransformation(waypoint - GetPosition(), -GetVelocity()); return transformation; @@ -242,23 +242,23 @@ private PositionTransformation GetRelativePositionTransformation(Vector3 relativ PositionTransformation positionTransformation = new PositionTransformation(); // Set the relative position in Cartesian coordinates. - positionTransformation.cartesian = relativePosition; + positionTransformation.Cartesian = relativePosition; // Calculate the distance (range) to the target. - positionTransformation.range = relativePosition.magnitude; + positionTransformation.Range = relativePosition.magnitude; Vector3 flatRelativePosition = Vector3.ProjectOnPlane(relativePosition, transform.up); Vector3 verticalRelativePosition = relativePosition - flatRelativePosition; // Calculate the elevation (vertical angle relative to forward). - positionTransformation.elevation = + positionTransformation.Elevation = Mathf.Atan(verticalRelativePosition.magnitude / flatRelativePosition.magnitude); // Calculate the azimuth (horizontal angle relative to forward). if (flatRelativePosition.magnitude == 0) { - positionTransformation.azimuth = 0; + positionTransformation.Azimuth = 0; } else { - positionTransformation.azimuth = + positionTransformation.Azimuth = Vector3.SignedAngle(transform.forward, flatRelativePosition, transform.up) * Mathf.PI / 180; } @@ -271,10 +271,10 @@ private VelocityTransformation GetRelativeVelocityTransformation(Vector3 relativ VelocityTransformation velocityTransformation = new VelocityTransformation(); // Set the relative velocity in Cartesian coordinates. - velocityTransformation.cartesian = relativeVelocity; + velocityTransformation.Cartesian = relativeVelocity; // Calculate range rate (radial velocity). - velocityTransformation.range = Vector3.Dot(relativeVelocity, relativePosition.normalized); + velocityTransformation.Range = Vector3.Dot(relativeVelocity, relativePosition.normalized); // Project relative velocity onto the sphere passing through the target. Vector3 tangentialVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); @@ -298,10 +298,10 @@ private VelocityTransformation GetRelativeVelocityTransformation(Vector3 relativ Vector3 tangentialVelocityOnAzimuth = Vector3.Project(tangentialVelocity, targetAzimuth); // Calculate the time derivative of the azimuth to the target. - velocityTransformation.azimuth = + velocityTransformation.Azimuth = tangentialVelocityOnAzimuth.magnitude / relativePosition.magnitude; if (Vector3.Dot(tangentialVelocityOnAzimuth, targetAzimuth) < 0) { - velocityTransformation.azimuth *= -1; + velocityTransformation.Azimuth *= -1; } // Project the velocity vector on the azimuth-elevation sphere onto the target @@ -309,10 +309,10 @@ private VelocityTransformation GetRelativeVelocityTransformation(Vector3 relativ Vector3 tangentialVelocityOnElevation = Vector3.Project(tangentialVelocity, targetElevation); // Calculate the time derivative of the elevation to the target. - velocityTransformation.elevation = + velocityTransformation.Elevation = tangentialVelocityOnElevation.magnitude / relativePosition.magnitude; if (Vector3.Dot(tangentialVelocityOnElevation, targetElevation) < 0) { - velocityTransformation.elevation *= -1; + velocityTransformation.Elevation *= -1; } return velocityTransformation; @@ -322,7 +322,7 @@ private AccelerationTransformation GetRelativeAccelerationTransformation(Agent a // Since the agent's acceleration is an input, the relative acceleration is just the agent's // acceleration. AccelerationTransformation accelerationTransformation = new AccelerationTransformation(); - accelerationTransformation.cartesian = agent.GetAcceleration(); + accelerationTransformation.Cartesian = agent.GetAcceleration(); return accelerationTransformation; } diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 2be5e70e7..e5a7f8e88 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -118,16 +118,16 @@ private Transformation GetRelativeTransformation(in Vector3 position, in Vector3 // Get the relative position transformation. var relativePosition = position - Position; - transformation.position = GetRelativePositionTransformation(relativePosition); + transformation.Position = GetRelativePositionTransformation(relativePosition); // Get the relative velocity transformation. - transformation.velocity = + transformation.Velocity = GetRelativeVelocityTransformation(relativePosition, velocity - Velocity); // Get the relative acceleration transformation. // Since the agent's acceleration is an input and can be set arbitrarily, the relative // acceleration is just the other agent's acceleration. - transformation.acceleration = GetRelativeAccelerationTransformation(acceleration); + transformation.Acceleration = GetRelativeAccelerationTransformation(acceleration); return transformation; } @@ -135,23 +135,23 @@ private PositionTransformation GetRelativePositionTransformation(in Vector3 rela PositionTransformation positionTransformation = new PositionTransformation(); // Set the relative position in Cartesian coordinates. - positionTransformation.cartesian = relativePosition; + positionTransformation.Cartesian = relativePosition; // Calculate the distance (range) to the target. - positionTransformation.range = relativePosition.magnitude; + positionTransformation.Range = relativePosition.magnitude; var flatRelativePosition = Vector3.ProjectOnPlane(relativePosition, transform.up); var verticalRelativePosition = relativePosition - flatRelativePosition; // Calculate the elevation (vertical angle relative to forward). - positionTransformation.elevation = + positionTransformation.Elevation = Mathf.Atan2(verticalRelativePosition.magnitude, flatRelativePosition.magnitude); // Calculate the azimuth (horizontal angle relative to forward). if (flatRelativePosition.sqrMagnitude < _epsilon) { - positionTransformation.azimuth = 0; + positionTransformation.Azimuth = 0; } else { - positionTransformation.azimuth = + positionTransformation.Azimuth = Vector3.SignedAngle(transform.forward, flatRelativePosition, transform.up) * Mathf.Deg2Rad; } @@ -164,17 +164,17 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela var velocityTransformation = new VelocityTransformation(); // Set the relative velocity in Cartesian coordinates. - velocityTransformation.cartesian = relativeVelocity; + velocityTransformation.Cartesian = relativeVelocity; if (relativePosition.sqrMagnitude < _epsilon) { - velocityTransformation.range = relativeVelocity.magnitude; - velocityTransformation.azimuth = 0; - velocityTransformation.elevation = 0; + velocityTransformation.Range = relativeVelocity.magnitude; + velocityTransformation.Azimuth = 0; + velocityTransformation.Elevation = 0; return velocityTransformation; } // Calculate range rate (radial velocity). - velocityTransformation.range = Vector3.Dot(relativeVelocity, relativePosition.normalized); + velocityTransformation.Range = Vector3.Dot(relativeVelocity, relativePosition.normalized); // Project relative velocity onto the sphere passing through the target. var tangentialVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); @@ -198,10 +198,10 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela var tangentialVelocityOnAzimuth = Vector3.Project(tangentialVelocity, targetAzimuth); // Calculate the time derivative of the azimuth to the target. - velocityTransformation.azimuth = + velocityTransformation.Azimuth = tangentialVelocityOnAzimuth.magnitude / relativePosition.magnitude; if (Vector3.Dot(tangentialVelocityOnAzimuth, targetAzimuth) < 0) { - velocityTransformation.azimuth *= -1; + velocityTransformation.Azimuth *= -1; } // Project the velocity vector on the azimuth-elevation sphere onto the target @@ -209,10 +209,10 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela var tangentialVelocityOnElevation = Vector3.Project(tangentialVelocity, targetElevation); // Calculate the time derivative of the elevation to the target. - velocityTransformation.elevation = + velocityTransformation.Elevation = tangentialVelocityOnElevation.magnitude / relativePosition.magnitude; if (Vector3.Dot(tangentialVelocityOnElevation, targetElevation) < 0) { - velocityTransformation.elevation *= -1; + velocityTransformation.Elevation *= -1; } return velocityTransformation; @@ -221,7 +221,7 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela private AccelerationTransformation GetRelativeAccelerationTransformation( in Vector3 relativeAcceleration) { var accelerationTransformation = new AccelerationTransformation(); - accelerationTransformation.cartesian = relativeAcceleration; + accelerationTransformation.Cartesian = relativeAcceleration; return accelerationTransformation; } } diff --git a/Assets/Scripts/Controller/ApnController.cs b/Assets/Scripts/Controller/ApnController.cs index 7e964cadd..d46c06eb4 100644 --- a/Assets/Scripts/Controller/ApnController.cs +++ b/Assets/Scripts/Controller/ApnController.cs @@ -10,7 +10,7 @@ protected override Vector3 PlanImpl(in Transformation relativeTransformation) { Vector3 pnAccelerationInput = base.PlanImpl(relativeTransformation); // Add a feedforward term proportional to the target's acceleration. - Vector3 targetAcceleration = relativeTransformation.acceleration.cartesian; + Vector3 targetAcceleration = relativeTransformation.Acceleration.Cartesian; Vector3 normalTargetAcceleration = Vector3.ProjectOnPlane(targetAcceleration, _agent.transform.forward); Vector3 accelerationInput = diff --git a/Assets/Scripts/Controller/PnController.cs b/Assets/Scripts/Controller/PnController.cs index 75853c97d..0e1a90f5c 100644 --- a/Assets/Scripts/Controller/PnController.cs +++ b/Assets/Scripts/Controller/PnController.cs @@ -29,12 +29,12 @@ protected override Vector3 PlanImpl(in Transformation relativeTransformation) { float speed = velocity.magnitude; // Extract the bearing and closing velocity from the relative transformation. - float losAz = relativeTransformation.position.azimuth; - float losEl = relativeTransformation.position.elevation; - float losRateAz = relativeTransformation.velocity.azimuth; - float losRateEl = relativeTransformation.velocity.elevation; + float losAz = relativeTransformation.Position.Azimuth; + float losEl = relativeTransformation.Position.Elevation; + float losRateAz = relativeTransformation.Velocity.Azimuth; + float losRateEl = relativeTransformation.Velocity.Elevation; // The closing velocity is negative because the closing velocity is opposite to the range rate. - float closingVelocity = -relativeTransformation.velocity.range; + float closingVelocity = -relativeTransformation.Velocity.Range; // Set the turn factor, which is equal to the closing velocity by default. float turnFactor = closingVelocity; diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointController.cs index 334e629d0..f93a16c34 100644 --- a/Assets/Scripts/Controller/WaypointController.cs +++ b/Assets/Scripts/Controller/WaypointController.cs @@ -11,7 +11,7 @@ public WaypointController(Agent agent, float desiredSpeed) : base(agent) { } protected override Vector3 PlanImpl(in Transformation relativeTransformation) { - Vector3 toWaypoint = relativeTransformation.position.cartesian; + Vector3 toWaypoint = relativeTransformation.Position.Cartesian; Vector3 desiredVelocity = toWaypoint.normalized * _desiredSpeed; // Calculate the acceleration needed to reach the desired velocity within one time step. diff --git a/Assets/Scripts/Sensors/IdealSensorLegacy.cs b/Assets/Scripts/Sensors/IdealSensorLegacy.cs index 287c3d448..a1ca0f559 100644 --- a/Assets/Scripts/Sensors/IdealSensorLegacy.cs +++ b/Assets/Scripts/Sensors/IdealSensorLegacy.cs @@ -6,8 +6,8 @@ public override SensorOutput Sense(Agent agent) { // Adapt the relative transformation to the agent for the sensor output. Transformation relativeTransformation = _agent.GetRelativeTransformation(agent); - agentSensorOutput.position = relativeTransformation.position; - agentSensorOutput.velocity = relativeTransformation.velocity; + agentSensorOutput.Position = relativeTransformation.Position; + agentSensorOutput.Velocity = relativeTransformation.Velocity; return agentSensorOutput; } @@ -17,8 +17,8 @@ public override SensorOutput SenseWaypoint(Vector3 waypoint) { // Adapt the agent's relative transformation to the waypoint for the sensor output. Transformation relativeTransformation = _agent.GetRelativeTransformationToWaypoint(waypoint); - waypointSensorOutput.position = relativeTransformation.position; - waypointSensorOutput.velocity = relativeTransformation.velocity; + waypointSensorOutput.Position = relativeTransformation.Position; + waypointSensorOutput.Velocity = relativeTransformation.Velocity; return waypointSensorOutput; } diff --git a/Assets/Scripts/Sensors/SensorOutput.cs b/Assets/Scripts/Sensors/SensorOutput.cs index f56a197df..9fadd10e4 100644 --- a/Assets/Scripts/Sensors/SensorOutput.cs +++ b/Assets/Scripts/Sensors/SensorOutput.cs @@ -2,8 +2,8 @@ // agent's state. public struct SensorOutput { // Relative position transformation. - public PositionTransformation position; + public PositionTransformation Position; // Relative velocity transformation. - public VelocityTransformation velocity; + public VelocityTransformation Velocity; } diff --git a/Assets/Scripts/Threats/Threat.cs b/Assets/Scripts/Threats/Threat.cs index ca919ac14..3d38da11c 100644 --- a/Assets/Scripts/Threats/Threat.cs +++ b/Assets/Scripts/Threats/Threat.cs @@ -92,9 +92,9 @@ protected Agent GetClosestInterceptor() { foreach (var interceptor in _interceptors) { if (!interceptor.IsTerminated()) { SensorOutput sensorOutput = _sensor.Sense(interceptor); - if (sensorOutput.position.range < minDistance) { + if (sensorOutput.Position.Range < minDistance) { closestInterceptor = interceptor; - minDistance = sensorOutput.position.range; + minDistance = sensorOutput.Position.Range; } } } @@ -114,7 +114,7 @@ protected bool ShouldEvade() { float evasionRangeThreshold = agentConfig.DynamicConfig.FlightConfig.EvasionConfig.RangeThreshold; SensorOutput sensorOutput = _sensor.Sense(closestInterceptor); - return sensorOutput.position.range <= evasionRangeThreshold && sensorOutput.velocity.range < 0; + return sensorOutput.Position.Range <= evasionRangeThreshold && sensorOutput.Velocity.Range < 0; } protected Vector3 EvadeInterceptor(Agent interceptor) { diff --git a/Assets/Scripts/Utils/Transformation.cs b/Assets/Scripts/Utils/Transformation.cs index c1a15c6a4..b04cdfd7d 100644 --- a/Assets/Scripts/Utils/Transformation.cs +++ b/Assets/Scripts/Utils/Transformation.cs @@ -1,25 +1,25 @@ using UnityEngine; public struct Transformation { - public PositionTransformation position; - public VelocityTransformation velocity; - public AccelerationTransformation acceleration; + public PositionTransformation Position; + public VelocityTransformation Velocity; + public AccelerationTransformation Acceleration; } public struct PositionTransformation { - public Vector3 cartesian; - public float range; - public float azimuth; - public float elevation; + public Vector3 Cartesian; + public float Range; + public float Azimuth; + public float Elevation; } public struct VelocityTransformation { - public Vector3 cartesian; - public float range; - public float azimuth; - public float elevation; + public Vector3 Cartesian; + public float Range; + public float Azimuth; + public float Elevation; } public struct AccelerationTransformation { - public Vector3 cartesian; + public Vector3 Cartesian; } diff --git a/Assets/Tests/EditMode/AgentBaseTests.cs b/Assets/Tests/EditMode/AgentBaseTests.cs index da26b3ed8..d408cd1f6 100644 --- a/Assets/Tests/EditMode/AgentBaseTests.cs +++ b/Assets/Tests/EditMode/AgentBaseTests.cs @@ -20,15 +20,15 @@ public void GetRelativeTransformation_TargetAtBoresight() { Transformation relativeTransformation = agent.GetRelativeTransformation(target); - Assert.AreEqual(relativeTransformation.position.cartesian, + Assert.AreEqual(relativeTransformation.Position.Cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); - Assert.AreEqual(relativeTransformation.position.azimuth, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.position.elevation, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); - Assert.AreEqual(relativeTransformation.velocity.range, -1f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.azimuth, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.elevation, 1f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Range, 20f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Elevation, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.Velocity.Range, -1f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Elevation, 1f, _epsilon); } [Test] @@ -47,15 +47,15 @@ public void GetRelativeTransformation_TargetAtStarboard() { Transformation relativeTransformation = agent.GetRelativeTransformation(target); - Assert.AreEqual(relativeTransformation.position.cartesian, + Assert.AreEqual(relativeTransformation.Position.Cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); - Assert.AreEqual(relativeTransformation.position.azimuth, Mathf.PI / 2, _epsilon); - Assert.AreEqual(relativeTransformation.position.elevation, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); - Assert.AreEqual(relativeTransformation.velocity.range, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.azimuth, -1f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.elevation, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Range, 20f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Azimuth, Mathf.PI / 2, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Elevation, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.Velocity.Range, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Azimuth, -1f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Elevation, 0f, _epsilon); } [Test] @@ -74,14 +74,14 @@ public void GetRelativeTransformation_TargetWithElevation() { Transformation relativeTransformation = agent.GetRelativeTransformation(target); - Assert.AreEqual(relativeTransformation.position.cartesian, + Assert.AreEqual(relativeTransformation.Position.Cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); - Assert.AreEqual(relativeTransformation.position.azimuth, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.position.elevation, Mathf.PI / 2, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); - Assert.AreEqual(relativeTransformation.velocity.range, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.azimuth, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.elevation, -1f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Range, 20f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Position.Elevation, Mathf.PI / 2, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.Velocity.Range, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Azimuth, 0f, _epsilon); + Assert.AreEqual(relativeTransformation.Velocity.Elevation, -1f, _epsilon); } } diff --git a/Assets/Tests/EditMode/TransformationTests.cs b/Assets/Tests/EditMode/TransformationTests.cs index ee9012f50..b88643674 100644 --- a/Assets/Tests/EditMode/TransformationTests.cs +++ b/Assets/Tests/EditMode/TransformationTests.cs @@ -25,16 +25,16 @@ public void Target_At_Boresight() { Transformation relativeTransformation = agent.GetRelativeTransformation(target); // Assert - Assert.AreEqual(relativeTransformation.position.cartesian, + Assert.AreEqual(relativeTransformation.Position.Cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20); - Assert.AreEqual(relativeTransformation.position.azimuth, 0); - Assert.AreEqual(relativeTransformation.position.elevation, 0); - Assert.AreEqual(relativeTransformation.velocity.cartesian, + Assert.AreEqual(relativeTransformation.Position.Range, 20); + Assert.AreEqual(relativeTransformation.Position.Azimuth, 0); + Assert.AreEqual(relativeTransformation.Position.Elevation, 0); + Assert.AreEqual(relativeTransformation.Velocity.Cartesian, target.GetVelocity() - agent.GetVelocity()); - Assert.AreEqual(relativeTransformation.velocity.range, -1); - Assert.AreEqual(relativeTransformation.velocity.azimuth, 0); - Assert.AreEqual(relativeTransformation.velocity.elevation, 1); + Assert.AreEqual(relativeTransformation.Velocity.Range, -1); + Assert.AreEqual(relativeTransformation.Velocity.Azimuth, 0); + Assert.AreEqual(relativeTransformation.Velocity.Elevation, 1); } [Test] @@ -55,16 +55,16 @@ public void Target_At_Starboard() { Transformation relativeTransformation = agent.GetRelativeTransformation(target); // Assert - Assert.AreEqual(relativeTransformation.position.cartesian, + Assert.AreEqual(relativeTransformation.Position.Cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20); - Assert.AreEqual(relativeTransformation.position.azimuth, Mathf.PI / 2); - Assert.AreEqual(relativeTransformation.position.elevation, 0); - Assert.AreEqual(relativeTransformation.velocity.cartesian, + Assert.AreEqual(relativeTransformation.Position.Range, 20); + Assert.AreEqual(relativeTransformation.Position.Azimuth, Mathf.PI / 2); + Assert.AreEqual(relativeTransformation.Position.Elevation, 0); + Assert.AreEqual(relativeTransformation.Velocity.Cartesian, target.GetVelocity() - agent.GetVelocity()); - Assert.AreEqual(relativeTransformation.velocity.range, 0); - Assert.AreEqual(relativeTransformation.velocity.azimuth, -1); - Assert.AreEqual(relativeTransformation.velocity.elevation, 0); + Assert.AreEqual(relativeTransformation.Velocity.Range, 0); + Assert.AreEqual(relativeTransformation.Velocity.Azimuth, -1); + Assert.AreEqual(relativeTransformation.Velocity.Elevation, 0); } [Test] @@ -87,15 +87,15 @@ public void Target_At_Elevation() { Transformation relativeTransformation = agent.GetRelativeTransformation(target); // Assert - Assert.AreEqual(relativeTransformation.position.cartesian, + Assert.AreEqual(relativeTransformation.Position.Cartesian, target.transform.position - agent.transform.position); - Assert.AreEqual(relativeTransformation.position.range, 20); - Assert.AreEqual(relativeTransformation.position.azimuth, 0); - Assert.AreEqual(relativeTransformation.position.elevation, Mathf.PI / 2); - Assert.AreEqual(relativeTransformation.velocity.cartesian, + Assert.AreEqual(relativeTransformation.Position.Range, 20); + Assert.AreEqual(relativeTransformation.Position.Azimuth, 0); + Assert.AreEqual(relativeTransformation.Position.Elevation, Mathf.PI / 2); + Assert.AreEqual(relativeTransformation.Velocity.Cartesian, target.GetVelocity() - agent.GetVelocity()); - Assert.AreEqual(relativeTransformation.velocity.range, 0); - Assert.AreEqual(relativeTransformation.velocity.azimuth, 0); - Assert.AreEqual(relativeTransformation.velocity.elevation, -1); + Assert.AreEqual(relativeTransformation.Velocity.Range, 0); + Assert.AreEqual(relativeTransformation.Velocity.Azimuth, 0); + Assert.AreEqual(relativeTransformation.Velocity.Elevation, -1); } } From 74c81b8fc0668bedffae492d8c9aa5647d27aa4a Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 16:28:47 -0700 Subject: [PATCH 23/74] Define sensor interface --- Assets/Scripts/Sensors/ISensor.cs | 14 +++++++++++ Assets/Scripts/Sensors/ISensor.cs.meta | 2 ++ Assets/Scripts/Sensors/IdealSensor.cs | 28 ++++++++++++++++++++++ Assets/Scripts/Sensors/IdealSensor.cs.meta | 2 ++ Assets/Scripts/Sensors/SensorBase.cs | 16 +++++++++++++ Assets/Scripts/Sensors/SensorBase.cs.meta | 2 ++ 6 files changed, 64 insertions(+) create mode 100644 Assets/Scripts/Sensors/ISensor.cs create mode 100644 Assets/Scripts/Sensors/ISensor.cs.meta create mode 100644 Assets/Scripts/Sensors/IdealSensor.cs create mode 100644 Assets/Scripts/Sensors/IdealSensor.cs.meta create mode 100644 Assets/Scripts/Sensors/SensorBase.cs create mode 100644 Assets/Scripts/Sensors/SensorBase.cs.meta diff --git a/Assets/Scripts/Sensors/ISensor.cs b/Assets/Scripts/Sensors/ISensor.cs new file mode 100644 index 000000000..b983a0657 --- /dev/null +++ b/Assets/Scripts/Sensors/ISensor.cs @@ -0,0 +1,14 @@ +using UnityEngine; + +// Interface for a sensor. +// +// The sensor finds the relative transformation from an agent to another agent or another waypoint. +public interface ISensor { + IAgent Agent { get; set; } + + // Sense the target hierarchical object. + SensorOutput Sense(IHierarchical hierarchical); + + // Sense the waypoint. + SensorOutput Sense(in Vector3 waypoint); +} diff --git a/Assets/Scripts/Sensors/ISensor.cs.meta b/Assets/Scripts/Sensors/ISensor.cs.meta new file mode 100644 index 000000000..7d3c65fb7 --- /dev/null +++ b/Assets/Scripts/Sensors/ISensor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 185f34f4150164445b4875399226f4c9 \ No newline at end of file diff --git a/Assets/Scripts/Sensors/IdealSensor.cs b/Assets/Scripts/Sensors/IdealSensor.cs new file mode 100644 index 000000000..240f1dda5 --- /dev/null +++ b/Assets/Scripts/Sensors/IdealSensor.cs @@ -0,0 +1,28 @@ +using UnityEngine; + +// Ideal sensor. +// +// The ideal sensor does not add any noise to the measurements. +public class IdealSensor : SensorBase { + public IdealSensor(IAgent agent) : base(agent) {} + + // Sense the target hierarchical object. + public override SensorOutput Sense(IHierarchical hierarchical) { + var relativeTransformation = Agent.GetRelativeTransformation(hierarchical); + return GenerateSensorOutput(relativeTransformation); + } + + // Sense the waypoint. + public override SensorOutput Sense(in Vector3 waypoint) { + var relativeTransformation = Agent.GetRelativeTransformation(waypoint); + return GenerateSensorOutput(relativeTransformation); + } + + // Generate the sensor output from the relative transformation. + private SensorOutput GenerateSensorOutput(in Transformation relativeTransformation) { + var sensorOutput = new SensorOutput(); + sensorOutput.Position = relativeTransformation.Position; + sensorOutput.Velocity = relativeTransformation.Velocity; + return sensorOutput; + } +} diff --git a/Assets/Scripts/Sensors/IdealSensor.cs.meta b/Assets/Scripts/Sensors/IdealSensor.cs.meta new file mode 100644 index 000000000..d94dd3918 --- /dev/null +++ b/Assets/Scripts/Sensors/IdealSensor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4d125bfffeaa048a1be91049f6c7b6d6 \ No newline at end of file diff --git a/Assets/Scripts/Sensors/SensorBase.cs b/Assets/Scripts/Sensors/SensorBase.cs new file mode 100644 index 000000000..39ad3d00d --- /dev/null +++ b/Assets/Scripts/Sensors/SensorBase.cs @@ -0,0 +1,16 @@ +using UnityEngine; + +// Base implementation of a sensor. +public abstract class SensorBase : ISensor { + public IAgent Agent { get; set; } + + public SensorBase(IAgent agent) { + Agent = agent; + } + + // Sense the target hierarchical object. + public abstract SensorOutput Sense(IHierarchical hierarchical); + + // Sense the waypoint. + public abstract SensorOutput Sense(in Vector3 waypoint); +} diff --git a/Assets/Scripts/Sensors/SensorBase.cs.meta b/Assets/Scripts/Sensors/SensorBase.cs.meta new file mode 100644 index 000000000..36df22afc --- /dev/null +++ b/Assets/Scripts/Sensors/SensorBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: a71cf36ad86e3440b8c89c9591ccf54a \ No newline at end of file From d063df8c9d52a657cd4c31f00e3d6b211a306bfc Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 16:35:03 -0700 Subject: [PATCH 24/74] Add unit tests for ideal sensor --- .../Scripts/Hierarchical/FixedHierarchical.cs | 2 +- Assets/Tests/EditMode/IdealSensorTests.cs | 36 +++++++++++++++++++ .../Tests/EditMode/IdealSensorTests.cs.meta | 2 ++ 3 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 Assets/Tests/EditMode/IdealSensorTests.cs create mode 100644 Assets/Tests/EditMode/IdealSensorTests.cs.meta diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs b/Assets/Scripts/Hierarchical/FixedHierarchical.cs index 3e29fd3d7..cebb575ff 100644 --- a/Assets/Scripts/Hierarchical/FixedHierarchical.cs +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs @@ -8,7 +8,7 @@ public class FixedHierarchical : HierarchicalBase { private Vector3 _acceleration; public FixedHierarchical(in Vector3 position, in Vector3 velocity) - : this(position, velocity, Vector3.zero) {} + : this(position, velocity, acceleration: Vector3.zero) {} public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { _position = position; _velocity = velocity; diff --git a/Assets/Tests/EditMode/IdealSensorTests.cs b/Assets/Tests/EditMode/IdealSensorTests.cs new file mode 100644 index 000000000..26e96e692 --- /dev/null +++ b/Assets/Tests/EditMode/IdealSensorTests.cs @@ -0,0 +1,36 @@ +using NUnit.Framework; +using UnityEngine; + +public class IdealSensorTests : TestBase { + private AgentBase _agent; + private IdealSensor _sensor; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _agent.Position = new Vector3(x: -5, y: 2, z: -5); + _agent.Velocity = new Vector3(x: 2, y: 4, z: 10); + _sensor = new IdealSensor(_agent); + } + + [Test] + public void Sense_ReturnsSameAsRelativeTransformation() { + var target = + new FixedHierarchical(position: new Vector3(10, 2, 20), velocity: new Vector3(-12, 20, -1)); + var relativeTransformation = _agent.GetRelativeTransformation(target); + var sensorOutput = _sensor.Sense(target); + Assert.AreEqual(relativeTransformation.Position, sensorOutput.Position); + Assert.AreEqual(relativeTransformation.Velocity, sensorOutput.Velocity); + } + + [Test] + public void Sense_WaypointReturnsSameAsRelativeTransformation() { + var target = new Vector3(-12, 20, -1); + var relativeTransformation = _agent.GetRelativeTransformation(target); + var sensorOutput = _sensor.Sense(target); + Assert.AreEqual(relativeTransformation.Position, sensorOutput.Position); + Assert.AreEqual(relativeTransformation.Velocity, sensorOutput.Velocity); + } +} diff --git a/Assets/Tests/EditMode/IdealSensorTests.cs.meta b/Assets/Tests/EditMode/IdealSensorTests.cs.meta new file mode 100644 index 000000000..3ca4861f2 --- /dev/null +++ b/Assets/Tests/EditMode/IdealSensorTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 349377821bac24d4993e49f8fb63d9d7 \ No newline at end of file From 0e4794ec0250a4bb84d1248b202611b27f702f7f Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 16:57:23 -0700 Subject: [PATCH 25/74] Clean up AgentBaseTests by using SetUp --- .../Scripts/Hierarchical/FixedHierarchical.cs | 27 +++++++ .../Hierarchical/FixedHierarchical.cs.meta | 2 + Assets/Tests/EditMode/AgentBaseTests.cs | 79 ++++++++----------- 3 files changed, 60 insertions(+), 48 deletions(-) create mode 100644 Assets/Scripts/Hierarchical/FixedHierarchical.cs create mode 100644 Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs b/Assets/Scripts/Hierarchical/FixedHierarchical.cs new file mode 100644 index 000000000..3e29fd3d7 --- /dev/null +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs @@ -0,0 +1,27 @@ +using UnityEngine; + +// The fixed hierarchical object represents a hierarchical object with a fixed position, velocity, +// and acceleration and should primarily be used for testing. +public class FixedHierarchical : HierarchicalBase { + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _acceleration; + + public FixedHierarchical(in Vector3 position, in Vector3 velocity) + : this(position, velocity, Vector3.zero) {} + public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { + _position = position; + _velocity = velocity; + _acceleration = acceleration; + } + + protected override Vector3 GetPosition() { + return _position; + } + protected override Vector3 GetVelocity() { + return _velocity; + } + protected override Vector3 GetAcceleration() { + return _acceleration; + } +} diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta b/Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta new file mode 100644 index 000000000..dea21cd2e --- /dev/null +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 76ccd61f72bad430a896ca4a5b9490b9 \ No newline at end of file diff --git a/Assets/Tests/EditMode/AgentBaseTests.cs b/Assets/Tests/EditMode/AgentBaseTests.cs index da26b3ed8..1908dd54a 100644 --- a/Assets/Tests/EditMode/AgentBaseTests.cs +++ b/Assets/Tests/EditMode/AgentBaseTests.cs @@ -4,28 +4,29 @@ public class AgentBaseTests : TestBase { private const float _epsilon = 1e-3f; - [Test] - public void GetRelativeTransformation_TargetAtBoresight() { - AgentBase agent = new GameObject("Agent").AddComponent(); - Rigidbody agentRb = agent.gameObject.AddComponent(); - InvokePrivateMethod(agent, "Awake"); - agent.Position = new Vector3(0, 0, 0); - agent.Velocity = new Vector3(0, 0, 0); + private AgentBase _agent; + private FixedHierarchical _target; - AgentBase target = new GameObject("Target").AddComponent(); - Rigidbody targetRb = target.gameObject.AddComponent(); - InvokePrivateMethod(target, "Awake"); - target.Position = new Vector3(0, 0, 20); - target.Velocity = new Vector3(0, 20, -1); + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + } - Transformation relativeTransformation = agent.GetRelativeTransformation(target); + [Test] + public void GetRelativeTransformation_TargetAtBoresight() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 0, 0); + _target = + new FixedHierarchical(position: new Vector3(0, 0, 20), velocity: new Vector3(0, 20, -1)); - Assert.AreEqual(relativeTransformation.position.cartesian, - target.transform.position - agent.transform.position); + var relativeTransformation = _agent.GetRelativeTransformation(_target); + Assert.AreEqual(relativeTransformation.position.cartesian, _target.Position - _agent.Position); Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); Assert.AreEqual(relativeTransformation.position.azimuth, 0f, _epsilon); Assert.AreEqual(relativeTransformation.position.elevation, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.velocity.cartesian, _target.Velocity - _agent.Velocity); Assert.AreEqual(relativeTransformation.velocity.range, -1f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.azimuth, 0f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.elevation, 1f, _epsilon); @@ -33,26 +34,17 @@ public void GetRelativeTransformation_TargetAtBoresight() { [Test] public void GetRelativeTransformation_TargetAtStarboard() { - AgentBase agent = new GameObject("Agent").AddComponent(); - Rigidbody agentRb = agent.gameObject.AddComponent(); - InvokePrivateMethod(agent, "Awake"); - agent.Position = new Vector3(0, 0, 0); - agent.Velocity = new Vector3(0, 0, 0); + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 0, 0); + _target = + new FixedHierarchical(position: new Vector3(20, 0, 0), velocity: new Vector3(0, 0, 20)); - AgentBase target = new GameObject("Target").AddComponent(); - Rigidbody targetRb = target.gameObject.AddComponent(); - InvokePrivateMethod(target, "Awake"); - target.Position = new Vector3(20, 0, 0); - target.Velocity = new Vector3(0, 0, 20); - - Transformation relativeTransformation = agent.GetRelativeTransformation(target); - - Assert.AreEqual(relativeTransformation.position.cartesian, - target.transform.position - agent.transform.position); + var relativeTransformation = _agent.GetRelativeTransformation(_target); + Assert.AreEqual(relativeTransformation.position.cartesian, _target.Position - _agent.Position); Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); Assert.AreEqual(relativeTransformation.position.azimuth, Mathf.PI / 2, _epsilon); Assert.AreEqual(relativeTransformation.position.elevation, 0f, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.velocity.cartesian, _target.Velocity - _agent.Velocity); Assert.AreEqual(relativeTransformation.velocity.range, 0f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.azimuth, -1f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.elevation, 0f, _epsilon); @@ -60,26 +52,17 @@ public void GetRelativeTransformation_TargetAtStarboard() { [Test] public void GetRelativeTransformation_TargetWithElevation() { - AgentBase agent = new GameObject("Agent").AddComponent(); - Rigidbody agentRb = agent.gameObject.AddComponent(); - InvokePrivateMethod(agent, "Awake"); - agent.Position = new Vector3(0, 0, 0); - agent.Velocity = new Vector3(0, 0, 0); - - AgentBase target = new GameObject("Target").AddComponent(); - Rigidbody targetRb = target.gameObject.AddComponent(); - InvokePrivateMethod(target, "Awake"); - target.Position = new Vector3(0, 20, 0); - target.Velocity = new Vector3(0, 0, 20); - - Transformation relativeTransformation = agent.GetRelativeTransformation(target); + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 0, 0); + _target = + new FixedHierarchical(position: new Vector3(0, 20, 0), velocity: new Vector3(0, 0, 20)); - Assert.AreEqual(relativeTransformation.position.cartesian, - target.transform.position - agent.transform.position); + var relativeTransformation = _agent.GetRelativeTransformation(_target); + Assert.AreEqual(relativeTransformation.position.cartesian, _target.Position - _agent.Position); Assert.AreEqual(relativeTransformation.position.range, 20f, _epsilon); Assert.AreEqual(relativeTransformation.position.azimuth, 0f, _epsilon); Assert.AreEqual(relativeTransformation.position.elevation, Mathf.PI / 2, _epsilon); - Assert.AreEqual(relativeTransformation.velocity.cartesian, target.Velocity - agent.Velocity); + Assert.AreEqual(relativeTransformation.velocity.cartesian, _target.Velocity - _agent.Velocity); Assert.AreEqual(relativeTransformation.velocity.range, 0f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.azimuth, 0f, _epsilon); Assert.AreEqual(relativeTransformation.velocity.elevation, -1f, _epsilon); From 37c8c700ad0dd5991690aab523d5d92e5b6e3d4a Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 17:02:41 -0700 Subject: [PATCH 26/74] Address comments --- Assets/Scripts/Evasion.meta | 8 -------- Assets/Scripts/Sensors/IdealSensor.cs | 8 ++++---- Assets/Scripts/Sensors/SensorOutput.cs | 4 ++-- Assets/Scripts/Utils/Transformation.cs | 24 +++++++++++------------ Assets/Tests/EditMode/IdealSensorTests.cs | 6 +++--- 5 files changed, 21 insertions(+), 29 deletions(-) delete mode 100644 Assets/Scripts/Evasion.meta diff --git a/Assets/Scripts/Evasion.meta b/Assets/Scripts/Evasion.meta deleted file mode 100644 index a17c8488c..000000000 --- a/Assets/Scripts/Evasion.meta +++ /dev/null @@ -1,8 +0,0 @@ -fileFormatVersion: 2 -guid: 578b89e2cd80e4a958b7e5eac564297b -folderAsset: yes -DefaultImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: diff --git a/Assets/Scripts/Sensors/IdealSensor.cs b/Assets/Scripts/Sensors/IdealSensor.cs index 240f1dda5..fe08c7522 100644 --- a/Assets/Scripts/Sensors/IdealSensor.cs +++ b/Assets/Scripts/Sensors/IdealSensor.cs @@ -20,9 +20,9 @@ public override SensorOutput Sense(in Vector3 waypoint) { // Generate the sensor output from the relative transformation. private SensorOutput GenerateSensorOutput(in Transformation relativeTransformation) { - var sensorOutput = new SensorOutput(); - sensorOutput.Position = relativeTransformation.Position; - sensorOutput.Velocity = relativeTransformation.Velocity; - return sensorOutput; + return new SensorOutput { + Position = relativeTransformation.Position, + Velocity = relativeTransformation.Velocity, + }; } } diff --git a/Assets/Scripts/Sensors/SensorOutput.cs b/Assets/Scripts/Sensors/SensorOutput.cs index 9fadd10e4..a8bdbd850 100644 --- a/Assets/Scripts/Sensors/SensorOutput.cs +++ b/Assets/Scripts/Sensors/SensorOutput.cs @@ -2,8 +2,8 @@ // agent's state. public struct SensorOutput { // Relative position transformation. - public PositionTransformation Position; + public PositionTransformation Position { get; set; } // Relative velocity transformation. - public VelocityTransformation Velocity; + public VelocityTransformation Velocity { get; set; } } diff --git a/Assets/Scripts/Utils/Transformation.cs b/Assets/Scripts/Utils/Transformation.cs index b04cdfd7d..343d231f3 100644 --- a/Assets/Scripts/Utils/Transformation.cs +++ b/Assets/Scripts/Utils/Transformation.cs @@ -1,25 +1,25 @@ using UnityEngine; public struct Transformation { - public PositionTransformation Position; - public VelocityTransformation Velocity; - public AccelerationTransformation Acceleration; + public PositionTransformation Position { get; set; } + public VelocityTransformation Velocity { get; set; } + public AccelerationTransformation Acceleration { get; set; } } public struct PositionTransformation { - public Vector3 Cartesian; - public float Range; - public float Azimuth; - public float Elevation; + public Vector3 Cartesian { get; set; } + public float Range { get; set; } + public float Azimuth { get; set; } + public float Elevation { get; set; } } public struct VelocityTransformation { - public Vector3 Cartesian; - public float Range; - public float Azimuth; - public float Elevation; + public Vector3 Cartesian { get; set; } + public float Range { get; set; } + public float Azimuth { get; set; } + public float Elevation { get; set; } } public struct AccelerationTransformation { - public Vector3 Cartesian; + public Vector3 Cartesian { get; set; } } diff --git a/Assets/Tests/EditMode/IdealSensorTests.cs b/Assets/Tests/EditMode/IdealSensorTests.cs index 26e96e692..029324834 100644 --- a/Assets/Tests/EditMode/IdealSensorTests.cs +++ b/Assets/Tests/EditMode/IdealSensorTests.cs @@ -27,9 +27,9 @@ public void Sense_ReturnsSameAsRelativeTransformation() { [Test] public void Sense_WaypointReturnsSameAsRelativeTransformation() { - var target = new Vector3(-12, 20, -1); - var relativeTransformation = _agent.GetRelativeTransformation(target); - var sensorOutput = _sensor.Sense(target); + var waypoint = new Vector3(-12, 20, -1); + var relativeTransformation = _agent.GetRelativeTransformation(waypoint); + var sensorOutput = _sensor.Sense(waypoint); Assert.AreEqual(relativeTransformation.Position, sensorOutput.Position); Assert.AreEqual(relativeTransformation.Velocity, sensorOutput.Velocity); } From a55fe1c4fbe014e33fddc78fec9c56cbb4144b1d Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 17:25:46 -0700 Subject: [PATCH 27/74] Remove unused flight plan attribute --- Assets/Scripts/Behavior/AttackBehaviorBase.cs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Assets/Scripts/Behavior/AttackBehaviorBase.cs b/Assets/Scripts/Behavior/AttackBehaviorBase.cs index 76c097b2a..32b977b5e 100644 --- a/Assets/Scripts/Behavior/AttackBehaviorBase.cs +++ b/Assets/Scripts/Behavior/AttackBehaviorBase.cs @@ -6,10 +6,6 @@ public abstract class AttackBehaviorBase : IAttackBehavior { [SerializeField] private Configs.AttackBehaviorConfig _config; - // Flight plan. - [SerializeField] - private FlightPlan _flightPlan; - public Configs.AttackBehaviorConfig Config { get => _config; set => _config = value; From 6829370f8e8baf63f2b89d3dfeea355dc53b2898 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 17:30:51 -0700 Subject: [PATCH 28/74] Clean up private properties --- Assets/Scripts/Agents/AgentBase.cs | 23 +++++++------------ .../Scripts/Hierarchical/HierarchicalAgent.cs | 9 ++------ .../Scripts/Hierarchical/HierarchicalBase.cs | 21 ++++++----------- Assets/Scripts/Movements/MovementBase.cs | 9 ++------ Assets/Scripts/Sensors/SensorBase.cs | 1 + Assets/Scripts/Threats/ThreatBase.cs | 13 ++--------- 6 files changed, 22 insertions(+), 54 deletions(-) diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index e5a7f8e88..92cff4334 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -18,13 +18,6 @@ public class AgentBase : MonoBehaviour, IAgent { // dynamic configuration, and sub-agent configuration (for interceptors). private Configs.AgentConfig _agentConfig; - // Movement behavior of the agent. - private IMovement _movement; - - // The controller calculates the acceleration input, given the agent's current state and its - // target's current state. - private IController _controller; - [SerializeField] // The acceleration field is not part of the rigid body component, so it is tracked separately. // The acceleration is applied as a force during each frame update. @@ -49,14 +42,14 @@ public Configs.AgentConfig AgentConfig { get => _agentConfig; set => _agentConfig = value; } - public IMovement Movement { - get => _movement; - set => _movement = value; - } - public IController Controller { - get => _controller; - set => _controller = value; - } + + // Movement behavior of the agent. + public IMovement Movement { get; set; } + + // The controller calculates the acceleration input, given the agent's current state and its + // target's current state. + public IController Controller { get; set; } + public Vector3 Position { get => transform.position; set => transform.position = value; diff --git a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs index 80a3c0603..7468cfc23 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs @@ -5,13 +5,8 @@ // Since agents cannot inherit from both HierarchicalBase and MonoBehaviour, agents use composition // to know their position within the hierarchical strategy. public class HierarchicalAgent : HierarchicalBase { - [SerializeField] - private AgentBase _agent; - - public AgentBase Agent { - get => _agent; - set => _agent = value; - } + // Agent to which the hierarchical node belongs. + public IAgent Agent { get; set; } protected override Vector3 GetPosition() { return Agent.Position; diff --git a/Assets/Scripts/Hierarchical/HierarchicalBase.cs b/Assets/Scripts/Hierarchical/HierarchicalBase.cs index 1aa6683d8..4934f196f 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalBase.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalBase.cs @@ -9,25 +9,18 @@ public class HierarchicalBase : IHierarchical { // List of hierarchical objects in the hierarchy level below. private List _subHierarchicals = new List(); + // List of hierarchical objects pursuing the hierarchical object. + private List _pursuers = new List(); + + public IReadOnlyList SubHierarchicals => _subHierarchicals.AsReadOnly(); + // Target of the hierarchical object. - private IHierarchical _target; + public IHierarchical Target { get; set; } // Target model of the hierarchical object. The target model is updated by the sensor and should // be used by the controller to model imperfect knowledge of the engagement. - private IHierarchical _targetModel; + public IHierarchical TargetModel { get; set; } - // List of hierarchical objects pursuing the hierarchical object. - private List _pursuers = new List(); - - public IReadOnlyList SubHierarchicals => _subHierarchicals.AsReadOnly(); - public IHierarchical Target { - get => _target; - set => _target = value; - } - public IHierarchical TargetModel { - get => _targetModel; - set => _targetModel = value; - } public IReadOnlyList Pursuers => _pursuers.AsReadOnly(); public Vector3 Position => GetPosition(); diff --git a/Assets/Scripts/Movements/MovementBase.cs b/Assets/Scripts/Movements/MovementBase.cs index 67d9dc1c1..38168868a 100644 --- a/Assets/Scripts/Movements/MovementBase.cs +++ b/Assets/Scripts/Movements/MovementBase.cs @@ -4,15 +4,10 @@ // Base implementation of a movement behavior. public abstract class MovementBase : IMovement { // Agent to which the movement behavior is assigned. - private IAgent _agent; - - public IAgent Agent { - get => _agent; - set => _agent = value; - } + public IAgent Agent { get; set; } public MovementBase(IAgent agent) { - _agent = agent; + Agent = agent; } // Determine the agent's actual acceleration input given its intended acceleration input by diff --git a/Assets/Scripts/Sensors/SensorBase.cs b/Assets/Scripts/Sensors/SensorBase.cs index 39ad3d00d..b2b8662d0 100644 --- a/Assets/Scripts/Sensors/SensorBase.cs +++ b/Assets/Scripts/Sensors/SensorBase.cs @@ -2,6 +2,7 @@ // Base implementation of a sensor. public abstract class SensorBase : ISensor { + // Agent to which the sensor belongs. public IAgent Agent { get; set; } public SensorBase(IAgent agent) { diff --git a/Assets/Scripts/Threats/ThreatBase.cs b/Assets/Scripts/Threats/ThreatBase.cs index 53cc11b36..0ab652aab 100644 --- a/Assets/Scripts/Threats/ThreatBase.cs +++ b/Assets/Scripts/Threats/ThreatBase.cs @@ -1,17 +1,8 @@ // Base implementation of a threat. public abstract class ThreatBase : AgentBase, IThreat { // The attack behavior determines how the threat navigates towards the asset. - private IAttackBehavior _attackBehavior; + public IAttackBehavior AttackBehavior { get; set; } // The evasion handles how the threat behaves in the vicinity of a pursuing interceptor. - private IEvasion _evasion; - - public IAttackBehavior AttackBehavior { - get => _attackBehavior; - set => _attackBehavior = value; - } - public IEvasion Evasion { - get => _evasion; - set => _evasion = value; - } + public IEvasion Evasion { get; set; } } From 728b2c16dbde9803992d3851f0a5d4c3cdc3f035 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 22:46:26 -0700 Subject: [PATCH 29/74] Implement evasion maneuvers --- Assets/Proto/Configs/dynamic_config.proto | 18 +- Assets/Scripts/Agents/AgentBase.cs | 14 + Assets/Scripts/Agents/IAgent.cs | 4 + Assets/Scripts/Evasion.meta | 8 + Assets/Scripts/Evasion/EvasionBase.cs | 11 +- Assets/Scripts/Evasion/IEvasion.cs | 6 +- Assets/Scripts/Evasion/NoEvasion.cs | 16 + Assets/Scripts/Evasion/NoEvasion.cs.meta | 2 + Assets/Scripts/Evasion/OrthogonalEvasion.cs | 86 +++ .../Scripts/Evasion/OrthogonalEvasion.cs.meta | 2 + .../Scripts/Generated/Proto/DynamicConfig.cs | 532 +++++++++--------- Assets/Scripts/Movements/MovementBase.cs | 18 +- Assets/Scripts/Sensors/ISensor.cs | 3 + Assets/Scripts/Sensors/IdealSensor.cs | 6 + Assets/Scripts/Sensors/SensorBase.cs | 3 + Assets/Scripts/Threats/IThreat.cs | 6 + Assets/Scripts/Threats/ThreatBase.cs | 11 + 17 files changed, 447 insertions(+), 299 deletions(-) create mode 100644 Assets/Scripts/Evasion.meta create mode 100644 Assets/Scripts/Evasion/NoEvasion.cs create mode 100644 Assets/Scripts/Evasion/NoEvasion.cs.meta create mode 100644 Assets/Scripts/Evasion/OrthogonalEvasion.cs create mode 100644 Assets/Scripts/Evasion/OrthogonalEvasion.cs.meta diff --git a/Assets/Proto/Configs/dynamic_config.proto b/Assets/Proto/Configs/dynamic_config.proto index 98158c994..eb088d61e 100644 --- a/Assets/Proto/Configs/dynamic_config.proto +++ b/Assets/Proto/Configs/dynamic_config.proto @@ -10,17 +10,17 @@ enum ControllerType { AUGMENTED_PROPORTIONAL_NAVIGATION = 1; } -// Flight configuration. -message FlightConfig { - // Evasion configuration. - message EvasionConfig { - // If true, evasion is enabled. - bool enabled = 1; +// Evasion configuration. +message EvasionConfig { + // If true, evasion is enabled. + bool enabled = 1; - // Range threshold in m at which the agent should evade. - float range_threshold = 2; - } + // Range threshold in m at which the agent should evade. + float range_threshold = 2; +} +// Flight configuration. +message FlightConfig { // Controller type. ControllerType controller_type = 1; diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 92cff4334..ad9aba15d 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -50,6 +50,9 @@ public Configs.AgentConfig AgentConfig { // target's current state. public IController Controller { get; set; } + // The sensor calculates the relative transformation from the current agent to a target. + public ISensor Sensor { get; set; } + public Vector3 Position { get => transform.position; set => transform.position = value; @@ -76,6 +79,17 @@ private void Awake() { // TODO(titan): Methods for updating the agent and terminating the agent. + public float MaxForwardAcceleration() { + return StaticConfig.AccelerationConfig?.MaxForwardAcceleration ?? 0; + } + + public float MaxNormalAcceleration() { + var maxReferenceNormalAcceleration = + (StaticConfig.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * Constants.kGravity; + var referenceSpeed = StaticConfig.AccelerationConfig?.ReferenceSpeed ?? 1; + return Mathf.Pow(Speed / referenceSpeed, 2) * maxReferenceNormalAcceleration; + } + public Transformation GetRelativeTransformation(IAgent target) { return GetRelativeTransformation(target.Position, target.Velocity, target.Acceleration); } diff --git a/Assets/Scripts/Agents/IAgent.cs b/Assets/Scripts/Agents/IAgent.cs index fdbe582f0..d82b1cd8b 100644 --- a/Assets/Scripts/Agents/IAgent.cs +++ b/Assets/Scripts/Agents/IAgent.cs @@ -12,6 +12,7 @@ public interface IAgent { IMovement Movement { get; set; } IController Controller { get; set; } + ISensor Sensor { get; set; } Vector3 Position { get; set; } Vector3 Velocity { get; set; } @@ -21,6 +22,9 @@ public interface IAgent { Transform transform { get; } + float MaxForwardAcceleration(); + float MaxNormalAcceleration(); + Transformation GetRelativeTransformation(IAgent target); Transformation GetRelativeTransformation(IHierarchical target); Transformation GetRelativeTransformation(in Vector3 waypoint); diff --git a/Assets/Scripts/Evasion.meta b/Assets/Scripts/Evasion.meta new file mode 100644 index 000000000..a17c8488c --- /dev/null +++ b/Assets/Scripts/Evasion.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 578b89e2cd80e4a958b7e5eac564297b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Evasion/EvasionBase.cs b/Assets/Scripts/Evasion/EvasionBase.cs index 5fa2816c7..d2ed2e21f 100644 --- a/Assets/Scripts/Evasion/EvasionBase.cs +++ b/Assets/Scripts/Evasion/EvasionBase.cs @@ -2,9 +2,16 @@ // Base implementation of an evasion. public abstract class EvasionBase : IEvasion { + // Agent that will evade the pursuers. + public IAgent Agent { get; set; } + + public EvasionBase(IAgent agent) { + Agent = agent; + } + // Determine whether to perform any evasive maneuvers. - public abstract bool ShouldEvade(IAgent agent, IAgent pursuer); + public abstract bool ShouldEvade(IAgent pursuer); // Calculate the acceleration input to evade the pursuer. - public abstract Vector3 Evade(IAgent agent, IAgent pursuer); + public abstract Vector3 Evade(IAgent pursuer); } diff --git a/Assets/Scripts/Evasion/IEvasion.cs b/Assets/Scripts/Evasion/IEvasion.cs index 0d07e2ad6..dcc626293 100644 --- a/Assets/Scripts/Evasion/IEvasion.cs +++ b/Assets/Scripts/Evasion/IEvasion.cs @@ -5,9 +5,11 @@ // When a threat detects a pursuing interceptor in its vicinity, it may attempt to evade the // interceptor before resuming its attack behavior. public interface IEvasion { + IAgent Agent { get; set; } + // Determine whether to perform any evasive maneuvers. - bool ShouldEvade(IAgent agent, IAgent pursuer); + bool ShouldEvade(IAgent pursuer); // Calculate the acceleration input to evade the pursuer. - Vector3 Evade(IAgent agent, IAgent pursuer); + Vector3 Evade(IAgent pursuer); } diff --git a/Assets/Scripts/Evasion/NoEvasion.cs b/Assets/Scripts/Evasion/NoEvasion.cs new file mode 100644 index 000000000..dd6ac9f3d --- /dev/null +++ b/Assets/Scripts/Evasion/NoEvasion.cs @@ -0,0 +1,16 @@ +using UnityEngine; + +// No evasion will be performed. +public class NoEvasion : EvasionBase { + public NoEvasion(IAgent agent) : base(agent) {} + + // Determine whether to perform any evasive maneuvers. + public override bool ShouldEvade(IAgent pursuer) { + return false; + } + + // Calculate the acceleration input to evade the pursuer. + public override Vector3 Evade(IAgent pursuer) { + return Vector3.zero; + } +} diff --git a/Assets/Scripts/Evasion/NoEvasion.cs.meta b/Assets/Scripts/Evasion/NoEvasion.cs.meta new file mode 100644 index 000000000..d59bec8ff --- /dev/null +++ b/Assets/Scripts/Evasion/NoEvasion.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 0bbb6138037f548f5bb3097fcc9d8948 \ No newline at end of file diff --git a/Assets/Scripts/Evasion/OrthogonalEvasion.cs b/Assets/Scripts/Evasion/OrthogonalEvasion.cs new file mode 100644 index 000000000..7c07706f7 --- /dev/null +++ b/Assets/Scripts/Evasion/OrthogonalEvasion.cs @@ -0,0 +1,86 @@ +using UnityEngine; + +// The agent will try to evade its pursuer by turning its velocity vector to be orthogonal to the +// pursuer's velocity vector. +// +// Turning orthogonal to the pursuer means that the pursuer needs to apply a normal acceleration to +// intercept the agent, incurring the maximum possible lift-induced drag. +public class OrthogonalEvasion : EvasionBase { + public OrthogonalEvasion(IAgent agent) : base(agent) {} + + // Determine whether to perform any evasive maneuvers. + public override bool ShouldEvade(IAgent pursuer) { + // Default evasion range threshold in meters. + const float defaultEvasionRangeThreshold = 1000f; + + if (!(Agent.AgentConfig?.DynamicConfig?.FlightConfig?.EvasionConfig?.Enabled ?? false)) { + return false; + } + + var evasionRangeThreshold = + Agent.AgentConfig?.DynamicConfig?.FlightConfig?.EvasionConfig?.RangeThreshold ?? + defaultEvasionRangeThreshold; + var sensorOutput = Agent.Sensor.Sense(pursuer); + return sensorOutput.Position.Range <= evasionRangeThreshold && sensorOutput.Velocity.Range < 0; + } + + // Calculate the acceleration input to evade the pursuer. + public override Vector3 Evade(IAgent pursuer) { + const float epsilon = 1e-12f; + const float groundProximityThresholdFactor = 5f; + const float groundAvoidanceUpFactor = 5f; + + var agentPosition = Agent.Position; + var agentVelocity = Agent.Velocity; + var pursuerPosition = pursuer.Position; + var pursuerVelocity = pursuer.Velocity; + + // Evade the pursuer by turning the velocity to be orthogonal to the pursuer's velocity. + var normalVelocity = Vector3.ProjectOnPlane(agentVelocity, pursuerVelocity); + if (normalVelocity.sqrMagnitude < epsilon) { + // If the agent's velocity is aligned with the pursuer's velocity, choose a random normal + // direction in which to evade. + normalVelocity = pursuer.transform.right; + } + var normalAccelerationDirection = + Vector3.ProjectOnPlane(normalVelocity, agentVelocity).normalized; + + // Turn away from the pursuer. + var relativePosition = pursuerPosition - agentPosition; + if (Vector3.Dot(relativePosition, normalAccelerationDirection) > 0) { + normalAccelerationDirection *= -1; + } + + // Avoid evading straight down when near the ground. + float altitude = agentPosition.y; + float groundProximityThreshold = Mathf.Abs(agentVelocity.y) * groundProximityThresholdFactor; + if (agentVelocity.y < 0 && altitude < groundProximityThreshold) { + // Determine the evasion direction based on the angle to pursuer. + var rightDirection = Vector3.Cross(Vector3.up, Agent.transform.forward); + var angle = Vector3.SignedAngle(Agent.transform.forward, relativePosition, Vector3.up); + + // Choose the direction that leads away from the pursuer. + var bestHorizontalDirection = angle > 0 ? -rightDirection : rightDirection; + + // Blend between horizontal evasion and slight upward movement. + var blendFactor = 1 - (altitude / groundProximityThreshold); + normalAccelerationDirection = + Vector3 + .Lerp(normalAccelerationDirection, + bestHorizontalDirection + Agent.transform.up * groundAvoidanceUpFactor, + blendFactor) + .normalized; + } + var normalAcceleration = normalAccelerationDirection * Agent.MaxNormalAcceleration(); + + // Set the forward acceleration. + var forwardAcceleration = Vector3.zero; + if (Agent is IThreat threat) { + // Set the forward acceleration to reach the target speed within one time step. + var targetSpeed = threat.LookupPowerTable(Configs.Power.Max); + var speedError = targetSpeed - Agent.Speed; + forwardAcceleration = speedError / Time.fixedDeltaTime * Agent.transform.forward; + } + return normalAcceleration + forwardAcceleration; + } +} diff --git a/Assets/Scripts/Evasion/OrthogonalEvasion.cs.meta b/Assets/Scripts/Evasion/OrthogonalEvasion.cs.meta new file mode 100644 index 000000000..424fb7334 --- /dev/null +++ b/Assets/Scripts/Evasion/OrthogonalEvasion.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 897724de3836b45e1bf6edd305a85167 \ No newline at end of file diff --git a/Assets/Scripts/Generated/Proto/DynamicConfig.cs b/Assets/Scripts/Generated/Proto/DynamicConfig.cs index f522317b4..b85656ead 100644 --- a/Assets/Scripts/Generated/Proto/DynamicConfig.cs +++ b/Assets/Scripts/Generated/Proto/DynamicConfig.cs @@ -25,20 +25,20 @@ static DynamicConfigReflection() { byte[] descriptorData = global::System.Convert.FromBase64String( string.Concat( "ChxDb25maWdzL2R5bmFtaWNfY29uZmlnLnByb3RvEgdjb25maWdzGhdTaW11", - "bGF0aW9uL3NlbnNvci5wcm90byK4AQoMRmxpZ2h0Q29uZmlnEjAKD2NvbnRy", - "b2xsZXJfdHlwZRgBIAEoDjIXLmNvbmZpZ3MuQ29udHJvbGxlclR5cGUSOwoO", - "ZXZhc2lvbl9jb25maWcYAiABKAsyIy5jb25maWdzLkZsaWdodENvbmZpZy5F", - "dmFzaW9uQ29uZmlnGjkKDUV2YXNpb25Db25maWcSDwoHZW5hYmxlZBgBIAEo", - "CBIXCg9yYW5nZV90aHJlc2hvbGQYAiABKAIibgoNRHluYW1pY0NvbmZpZxIs", - "Cg1mbGlnaHRfY29uZmlnGAEgASgLMhUuY29uZmlncy5GbGlnaHRDb25maWcS", - "LwoNc2Vuc29yX2NvbmZpZxgCIAEoCzIYLnNpbXVsYXRpb24uU2Vuc29yQ29u", - "ZmlnKlQKDkNvbnRyb2xsZXJUeXBlEhsKF1BST1BPUlRJT05BTF9OQVZJR0FU", - "SU9OEAASJQohQVVHTUVOVEVEX1BST1BPUlRJT05BTF9OQVZJR0FUSU9OEAFi", - "BnByb3RvMw==")); + "bGF0aW9uL3NlbnNvci5wcm90byI5Cg1FdmFzaW9uQ29uZmlnEg8KB2VuYWJs", + "ZWQYASABKAgSFwoPcmFuZ2VfdGhyZXNob2xkGAIgASgCInAKDEZsaWdodENv", + "bmZpZxIwCg9jb250cm9sbGVyX3R5cGUYASABKA4yFy5jb25maWdzLkNvbnRy", + "b2xsZXJUeXBlEi4KDmV2YXNpb25fY29uZmlnGAIgASgLMhYuY29uZmlncy5F", + "dmFzaW9uQ29uZmlnIm4KDUR5bmFtaWNDb25maWcSLAoNZmxpZ2h0X2NvbmZp", + "ZxgBIAEoCzIVLmNvbmZpZ3MuRmxpZ2h0Q29uZmlnEi8KDXNlbnNvcl9jb25m", + "aWcYAiABKAsyGC5zaW11bGF0aW9uLlNlbnNvckNvbmZpZypUCg5Db250cm9s", + "bGVyVHlwZRIbChdQUk9QT1JUSU9OQUxfTkFWSUdBVElPThAAEiUKIUFVR01F", + "TlRFRF9QUk9QT1JUSU9OQUxfTkFWSUdBVElPThABYgZwcm90bzM=")); descriptor = pbr::FileDescriptor.FromGeneratedCode(descriptorData, new pbr::FileDescriptor[] { global::Simulation.SensorReflection.Descriptor, }, new pbr::GeneratedClrTypeInfo(new[] {typeof(global::Configs.ControllerType), }, null, new pbr::GeneratedClrTypeInfo[] { - new pbr::GeneratedClrTypeInfo(typeof(global::Configs.FlightConfig), global::Configs.FlightConfig.Parser, new[]{ "ControllerType", "EvasionConfig" }, null, null, null, new pbr::GeneratedClrTypeInfo[] { new pbr::GeneratedClrTypeInfo(typeof(global::Configs.FlightConfig.Types.EvasionConfig), global::Configs.FlightConfig.Types.EvasionConfig.Parser, new[]{ "Enabled", "RangeThreshold" }, null, null, null, null)}), + new pbr::GeneratedClrTypeInfo(typeof(global::Configs.EvasionConfig), global::Configs.EvasionConfig.Parser, new[]{ "Enabled", "RangeThreshold" }, null, null, null, null), + new pbr::GeneratedClrTypeInfo(typeof(global::Configs.FlightConfig), global::Configs.FlightConfig.Parser, new[]{ "ControllerType", "EvasionConfig" }, null, null, null, null), new pbr::GeneratedClrTypeInfo(typeof(global::Configs.DynamicConfig), global::Configs.DynamicConfig.Parser, new[]{ "FlightConfig", "SensorConfig" }, null, null, null, null) })); } @@ -57,6 +57,250 @@ public enum ControllerType { #endregion #region Messages + /// + /// Evasion configuration. + /// + [global::System.Diagnostics.DebuggerDisplayAttribute("{ToString(),nq}")] + public sealed partial class EvasionConfig : pb::IMessage + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + , pb::IBufferMessage + #endif + { + private static readonly pb::MessageParser _parser = new pb::MessageParser(() => new EvasionConfig()); + private pb::UnknownFieldSet _unknownFields; + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public static pb::MessageParser Parser { get { return _parser; } } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public static pbr::MessageDescriptor Descriptor { + get { return global::Configs.DynamicConfigReflection.Descriptor.MessageTypes[0]; } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + pbr::MessageDescriptor pb::IMessage.Descriptor { + get { return Descriptor; } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public EvasionConfig() { + OnConstruction(); + } + + partial void OnConstruction(); + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public EvasionConfig(EvasionConfig other) : this() { + enabled_ = other.enabled_; + rangeThreshold_ = other.rangeThreshold_; + _unknownFields = pb::UnknownFieldSet.Clone(other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public EvasionConfig Clone() { + return new EvasionConfig(this); + } + + /// Field number for the "enabled" field. + public const int EnabledFieldNumber = 1; + private bool enabled_; + /// + /// If true, evasion is enabled. + /// + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public bool Enabled { + get { return enabled_; } + set { + enabled_ = value; + } + } + + /// Field number for the "range_threshold" field. + public const int RangeThresholdFieldNumber = 2; + private float rangeThreshold_; + /// + /// Range threshold in m at which the agent should evade. + /// + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public float RangeThreshold { + get { return rangeThreshold_; } + set { + rangeThreshold_ = value; + } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override bool Equals(object other) { + return Equals(other as EvasionConfig); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public bool Equals(EvasionConfig other) { + if (ReferenceEquals(other, null)) { + return false; + } + if (ReferenceEquals(other, this)) { + return true; + } + if (Enabled != other.Enabled) return false; + if (!pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.Equals(RangeThreshold, other.RangeThreshold)) return false; + return Equals(_unknownFields, other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override int GetHashCode() { + int hash = 1; + if (Enabled != false) hash ^= Enabled.GetHashCode(); + if (RangeThreshold != 0F) hash ^= pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.GetHashCode(RangeThreshold); + if (_unknownFields != null) { + hash ^= _unknownFields.GetHashCode(); + } + return hash; + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override string ToString() { + return pb::JsonFormatter.ToDiagnosticString(this); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void WriteTo(pb::CodedOutputStream output) { + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + output.WriteRawMessage(this); + #else + if (Enabled != false) { + output.WriteRawTag(8); + output.WriteBool(Enabled); + } + if (RangeThreshold != 0F) { + output.WriteRawTag(21); + output.WriteFloat(RangeThreshold); + } + if (_unknownFields != null) { + _unknownFields.WriteTo(output); + } + #endif + } + + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + void pb::IBufferMessage.InternalWriteTo(ref pb::WriteContext output) { + if (Enabled != false) { + output.WriteRawTag(8); + output.WriteBool(Enabled); + } + if (RangeThreshold != 0F) { + output.WriteRawTag(21); + output.WriteFloat(RangeThreshold); + } + if (_unknownFields != null) { + _unknownFields.WriteTo(ref output); + } + } + #endif + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public int CalculateSize() { + int size = 0; + if (Enabled != false) { + size += 1 + 1; + } + if (RangeThreshold != 0F) { + size += 1 + 4; + } + if (_unknownFields != null) { + size += _unknownFields.CalculateSize(); + } + return size; + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void MergeFrom(EvasionConfig other) { + if (other == null) { + return; + } + if (other.Enabled != false) { + Enabled = other.Enabled; + } + if (other.RangeThreshold != 0F) { + RangeThreshold = other.RangeThreshold; + } + _unknownFields = pb::UnknownFieldSet.MergeFrom(_unknownFields, other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void MergeFrom(pb::CodedInputStream input) { + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + input.ReadRawMessage(this); + #else + uint tag; + while ((tag = input.ReadTag()) != 0) { + if ((tag & 7) == 4) { + // Abort on any end group tag. + return; + } + switch(tag) { + default: + _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, input); + break; + case 8: { + Enabled = input.ReadBool(); + break; + } + case 21: { + RangeThreshold = input.ReadFloat(); + break; + } + } + } + #endif + } + + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + void pb::IBufferMessage.InternalMergeFrom(ref pb::ParseContext input) { + uint tag; + while ((tag = input.ReadTag()) != 0) { + if ((tag & 7) == 4) { + // Abort on any end group tag. + return; + } + switch(tag) { + default: + _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, ref input); + break; + case 8: { + Enabled = input.ReadBool(); + break; + } + case 21: { + RangeThreshold = input.ReadFloat(); + break; + } + } + } + } + #endif + + } + /// /// Flight configuration. /// @@ -75,7 +319,7 @@ public sealed partial class FlightConfig : pb::IMessage [global::System.Diagnostics.DebuggerNonUserCodeAttribute] [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] public static pbr::MessageDescriptor Descriptor { - get { return global::Configs.DynamicConfigReflection.Descriptor.MessageTypes[0]; } + get { return global::Configs.DynamicConfigReflection.Descriptor.MessageTypes[1]; } } [global::System.Diagnostics.DebuggerNonUserCodeAttribute] @@ -123,13 +367,13 @@ public FlightConfig Clone() { /// Field number for the "evasion_config" field. public const int EvasionConfigFieldNumber = 2; - private global::Configs.FlightConfig.Types.EvasionConfig evasionConfig_; + private global::Configs.EvasionConfig evasionConfig_; /// /// Evasion configuration. /// [global::System.Diagnostics.DebuggerNonUserCodeAttribute] [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public global::Configs.FlightConfig.Types.EvasionConfig EvasionConfig { + public global::Configs.EvasionConfig EvasionConfig { get { return evasionConfig_; } set { evasionConfig_ = value; @@ -239,7 +483,7 @@ public void MergeFrom(FlightConfig other) { } if (other.evasionConfig_ != null) { if (evasionConfig_ == null) { - EvasionConfig = new global::Configs.FlightConfig.Types.EvasionConfig(); + EvasionConfig = new global::Configs.EvasionConfig(); } EvasionConfig.MergeFrom(other.EvasionConfig); } @@ -268,7 +512,7 @@ public void MergeFrom(pb::CodedInputStream input) { } case 18: { if (evasionConfig_ == null) { - EvasionConfig = new global::Configs.FlightConfig.Types.EvasionConfig(); + EvasionConfig = new global::Configs.EvasionConfig(); } input.ReadMessage(EvasionConfig); break; @@ -298,7 +542,7 @@ public void MergeFrom(pb::CodedInputStream input) { } case 18: { if (evasionConfig_ == null) { - EvasionConfig = new global::Configs.FlightConfig.Types.EvasionConfig(); + EvasionConfig = new global::Configs.EvasionConfig(); } input.ReadMessage(EvasionConfig); break; @@ -308,258 +552,6 @@ public void MergeFrom(pb::CodedInputStream input) { } #endif - #region Nested types - /// Container for nested types declared in the FlightConfig message type. - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static partial class Types { - /// - /// Evasion configuration. - /// - [global::System.Diagnostics.DebuggerDisplayAttribute("{ToString(),nq}")] - public sealed partial class EvasionConfig : pb::IMessage - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - , pb::IBufferMessage - #endif - { - private static readonly pb::MessageParser _parser = new pb::MessageParser(() => new EvasionConfig()); - private pb::UnknownFieldSet _unknownFields; - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static pb::MessageParser Parser { get { return _parser; } } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static pbr::MessageDescriptor Descriptor { - get { return global::Configs.FlightConfig.Descriptor.NestedTypes[0]; } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - pbr::MessageDescriptor pb::IMessage.Descriptor { - get { return Descriptor; } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public EvasionConfig() { - OnConstruction(); - } - - partial void OnConstruction(); - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public EvasionConfig(EvasionConfig other) : this() { - enabled_ = other.enabled_; - rangeThreshold_ = other.rangeThreshold_; - _unknownFields = pb::UnknownFieldSet.Clone(other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public EvasionConfig Clone() { - return new EvasionConfig(this); - } - - /// Field number for the "enabled" field. - public const int EnabledFieldNumber = 1; - private bool enabled_; - /// - /// If true, evasion is enabled. - /// - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public bool Enabled { - get { return enabled_; } - set { - enabled_ = value; - } - } - - /// Field number for the "range_threshold" field. - public const int RangeThresholdFieldNumber = 2; - private float rangeThreshold_; - /// - /// Range threshold in m at which the agent should evade. - /// - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public float RangeThreshold { - get { return rangeThreshold_; } - set { - rangeThreshold_ = value; - } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override bool Equals(object other) { - return Equals(other as EvasionConfig); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public bool Equals(EvasionConfig other) { - if (ReferenceEquals(other, null)) { - return false; - } - if (ReferenceEquals(other, this)) { - return true; - } - if (Enabled != other.Enabled) return false; - if (!pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.Equals(RangeThreshold, other.RangeThreshold)) return false; - return Equals(_unknownFields, other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override int GetHashCode() { - int hash = 1; - if (Enabled != false) hash ^= Enabled.GetHashCode(); - if (RangeThreshold != 0F) hash ^= pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.GetHashCode(RangeThreshold); - if (_unknownFields != null) { - hash ^= _unknownFields.GetHashCode(); - } - return hash; - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override string ToString() { - return pb::JsonFormatter.ToDiagnosticString(this); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void WriteTo(pb::CodedOutputStream output) { - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - output.WriteRawMessage(this); - #else - if (Enabled != false) { - output.WriteRawTag(8); - output.WriteBool(Enabled); - } - if (RangeThreshold != 0F) { - output.WriteRawTag(21); - output.WriteFloat(RangeThreshold); - } - if (_unknownFields != null) { - _unknownFields.WriteTo(output); - } - #endif - } - - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - void pb::IBufferMessage.InternalWriteTo(ref pb::WriteContext output) { - if (Enabled != false) { - output.WriteRawTag(8); - output.WriteBool(Enabled); - } - if (RangeThreshold != 0F) { - output.WriteRawTag(21); - output.WriteFloat(RangeThreshold); - } - if (_unknownFields != null) { - _unknownFields.WriteTo(ref output); - } - } - #endif - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public int CalculateSize() { - int size = 0; - if (Enabled != false) { - size += 1 + 1; - } - if (RangeThreshold != 0F) { - size += 1 + 4; - } - if (_unknownFields != null) { - size += _unknownFields.CalculateSize(); - } - return size; - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void MergeFrom(EvasionConfig other) { - if (other == null) { - return; - } - if (other.Enabled != false) { - Enabled = other.Enabled; - } - if (other.RangeThreshold != 0F) { - RangeThreshold = other.RangeThreshold; - } - _unknownFields = pb::UnknownFieldSet.MergeFrom(_unknownFields, other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void MergeFrom(pb::CodedInputStream input) { - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - input.ReadRawMessage(this); - #else - uint tag; - while ((tag = input.ReadTag()) != 0) { - if ((tag & 7) == 4) { - // Abort on any end group tag. - return; - } - switch(tag) { - default: - _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, input); - break; - case 8: { - Enabled = input.ReadBool(); - break; - } - case 21: { - RangeThreshold = input.ReadFloat(); - break; - } - } - } - #endif - } - - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - void pb::IBufferMessage.InternalMergeFrom(ref pb::ParseContext input) { - uint tag; - while ((tag = input.ReadTag()) != 0) { - if ((tag & 7) == 4) { - // Abort on any end group tag. - return; - } - switch(tag) { - default: - _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, ref input); - break; - case 8: { - Enabled = input.ReadBool(); - break; - } - case 21: { - RangeThreshold = input.ReadFloat(); - break; - } - } - } - } - #endif - - } - - } - #endregion - } /// @@ -580,7 +572,7 @@ public sealed partial class DynamicConfig : pb::IMessage [global::System.Diagnostics.DebuggerNonUserCodeAttribute] [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] public static pbr::MessageDescriptor Descriptor { - get { return global::Configs.DynamicConfigReflection.Descriptor.MessageTypes[1]; } + get { return global::Configs.DynamicConfigReflection.Descriptor.MessageTypes[2]; } } [global::System.Diagnostics.DebuggerNonUserCodeAttribute] diff --git a/Assets/Scripts/Movements/MovementBase.cs b/Assets/Scripts/Movements/MovementBase.cs index 38168868a..a313dbca4 100644 --- a/Assets/Scripts/Movements/MovementBase.cs +++ b/Assets/Scripts/Movements/MovementBase.cs @@ -14,20 +14,6 @@ public MovementBase(IAgent agent) { // applying physics and other constraints. public abstract Vector3 Act(in Vector3 accelerationInput); - // Get the maximum forward acceleration from the agent's static configuration. - public float MaxForwardAcceleration() { - return Agent.StaticConfig?.AccelerationConfig?.MaxForwardAcceleration ?? 0; - } - - // Get the maximum normal acceleration from the agent's static configuration. - public float MaxNormalAcceleration() { - var maxReferenceNormalAcceleration = - (Agent.StaticConfig?.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * - Constants.kGravity; - var referenceSpeed = Agent.StaticConfig?.AccelerationConfig?.ReferenceSpeed ?? 1; - return Mathf.Pow(Agent.Speed / referenceSpeed, 2) * maxReferenceNormalAcceleration; - } - // Limit acceleration input to the agent's maximum forward and normal accelerations. protected Vector3 LimitAccelerationInput(in Vector3 accelerationInput) { var forwardAccelerationInput = Vector3.Project(accelerationInput, Agent.transform.forward); @@ -35,9 +21,9 @@ protected Vector3 LimitAccelerationInput(in Vector3 accelerationInput) { // Limit the forward and the normal acceleration magnitude. forwardAccelerationInput = - Vector3.ClampMagnitude(forwardAccelerationInput, MaxForwardAcceleration()); + Vector3.ClampMagnitude(forwardAccelerationInput, Agent.MaxForwardAcceleration()); normalAccelerationInput = - Vector3.ClampMagnitude(normalAccelerationInput, MaxNormalAcceleration()); + Vector3.ClampMagnitude(normalAccelerationInput, Agent.MaxNormalAcceleration()); return forwardAccelerationInput + normalAccelerationInput; } } diff --git a/Assets/Scripts/Sensors/ISensor.cs b/Assets/Scripts/Sensors/ISensor.cs index b983a0657..40152aa10 100644 --- a/Assets/Scripts/Sensors/ISensor.cs +++ b/Assets/Scripts/Sensors/ISensor.cs @@ -9,6 +9,9 @@ public interface ISensor { // Sense the target hierarchical object. SensorOutput Sense(IHierarchical hierarchical); + // Sense the target agent. + SensorOutput Sense(IAgent agent); + // Sense the waypoint. SensorOutput Sense(in Vector3 waypoint); } diff --git a/Assets/Scripts/Sensors/IdealSensor.cs b/Assets/Scripts/Sensors/IdealSensor.cs index fe08c7522..dafa28dc5 100644 --- a/Assets/Scripts/Sensors/IdealSensor.cs +++ b/Assets/Scripts/Sensors/IdealSensor.cs @@ -12,6 +12,12 @@ public override SensorOutput Sense(IHierarchical hierarchical) { return GenerateSensorOutput(relativeTransformation); } + // Sense the target agent. + public override SensorOutput Sense(IAgent agent) { + var relativeTransformation = Agent.GetRelativeTransformation(agent); + return GenerateSensorOutput(relativeTransformation); + } + // Sense the waypoint. public override SensorOutput Sense(in Vector3 waypoint) { var relativeTransformation = Agent.GetRelativeTransformation(waypoint); diff --git a/Assets/Scripts/Sensors/SensorBase.cs b/Assets/Scripts/Sensors/SensorBase.cs index b2b8662d0..dd8251a9c 100644 --- a/Assets/Scripts/Sensors/SensorBase.cs +++ b/Assets/Scripts/Sensors/SensorBase.cs @@ -12,6 +12,9 @@ public SensorBase(IAgent agent) { // Sense the target hierarchical object. public abstract SensorOutput Sense(IHierarchical hierarchical); + // Sense the target hierarchical object. + public abstract SensorOutput Sense(IAgent agent); + // Sense the waypoint. public abstract SensorOutput Sense(in Vector3 waypoint); } diff --git a/Assets/Scripts/Threats/IThreat.cs b/Assets/Scripts/Threats/IThreat.cs index f63155085..db528bfee 100644 --- a/Assets/Scripts/Threats/IThreat.cs +++ b/Assets/Scripts/Threats/IThreat.cs @@ -1,7 +1,13 @@ +using System.Collections.Generic; + // Interface for a threat. // // Threats attempt to reach the asset while avoiding the defending interceptors. public interface IThreat { IAttackBehavior AttackBehavior { get; set; } IEvasion Evasion { get; set; } + + Dictionary PowerTable { get; } + + float LookupPowerTable(Configs.Power power); } diff --git a/Assets/Scripts/Threats/ThreatBase.cs b/Assets/Scripts/Threats/ThreatBase.cs index 0ab652aab..0fbe3c390 100644 --- a/Assets/Scripts/Threats/ThreatBase.cs +++ b/Assets/Scripts/Threats/ThreatBase.cs @@ -1,3 +1,6 @@ +using System.Collections.Generic; +using System.Linq; + // Base implementation of a threat. public abstract class ThreatBase : AgentBase, IThreat { // The attack behavior determines how the threat navigates towards the asset. @@ -5,4 +8,12 @@ public abstract class ThreatBase : AgentBase, IThreat { // The evasion handles how the threat behaves in the vicinity of a pursuing interceptor. public IEvasion Evasion { get; set; } + + public Dictionary PowerTable => + StaticConfig.PowerTable.ToDictionary(entry => entry.Power, entry => entry.Speed); + + public float LookupPowerTable(Configs.Power power) { + PowerTable.TryGetValue(power, out float speed); + return speed; + } } From d68be3956dbec17e657e849c1ba95f97dc7d07be Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 22:49:13 -0700 Subject: [PATCH 30/74] Add evasion unit tests --- Assets/Tests/EditMode/NoEvasionTests.cs | 29 +++++++ Assets/Tests/EditMode/NoEvasionTests.cs.meta | 2 + .../Tests/EditMode/OrthogonalEvasionTests.cs | 86 +++++++++++++++++++ .../EditMode/OrthogonalEvasionTests.cs.meta | 2 + 4 files changed, 119 insertions(+) create mode 100644 Assets/Tests/EditMode/NoEvasionTests.cs create mode 100644 Assets/Tests/EditMode/NoEvasionTests.cs.meta create mode 100644 Assets/Tests/EditMode/OrthogonalEvasionTests.cs create mode 100644 Assets/Tests/EditMode/OrthogonalEvasionTests.cs.meta diff --git a/Assets/Tests/EditMode/NoEvasionTests.cs b/Assets/Tests/EditMode/NoEvasionTests.cs new file mode 100644 index 000000000..82cc0090a --- /dev/null +++ b/Assets/Tests/EditMode/NoEvasionTests.cs @@ -0,0 +1,29 @@ +using NUnit.Framework; +using UnityEngine; + +public class NoEvasionTests : TestBase { + private AgentBase _agent; + private AgentBase _pursuer; + private NoEvasion _evasion; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _pursuer = new GameObject("Agent").AddComponent(); + Rigidbody pursuerRb = _pursuer.gameObject.AddComponent(); + InvokePrivateMethod(_pursuer, "Awake"); + _evasion = new NoEvasion(_agent); + } + + [Test] + public void ShouldEvade_ReturnsFalse() { + Assert.IsFalse(_evasion.ShouldEvade(_pursuer)); + } + + [Test] + public void Evade_ReturnsZero() { + Assert.AreEqual(Vector3.zero, _evasion.Evade(_pursuer)); + } +} diff --git a/Assets/Tests/EditMode/NoEvasionTests.cs.meta b/Assets/Tests/EditMode/NoEvasionTests.cs.meta new file mode 100644 index 000000000..0e96a9fae --- /dev/null +++ b/Assets/Tests/EditMode/NoEvasionTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 5e02c6df201cf42eeb03df4ef8f610cc \ No newline at end of file diff --git a/Assets/Tests/EditMode/OrthogonalEvasionTests.cs b/Assets/Tests/EditMode/OrthogonalEvasionTests.cs new file mode 100644 index 000000000..e7734a0a9 --- /dev/null +++ b/Assets/Tests/EditMode/OrthogonalEvasionTests.cs @@ -0,0 +1,86 @@ +using NUnit.Framework; +using UnityEngine; + +public class OrthogonalEvasionTests : TestBase { + private const float _epsilon = 1e-3f; + + private AgentBase _agent; + private AgentBase _pursuer; + private OrthogonalEvasion _evasion; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + _agent.StaticConfig = new Configs.StaticConfig() { + AccelerationConfig = + new Configs.AccelerationConfig() { + MaxForwardAcceleration = 10, + MaxReferenceNormalAcceleration = 5 / Constants.kGravity, + ReferenceSpeed = 100, + }, + }; + _agent.AgentConfig = new Configs.AgentConfig() { + DynamicConfig = + new Configs.DynamicConfig() { + FlightConfig = new Configs.FlightConfig() { EvasionConfig = + new Configs.EvasionConfig() { + Enabled = true, + RangeThreshold = 1000, + } }, + }, + }; + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _agent.Sensor = new IdealSensor(_agent); + _pursuer = new GameObject("Agent").AddComponent(); + Rigidbody pursuerRb = _pursuer.gameObject.AddComponent(); + InvokePrivateMethod(_pursuer, "Awake"); + _evasion = new OrthogonalEvasion(_agent); + } + + [Test] + public void ShouldEvade_EvasionDisabled_ReturnsFalse() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 2, 0); + _pursuer.Position = new Vector3(0, 100, 0); + _pursuer.Velocity = new Vector3(0, -1, 0); + _agent.AgentConfig.DynamicConfig.FlightConfig.EvasionConfig.Enabled = false; + Assert.IsFalse(_evasion.ShouldEvade(_pursuer)); + } + + [Test] + public void ShouldEvade_OutsideRange_ReturnsFalse() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 2, 0); + _pursuer.Position = new Vector3(0, 2000, 0); + _pursuer.Velocity = new Vector3(0, -1, 0); + Assert.IsFalse(_evasion.ShouldEvade(_pursuer)); + } + + [Test] + public void ShouldEvade_MovingAway_ReturnsFalse() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 1, 0); + _pursuer.Position = new Vector3(0, 500, 0); + _pursuer.Velocity = new Vector3(0, 2, 0); + Assert.IsFalse(_evasion.ShouldEvade(_pursuer)); + } + + [Test] + public void Evade_NonZeroNormalAcceleration() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(1, 2, 0); + _pursuer.Position = new Vector3(0, 100, 0); + _pursuer.Velocity = new Vector3(0, -1, 0); + Assert.AreEqual(0, Vector3.Dot(_evasion.Evade(_pursuer), _agent.Velocity)); + } + + [Test] + public void Evade_OrthogonalToPursuerVelocity() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 2, 0); + _pursuer.Position = new Vector3(0, 100, 0); + _pursuer.Velocity = new Vector3(0, -1, 0); + Assert.AreEqual(0, Vector3.Dot(_evasion.Evade(_pursuer), _pursuer.Velocity)); + } +} diff --git a/Assets/Tests/EditMode/OrthogonalEvasionTests.cs.meta b/Assets/Tests/EditMode/OrthogonalEvasionTests.cs.meta new file mode 100644 index 000000000..eeb597c28 --- /dev/null +++ b/Assets/Tests/EditMode/OrthogonalEvasionTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: f0a80d618bbfc410d8596820ec8eef8f \ No newline at end of file From a3d1a1a1eab658223dc4afa4acf8ce9a6199d11c Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 13 Oct 2025 23:24:23 -0700 Subject: [PATCH 31/74] Address comments --- Assets/Scripts/Evasion/OrthogonalEvasion.cs | 2 +- Assets/Scripts/Threats/ThreatBase.cs | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/Evasion/OrthogonalEvasion.cs b/Assets/Scripts/Evasion/OrthogonalEvasion.cs index 7c07706f7..9747f06d4 100644 --- a/Assets/Scripts/Evasion/OrthogonalEvasion.cs +++ b/Assets/Scripts/Evasion/OrthogonalEvasion.cs @@ -56,10 +56,10 @@ public override Vector3 Evade(IAgent pursuer) { float groundProximityThreshold = Mathf.Abs(agentVelocity.y) * groundProximityThresholdFactor; if (agentVelocity.y < 0 && altitude < groundProximityThreshold) { // Determine the evasion direction based on the angle to pursuer. - var rightDirection = Vector3.Cross(Vector3.up, Agent.transform.forward); var angle = Vector3.SignedAngle(Agent.transform.forward, relativePosition, Vector3.up); // Choose the direction that leads away from the pursuer. + var rightDirection = Agent.transform.right; var bestHorizontalDirection = angle > 0 ? -rightDirection : rightDirection; // Blend between horizontal evasion and slight upward movement. diff --git a/Assets/Scripts/Threats/ThreatBase.cs b/Assets/Scripts/Threats/ThreatBase.cs index 0fbe3c390..12462d8fe 100644 --- a/Assets/Scripts/Threats/ThreatBase.cs +++ b/Assets/Scripts/Threats/ThreatBase.cs @@ -3,14 +3,24 @@ // Base implementation of a threat. public abstract class ThreatBase : AgentBase, IThreat { + // Power table map from the power to the speed. + private Dictionary _powerTable; + // The attack behavior determines how the threat navigates towards the asset. public IAttackBehavior AttackBehavior { get; set; } // The evasion handles how the threat behaves in the vicinity of a pursuing interceptor. public IEvasion Evasion { get; set; } - public Dictionary PowerTable => - StaticConfig.PowerTable.ToDictionary(entry => entry.Power, entry => entry.Speed); + public Dictionary PowerTable { + get { + if (_powerTable == null) { + _powerTable = + StaticConfig.PowerTable.ToDictionary(entry => entry.Power, entry => entry.Speed); + } + return _powerTable; + } + } public float LookupPowerTable(Configs.Power power) { PowerTable.TryGetValue(power, out float speed); From a4076a6d6be252de95dd44a3c86d6b9ca9125934 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Tue, 14 Oct 2025 00:36:40 -0700 Subject: [PATCH 32/74] Set power table to IReadOnlyDictionary --- Assets/Scripts/Threats/IThreat.cs | 2 +- Assets/Scripts/Threats/ThreatBase.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Threats/IThreat.cs b/Assets/Scripts/Threats/IThreat.cs index db528bfee..484515cee 100644 --- a/Assets/Scripts/Threats/IThreat.cs +++ b/Assets/Scripts/Threats/IThreat.cs @@ -7,7 +7,7 @@ public interface IThreat { IAttackBehavior AttackBehavior { get; set; } IEvasion Evasion { get; set; } - Dictionary PowerTable { get; } + IReadOnlyDictionary PowerTable { get; } float LookupPowerTable(Configs.Power power); } diff --git a/Assets/Scripts/Threats/ThreatBase.cs b/Assets/Scripts/Threats/ThreatBase.cs index 12462d8fe..432a5f0fa 100644 --- a/Assets/Scripts/Threats/ThreatBase.cs +++ b/Assets/Scripts/Threats/ThreatBase.cs @@ -12,7 +12,7 @@ public abstract class ThreatBase : AgentBase, IThreat { // The evasion handles how the threat behaves in the vicinity of a pursuing interceptor. public IEvasion Evasion { get; set; } - public Dictionary PowerTable { + public IReadOnlyDictionary PowerTable { get { if (_powerTable == null) { _powerTable = From 51b36e07cdcb6aca224a01acf4d41cb483b55440 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Tue, 14 Oct 2025 00:53:31 -0700 Subject: [PATCH 33/74] Implement attack behaviors --- .../Configs/attack_behavior_config.proto | 45 +- Assets/Scripts/Behavior/AttackBehaviorBase.cs | 20 +- .../Scripts/Behavior/AttackBehaviorFactory.cs | 2 +- .../Scripts/Behavior/DirectAttackBehavior.cs | 20 +- .../Behavior/DirectAttackBehavior.cs.meta | 2 +- .../Behavior/DirectAttackBehaviorLegacy.cs | 43 + .../DirectAttackBehaviorLegacy.cs.meta | 2 + Assets/Scripts/Behavior/FlightPlan.cs | 27 +- Assets/Scripts/Behavior/IAttackBehavior.cs | 6 +- Assets/Scripts/Behavior/Waypoint.cs | 11 - Assets/Scripts/Behavior/Waypoint.cs.meta | 2 - .../Generated/Proto/AttackBehaviorConfig.cs | 1106 ++++++++--------- Assets/Tests/EditMode/BehaviorTests.cs | 67 +- Assets/Tests/EditMode/ThreatTests.cs | 12 +- 14 files changed, 702 insertions(+), 663 deletions(-) create mode 100644 Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs create mode 100644 Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs.meta delete mode 100644 Assets/Scripts/Behavior/Waypoint.cs delete mode 100644 Assets/Scripts/Behavior/Waypoint.cs.meta diff --git a/Assets/Proto/Configs/attack_behavior_config.proto b/Assets/Proto/Configs/attack_behavior_config.proto index 51f9aa5f4..3dfd834a3 100644 --- a/Assets/Proto/Configs/attack_behavior_config.proto +++ b/Assets/Proto/Configs/attack_behavior_config.proto @@ -12,34 +12,35 @@ enum AttackType { SLALOM_ATTACK = 3; } -// Attack behavior configuration. -message AttackBehaviorConfig { - // Flight plan type enumeration. - enum FlightPlanType { - INVALID_FLIGHT_PLAN = 0; - DISTANCE_TO_TARGET = 1; - } +// Flight plan type enumeration. +enum FlightPlanType { + INVALID_FLIGHT_PLAN = 0; + DISTANCE_TO_TARGET = 1; +} - // Flight plan. - message FlightPlan { - message Waypoint { - // Distance. - float distance = 1; +// Flight plan waypoint. +message FlightPlanWaypoint { + // Distance. + float distance = 1; - // Altitude. - float altitude = 2; + // Altitude. + float altitude = 2; - // Power. - Power power = 3; - } + // Power. + Power power = 3; +} - // Flight plan type. - FlightPlanType type = 1; +// Flight plan. +message FlightPlan { + // Flight plan type. + FlightPlanType type = 1; - // Waypoints. - repeated Waypoint waypoints = 2; - } + // Waypoints. + repeated FlightPlanWaypoint waypoints = 2; +} +// Attack behavior configuration. +message AttackBehaviorConfig { // Attack behavior name. string name = 1; diff --git a/Assets/Scripts/Behavior/AttackBehaviorBase.cs b/Assets/Scripts/Behavior/AttackBehaviorBase.cs index 76c097b2a..7c9a5769a 100644 --- a/Assets/Scripts/Behavior/AttackBehaviorBase.cs +++ b/Assets/Scripts/Behavior/AttackBehaviorBase.cs @@ -7,16 +7,32 @@ public abstract class AttackBehaviorBase : IAttackBehavior { private Configs.AttackBehaviorConfig _config; // Flight plan. - [SerializeField] private FlightPlan _flightPlan; + // Agent that will execute the attack behavior. + public IAgent Agent { get; set; } + public Configs.AttackBehaviorConfig Config { get => _config; set => _config = value; } + public FlightPlan FlightPlan { + get { + if (_flightPlan == null) { + _flightPlan = new FlightPlan(Config.FlightPlan); + } + return _flightPlan; + } + } + + public AttackBehaviorBase(IAgent agent, Configs.AttackBehaviorConfig config) { + Agent = agent; + Config = config; + } + // Return the next waypoint for the agent to navigate to and the power setting to use towards the // waypoint. public abstract (Vector3 waypointPosition, Configs.Power power) - GetNextWaypoint(IAgent agent, in Vector3 targetPosition); + GetNextWaypoint(in Vector3 targetPosition); } diff --git a/Assets/Scripts/Behavior/AttackBehaviorFactory.cs b/Assets/Scripts/Behavior/AttackBehaviorFactory.cs index d8f783c38..bb4496c2e 100644 --- a/Assets/Scripts/Behavior/AttackBehaviorFactory.cs +++ b/Assets/Scripts/Behavior/AttackBehaviorFactory.cs @@ -1,7 +1,7 @@ public static class AttackBehaviorFactory { public static AttackBehavior Create(in Configs.AttackBehaviorConfig config) { return config.Type switch { - Configs.AttackType.DirectAttack => new DirectAttackBehavior(config), + Configs.AttackType.DirectAttack => new DirectAttackBehaviorLegacy(config), Configs.AttackType.PreplannedAttack => null, Configs.AttackType.SlalomAttack => null, _ => null, diff --git a/Assets/Scripts/Behavior/DirectAttackBehavior.cs b/Assets/Scripts/Behavior/DirectAttackBehavior.cs index ee0026995..8d7e64f32 100644 --- a/Assets/Scripts/Behavior/DirectAttackBehavior.cs +++ b/Assets/Scripts/Behavior/DirectAttackBehavior.cs @@ -1,19 +1,23 @@ using UnityEngine; -public class DirectAttackBehavior : AttackBehavior { - public DirectAttackBehavior(in Configs.AttackBehaviorConfig config) : base(config) {} +// Direct attack behavior. +// +// The agent will navigate directly to each waypoint on the way to the final target. +public abstract class DirectAttackBehavior : AttackBehaviorBase { + public DirectAttackBehavior(IAgent agent, Configs.AttackBehaviorConfig config) + : base(agent, config) {} - // Return the next waypoint for the threat to navigate to and the power setting to use towards the + // Return the next waypoint for the agent to navigate to and the power setting to use towards the // waypoint. public override (Vector3 waypointPosition, Configs.Power power) - GetNextWaypoint(Vector3 currentPosition, Vector3 targetPosition) { + GetNextWaypoint(in Vector3 targetPosition) { if (FlightPlan.Waypoints.Count == 0) { // If no waypoints are defined, directly target the target position. return (targetPosition, Configs.Power.Max); } - Vector3 directionToTarget = targetPosition - currentPosition; - float distanceToTarget = directionToTarget.magnitude; + var directionToTarget = targetPosition - Agent.Position; + var distanceToTarget = directionToTarget.magnitude; // Find the index of the first waypoint whose position is closer to the target than the current // position. @@ -24,8 +28,8 @@ public override (Vector3 waypointPosition, Configs.Power power) } } - Vector3 waypointPosition = targetPosition; - Configs.Power power = Configs.Power.Idle; + var waypointPosition = targetPosition; + var power = Configs.Power.Idle; if (waypointIndex == FlightPlan.Waypoints.Count) { // This is the last waypoint, so target the final position with the last waypoint's power // setting. diff --git a/Assets/Scripts/Behavior/DirectAttackBehavior.cs.meta b/Assets/Scripts/Behavior/DirectAttackBehavior.cs.meta index f95dd73db..2ec9e3627 100644 --- a/Assets/Scripts/Behavior/DirectAttackBehavior.cs.meta +++ b/Assets/Scripts/Behavior/DirectAttackBehavior.cs.meta @@ -1,2 +1,2 @@ fileFormatVersion: 2 -guid: a3fab0e342bc4c844b5bffde2abb83ec \ No newline at end of file +guid: d0af367dfb71b4c45b2667715189ae90 \ No newline at end of file diff --git a/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs b/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs new file mode 100644 index 000000000..66d07ebb5 --- /dev/null +++ b/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs @@ -0,0 +1,43 @@ +using UnityEngine; + +public class DirectAttackBehaviorLegacy : AttackBehavior { + public DirectAttackBehaviorLegacy(in Configs.AttackBehaviorConfig config) : base(config) {} + + // Return the next waypoint for the threat to navigate to and the power setting to use towards the + // waypoint. + public override (Vector3 waypointPosition, Configs.Power power) + GetNextWaypoint(Vector3 currentPosition, Vector3 targetPosition) { + if (FlightPlan.Waypoints.Count == 0) { + // If no waypoints are defined, directly target the target position. + return (targetPosition, Configs.Power.Max); + } + + Vector3 directionToTarget = targetPosition - currentPosition; + float distanceToTarget = directionToTarget.magnitude; + + // Find the index of the first waypoint whose position is closer to the target than the current + // position. + int waypointIndex = 0; + for (waypointIndex = 0; waypointIndex < FlightPlan.Waypoints.Count; ++waypointIndex) { + if (distanceToTarget > FlightPlan.Waypoints[waypointIndex].Distance) { + break; + } + } + + Vector3 waypointPosition = targetPosition; + Configs.Power power = Configs.Power.Idle; + if (waypointIndex == FlightPlan.Waypoints.Count) { + // This is the last waypoint, so target the final position with the last waypoint's power + // setting. + waypointPosition = targetPosition; + power = FlightPlan.Waypoints[FlightPlan.Waypoints.Count - 1].Power; + return (waypointPosition, power); + } + // There is a next waypoint. + var waypoint = FlightPlan.Waypoints[waypointIndex]; + waypointPosition = targetPosition - directionToTarget.normalized * waypoint.Distance; + waypointPosition.y = waypoint.Altitude; + power = waypoint.Power; + return (waypointPosition, power); + } +} diff --git a/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs.meta b/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs.meta new file mode 100644 index 000000000..f95dd73db --- /dev/null +++ b/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: a3fab0e342bc4c844b5bffde2abb83ec \ No newline at end of file diff --git a/Assets/Scripts/Behavior/FlightPlan.cs b/Assets/Scripts/Behavior/FlightPlan.cs index c37aad48a..b3601dee4 100644 --- a/Assets/Scripts/Behavior/FlightPlan.cs +++ b/Assets/Scripts/Behavior/FlightPlan.cs @@ -1,20 +1,25 @@ using System.Collections.Generic; using System.Linq; +// Flight plan. +// +// The flight plan contains a sorted list of waypoints for the agent on the way to the asset. public class FlightPlan { - private Configs.AttackBehaviorConfig.Types.FlightPlan _flightPlan; + // List of waypoints sorted by distance in descending order. + List _waypoints; - public Configs.AttackBehaviorConfig.Types.FlightPlanType Type { - get { return _flightPlan.Type; } - } + public Configs.FlightPlan Config { get; set; } - public List Waypoints { get; } + public IReadOnlyList Waypoints { + get { + if (_waypoints == null) { + _waypoints = Config.Waypoints.OrderByDescending(waypoint => waypoint.Distance).ToList(); + } + return _waypoints.AsReadOnly(); + } + } - public FlightPlan(in Configs.AttackBehaviorConfig.Types.FlightPlan flightPlan) { - _flightPlan = flightPlan ?? new Configs.AttackBehaviorConfig.Types.FlightPlan(); - // Sort waypoints in descending order based on distance. - Waypoints = _flightPlan.Waypoints.Select(waypoint => new Waypoint(waypoint)) - .OrderByDescending(waypoint => waypoint.Distance) - .ToList(); + public FlightPlan(Configs.FlightPlan config) { + Config = config; } } diff --git a/Assets/Scripts/Behavior/IAttackBehavior.cs b/Assets/Scripts/Behavior/IAttackBehavior.cs index c5024a002..7f6b5ee10 100644 --- a/Assets/Scripts/Behavior/IAttackBehavior.cs +++ b/Assets/Scripts/Behavior/IAttackBehavior.cs @@ -4,10 +4,12 @@ // // The attack behavior determines how the the threat navigates towards the asset. public interface IAttackBehavior { + IAgent Agent { get; set; } + Configs.AttackBehaviorConfig Config { get; set; } + FlightPlan FlightPlan { get; } // Return the next waypoint for the agent to navigate to and the power setting to use towards the // waypoint. - (Vector3 waypointPosition, Configs.Power power) - GetNextWaypoint(IAgent agent, in Vector3 targetPosition); + (Vector3 waypointPosition, Configs.Power power) GetNextWaypoint(in Vector3 targetPosition); } diff --git a/Assets/Scripts/Behavior/Waypoint.cs b/Assets/Scripts/Behavior/Waypoint.cs deleted file mode 100644 index 55a27946c..000000000 --- a/Assets/Scripts/Behavior/Waypoint.cs +++ /dev/null @@ -1,11 +0,0 @@ -public class Waypoint { - public float Distance { get; } - public float Altitude { get; } - public Configs.Power Power { get; } - - public Waypoint(in Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint waypoint) { - Distance = waypoint.Distance; - Altitude = waypoint.Altitude; - Power = waypoint.Power; - } -} diff --git a/Assets/Scripts/Behavior/Waypoint.cs.meta b/Assets/Scripts/Behavior/Waypoint.cs.meta deleted file mode 100644 index 3a1e18b81..000000000 --- a/Assets/Scripts/Behavior/Waypoint.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: d736791c73ba74056b77d9aa38bdc5ab \ No newline at end of file diff --git a/Assets/Scripts/Generated/Proto/AttackBehaviorConfig.cs b/Assets/Scripts/Generated/Proto/AttackBehaviorConfig.cs index 0efeb597f..47d0c2793 100644 --- a/Assets/Scripts/Generated/Proto/AttackBehaviorConfig.cs +++ b/Assets/Scripts/Generated/Proto/AttackBehaviorConfig.cs @@ -25,23 +25,24 @@ static AttackBehaviorConfigReflection() { byte[] descriptorData = global::System.Convert.FromBase64String( string.Concat( "CiRDb25maWdzL2F0dGFja19iZWhhdmlvcl9jb25maWcucHJvdG8SB2NvbmZp", - "Z3MaG0NvbmZpZ3Mvc3RhdGljX2NvbmZpZy5wcm90byKpAwoUQXR0YWNrQmVo", - "YXZpb3JDb25maWcSDAoEbmFtZRgBIAEoCRIhCgR0eXBlGAIgASgOMhMuY29u", - "Zmlncy5BdHRhY2tUeXBlEj0KC2ZsaWdodF9wbGFuGAMgASgLMiguY29uZmln", - "cy5BdHRhY2tCZWhhdmlvckNvbmZpZy5GbGlnaHRQbGFuGt0BCgpGbGlnaHRQ", - "bGFuEjoKBHR5cGUYASABKA4yLC5jb25maWdzLkF0dGFja0JlaGF2aW9yQ29u", - "ZmlnLkZsaWdodFBsYW5UeXBlEkQKCXdheXBvaW50cxgCIAMoCzIxLmNvbmZp", - "Z3MuQXR0YWNrQmVoYXZpb3JDb25maWcuRmxpZ2h0UGxhbi5XYXlwb2ludBpN", - "CghXYXlwb2ludBIQCghkaXN0YW5jZRgBIAEoAhIQCghhbHRpdHVkZRgCIAEo", - "AhIdCgVwb3dlchgDIAEoDjIOLmNvbmZpZ3MuUG93ZXIiQQoORmxpZ2h0UGxh", - "blR5cGUSFwoTSU5WQUxJRF9GTElHSFRfUExBThAAEhYKEkRJU1RBTkNFX1RP", - "X1RBUkdFVBABKl0KCkF0dGFja1R5cGUSEgoOSU5WQUxJRF9BVFRBQ0sQABIR", - "Cg1ESVJFQ1RfQVRUQUNLEAESFQoRUFJFUExBTk5FRF9BVFRBQ0sQAhIRCg1T", - "TEFMT01fQVRUQUNLEANiBnByb3RvMw==")); + "Z3MaG0NvbmZpZ3Mvc3RhdGljX2NvbmZpZy5wcm90byJXChJGbGlnaHRQbGFu", + "V2F5cG9pbnQSEAoIZGlzdGFuY2UYASABKAISEAoIYWx0aXR1ZGUYAiABKAIS", + "HQoFcG93ZXIYAyABKA4yDi5jb25maWdzLlBvd2VyImMKCkZsaWdodFBsYW4S", + "JQoEdHlwZRgBIAEoDjIXLmNvbmZpZ3MuRmxpZ2h0UGxhblR5cGUSLgoJd2F5", + "cG9pbnRzGAIgAygLMhsuY29uZmlncy5GbGlnaHRQbGFuV2F5cG9pbnQicQoU", + "QXR0YWNrQmVoYXZpb3JDb25maWcSDAoEbmFtZRgBIAEoCRIhCgR0eXBlGAIg", + "ASgOMhMuY29uZmlncy5BdHRhY2tUeXBlEigKC2ZsaWdodF9wbGFuGAMgASgL", + "MhMuY29uZmlncy5GbGlnaHRQbGFuKl0KCkF0dGFja1R5cGUSEgoOSU5WQUxJ", + "RF9BVFRBQ0sQABIRCg1ESVJFQ1RfQVRUQUNLEAESFQoRUFJFUExBTk5FRF9B", + "VFRBQ0sQAhIRCg1TTEFMT01fQVRUQUNLEAMqQQoORmxpZ2h0UGxhblR5cGUS", + "FwoTSU5WQUxJRF9GTElHSFRfUExBThAAEhYKEkRJU1RBTkNFX1RPX1RBUkdF", + "VBABYgZwcm90bzM=")); descriptor = pbr::FileDescriptor.FromGeneratedCode(descriptorData, new pbr::FileDescriptor[] { global::Configs.StaticConfigReflection.Descriptor, }, - new pbr::GeneratedClrTypeInfo(new[] {typeof(global::Configs.AttackType), }, null, new pbr::GeneratedClrTypeInfo[] { - new pbr::GeneratedClrTypeInfo(typeof(global::Configs.AttackBehaviorConfig), global::Configs.AttackBehaviorConfig.Parser, new[]{ "Name", "Type", "FlightPlan" }, null, new[]{ typeof(global::Configs.AttackBehaviorConfig.Types.FlightPlanType) }, null, new pbr::GeneratedClrTypeInfo[] { new pbr::GeneratedClrTypeInfo(typeof(global::Configs.AttackBehaviorConfig.Types.FlightPlan), global::Configs.AttackBehaviorConfig.Types.FlightPlan.Parser, new[]{ "Type", "Waypoints" }, null, null, null, new pbr::GeneratedClrTypeInfo[] { new pbr::GeneratedClrTypeInfo(typeof(global::Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint), global::Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint.Parser, new[]{ "Distance", "Altitude", "Power" }, null, null, null, null)})}) + new pbr::GeneratedClrTypeInfo(new[] {typeof(global::Configs.AttackType), typeof(global::Configs.FlightPlanType), }, null, new pbr::GeneratedClrTypeInfo[] { + new pbr::GeneratedClrTypeInfo(typeof(global::Configs.FlightPlanWaypoint), global::Configs.FlightPlanWaypoint.Parser, new[]{ "Distance", "Altitude", "Power" }, null, null, null, null), + new pbr::GeneratedClrTypeInfo(typeof(global::Configs.FlightPlan), global::Configs.FlightPlan.Parser, new[]{ "Type", "Waypoints" }, null, null, null, null), + new pbr::GeneratedClrTypeInfo(typeof(global::Configs.AttackBehaviorConfig), global::Configs.AttackBehaviorConfig.Parser, new[]{ "Name", "Type", "FlightPlan" }, null, null, null, null) })); } #endregion @@ -58,9 +59,534 @@ public enum AttackType { [pbr::OriginalName("SLALOM_ATTACK")] SlalomAttack = 3, } + /// + /// Flight plan type enumeration. + /// + public enum FlightPlanType { + [pbr::OriginalName("INVALID_FLIGHT_PLAN")] InvalidFlightPlan = 0, + [pbr::OriginalName("DISTANCE_TO_TARGET")] DistanceToTarget = 1, + } + #endregion #region Messages + /// + /// Flight plan waypoint. + /// + [global::System.Diagnostics.DebuggerDisplayAttribute("{ToString(),nq}")] + public sealed partial class FlightPlanWaypoint : pb::IMessage + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + , pb::IBufferMessage + #endif + { + private static readonly pb::MessageParser _parser = new pb::MessageParser(() => new FlightPlanWaypoint()); + private pb::UnknownFieldSet _unknownFields; + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public static pb::MessageParser Parser { get { return _parser; } } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public static pbr::MessageDescriptor Descriptor { + get { return global::Configs.AttackBehaviorConfigReflection.Descriptor.MessageTypes[0]; } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + pbr::MessageDescriptor pb::IMessage.Descriptor { + get { return Descriptor; } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public FlightPlanWaypoint() { + OnConstruction(); + } + + partial void OnConstruction(); + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public FlightPlanWaypoint(FlightPlanWaypoint other) : this() { + distance_ = other.distance_; + altitude_ = other.altitude_; + power_ = other.power_; + _unknownFields = pb::UnknownFieldSet.Clone(other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public FlightPlanWaypoint Clone() { + return new FlightPlanWaypoint(this); + } + + /// Field number for the "distance" field. + public const int DistanceFieldNumber = 1; + private float distance_; + /// + /// Distance. + /// + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public float Distance { + get { return distance_; } + set { + distance_ = value; + } + } + + /// Field number for the "altitude" field. + public const int AltitudeFieldNumber = 2; + private float altitude_; + /// + /// Altitude. + /// + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public float Altitude { + get { return altitude_; } + set { + altitude_ = value; + } + } + + /// Field number for the "power" field. + public const int PowerFieldNumber = 3; + private global::Configs.Power power_ = global::Configs.Power.Idle; + /// + /// Power. + /// + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public global::Configs.Power Power { + get { return power_; } + set { + power_ = value; + } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override bool Equals(object other) { + return Equals(other as FlightPlanWaypoint); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public bool Equals(FlightPlanWaypoint other) { + if (ReferenceEquals(other, null)) { + return false; + } + if (ReferenceEquals(other, this)) { + return true; + } + if (!pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.Equals(Distance, other.Distance)) return false; + if (!pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.Equals(Altitude, other.Altitude)) return false; + if (Power != other.Power) return false; + return Equals(_unknownFields, other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override int GetHashCode() { + int hash = 1; + if (Distance != 0F) hash ^= pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.GetHashCode(Distance); + if (Altitude != 0F) hash ^= pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.GetHashCode(Altitude); + if (Power != global::Configs.Power.Idle) hash ^= Power.GetHashCode(); + if (_unknownFields != null) { + hash ^= _unknownFields.GetHashCode(); + } + return hash; + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override string ToString() { + return pb::JsonFormatter.ToDiagnosticString(this); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void WriteTo(pb::CodedOutputStream output) { + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + output.WriteRawMessage(this); + #else + if (Distance != 0F) { + output.WriteRawTag(13); + output.WriteFloat(Distance); + } + if (Altitude != 0F) { + output.WriteRawTag(21); + output.WriteFloat(Altitude); + } + if (Power != global::Configs.Power.Idle) { + output.WriteRawTag(24); + output.WriteEnum((int) Power); + } + if (_unknownFields != null) { + _unknownFields.WriteTo(output); + } + #endif + } + + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + void pb::IBufferMessage.InternalWriteTo(ref pb::WriteContext output) { + if (Distance != 0F) { + output.WriteRawTag(13); + output.WriteFloat(Distance); + } + if (Altitude != 0F) { + output.WriteRawTag(21); + output.WriteFloat(Altitude); + } + if (Power != global::Configs.Power.Idle) { + output.WriteRawTag(24); + output.WriteEnum((int) Power); + } + if (_unknownFields != null) { + _unknownFields.WriteTo(ref output); + } + } + #endif + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public int CalculateSize() { + int size = 0; + if (Distance != 0F) { + size += 1 + 4; + } + if (Altitude != 0F) { + size += 1 + 4; + } + if (Power != global::Configs.Power.Idle) { + size += 1 + pb::CodedOutputStream.ComputeEnumSize((int) Power); + } + if (_unknownFields != null) { + size += _unknownFields.CalculateSize(); + } + return size; + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void MergeFrom(FlightPlanWaypoint other) { + if (other == null) { + return; + } + if (other.Distance != 0F) { + Distance = other.Distance; + } + if (other.Altitude != 0F) { + Altitude = other.Altitude; + } + if (other.Power != global::Configs.Power.Idle) { + Power = other.Power; + } + _unknownFields = pb::UnknownFieldSet.MergeFrom(_unknownFields, other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void MergeFrom(pb::CodedInputStream input) { + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + input.ReadRawMessage(this); + #else + uint tag; + while ((tag = input.ReadTag()) != 0) { + if ((tag & 7) == 4) { + // Abort on any end group tag. + return; + } + switch(tag) { + default: + _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, input); + break; + case 13: { + Distance = input.ReadFloat(); + break; + } + case 21: { + Altitude = input.ReadFloat(); + break; + } + case 24: { + Power = (global::Configs.Power) input.ReadEnum(); + break; + } + } + } + #endif + } + + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + void pb::IBufferMessage.InternalMergeFrom(ref pb::ParseContext input) { + uint tag; + while ((tag = input.ReadTag()) != 0) { + if ((tag & 7) == 4) { + // Abort on any end group tag. + return; + } + switch(tag) { + default: + _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, ref input); + break; + case 13: { + Distance = input.ReadFloat(); + break; + } + case 21: { + Altitude = input.ReadFloat(); + break; + } + case 24: { + Power = (global::Configs.Power) input.ReadEnum(); + break; + } + } + } + } + #endif + + } + + /// + /// Flight plan. + /// + [global::System.Diagnostics.DebuggerDisplayAttribute("{ToString(),nq}")] + public sealed partial class FlightPlan : pb::IMessage + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + , pb::IBufferMessage + #endif + { + private static readonly pb::MessageParser _parser = new pb::MessageParser(() => new FlightPlan()); + private pb::UnknownFieldSet _unknownFields; + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public static pb::MessageParser Parser { get { return _parser; } } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public static pbr::MessageDescriptor Descriptor { + get { return global::Configs.AttackBehaviorConfigReflection.Descriptor.MessageTypes[1]; } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + pbr::MessageDescriptor pb::IMessage.Descriptor { + get { return Descriptor; } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public FlightPlan() { + OnConstruction(); + } + + partial void OnConstruction(); + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public FlightPlan(FlightPlan other) : this() { + type_ = other.type_; + waypoints_ = other.waypoints_.Clone(); + _unknownFields = pb::UnknownFieldSet.Clone(other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public FlightPlan Clone() { + return new FlightPlan(this); + } + + /// Field number for the "type" field. + public const int TypeFieldNumber = 1; + private global::Configs.FlightPlanType type_ = global::Configs.FlightPlanType.InvalidFlightPlan; + /// + /// Flight plan type. + /// + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public global::Configs.FlightPlanType Type { + get { return type_; } + set { + type_ = value; + } + } + + /// Field number for the "waypoints" field. + public const int WaypointsFieldNumber = 2; + private static readonly pb::FieldCodec _repeated_waypoints_codec + = pb::FieldCodec.ForMessage(18, global::Configs.FlightPlanWaypoint.Parser); + private readonly pbc::RepeatedField waypoints_ = new pbc::RepeatedField(); + /// + /// Waypoints. + /// + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public pbc::RepeatedField Waypoints { + get { return waypoints_; } + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override bool Equals(object other) { + return Equals(other as FlightPlan); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public bool Equals(FlightPlan other) { + if (ReferenceEquals(other, null)) { + return false; + } + if (ReferenceEquals(other, this)) { + return true; + } + if (Type != other.Type) return false; + if(!waypoints_.Equals(other.waypoints_)) return false; + return Equals(_unknownFields, other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override int GetHashCode() { + int hash = 1; + if (Type != global::Configs.FlightPlanType.InvalidFlightPlan) hash ^= Type.GetHashCode(); + hash ^= waypoints_.GetHashCode(); + if (_unknownFields != null) { + hash ^= _unknownFields.GetHashCode(); + } + return hash; + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public override string ToString() { + return pb::JsonFormatter.ToDiagnosticString(this); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void WriteTo(pb::CodedOutputStream output) { + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + output.WriteRawMessage(this); + #else + if (Type != global::Configs.FlightPlanType.InvalidFlightPlan) { + output.WriteRawTag(8); + output.WriteEnum((int) Type); + } + waypoints_.WriteTo(output, _repeated_waypoints_codec); + if (_unknownFields != null) { + _unknownFields.WriteTo(output); + } + #endif + } + + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + void pb::IBufferMessage.InternalWriteTo(ref pb::WriteContext output) { + if (Type != global::Configs.FlightPlanType.InvalidFlightPlan) { + output.WriteRawTag(8); + output.WriteEnum((int) Type); + } + waypoints_.WriteTo(ref output, _repeated_waypoints_codec); + if (_unknownFields != null) { + _unknownFields.WriteTo(ref output); + } + } + #endif + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public int CalculateSize() { + int size = 0; + if (Type != global::Configs.FlightPlanType.InvalidFlightPlan) { + size += 1 + pb::CodedOutputStream.ComputeEnumSize((int) Type); + } + size += waypoints_.CalculateSize(_repeated_waypoints_codec); + if (_unknownFields != null) { + size += _unknownFields.CalculateSize(); + } + return size; + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void MergeFrom(FlightPlan other) { + if (other == null) { + return; + } + if (other.Type != global::Configs.FlightPlanType.InvalidFlightPlan) { + Type = other.Type; + } + waypoints_.Add(other.waypoints_); + _unknownFields = pb::UnknownFieldSet.MergeFrom(_unknownFields, other._unknownFields); + } + + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + public void MergeFrom(pb::CodedInputStream input) { + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + input.ReadRawMessage(this); + #else + uint tag; + while ((tag = input.ReadTag()) != 0) { + if ((tag & 7) == 4) { + // Abort on any end group tag. + return; + } + switch(tag) { + default: + _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, input); + break; + case 8: { + Type = (global::Configs.FlightPlanType) input.ReadEnum(); + break; + } + case 18: { + waypoints_.AddEntriesFrom(input, _repeated_waypoints_codec); + break; + } + } + } + #endif + } + + #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE + [global::System.Diagnostics.DebuggerNonUserCodeAttribute] + [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] + void pb::IBufferMessage.InternalMergeFrom(ref pb::ParseContext input) { + uint tag; + while ((tag = input.ReadTag()) != 0) { + if ((tag & 7) == 4) { + // Abort on any end group tag. + return; + } + switch(tag) { + default: + _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, ref input); + break; + case 8: { + Type = (global::Configs.FlightPlanType) input.ReadEnum(); + break; + } + case 18: { + waypoints_.AddEntriesFrom(ref input, _repeated_waypoints_codec); + break; + } + } + } + } + #endif + + } + /// /// Attack behavior configuration. /// @@ -79,7 +605,7 @@ public sealed partial class AttackBehaviorConfig : pb::IMessageField number for the "flight_plan" field. public const int FlightPlanFieldNumber = 3; - private global::Configs.AttackBehaviorConfig.Types.FlightPlan flightPlan_; + private global::Configs.FlightPlan flightPlan_; /// /// Flight plan. /// [global::System.Diagnostics.DebuggerNonUserCodeAttribute] [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public global::Configs.AttackBehaviorConfig.Types.FlightPlan FlightPlan { + public global::Configs.FlightPlan FlightPlan { get { return flightPlan_; } set { flightPlan_ = value; @@ -275,7 +801,7 @@ public void MergeFrom(AttackBehaviorConfig other) { } if (other.flightPlan_ != null) { if (flightPlan_ == null) { - FlightPlan = new global::Configs.AttackBehaviorConfig.Types.FlightPlan(); + FlightPlan = new global::Configs.FlightPlan(); } FlightPlan.MergeFrom(other.FlightPlan); } @@ -308,7 +834,7 @@ public void MergeFrom(pb::CodedInputStream input) { } case 26: { if (flightPlan_ == null) { - FlightPlan = new global::Configs.AttackBehaviorConfig.Types.FlightPlan(); + FlightPlan = new global::Configs.FlightPlan(); } input.ReadMessage(FlightPlan); break; @@ -342,7 +868,7 @@ public void MergeFrom(pb::CodedInputStream input) { } case 26: { if (flightPlan_ == null) { - FlightPlan = new global::Configs.AttackBehaviorConfig.Types.FlightPlan(); + FlightPlan = new global::Configs.FlightPlan(); } input.ReadMessage(FlightPlan); break; @@ -352,544 +878,6 @@ public void MergeFrom(pb::CodedInputStream input) { } #endif - #region Nested types - /// Container for nested types declared in the AttackBehaviorConfig message type. - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static partial class Types { - /// - /// Flight plan type enumeration. - /// - public enum FlightPlanType { - [pbr::OriginalName("INVALID_FLIGHT_PLAN")] InvalidFlightPlan = 0, - [pbr::OriginalName("DISTANCE_TO_TARGET")] DistanceToTarget = 1, - } - - /// - /// Flight plan. - /// - [global::System.Diagnostics.DebuggerDisplayAttribute("{ToString(),nq}")] - public sealed partial class FlightPlan : pb::IMessage - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - , pb::IBufferMessage - #endif - { - private static readonly pb::MessageParser _parser = new pb::MessageParser(() => new FlightPlan()); - private pb::UnknownFieldSet _unknownFields; - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static pb::MessageParser Parser { get { return _parser; } } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static pbr::MessageDescriptor Descriptor { - get { return global::Configs.AttackBehaviorConfig.Descriptor.NestedTypes[0]; } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - pbr::MessageDescriptor pb::IMessage.Descriptor { - get { return Descriptor; } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public FlightPlan() { - OnConstruction(); - } - - partial void OnConstruction(); - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public FlightPlan(FlightPlan other) : this() { - type_ = other.type_; - waypoints_ = other.waypoints_.Clone(); - _unknownFields = pb::UnknownFieldSet.Clone(other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public FlightPlan Clone() { - return new FlightPlan(this); - } - - /// Field number for the "type" field. - public const int TypeFieldNumber = 1; - private global::Configs.AttackBehaviorConfig.Types.FlightPlanType type_ = global::Configs.AttackBehaviorConfig.Types.FlightPlanType.InvalidFlightPlan; - /// - /// Flight plan type. - /// - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public global::Configs.AttackBehaviorConfig.Types.FlightPlanType Type { - get { return type_; } - set { - type_ = value; - } - } - - /// Field number for the "waypoints" field. - public const int WaypointsFieldNumber = 2; - private static readonly pb::FieldCodec _repeated_waypoints_codec - = pb::FieldCodec.ForMessage(18, global::Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint.Parser); - private readonly pbc::RepeatedField waypoints_ = new pbc::RepeatedField(); - /// - /// Waypoints. - /// - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public pbc::RepeatedField Waypoints { - get { return waypoints_; } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override bool Equals(object other) { - return Equals(other as FlightPlan); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public bool Equals(FlightPlan other) { - if (ReferenceEquals(other, null)) { - return false; - } - if (ReferenceEquals(other, this)) { - return true; - } - if (Type != other.Type) return false; - if(!waypoints_.Equals(other.waypoints_)) return false; - return Equals(_unknownFields, other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override int GetHashCode() { - int hash = 1; - if (Type != global::Configs.AttackBehaviorConfig.Types.FlightPlanType.InvalidFlightPlan) hash ^= Type.GetHashCode(); - hash ^= waypoints_.GetHashCode(); - if (_unknownFields != null) { - hash ^= _unknownFields.GetHashCode(); - } - return hash; - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override string ToString() { - return pb::JsonFormatter.ToDiagnosticString(this); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void WriteTo(pb::CodedOutputStream output) { - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - output.WriteRawMessage(this); - #else - if (Type != global::Configs.AttackBehaviorConfig.Types.FlightPlanType.InvalidFlightPlan) { - output.WriteRawTag(8); - output.WriteEnum((int) Type); - } - waypoints_.WriteTo(output, _repeated_waypoints_codec); - if (_unknownFields != null) { - _unknownFields.WriteTo(output); - } - #endif - } - - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - void pb::IBufferMessage.InternalWriteTo(ref pb::WriteContext output) { - if (Type != global::Configs.AttackBehaviorConfig.Types.FlightPlanType.InvalidFlightPlan) { - output.WriteRawTag(8); - output.WriteEnum((int) Type); - } - waypoints_.WriteTo(ref output, _repeated_waypoints_codec); - if (_unknownFields != null) { - _unknownFields.WriteTo(ref output); - } - } - #endif - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public int CalculateSize() { - int size = 0; - if (Type != global::Configs.AttackBehaviorConfig.Types.FlightPlanType.InvalidFlightPlan) { - size += 1 + pb::CodedOutputStream.ComputeEnumSize((int) Type); - } - size += waypoints_.CalculateSize(_repeated_waypoints_codec); - if (_unknownFields != null) { - size += _unknownFields.CalculateSize(); - } - return size; - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void MergeFrom(FlightPlan other) { - if (other == null) { - return; - } - if (other.Type != global::Configs.AttackBehaviorConfig.Types.FlightPlanType.InvalidFlightPlan) { - Type = other.Type; - } - waypoints_.Add(other.waypoints_); - _unknownFields = pb::UnknownFieldSet.MergeFrom(_unknownFields, other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void MergeFrom(pb::CodedInputStream input) { - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - input.ReadRawMessage(this); - #else - uint tag; - while ((tag = input.ReadTag()) != 0) { - if ((tag & 7) == 4) { - // Abort on any end group tag. - return; - } - switch(tag) { - default: - _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, input); - break; - case 8: { - Type = (global::Configs.AttackBehaviorConfig.Types.FlightPlanType) input.ReadEnum(); - break; - } - case 18: { - waypoints_.AddEntriesFrom(input, _repeated_waypoints_codec); - break; - } - } - } - #endif - } - - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - void pb::IBufferMessage.InternalMergeFrom(ref pb::ParseContext input) { - uint tag; - while ((tag = input.ReadTag()) != 0) { - if ((tag & 7) == 4) { - // Abort on any end group tag. - return; - } - switch(tag) { - default: - _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, ref input); - break; - case 8: { - Type = (global::Configs.AttackBehaviorConfig.Types.FlightPlanType) input.ReadEnum(); - break; - } - case 18: { - waypoints_.AddEntriesFrom(ref input, _repeated_waypoints_codec); - break; - } - } - } - } - #endif - - #region Nested types - /// Container for nested types declared in the FlightPlan message type. - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static partial class Types { - [global::System.Diagnostics.DebuggerDisplayAttribute("{ToString(),nq}")] - public sealed partial class Waypoint : pb::IMessage - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - , pb::IBufferMessage - #endif - { - private static readonly pb::MessageParser _parser = new pb::MessageParser(() => new Waypoint()); - private pb::UnknownFieldSet _unknownFields; - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static pb::MessageParser Parser { get { return _parser; } } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public static pbr::MessageDescriptor Descriptor { - get { return global::Configs.AttackBehaviorConfig.Types.FlightPlan.Descriptor.NestedTypes[0]; } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - pbr::MessageDescriptor pb::IMessage.Descriptor { - get { return Descriptor; } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public Waypoint() { - OnConstruction(); - } - - partial void OnConstruction(); - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public Waypoint(Waypoint other) : this() { - distance_ = other.distance_; - altitude_ = other.altitude_; - power_ = other.power_; - _unknownFields = pb::UnknownFieldSet.Clone(other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public Waypoint Clone() { - return new Waypoint(this); - } - - /// Field number for the "distance" field. - public const int DistanceFieldNumber = 1; - private float distance_; - /// - /// Distance. - /// - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public float Distance { - get { return distance_; } - set { - distance_ = value; - } - } - - /// Field number for the "altitude" field. - public const int AltitudeFieldNumber = 2; - private float altitude_; - /// - /// Altitude. - /// - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public float Altitude { - get { return altitude_; } - set { - altitude_ = value; - } - } - - /// Field number for the "power" field. - public const int PowerFieldNumber = 3; - private global::Configs.Power power_ = global::Configs.Power.Idle; - /// - /// Power. - /// - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public global::Configs.Power Power { - get { return power_; } - set { - power_ = value; - } - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override bool Equals(object other) { - return Equals(other as Waypoint); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public bool Equals(Waypoint other) { - if (ReferenceEquals(other, null)) { - return false; - } - if (ReferenceEquals(other, this)) { - return true; - } - if (!pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.Equals(Distance, other.Distance)) return false; - if (!pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.Equals(Altitude, other.Altitude)) return false; - if (Power != other.Power) return false; - return Equals(_unknownFields, other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override int GetHashCode() { - int hash = 1; - if (Distance != 0F) hash ^= pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.GetHashCode(Distance); - if (Altitude != 0F) hash ^= pbc::ProtobufEqualityComparers.BitwiseSingleEqualityComparer.GetHashCode(Altitude); - if (Power != global::Configs.Power.Idle) hash ^= Power.GetHashCode(); - if (_unknownFields != null) { - hash ^= _unknownFields.GetHashCode(); - } - return hash; - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public override string ToString() { - return pb::JsonFormatter.ToDiagnosticString(this); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void WriteTo(pb::CodedOutputStream output) { - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - output.WriteRawMessage(this); - #else - if (Distance != 0F) { - output.WriteRawTag(13); - output.WriteFloat(Distance); - } - if (Altitude != 0F) { - output.WriteRawTag(21); - output.WriteFloat(Altitude); - } - if (Power != global::Configs.Power.Idle) { - output.WriteRawTag(24); - output.WriteEnum((int) Power); - } - if (_unknownFields != null) { - _unknownFields.WriteTo(output); - } - #endif - } - - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - void pb::IBufferMessage.InternalWriteTo(ref pb::WriteContext output) { - if (Distance != 0F) { - output.WriteRawTag(13); - output.WriteFloat(Distance); - } - if (Altitude != 0F) { - output.WriteRawTag(21); - output.WriteFloat(Altitude); - } - if (Power != global::Configs.Power.Idle) { - output.WriteRawTag(24); - output.WriteEnum((int) Power); - } - if (_unknownFields != null) { - _unknownFields.WriteTo(ref output); - } - } - #endif - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public int CalculateSize() { - int size = 0; - if (Distance != 0F) { - size += 1 + 4; - } - if (Altitude != 0F) { - size += 1 + 4; - } - if (Power != global::Configs.Power.Idle) { - size += 1 + pb::CodedOutputStream.ComputeEnumSize((int) Power); - } - if (_unknownFields != null) { - size += _unknownFields.CalculateSize(); - } - return size; - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void MergeFrom(Waypoint other) { - if (other == null) { - return; - } - if (other.Distance != 0F) { - Distance = other.Distance; - } - if (other.Altitude != 0F) { - Altitude = other.Altitude; - } - if (other.Power != global::Configs.Power.Idle) { - Power = other.Power; - } - _unknownFields = pb::UnknownFieldSet.MergeFrom(_unknownFields, other._unknownFields); - } - - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - public void MergeFrom(pb::CodedInputStream input) { - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - input.ReadRawMessage(this); - #else - uint tag; - while ((tag = input.ReadTag()) != 0) { - if ((tag & 7) == 4) { - // Abort on any end group tag. - return; - } - switch(tag) { - default: - _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, input); - break; - case 13: { - Distance = input.ReadFloat(); - break; - } - case 21: { - Altitude = input.ReadFloat(); - break; - } - case 24: { - Power = (global::Configs.Power) input.ReadEnum(); - break; - } - } - } - #endif - } - - #if !GOOGLE_PROTOBUF_REFSTRUCT_COMPATIBILITY_MODE - [global::System.Diagnostics.DebuggerNonUserCodeAttribute] - [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] - void pb::IBufferMessage.InternalMergeFrom(ref pb::ParseContext input) { - uint tag; - while ((tag = input.ReadTag()) != 0) { - if ((tag & 7) == 4) { - // Abort on any end group tag. - return; - } - switch(tag) { - default: - _unknownFields = pb::UnknownFieldSet.MergeFieldFrom(_unknownFields, ref input); - break; - case 13: { - Distance = input.ReadFloat(); - break; - } - case 21: { - Altitude = input.ReadFloat(); - break; - } - case 24: { - Power = (global::Configs.Power) input.ReadEnum(); - break; - } - } - } - } - #endif - - } - - } - #endregion - - } - - } - #endregion - } #endregion diff --git a/Assets/Tests/EditMode/BehaviorTests.cs b/Assets/Tests/EditMode/BehaviorTests.cs index e09f379de..efd3a8fbc 100644 --- a/Assets/Tests/EditMode/BehaviorTests.cs +++ b/Assets/Tests/EditMode/BehaviorTests.cs @@ -7,17 +7,14 @@ public class BehaviorTests : TestBase { public void TestDirectAttackBehaviorFactoryUsesOverride() { var config = new Configs.AttackBehaviorConfig() { Name = "Sample Attack", Type = Configs.AttackType.DirectAttack, - FlightPlan = - new Configs.AttackBehaviorConfig.Types.FlightPlan() { - Type = Configs.AttackBehaviorConfig.Types.FlightPlanType.DistanceToTarget, - Waypoints = { new List() { - new Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint() { - Distance = 1000, - Altitude = 100, - Power = Configs.Power.Cruise, - } - } } - } + FlightPlan = new Configs.FlightPlan() { Type = Configs.FlightPlanType.DistanceToTarget, + Waypoints = { new List() { + new Configs.FlightPlanWaypoint() { + Distance = 1000, + Altitude = 100, + Power = Configs.Power.Cruise, + } + } } } }; AttackBehavior attackBehavior = AttackBehaviorFactory.Create(config); @@ -33,33 +30,27 @@ public void TestDirectAttackBehaviorFactoryUsesOverride() { [Test] public void TestDirectAttackBehaviorWaypoints() { // Create a sample direct attack behavior. - DirectAttackBehavior attackBehavior = - new DirectAttackBehavior( - new Configs.AttackBehaviorConfig() { - Name = "Sample Attack", Type = Configs.AttackType.DirectAttack, - FlightPlan = - new Configs.AttackBehaviorConfig.Types.FlightPlan() { - Type = Configs.AttackBehaviorConfig.Types.FlightPlanType.DistanceToTarget, - Waypoints = - { new List() { - new Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint() { - Distance = 1000, - Altitude = 100, - Power = Configs.Power.Cruise, - }, - new Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint() { - Distance = 500, - Altitude = 50, - Power = Configs.Power.Mil, - }, - new Configs.AttackBehaviorConfig.Types.FlightPlan.Types.Waypoint() { - Distance = 100, - Altitude = 25, - Power = Configs.Power.Max, - } - } } - } - }); + var attackBehavior = new DirectAttackBehaviorLegacy(new Configs.AttackBehaviorConfig() { + Name = "Sample Attack", Type = Configs.AttackType.DirectAttack, + FlightPlan = new Configs.FlightPlan() { Type = Configs.FlightPlanType.DistanceToTarget, + Waypoints = { new List() { + new Configs.FlightPlanWaypoint() { + Distance = 1000, + Altitude = 100, + Power = Configs.Power.Cruise, + }, + new Configs.FlightPlanWaypoint() { + Distance = 500, + Altitude = 50, + Power = Configs.Power.Mil, + }, + new Configs.FlightPlanWaypoint() { + Distance = 100, + Altitude = 25, + Power = Configs.Power.Max, + } + } } } + }); Vector3 targetPosition = new Vector3(1000, 0, 0); const float epsilon = 0.001f; diff --git a/Assets/Tests/EditMode/ThreatTests.cs b/Assets/Tests/EditMode/ThreatTests.cs index bc43285fb..bc5fa8b3b 100644 --- a/Assets/Tests/EditMode/ThreatTests.cs +++ b/Assets/Tests/EditMode/ThreatTests.cs @@ -219,15 +219,15 @@ public void AttackBehavior_LoadedCorrectly() { Assert.AreEqual("Test Direct Attack", attackBehavior.Name); Assert.AreEqual(Configs.AttackType.DirectAttack, attackBehavior.Type); - Assert.IsTrue(attackBehavior is DirectAttackBehavior, - "Attack behavior should be a DirectAttackBehavior."); - DirectAttackBehavior directAttackBehavior = (DirectAttackBehavior)attackBehavior; + Assert.IsTrue(attackBehavior is DirectAttackBehaviorLegacy, + "Attack behavior should be a DirectAttackBehaviorLegacy."); + var directAttackBehavior = (DirectAttackBehaviorLegacy)attackBehavior; Assert.IsNotNull(attackBehavior.FlightPlan, "Flight plan should not be null."); - Assert.AreEqual(Configs.AttackBehaviorConfig.Types.FlightPlanType.DistanceToTarget, - attackBehavior.FlightPlan.Type); + Assert.AreEqual(Configs.FlightPlanType.DistanceToTarget, + attackBehavior.FlightPlan.Config.Type); - List waypoints = attackBehavior.FlightPlan.Waypoints; + var waypoints = attackBehavior.FlightPlan.Waypoints; Assert.IsNotNull(waypoints, "Waypoints should not be null."); Assert.AreEqual(2, waypoints.Count, "There should be 2 waypoints."); From 1b6f9993414639a0a35ba35225843aaf2d5d03f9 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Tue, 14 Oct 2025 11:58:15 -0700 Subject: [PATCH 34/74] Add unit tests for direct attack behavior --- .../Scripts/Behavior/DirectAttackBehavior.cs | 2 +- Assets/Tests/EditMode/BehaviorTests.cs | 2 +- .../EditMode/DirectAttackBehaviorTests.cs | 94 +++++++++++++++++++ .../DirectAttackBehaviorTests.cs.meta | 2 + Assets/Tests/EditMode/ThreatTests.cs | 6 +- 5 files changed, 101 insertions(+), 5 deletions(-) create mode 100644 Assets/Tests/EditMode/DirectAttackBehaviorTests.cs create mode 100644 Assets/Tests/EditMode/DirectAttackBehaviorTests.cs.meta diff --git a/Assets/Scripts/Behavior/DirectAttackBehavior.cs b/Assets/Scripts/Behavior/DirectAttackBehavior.cs index 8d7e64f32..b7810ca10 100644 --- a/Assets/Scripts/Behavior/DirectAttackBehavior.cs +++ b/Assets/Scripts/Behavior/DirectAttackBehavior.cs @@ -3,7 +3,7 @@ // Direct attack behavior. // // The agent will navigate directly to each waypoint on the way to the final target. -public abstract class DirectAttackBehavior : AttackBehaviorBase { +public class DirectAttackBehavior : AttackBehaviorBase { public DirectAttackBehavior(IAgent agent, Configs.AttackBehaviorConfig config) : base(agent, config) {} diff --git a/Assets/Tests/EditMode/BehaviorTests.cs b/Assets/Tests/EditMode/BehaviorTests.cs index efd3a8fbc..bc9e3441f 100644 --- a/Assets/Tests/EditMode/BehaviorTests.cs +++ b/Assets/Tests/EditMode/BehaviorTests.cs @@ -1,6 +1,6 @@ using NUnit.Framework; -using UnityEngine; using System.Collections.Generic; +using UnityEngine; public class BehaviorTests : TestBase { [Test] diff --git a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs new file mode 100644 index 000000000..17108cdaa --- /dev/null +++ b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs @@ -0,0 +1,94 @@ +using NUnit.Framework; +using System.Collections.Generic; +using UnityEngine; + +public class DirectAttackBehaviorTests : TestBase { + private AgentBase _agent; + private DirectAttackBehavior _attackBehavior; + private Vector3 _targetPosition = new Vector3(1000, 0, 0); + private Vector3 _targetPositionWithNonzeroZ = new Vector3(800, 0, 600); + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + var attackBehaviorConfig = new Configs.AttackBehaviorConfig() { + Name = "Direct Attack", + Type = Configs.AttackType.DirectAttack, + FlightPlan = + new Configs.FlightPlan() { + Type = Configs.FlightPlanType.DistanceToTarget, + Waypoints = + { + new List() { + new Configs.FlightPlanWaypoint() { + Distance = 1000, + Altitude = 100, + Power = Configs.Power.Cruise, + }, + new Configs.FlightPlanWaypoint() { + Distance = 500, + Altitude = 50, + Power = Configs.Power.Mil, + }, + new Configs.FlightPlanWaypoint() { + Distance = 100, + Altitude = 25, + Power = Configs.Power.Max, + }, + }, + }, + }, + }; + _attackBehavior = new DirectAttackBehavior(_agent, attackBehaviorConfig); + } + + [Test] + public void GetNextWaypoint_BeyondFarthestWaypoint() { + _agent.Position = new Vector3(-100, 0, 0); + (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + Assert.AreEqual(new Vector3(0, 100, 0), waypoint); + Assert.AreEqual(Configs.Power.Cruise, power); + } + + [Test] + public void GetNextWaypoint_AtFarthestWaypoint() { + _agent.Position = new Vector3(0, 0, 0); + (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + Assert.AreEqual(new Vector3(500, 50, 0), waypoint); + Assert.AreEqual(Configs.Power.Mil, power); + } + + [Test] + public void GetNextWaypoint_BetweenWaypoints() { + _agent.Position = new Vector3(600, 0, 0); + (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + Assert.AreEqual(new Vector3(900, 25, 0), waypoint); + Assert.AreEqual(Configs.Power.Max, power); + } + + [Test] + public void GetNextWaypoint_PastClosestWaypoint() { + _agent.Position = new Vector3(920, 0, 0); + (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + Assert.AreEqual(new Vector3(1000, 0, 0), waypoint); + Assert.AreEqual(Configs.Power.Max, power); + } + + [Test] + public void GetNextWaypoint_AtFarthestWaypoint_WithNonzeroZ() { + _agent.Position = new Vector3(0, 0, 0); + (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); + Assert.AreEqual(new Vector3(400, 50, 300), waypoint); + Assert.AreEqual(Configs.Power.Mil, power); + } + + [Test] + public void GetNextWaypoint_BetweenWaypoints_WithNonzeroZ() { + _agent.Position = new Vector3(440, 0, 330); + (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); + Assert.AreEqual(new Vector3(720, 25, 540), waypoint); + Assert.AreEqual(Configs.Power.Max, power); + } +} diff --git a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs.meta b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs.meta new file mode 100644 index 000000000..d68cd4294 --- /dev/null +++ b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 14dcdc17a2b734a3e970303772e793c0 \ No newline at end of file diff --git a/Assets/Tests/EditMode/ThreatTests.cs b/Assets/Tests/EditMode/ThreatTests.cs index bc5fa8b3b..c8f6b09bf 100644 --- a/Assets/Tests/EditMode/ThreatTests.cs +++ b/Assets/Tests/EditMode/ThreatTests.cs @@ -1,9 +1,9 @@ -using System; using NUnit.Framework; -using UnityEngine; +using System; using System.Collections.Generic; -using System.Linq; using System.IO; +using System.Linq; +using UnityEngine; public class ThreatTests : AgentTestBase { private FixedWingThreat _fixedWingThreat; From 79362813f56a48575dce2f6721b3e6d39efc1bb4 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Tue, 14 Oct 2025 12:27:22 -0700 Subject: [PATCH 35/74] Address comments --- Assets/Scripts/Behavior/FlightPlan.cs | 2 +- Assets/Tests/EditMode/BehaviorTests.cs | 67 +++++++++++-------- .../EditMode/DirectAttackBehaviorTests.cs | 31 ++++----- Assets/Tests/EditMode/FlightPlanTests.cs | 46 +++++++++++++ Assets/Tests/EditMode/FlightPlanTests.cs.meta | 2 + 5 files changed, 101 insertions(+), 47 deletions(-) create mode 100644 Assets/Tests/EditMode/FlightPlanTests.cs create mode 100644 Assets/Tests/EditMode/FlightPlanTests.cs.meta diff --git a/Assets/Scripts/Behavior/FlightPlan.cs b/Assets/Scripts/Behavior/FlightPlan.cs index b3601dee4..29bb79d55 100644 --- a/Assets/Scripts/Behavior/FlightPlan.cs +++ b/Assets/Scripts/Behavior/FlightPlan.cs @@ -20,6 +20,6 @@ public IReadOnlyList Waypoints { } public FlightPlan(Configs.FlightPlan config) { - Config = config; + Config = config ?? new Configs.FlightPlan(); } } diff --git a/Assets/Tests/EditMode/BehaviorTests.cs b/Assets/Tests/EditMode/BehaviorTests.cs index bc9e3441f..08d33e409 100644 --- a/Assets/Tests/EditMode/BehaviorTests.cs +++ b/Assets/Tests/EditMode/BehaviorTests.cs @@ -1,20 +1,24 @@ using NUnit.Framework; -using System.Collections.Generic; using UnityEngine; public class BehaviorTests : TestBase { [Test] public void TestDirectAttackBehaviorFactoryUsesOverride() { var config = new Configs.AttackBehaviorConfig() { - Name = "Sample Attack", Type = Configs.AttackType.DirectAttack, - FlightPlan = new Configs.FlightPlan() { Type = Configs.FlightPlanType.DistanceToTarget, - Waypoints = { new List() { - new Configs.FlightPlanWaypoint() { - Distance = 1000, - Altitude = 100, - Power = Configs.Power.Cruise, - } - } } } + Name = "Sample Attack", + Type = Configs.AttackType.DirectAttack, + FlightPlan = + new Configs.FlightPlan() { + Type = Configs.FlightPlanType.DistanceToTarget, + Waypoints = + { + new Configs.FlightPlanWaypoint() { + Distance = 1000, + Altitude = 100, + Power = Configs.Power.Cruise, + }, + }, + }, }; AttackBehavior attackBehavior = AttackBehaviorFactory.Create(config); @@ -31,25 +35,30 @@ public void TestDirectAttackBehaviorFactoryUsesOverride() { public void TestDirectAttackBehaviorWaypoints() { // Create a sample direct attack behavior. var attackBehavior = new DirectAttackBehaviorLegacy(new Configs.AttackBehaviorConfig() { - Name = "Sample Attack", Type = Configs.AttackType.DirectAttack, - FlightPlan = new Configs.FlightPlan() { Type = Configs.FlightPlanType.DistanceToTarget, - Waypoints = { new List() { - new Configs.FlightPlanWaypoint() { - Distance = 1000, - Altitude = 100, - Power = Configs.Power.Cruise, - }, - new Configs.FlightPlanWaypoint() { - Distance = 500, - Altitude = 50, - Power = Configs.Power.Mil, - }, - new Configs.FlightPlanWaypoint() { - Distance = 100, - Altitude = 25, - Power = Configs.Power.Max, - } - } } } + Name = "Sample Attack", + Type = Configs.AttackType.DirectAttack, + FlightPlan = + new Configs.FlightPlan() { + Type = Configs.FlightPlanType.DistanceToTarget, + Waypoints = + { + new Configs.FlightPlanWaypoint() { + Distance = 1000, + Altitude = 100, + Power = Configs.Power.Cruise, + }, + new Configs.FlightPlanWaypoint() { + Distance = 500, + Altitude = 50, + Power = Configs.Power.Mil, + }, + new Configs.FlightPlanWaypoint() { + Distance = 100, + Altitude = 25, + Power = Configs.Power.Max, + }, + }, + }, }); Vector3 targetPosition = new Vector3(1000, 0, 0); diff --git a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs index 17108cdaa..1c2e1e0f2 100644 --- a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs +++ b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs @@ -1,5 +1,4 @@ using NUnit.Framework; -using System.Collections.Generic; using UnityEngine; public class DirectAttackBehaviorTests : TestBase { @@ -21,22 +20,20 @@ public void SetUp() { Type = Configs.FlightPlanType.DistanceToTarget, Waypoints = { - new List() { - new Configs.FlightPlanWaypoint() { - Distance = 1000, - Altitude = 100, - Power = Configs.Power.Cruise, - }, - new Configs.FlightPlanWaypoint() { - Distance = 500, - Altitude = 50, - Power = Configs.Power.Mil, - }, - new Configs.FlightPlanWaypoint() { - Distance = 100, - Altitude = 25, - Power = Configs.Power.Max, - }, + new Configs.FlightPlanWaypoint() { + Distance = 1000, + Altitude = 100, + Power = Configs.Power.Cruise, + }, + new Configs.FlightPlanWaypoint() { + Distance = 500, + Altitude = 50, + Power = Configs.Power.Mil, + }, + new Configs.FlightPlanWaypoint() { + Distance = 100, + Altitude = 25, + Power = Configs.Power.Max, }, }, }, diff --git a/Assets/Tests/EditMode/FlightPlanTests.cs b/Assets/Tests/EditMode/FlightPlanTests.cs new file mode 100644 index 000000000..c469215cf --- /dev/null +++ b/Assets/Tests/EditMode/FlightPlanTests.cs @@ -0,0 +1,46 @@ +using NUnit.Framework; +using UnityEngine; + +public class FlightPlanTests { + [Test] + public void Waypoints_ReturnsWaypointsSortedByDescendingDistance() { + var flightPlanConfig = new Configs.FlightPlan() { + Type = Configs.FlightPlanType.DistanceToTarget, + Waypoints = + { + new Configs.FlightPlanWaypoint() { + Distance = 100, + Altitude = 25, + Power = Configs.Power.Max, + }, + new Configs.FlightPlanWaypoint() { + Distance = 1000, + Altitude = 100, + Power = Configs.Power.Cruise, + }, + new Configs.FlightPlanWaypoint() { + Distance = 500, + Altitude = 50, + Power = Configs.Power.Mil, + }, + new Configs.FlightPlanWaypoint() { + Distance = 2000, + Altitude = 200, + Power = Configs.Power.Cruise, + }, + }, + }; + var flightPlan = new FlightPlan(flightPlanConfig); + Assert.AreEqual(4, flightPlan.Waypoints.Count); + Assert.AreEqual(2000, flightPlan.Waypoints[0].Distance); + Assert.AreEqual(1000, flightPlan.Waypoints[1].Distance); + Assert.AreEqual(500, flightPlan.Waypoints[2].Distance); + Assert.AreEqual(100, flightPlan.Waypoints[3].Distance); + } + + [Test] + public void Waypoints_HandlesNullConfig() { + var flightPlan = new FlightPlan(config: null); + Assert.AreEqual(0, flightPlan.Waypoints.Count); + } +} diff --git a/Assets/Tests/EditMode/FlightPlanTests.cs.meta b/Assets/Tests/EditMode/FlightPlanTests.cs.meta new file mode 100644 index 000000000..33d710561 --- /dev/null +++ b/Assets/Tests/EditMode/FlightPlanTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 8b89ea1e6c8ce44d8bb106a27a8cff68 \ No newline at end of file From 32958ad7f352ff9a34b046a9ddf4e2788fe08e40 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Wed, 15 Oct 2025 12:45:48 -0700 Subject: [PATCH 36/74] Add Legacy suffix to existing controller class names --- .../Controller/{ApnController.cs => ApnControllerLegacy.cs} | 4 ++-- .../{ApnController.cs.meta => ApnControllerLegacy.cs.meta} | 0 .../Controller/{PnController.cs => PnControllerLegacy.cs} | 4 ++-- .../{PnController.cs.meta => PnControllerLegacy.cs.meta} | 0 .../{WaypointController.cs => WaypointControllerLegacy.cs} | 4 ++-- ...tController.cs.meta => WaypointControllerLegacy.cs.meta} | 0 Assets/Scripts/Interceptors/Interceptor.cs | 6 +++--- Assets/Scripts/Threats/FixedWingThreat.cs | 2 +- Assets/Scripts/Threats/RotaryWingThreat.cs | 2 +- 9 files changed, 11 insertions(+), 11 deletions(-) rename Assets/Scripts/Controller/{ApnController.cs => ApnControllerLegacy.cs} (82%) rename Assets/Scripts/Controller/{ApnController.cs.meta => ApnControllerLegacy.cs.meta} (100%) rename Assets/Scripts/Controller/{PnController.cs => PnControllerLegacy.cs} (95%) rename Assets/Scripts/Controller/{PnController.cs.meta => PnControllerLegacy.cs.meta} (100%) rename Assets/Scripts/Controller/{WaypointController.cs => WaypointControllerLegacy.cs} (82%) rename Assets/Scripts/Controller/{WaypointController.cs.meta => WaypointControllerLegacy.cs.meta} (100%) diff --git a/Assets/Scripts/Controller/ApnController.cs b/Assets/Scripts/Controller/ApnControllerLegacy.cs similarity index 82% rename from Assets/Scripts/Controller/ApnController.cs rename to Assets/Scripts/Controller/ApnControllerLegacy.cs index d46c06eb4..191ec182a 100644 --- a/Assets/Scripts/Controller/ApnController.cs +++ b/Assets/Scripts/Controller/ApnControllerLegacy.cs @@ -3,8 +3,8 @@ // The augmented proportional navigation controller applies augmented proportional navigation to // steer the agent towards its target. -public class ApnController : PnController { - public ApnController(Agent agent, float navigationGain) : base(agent, navigationGain) {} +public class ApnControllerLegacy : PnControllerLegacy { + public ApnControllerLegacy(Agent agent, float navigationGain) : base(agent, navigationGain) {} protected override Vector3 PlanImpl(in Transformation relativeTransformation) { Vector3 pnAccelerationInput = base.PlanImpl(relativeTransformation); diff --git a/Assets/Scripts/Controller/ApnController.cs.meta b/Assets/Scripts/Controller/ApnControllerLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Controller/ApnController.cs.meta rename to Assets/Scripts/Controller/ApnControllerLegacy.cs.meta diff --git a/Assets/Scripts/Controller/PnController.cs b/Assets/Scripts/Controller/PnControllerLegacy.cs similarity index 95% rename from Assets/Scripts/Controller/PnController.cs rename to Assets/Scripts/Controller/PnControllerLegacy.cs index 0e1a90f5c..48bb5f66d 100644 --- a/Assets/Scripts/Controller/PnController.cs +++ b/Assets/Scripts/Controller/PnControllerLegacy.cs @@ -3,7 +3,7 @@ // The proportional navigation controller applies proportional navigation to steer the agent towards // its target. -public class PnController : IControllerLegacy { +public class PnControllerLegacy : IControllerLegacy { // Negative closing velocity turn factor. protected const float negativeClosingVelocityTurnFactor = 100f; @@ -13,7 +13,7 @@ public class PnController : IControllerLegacy { // Proportional navigation gain. protected float _navigationGain; - public PnController(Agent agent, float navigationGain) : base(agent) { + public PnControllerLegacy(Agent agent, float navigationGain) : base(agent) { _navigationGain = navigationGain; } diff --git a/Assets/Scripts/Controller/PnController.cs.meta b/Assets/Scripts/Controller/PnControllerLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Controller/PnController.cs.meta rename to Assets/Scripts/Controller/PnControllerLegacy.cs.meta diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointControllerLegacy.cs similarity index 82% rename from Assets/Scripts/Controller/WaypointController.cs rename to Assets/Scripts/Controller/WaypointControllerLegacy.cs index f93a16c34..f590cc401 100644 --- a/Assets/Scripts/Controller/WaypointController.cs +++ b/Assets/Scripts/Controller/WaypointControllerLegacy.cs @@ -2,11 +2,11 @@ using UnityEngine; // The waypoint controller steers the agent to the target using direct linear guidance. -public class WaypointController : IControllerLegacy { +public class WaypointControllerLegacy : IControllerLegacy { // Desired speed in m/s. float _desiredSpeed; - public WaypointController(Agent agent, float desiredSpeed) : base(agent) { + public WaypointControllerLegacy(Agent agent, float desiredSpeed) : base(agent) { _desiredSpeed = desiredSpeed; } diff --git a/Assets/Scripts/Controller/WaypointController.cs.meta b/Assets/Scripts/Controller/WaypointControllerLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Controller/WaypointController.cs.meta rename to Assets/Scripts/Controller/WaypointControllerLegacy.cs.meta diff --git a/Assets/Scripts/Interceptors/Interceptor.cs b/Assets/Scripts/Interceptors/Interceptor.cs index 7af164616..22a11b6e1 100644 --- a/Assets/Scripts/Interceptors/Interceptor.cs +++ b/Assets/Scripts/Interceptors/Interceptor.cs @@ -100,17 +100,17 @@ private Vector3 CalculateAccelerationInput(double deltaTime) { IControllerLegacy controller; switch (agentConfig.DynamicConfig.FlightConfig.ControllerType) { case Configs.ControllerType.ProportionalNavigation: { - controller = new PnController(this, _navigationGain); + controller = new PnControllerLegacy(this, _navigationGain); break; } case Configs.ControllerType.AugmentedProportionalNavigation: { - controller = new ApnController(this, _navigationGain); + controller = new ApnControllerLegacy(this, _navigationGain); break; } default: { Debug.LogError( $"Controller type {agentConfig.DynamicConfig.FlightConfig.ControllerType} not found."); - controller = new PnController(this, _navigationGain); + controller = new PnControllerLegacy(this, _navigationGain); break; } } diff --git a/Assets/Scripts/Threats/FixedWingThreat.cs b/Assets/Scripts/Threats/FixedWingThreat.cs index ff1e51fac..a8f4ccd32 100644 --- a/Assets/Scripts/Threats/FixedWingThreat.cs +++ b/Assets/Scripts/Threats/FixedWingThreat.cs @@ -70,7 +70,7 @@ private Vector3 CalculateAccelerationInput() { Vector3 velocity = GetVelocity(); float speed = velocity.magnitude; - IControllerLegacy controller = new PnController(this, _navigationGain); + IControllerLegacy controller = new PnControllerLegacy(this, _navigationGain); Vector3 accelerationInput = controller.PlanToWaypoint(_currentWaypoint); // Counter gravity as much as possible. diff --git a/Assets/Scripts/Threats/RotaryWingThreat.cs b/Assets/Scripts/Threats/RotaryWingThreat.cs index 6c69e2ba7..a0031e20f 100644 --- a/Assets/Scripts/Threats/RotaryWingThreat.cs +++ b/Assets/Scripts/Threats/RotaryWingThreat.cs @@ -42,7 +42,7 @@ private void UpdateWaypointAndPower() { private Vector3 CalculateAccelerationToWaypoint() { float desiredSpeed = LookupPowerTable(_currentPower); - IControllerLegacy controller = new WaypointController(this, desiredSpeed); + IControllerLegacy controller = new WaypointControllerLegacy(this, desiredSpeed); Vector3 accelerationInput = controller.PlanToWaypoint(_currentWaypoint); Vector3 forwardAccelerationInput = Vector3.Project(accelerationInput, transform.forward); From 2bc261c1d5f0c4b2f212830af21c8f198e7898c0 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Wed, 15 Oct 2025 17:43:31 -0700 Subject: [PATCH 37/74] Define all controllers --- Assets/Scripts/Controller/ApnController.cs | 18 ++++++ .../Scripts/Controller/ApnController.cs.meta | 2 + Assets/Scripts/Controller/ControllerBase.cs | 19 ++++-- Assets/Scripts/Controller/IController.cs | 6 +- Assets/Scripts/Controller/PnController.cs | 61 +++++++++++++++++++ .../Scripts/Controller/PnController.cs.meta | 2 + .../Scripts/Controller/WaypointController.cs | 20 ++++++ .../Controller/WaypointController.cs.meta | 2 + 8 files changed, 124 insertions(+), 6 deletions(-) create mode 100644 Assets/Scripts/Controller/ApnController.cs create mode 100644 Assets/Scripts/Controller/ApnController.cs.meta create mode 100644 Assets/Scripts/Controller/PnController.cs create mode 100644 Assets/Scripts/Controller/PnController.cs.meta create mode 100644 Assets/Scripts/Controller/WaypointController.cs create mode 100644 Assets/Scripts/Controller/WaypointController.cs.meta diff --git a/Assets/Scripts/Controller/ApnController.cs b/Assets/Scripts/Controller/ApnController.cs new file mode 100644 index 000000000..b49f1eea1 --- /dev/null +++ b/Assets/Scripts/Controller/ApnController.cs @@ -0,0 +1,18 @@ +using UnityEngine; + +// The augmented proportional navigation controller applies an input that is proportional to the +// line of sight's rotation rate and adds a term to compensate for the target's acceleration. +public class ApnController : PnController { + public ApnController(IAgent agent, float gain) : base(agent, gain) {} + + // Controller-dependent implementation of the control law. + protected override Vector3 Plan(in Transformation relativeTransformation) { + var accelerationInput = base.Plan(relativeTransformation); + + // Add a feedforward term proportional to the target's acceleration. + var targetAcceleration = relativeTransformation.Acceleration.Cartesian; + var normalTargetAcceleration = + Vector3.ProjectOnPlane(targetAcceleration, Agent.transform.forward); + return accelerationInput + Gain / 2 * normalTargetAcceleration; + } +} diff --git a/Assets/Scripts/Controller/ApnController.cs.meta b/Assets/Scripts/Controller/ApnController.cs.meta new file mode 100644 index 000000000..14132c2ef --- /dev/null +++ b/Assets/Scripts/Controller/ApnController.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: b06b543dfe22c40a4abff190c58b4066 \ No newline at end of file diff --git a/Assets/Scripts/Controller/ControllerBase.cs b/Assets/Scripts/Controller/ControllerBase.cs index 8a3bebb2a..99d824527 100644 --- a/Assets/Scripts/Controller/ControllerBase.cs +++ b/Assets/Scripts/Controller/ControllerBase.cs @@ -2,16 +2,27 @@ // Base implementation of a controller. public abstract class ControllerBase : IController { + // Agent that the controller is steering. + public IAgent Agent { get; set; } + + public ControllerBase(IAgent agent) { + Agent = agent; + } + // Plan the next optimal control to intercept the target. - public Vector3 PlanToTarget(IAgent agent) { + public Vector3 Plan() { + if (Agent.HierarchicalAgent == null || Agent.HierarchicalAgent.TargetModel == null) { + return Vector3.zero; + } + Transformation relativeTransformation = - agent.GetRelativeTransformation(agent.HierarchicalAgent.TargetModel); + Agent.GetRelativeTransformation(Agent.HierarchicalAgent.TargetModel); return Plan(relativeTransformation); } // Plan the next optimal control to the waypoint. - public Vector3 PlanToWaypoint(IAgent agent, in Vector3 waypoint) { - Transformation relativeTransformation = agent.GetRelativeTransformation(waypoint); + public Vector3 Plan(in Vector3 waypoint) { + Transformation relativeTransformation = Agent.GetRelativeTransformation(waypoint); return Plan(relativeTransformation); } diff --git a/Assets/Scripts/Controller/IController.cs b/Assets/Scripts/Controller/IController.cs index 6f9735a63..03c23e15a 100644 --- a/Assets/Scripts/Controller/IController.cs +++ b/Assets/Scripts/Controller/IController.cs @@ -5,9 +5,11 @@ // The controller determines the acceleration input given the agent's current state and its intended // target or waypoint. public interface IController { + IAgent Agent { get; set; } + // Plan the next optimal control to intercept the target. - Vector3 PlanToTarget(IAgent agent); + Vector3 Plan(); // Plan the next optimal control to the waypoint. - Vector3 PlanToWaypoint(IAgent agent, in Vector3 waypoint); + Vector3 Plan(in Vector3 waypoint); } diff --git a/Assets/Scripts/Controller/PnController.cs b/Assets/Scripts/Controller/PnController.cs new file mode 100644 index 000000000..1e7548430 --- /dev/null +++ b/Assets/Scripts/Controller/PnController.cs @@ -0,0 +1,61 @@ +using UnityEngine; + +// The proportional navigation controller applies an input that is proportional to the line of +// sight's rotation rate to steer the agent towards its target. +public class PnController : ControllerBase { + // Negative closing velocity turn factor. + private const float _negativeClosingVelocityTurnFactor = 100f; + + // Angular threshold in degrees for detecting spiral behavior. + private const float _spiralBearingThreshold = 10f; + + // Minimum line of sight rate. + private const float _minimumLosRate = 0.2f; + + public float Gain { get; set; } + + public PnController(IAgent agent, float gain) : base(agent) { + Gain = gain; + } + + // Controller-dependent implementation of the control law. + protected override Vector3 Plan(in Transformation relativeTransformation) { + // Cache the transform, velocity, and speed. + var agentTransform = Agent.transform; + var right = agentTransform.right; + var up = agentTransform.up; + var velocity = Agent.Velocity; + var speed = Agent.Speed; + + // Extract the bearing and closing velocity from the relative transformation. + var losAz = relativeTransformation.Position.Azimuth; + var losEl = relativeTransformation.Position.Elevation; + var losRateAz = relativeTransformation.Velocity.Azimuth; + var losRateEl = relativeTransformation.Velocity.Elevation; + // The closing velocity is negative because the closing velocity is opposite to the range rate. + var closingVelocity = -relativeTransformation.Velocity.Range; + + // Set the turn factor, which is equal to the closing velocity by default. + var turnFactor = closingVelocity; + // Handle a negative closing velocity. In this case, since the target is moving away from the + // agent, apply a stronger turn. + if (closingVelocity < 0) { + turnFactor = Mathf.Max(1f, Mathf.Abs(closingVelocity) * _negativeClosingVelocityTurnFactor); + } + + // Handle the spiral behavior if the target is at a bearing of around 90 degrees. + if (Mathf.Abs(Mathf.Abs(losAz) - 90f * Mathf.Deg2Rad) < + _spiralBearingThreshold * Mathf.Deg2Rad || + Mathf.Abs(Mathf.Abs(losEl) - 90f * Mathf.Deg2Rad) < + _spiralBearingThreshold * Mathf.Deg2Rad) { + // Check that the agent is not moving in a spiral by clamping the line of sight rate. + losRateAz = Mathf.Sign(losRateAz) * Mathf.Max(Mathf.Abs(losRateAz), _minimumLosRate); + losRateEl = Mathf.Sign(losRateEl) * Mathf.Max(Mathf.Abs(losRateEl), _minimumLosRate); + turnFactor = Mathf.Abs(closingVelocity) * _negativeClosingVelocityTurnFactor; + } + + var accelerationAz = Gain * turnFactor * losRateAz; + var accelerationEl = Gain * turnFactor * losRateEl; + return right * accelerationAz + up * accelerationEl; + } +} diff --git a/Assets/Scripts/Controller/PnController.cs.meta b/Assets/Scripts/Controller/PnController.cs.meta new file mode 100644 index 000000000..6bf0082ab --- /dev/null +++ b/Assets/Scripts/Controller/PnController.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 36f3446cca3874458901a466f663b2fa \ No newline at end of file diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointController.cs new file mode 100644 index 000000000..ee84d6bc5 --- /dev/null +++ b/Assets/Scripts/Controller/WaypointController.cs @@ -0,0 +1,20 @@ +using UnityEngine; + +// The waypoint controller steers the agent to the target using direct linear guidance. +public class WaypointController : ControllerBase { + public WaypointController(IAgent agent) : base(agent) {} + + // Controller-dependent implementation of the control law. + protected override Vector3 Plan(in Transformation relativeTransformation) { + var relativePosition = relativeTransformation.Position.Cartesian; + + // To reach the waypoint as fast as possible, use the maximum forward acceleration. + var forward = Agent.transform.forward; + var forwardAccelerationInput = forward * Agent.MaxForwardAcceleration(); + + // Steer as hard as possible towards the waypoint. + var normalAcceleration = Vector3.ProjectOnPlane(relativePosition, forward).normalized; + var normalAccelerationInput = normalAcceleration * Agent.MaxNormalAcceleration(); + return forwardAccelerationInput + normalAccelerationInput; + } +} diff --git a/Assets/Scripts/Controller/WaypointController.cs.meta b/Assets/Scripts/Controller/WaypointController.cs.meta new file mode 100644 index 000000000..1fc64e461 --- /dev/null +++ b/Assets/Scripts/Controller/WaypointController.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 3d9607f2d2d754fecb6c17014b336b72 \ No newline at end of file From 1c207ae39b955a29e29efdf60958adaa51404b18 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Wed, 15 Oct 2025 18:18:09 -0700 Subject: [PATCH 38/74] Add unit tests for all controllers --- .../Scripts/Hierarchical/HierarchicalAgent.cs | 4 ++ Assets/Tests/EditMode/ApnControllerTests.cs | 33 ++++++++++++ .../Tests/EditMode/ApnControllerTests.cs.meta | 2 + Assets/Tests/EditMode/PnControllerTests.cs | 51 +++++++++++++++++++ .../Tests/EditMode/PnControllerTests.cs.meta | 2 + .../Tests/EditMode/WaypointControllerTests.cs | 42 +++++++++++++++ .../EditMode/WaypointControllerTests.cs.meta | 2 + 7 files changed, 136 insertions(+) create mode 100644 Assets/Tests/EditMode/ApnControllerTests.cs create mode 100644 Assets/Tests/EditMode/ApnControllerTests.cs.meta create mode 100644 Assets/Tests/EditMode/PnControllerTests.cs create mode 100644 Assets/Tests/EditMode/PnControllerTests.cs.meta create mode 100644 Assets/Tests/EditMode/WaypointControllerTests.cs create mode 100644 Assets/Tests/EditMode/WaypointControllerTests.cs.meta diff --git a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs index 7468cfc23..c783bd2f3 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs @@ -8,6 +8,10 @@ public class HierarchicalAgent : HierarchicalBase { // Agent to which the hierarchical node belongs. public IAgent Agent { get; set; } + public HierarchicalAgent(IAgent agent) { + Agent = agent; + } + protected override Vector3 GetPosition() { return Agent.Position; } diff --git a/Assets/Tests/EditMode/ApnControllerTests.cs b/Assets/Tests/EditMode/ApnControllerTests.cs new file mode 100644 index 000000000..495b4c6ed --- /dev/null +++ b/Assets/Tests/EditMode/ApnControllerTests.cs @@ -0,0 +1,33 @@ +using NUnit.Framework; +using UnityEngine; + +public class ApnControllerTests : TestBase { + private AgentBase _agent; + private ApnController _controller; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + _agent.transform.rotation = Quaternion.LookRotation(new Vector3(0, 0, 1)); + InvokePrivateMethod(_agent, "Awake"); + _agent.HierarchicalAgent = new HierarchicalAgent(_agent); + _controller = new ApnController(_agent, gain: 1); + } + + [Test] + public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingUpwards() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(new Vector3(0, 1.5f, 0), _controller.Plan()); + } + + [Test] + public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingLeft() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(-1, 0, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(new Vector3(-1, 0.5f, 0), _controller.Plan()); + } +} diff --git a/Assets/Tests/EditMode/ApnControllerTests.cs.meta b/Assets/Tests/EditMode/ApnControllerTests.cs.meta new file mode 100644 index 000000000..b5f56e671 --- /dev/null +++ b/Assets/Tests/EditMode/ApnControllerTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: d5eca4063ada44b50a9c373e7b141a2d \ No newline at end of file diff --git a/Assets/Tests/EditMode/PnControllerTests.cs b/Assets/Tests/EditMode/PnControllerTests.cs new file mode 100644 index 000000000..9de277515 --- /dev/null +++ b/Assets/Tests/EditMode/PnControllerTests.cs @@ -0,0 +1,51 @@ +using NUnit.Framework; +using UnityEngine; + +public class PnControllerTests : TestBase { + private AgentBase _agent; + private PnController _controller; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + _agent.transform.rotation = Quaternion.LookRotation(new Vector3(0, 0, 1)); + InvokePrivateMethod(_agent, "Awake"); + _agent.HierarchicalAgent = new HierarchicalAgent(_agent); + _controller = new PnController(_agent, gain: 1); + } + + [Test] + public void Plan_NoHierarchicalAgent_ReturnsZero() { + _agent.HierarchicalAgent = null; + Assert.AreEqual(Vector3.zero, _controller.Plan()); + } + + [Test] + public void Plan_NoTargetModel_ReturnsZero() { + _agent.HierarchicalAgent.TargetModel = null; + Assert.AreEqual(Vector3.zero, _controller.Plan()); + } + + [Test] + public void Plan_TargetAtBoresight_ZeroClosingVelocity_ReturnsZero() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, 0)); + Assert.AreEqual(Vector3.zero, _controller.Plan()); + Assert.AreEqual(Vector3.zero, _controller.Plan()); + } + + [Test] + public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingUpwards() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, -1)); + Assert.AreEqual(new Vector3(0, 1, 0), _controller.Plan()); + } + + [Test] + public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingLeft() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(-1, 0, -1)); + Assert.AreEqual(new Vector3(-1, 0, 0), _controller.Plan()); + } +} diff --git a/Assets/Tests/EditMode/PnControllerTests.cs.meta b/Assets/Tests/EditMode/PnControllerTests.cs.meta new file mode 100644 index 000000000..b2a4ec987 --- /dev/null +++ b/Assets/Tests/EditMode/PnControllerTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: a20001bd81a6d4d1ba3bc7ebe17debed \ No newline at end of file diff --git a/Assets/Tests/EditMode/WaypointControllerTests.cs b/Assets/Tests/EditMode/WaypointControllerTests.cs new file mode 100644 index 000000000..c1167cb15 --- /dev/null +++ b/Assets/Tests/EditMode/WaypointControllerTests.cs @@ -0,0 +1,42 @@ +using NUnit.Framework; +using UnityEngine; + +public class WaypointControllerTests : TestBase { + private AgentBase _agent; + private WaypointController _controller; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + _agent.StaticConfig = new Configs.StaticConfig() { + AccelerationConfig = + new Configs.AccelerationConfig() { + MaxForwardAcceleration = 10, + MaxReferenceNormalAcceleration = 5 / Constants.kGravity, + ReferenceSpeed = 1, + }, + }; + Rigidbody agentRb = _agent.gameObject.AddComponent(); + _agent.transform.rotation = Quaternion.LookRotation(new Vector3(0, 0, 1)); + InvokePrivateMethod(_agent, "Awake"); + _agent.Velocity = new Vector3(0, 0, 1); + _agent.HierarchicalAgent = new HierarchicalAgent(_agent); + _controller = new WaypointController(_agent); + } + + [Test] + public void Plan_UsesMaximumForwardAcceleration() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 1, 1), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(10, _controller.Plan().z); + } + + [Test] + public void Plan_UsesMaximumNormalAcceleration() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 1, 1), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(5, _controller.Plan().y); + } +} diff --git a/Assets/Tests/EditMode/WaypointControllerTests.cs.meta b/Assets/Tests/EditMode/WaypointControllerTests.cs.meta new file mode 100644 index 000000000..1377b930b --- /dev/null +++ b/Assets/Tests/EditMode/WaypointControllerTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 0e7f94f37288b41ea9bb84ef66a4425c \ No newline at end of file From 5e84fa5057cc375aa544f7c63f6c184de81b9f67 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Wed, 15 Oct 2025 19:13:10 -0700 Subject: [PATCH 39/74] Address comments --- Assets/Scripts/Controller/PnController.cs | 2 -- Assets/Scripts/Controller/WaypointController.cs | 7 +++++++ Assets/Tests/EditMode/WaypointControllerTests.cs | 8 ++++++++ 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Controller/PnController.cs b/Assets/Scripts/Controller/PnController.cs index 1e7548430..25ff821f6 100644 --- a/Assets/Scripts/Controller/PnController.cs +++ b/Assets/Scripts/Controller/PnController.cs @@ -24,8 +24,6 @@ protected override Vector3 Plan(in Transformation relativeTransformation) { var agentTransform = Agent.transform; var right = agentTransform.right; var up = agentTransform.up; - var velocity = Agent.Velocity; - var speed = Agent.Speed; // Extract the bearing and closing velocity from the relative transformation. var losAz = relativeTransformation.Position.Azimuth; diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointController.cs index ee84d6bc5..1233e64b0 100644 --- a/Assets/Scripts/Controller/WaypointController.cs +++ b/Assets/Scripts/Controller/WaypointController.cs @@ -2,11 +2,18 @@ // The waypoint controller steers the agent to the target using direct linear guidance. public class WaypointController : ControllerBase { + // To prevent overshooting near the waypoint, if the squared distance to the waypoint is less than + // the threshold, do not apply any acceleration. + private const float AccelerationCutoffDistanceSqr = 1.0f; + public WaypointController(IAgent agent) : base(agent) {} // Controller-dependent implementation of the control law. protected override Vector3 Plan(in Transformation relativeTransformation) { var relativePosition = relativeTransformation.Position.Cartesian; + if (relativeTransformation.Position.Range < AccelerationCutoffDistanceSqr) { + return Vector3.zero; + } // To reach the waypoint as fast as possible, use the maximum forward acceleration. var forward = Agent.transform.forward; diff --git a/Assets/Tests/EditMode/WaypointControllerTests.cs b/Assets/Tests/EditMode/WaypointControllerTests.cs index c1167cb15..401788eab 100644 --- a/Assets/Tests/EditMode/WaypointControllerTests.cs +++ b/Assets/Tests/EditMode/WaypointControllerTests.cs @@ -24,6 +24,14 @@ public void SetUp() { _controller = new WaypointController(_agent); } + [Test] + public void Plan_WithinCutoffDistance_ReturnsZero() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0.5f, 0.5f), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(Vector3.zero, _controller.Plan()); + } + [Test] public void Plan_UsesMaximumForwardAcceleration() { _agent.HierarchicalAgent.TargetModel = From fe294d236eabbc8c087719443d679125cc14e97e Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Wed, 15 Oct 2025 19:42:15 -0700 Subject: [PATCH 40/74] Add Legacy suffix to linear extrapolator --- .../{LinearExtrapolator.cs => LinearExtrapolatorLegacy.cs} | 4 ++-- ...rExtrapolator.cs.meta => LinearExtrapolatorLegacy.cs.meta} | 0 Assets/Scripts/IADS/IADS.cs | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) rename Assets/Scripts/Algorithms/Prediction/{LinearExtrapolator.cs => LinearExtrapolatorLegacy.cs} (77%) rename Assets/Scripts/Algorithms/Prediction/{LinearExtrapolator.cs.meta => LinearExtrapolatorLegacy.cs.meta} (100%) diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolatorLegacy.cs similarity index 77% rename from Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs rename to Assets/Scripts/Algorithms/Prediction/LinearExtrapolatorLegacy.cs index 047dde9ef..ac937ccc2 100644 --- a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs +++ b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolatorLegacy.cs @@ -3,8 +3,8 @@ // The linear extrapolator class predicts the trajectory of an agent by linearly extrapolating its // current position and velocity. -public class LinearExtrapolator : IPredictorLegacy { - public LinearExtrapolator(in Agent agent) : base(agent) {} +public class LinearExtrapolatorLegacy : IPredictorLegacy { + public LinearExtrapolatorLegacy(in Agent agent) : base(agent) {} // Predict the state at the given time. public override PredictorStateLegacy Predict(float time) { diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs.meta b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolatorLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs.meta rename to Assets/Scripts/Algorithms/Prediction/LinearExtrapolatorLegacy.cs.meta diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index 7077f6ae1..4d925b362 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -109,7 +109,7 @@ private void CheckAndLaunchInterceptors() { } // Create a predictor to track the cluster's centroid. - var predictor = new LinearExtrapolator(_threatClusterMap[cluster].Centroid); + var predictor = new LinearExtrapolatorLegacy(_threatClusterMap[cluster].Centroid); // Create a launch planner. ILaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); @@ -166,7 +166,7 @@ public bool ShouldLaunchSubmunitions(Interceptor carrier) { Vector3 carrierPosition = carrier.GetPosition(); Vector3 carrierVelocity = carrier.GetVelocity(); foreach (var threat in threats) { - var predictor = new LinearExtrapolator(threat); + var predictor = new LinearExtrapolatorLegacy(threat); var predictedState = predictor.Predict(SubmunitionSpawnPredictionTime); Vector3 positionToPredictedThreat = predictedState.Position - carrierPosition; float predictedDistanceToThreat = positionToPredictedThreat.magnitude; From 086ea47a64a219104a352976d42304d6fb6bb338 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Wed, 15 Oct 2025 19:55:50 -0700 Subject: [PATCH 41/74] Implement linear extrapolator --- .../Prediction/LinearExtrapolator.cs | 17 +++++++ .../Prediction/LinearExtrapolator.cs.meta | 2 + .../Algorithms/Prediction/PredictorState.cs | 2 +- .../Tests/EditMode/LinearExtrapolatorTests.cs | 47 +++++++++---------- 4 files changed, 42 insertions(+), 26 deletions(-) create mode 100644 Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs create mode 100644 Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs.meta diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs new file mode 100644 index 000000000..47333a250 --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs @@ -0,0 +1,17 @@ +using System.Linq; +using UnityEngine; + +// The linear extrapolator class predicts the trajectory of an agent by linearly extrapolating its +// current position and velocity. +public class LinearExtrapolator : PredictorBase { + // Predict the future state of the hierarchical object by linearly extrapolating its current + // position and velocity. + public override PredictorState Predict(IHierarchical hierarchical, float time) { + var position = hierarchical.Position + hierarchical.Velocity * time; + return new PredictorState { + Position = position, + Velocity = hierarchical.Velocity, + Acceleration = hierarchical.Acceleration, + }; + } +} diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs.meta b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs.meta new file mode 100644 index 000000000..685019579 --- /dev/null +++ b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 9733c7760103b4f31b1ed62bba13c6b0 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorState.cs b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs index 09f4e80f7..24a3a59b1 100644 --- a/Assets/Scripts/Algorithms/Prediction/PredictorState.cs +++ b/Assets/Scripts/Algorithms/Prediction/PredictorState.cs @@ -1,7 +1,7 @@ using UnityEngine; // The predictor state defines the output of a predictor. -public class PredictorState { +public struct PredictorState { // Position. public Vector3 Position { get; set; } diff --git a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs index 526b6ca78..a845fbf77 100644 --- a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs +++ b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs @@ -1,42 +1,39 @@ using NUnit.Framework; -using System.Collections; -using System.Collections.Generic; using UnityEngine; -using UnityEngine.TestTools; public class LinearExtrapolatorTests { - public static Agent GenerateAgent() { - Agent agent = new GameObject().AddComponent(); - Rigidbody rb = agent.gameObject.AddComponent(); - agent.transform.position = new Vector3(10, 0, 5); - rb.linearVelocity = new Vector3(1, -2, 2); - return agent; + private LinearExtrapolator _predictor; + private FixedHierarchical _hierarchical; + + [SetUp] + public void SetUp() { + _predictor = new LinearExtrapolator(); + _hierarchical = + new FixedHierarchical(position: new Vector3(10, 0, 5), velocity: new Vector3(1, -2, 2), + acceleration: new Vector3(-1, 1, 2)); } [Test] - public void TestPresent() { - Agent agent = GenerateAgent(); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - var predictedState = predictor.Predict(time: 0f); - Assert.AreEqual(agent.GetPosition(), predictedState.Position); - Assert.AreEqual(agent.GetVelocity(), predictedState.Velocity); + public void Predict_AtTimeZero_ReturnsCurrentState() { + var predictedState = _predictor.Predict(_hierarchical, time: 0f); + Assert.AreEqual(_hierarchical.Position, predictedState.Position); + Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); + Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); } [Test] - public void TestPast() { - Agent agent = GenerateAgent(); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - var predictedState = predictor.Predict(time: -5f); + public void Predict_InThePast_ReturnsExtrapolatedState() { + var predictedState = _predictor.Predict(_hierarchical, time: -5f); Assert.AreEqual(new Vector3(5, 10, -5), predictedState.Position); - Assert.AreEqual(agent.GetVelocity(), predictedState.Velocity); + Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); + Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); } [Test] - public void TestFuture() { - Agent agent = GenerateAgent(); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - var predictedState = predictor.Predict(time: 10f); + public void Predict_InTheFuture_ReturnsExtrapolatedState() { + var predictedState = _predictor.Predict(_hierarchical, time: 10f); Assert.AreEqual(new Vector3(20, -20, 25), predictedState.Position); - Assert.AreEqual(agent.GetVelocity(), predictedState.Velocity); + Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); + Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); } } From 515e488a27c20d825f77df5d3d7b31d1f46c6dba Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Wed, 15 Oct 2025 23:27:41 -0700 Subject: [PATCH 42/74] Refactor launch plan class --- ...ner.cs => IterativeLaunchPlannerLegacy.cs} | 17 ++++-- ...a => IterativeLaunchPlannerLegacy.cs.meta} | 0 .../Scripts/Algorithms/Planning/LaunchPlan.cs | 25 ++++++++ .../Algorithms/Planning/LaunchPlan.cs.meta | 2 + .../Algorithms/Planning/LaunchPlanner.cs | 50 ---------------- .../Planning/LaunchPlannerLegacy.cs | 19 ++++++ ...er.cs.meta => LaunchPlannerLegacy.cs.meta} | 0 Assets/Scripts/IADS/IADS.cs | 9 +-- Assets/Tests/EditMode/LaunchPlanTests.cs | 60 +++++++++++++++++++ Assets/Tests/EditMode/LaunchPlanTests.cs.meta | 2 + docs/Simulator_Overview.md | 6 +- 11 files changed, 127 insertions(+), 63 deletions(-) rename Assets/Scripts/Algorithms/Planning/{IterativeLaunchPlanner.cs => IterativeLaunchPlannerLegacy.cs} (88%) rename Assets/Scripts/Algorithms/Planning/{IterativeLaunchPlanner.cs.meta => IterativeLaunchPlannerLegacy.cs.meta} (100%) create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchPlan.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchPlan.cs.meta delete mode 100644 Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs rename Assets/Scripts/Algorithms/Planning/{LaunchPlanner.cs.meta => LaunchPlannerLegacy.cs.meta} (100%) create mode 100644 Assets/Tests/EditMode/LaunchPlanTests.cs create mode 100644 Assets/Tests/EditMode/LaunchPlanTests.cs.meta diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs similarity index 88% rename from Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs rename to Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs index 066668910..318977e8d 100644 --- a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs +++ b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs @@ -8,7 +8,7 @@ // current position. // 2. Intercept position estimation: The algorithm predicts the target position at the estimated // time-to-intercept. -public class IterativeLaunchPlanner : ILaunchPlanner { +public class IterativeLaunchPlannerLegacy : ILaunchPlannerLegacy { // Maximum number of iterations before declaring failure. In certain cases, the predictor and // planner do not converge, so we need to limit the number of iterations. private const int MaxNumIterations = 10; @@ -23,7 +23,8 @@ public class IterativeLaunchPlanner : ILaunchPlanner { // granularity of the possible intercept positions. private const float InterceptPositionThreshold = 1000f; - public IterativeLaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictorLegacy predictor) + public IterativeLaunchPlannerLegacy(ILaunchAnglePlanner launchAnglePlanner, + IPredictorLegacy predictor) : base(launchAnglePlanner, predictor) {} // Plan the launch. @@ -56,7 +57,7 @@ public override LaunchPlan Plan() { Vector3 targetToInterceptPosition = interceptPosition - initialState.Position; Vector3 targetToPredictedPosition = targetPosition - initialState.Position; if (Vector3.Dot(targetToInterceptPosition, targetToPredictedPosition) < 0) { - return LaunchPlan.NoLaunch; + return LaunchPlan.NoLaunch(); } } @@ -66,14 +67,18 @@ public override LaunchPlan Plan() { Vector3 interceptorToInterceptPosition = interceptPosition; Vector3 threatToPredictedPosition = targetPosition - initialState.Position; if (Vector3.Dot(interceptorToInterceptPosition, threatToPredictedPosition) > 0) { - return LaunchPlan.NoLaunch; + return LaunchPlan.NoLaunch(); } // Check that the intercept position and the predicted position are within some threshold // distance of each other. if (Vector3.Distance(interceptPosition, targetPosition) < InterceptPositionThreshold) { - return new LaunchPlan(launchAngleOutput.LaunchAngle, targetPosition); + return new LaunchPlan { + ShouldLaunch = true, + LaunchAngle = launchAngleOutput.LaunchAngle, + RelativeInterceptPosition = targetPosition, + }; } - return LaunchPlan.NoLaunch; + return LaunchPlan.NoLaunch(); } } diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs.meta b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs.meta rename to Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs.meta diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs new file mode 100644 index 000000000..bad2a7d2b --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs @@ -0,0 +1,25 @@ +using UnityEngine; + +// The launch planner outputs a launch plan. +public struct LaunchPlan { + // If true, the interceptor should be launched. + public bool ShouldLaunch { get; set; } + + // Launch angle in degrees measured from the horizon. + public float LaunchAngle { get; set; } + + // Intercept position relative to the launcher's position. + public Vector3 RelativeInterceptPosition { get; set; } + + // No-launch launch plan. + public static LaunchPlan NoLaunch() { + return new LaunchPlan { ShouldLaunch = false }; + } + + // Get the normalized launch vector. + public Vector3 NormalizedLaunchVector() { + var interceptDirection = Coordinates3.ConvertCartesianToSpherical(RelativeInterceptPosition); + return Coordinates3.ConvertSphericalToCartesian(r: 1, azimuth: interceptDirection[1], + elevation: LaunchAngle); + } +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs.meta new file mode 100644 index 000000000..ab195ed67 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e60ee337cfa9740c1a0cc6de8ba83ee0 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs deleted file mode 100644 index c47bf4034..000000000 --- a/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs +++ /dev/null @@ -1,50 +0,0 @@ -using UnityEngine; - -// Launch planner output. -public class LaunchPlan { - // No-launch launch plan. - public static LaunchPlan NoLaunch = new LaunchPlan(); - - // Whether the interceptor should be launched. - public bool ShouldLaunch { get; } - - // Launch angle in degrees measured from the horizon. - public float LaunchAngle { get; } - - // Intercept position. - public Vector3 InterceptPosition { get; } - - public LaunchPlan() { - ShouldLaunch = false; - } - public LaunchPlan(float launchAngle, Vector3 interceptPosition, bool shouldLaunch = true) { - LaunchAngle = launchAngle; - InterceptPosition = interceptPosition; - ShouldLaunch = shouldLaunch; - } - - // Get the normalized launch vector. - public Vector3 GetNormalizedLaunchVector() { - Vector3 interceptDirection = Coordinates3.ConvertCartesianToSpherical(InterceptPosition); - return Coordinates3.ConvertSphericalToCartesian(r: 1, azimuth: interceptDirection[1], - elevation: LaunchAngle); - } -} - -// The launch planner class is an interface for planning when and where to launch an interceptor to -// intercept a target. -public abstract class ILaunchPlanner { - // Launch angle planner. - protected ILaunchAnglePlanner _launchAnglePlanner; - - // Agent trajectory predictor. - protected IPredictorLegacy _predictor; - - public ILaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictorLegacy predictor) { - _launchAnglePlanner = launchAnglePlanner; - _predictor = predictor; - } - - // Plan the launch. - public abstract LaunchPlan Plan(); -} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs new file mode 100644 index 000000000..de12bf7d1 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs @@ -0,0 +1,19 @@ +using UnityEngine; + +// The launch planner class is an interface for planning when and where to launch an interceptor to +// intercept a target. +public abstract class ILaunchPlannerLegacy { + // Launch angle planner. + protected ILaunchAnglePlanner _launchAnglePlanner; + + // Agent trajectory predictor. + protected IPredictorLegacy _predictor; + + public ILaunchPlannerLegacy(ILaunchAnglePlanner launchAnglePlanner, IPredictorLegacy predictor) { + _launchAnglePlanner = launchAnglePlanner; + _predictor = predictor; + } + + // Plan the launch. + public abstract LaunchPlan Plan(); +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Algorithms/Planning/LaunchPlanner.cs.meta rename to Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs.meta diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index 4d925b362..6ea19c67e 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -112,15 +112,16 @@ private void CheckAndLaunchInterceptors() { var predictor = new LinearExtrapolatorLegacy(_threatClusterMap[cluster].Centroid); // Create a launch planner. - ILaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); + ILaunchPlannerLegacy planner = + new IterativeLaunchPlannerLegacy(_launchAnglePlanner, predictor); LaunchPlan plan = planner.Plan(); // Check whether an interceptor should be launched. if (plan.ShouldLaunch) { Debug.Log( - $"Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.InterceptPosition}."); + $"Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.RelativeInterceptPosition}."); UIManager.Instance.LogActionMessage( - $"[IADS] Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.InterceptPosition}."); + $"[IADS] Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.RelativeInterceptPosition}."); // Create a new interceptor. Configs.AgentConfig config = @@ -133,7 +134,7 @@ private void CheckAndLaunchInterceptors() { initialState.Position = Coordinates3.ToProto(Vector3.zero); // Set the initial velocity to point along the launch vector. - initialState.Velocity = Coordinates3.ToProto(plan.GetNormalizedLaunchVector() * 1e-3f); + initialState.Velocity = Coordinates3.ToProto(plan.NormalizedLaunchVector() * 1e-3f); Interceptor interceptor = SimManager.Instance.CreateInterceptor(config, initialState); // Assign the interceptor to the cluster. diff --git a/Assets/Tests/EditMode/LaunchPlanTests.cs b/Assets/Tests/EditMode/LaunchPlanTests.cs new file mode 100644 index 000000000..a9db0db03 --- /dev/null +++ b/Assets/Tests/EditMode/LaunchPlanTests.cs @@ -0,0 +1,60 @@ +using NUnit.Framework; +using UnityEngine; +using UnityEngine.TestTools.Utils; + +public class LaunchPlanTests { + private const float _epsilon = 1e-3f; + + [Test] + public void NoLaunch_ShouldHaveShouldLaunchFalse() { + var plan = LaunchPlan.NoLaunch(); + Assert.IsFalse(plan.ShouldLaunch); + } + + [Test] + public void NormalizedLaunchVector_IsNormalized() { + var launchPlan = new LaunchPlan { + ShouldLaunch = true, + LaunchAngle = 45f, + RelativeInterceptPosition = new Vector3(1, 1, 0), + }; + var launchVector = launchPlan.NormalizedLaunchVector(); + Assert.AreEqual(1f, launchVector.magnitude, _epsilon); + } + + [Test] + public void NormalizedLaunchVector_PointsVertically() { + var launchPlan = new LaunchPlan { + ShouldLaunch = true, + LaunchAngle = 90f, + RelativeInterceptPosition = new Vector3(1, 1, 0), + }; + var launchVector = launchPlan.NormalizedLaunchVector(); + Assert.That(launchVector, + Is.EqualTo(new Vector3(0, 1f, 0)).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void NormalizedLaunchVector_PointsHorizontally() { + var launchPlan = new LaunchPlan { + ShouldLaunch = true, + LaunchAngle = 0f, + RelativeInterceptPosition = new Vector3(1, 1, 0), + }; + var launchVector = launchPlan.NormalizedLaunchVector(); + Assert.That(launchVector, + Is.EqualTo(new Vector3(1f, 0, 0)).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void NormalizedLaunchVector_PointsDiagonally() { + var launchPlan = new LaunchPlan { + ShouldLaunch = true, + LaunchAngle = 45f, + RelativeInterceptPosition = new Vector3(1, 0, 1), + }; + var launchVector = launchPlan.NormalizedLaunchVector(); + Assert.That(launchVector, Is.EqualTo(new Vector3(1, Mathf.Sqrt(2f), 1).normalized) + .Using(Vector3EqualityComparer.Instance)); + } +} diff --git a/Assets/Tests/EditMode/LaunchPlanTests.cs.meta b/Assets/Tests/EditMode/LaunchPlanTests.cs.meta new file mode 100644 index 000000000..5542ff825 --- /dev/null +++ b/Assets/Tests/EditMode/LaunchPlanTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 48c29364e3fff4d04813ce5be04831f5 \ No newline at end of file diff --git a/docs/Simulator_Overview.md b/docs/Simulator_Overview.md index d0137eb8e..a039f2fb3 100644 --- a/docs/Simulator_Overview.md +++ b/docs/Simulator_Overview.md @@ -190,7 +190,7 @@ For example, this algorithm may not always converge to a solution, so to prevent // Check that the intercept position and the predicted position are within some threshold // distance of each other. if (Vector3.Distance(interceptPosition, targetPosition) >= InterceptPositionThreshold) { - return LaunchPlan.NoLaunch; + return LaunchPlan.NoLaunch(); } ``` Furthermore, the algorithm checks that the threat is indeed heading towards the intercept position. @@ -200,7 +200,7 @@ Furthermore, the algorithm checks that the threat is indeed heading towards the Vector3 targetToInterceptPosition = interceptPosition - initialState.Position; Vector3 targetToPredictedPosition = targetPosition - initialState.Position; if (Vector3.Dot(targetToInterceptPosition, targetToPredictedPosition) < 0) { - return LaunchPlan.NoLaunch; + return LaunchPlan.NoLaunch(); } ``` Finally, the interceptor might be launched into the posterior hemisphere away from the threat if the estimated intercept position is behind the launch position. @@ -212,7 +212,7 @@ The solution is to delay the launch until the intercept position is in the anter Vector3 interceptorToInterceptPosition = interceptPosition; Vector3 threatToPredictedPosition = targetPosition - initialState.Position; if (Vector3.Dot(interceptorToInterceptPosition, threatToPredictedPosition) > 0) { - return LaunchPlan.NoLaunch; + return LaunchPlan.NoLaunch(); } ``` From f9bdd9ae6254c6b51cfea0e2771c47eb88f1a3bb Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 16 Oct 2025 10:00:39 -0700 Subject: [PATCH 43/74] Clean up interpolator classes --- Assets/Scripts/Utils/IInterpolator2D.cs | 16 +++ Assets/Scripts/Utils/IInterpolator2D.cs.meta | 2 + Assets/Scripts/Utils/Interpolator2D.cs | 86 ------------- Assets/Scripts/Utils/Interpolator2D.cs.meta | 2 - Assets/Scripts/Utils/Interpolator2DBase.cs | 41 +++++++ .../Scripts/Utils/Interpolator2DBase.cs.meta | 2 + .../Scripts/Utils/Interpolator2DDataPoint.cs | 23 ++++ .../Utils/Interpolator2DDataPoint.cs.meta | 2 + Assets/Scripts/Utils/KDTree.cs | 115 +++++++++--------- .../Utils/NearestNeighborInterpolator2D.cs | 29 +++++ .../NearestNeighborInterpolator2D.cs.meta | 2 + Assets/Scripts/Utils/PriorityQueue.cs | 12 +- 12 files changed, 182 insertions(+), 150 deletions(-) create mode 100644 Assets/Scripts/Utils/IInterpolator2D.cs create mode 100644 Assets/Scripts/Utils/IInterpolator2D.cs.meta delete mode 100644 Assets/Scripts/Utils/Interpolator2D.cs delete mode 100644 Assets/Scripts/Utils/Interpolator2D.cs.meta create mode 100644 Assets/Scripts/Utils/Interpolator2DBase.cs create mode 100644 Assets/Scripts/Utils/Interpolator2DBase.cs.meta create mode 100644 Assets/Scripts/Utils/Interpolator2DDataPoint.cs create mode 100644 Assets/Scripts/Utils/Interpolator2DDataPoint.cs.meta create mode 100644 Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs create mode 100644 Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs.meta diff --git a/Assets/Scripts/Utils/IInterpolator2D.cs b/Assets/Scripts/Utils/IInterpolator2D.cs new file mode 100644 index 000000000..5c60a9c42 --- /dev/null +++ b/Assets/Scripts/Utils/IInterpolator2D.cs @@ -0,0 +1,16 @@ +using System.Collections.Generic; +using UnityEngine; + +// Interface for a 2D interpolator. +// +// The 2D interpolator class interpolates values on a 2D grid. +// The first two columns specify the coordinates of each data point that are used for interpolating +// the remaining data values. +public interface IInterpolator2D { + // 2D interpolator data points. + IReadOnlyList Data { get; } + + // Interpolate the value. + Interpolator2DDataPoint Interpolate(float x, float y); + Interpolator2DDataPoint Interpolate(in Vector2 coordinates); +} diff --git a/Assets/Scripts/Utils/IInterpolator2D.cs.meta b/Assets/Scripts/Utils/IInterpolator2D.cs.meta new file mode 100644 index 000000000..a1c63818b --- /dev/null +++ b/Assets/Scripts/Utils/IInterpolator2D.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 841f54705653848eaa5ca538d40a791c \ No newline at end of file diff --git a/Assets/Scripts/Utils/Interpolator2D.cs b/Assets/Scripts/Utils/Interpolator2D.cs deleted file mode 100644 index ab488ce2e..000000000 --- a/Assets/Scripts/Utils/Interpolator2D.cs +++ /dev/null @@ -1,86 +0,0 @@ -using System.Collections; -using System.Collections.Generic; -using System.Linq; -using UnityEngine; - -// 2D interpolator data point. -public class Interpolator2DDataPoint { - // 2D coordinates. - public Vector2 Coordinates { get; } - - // Arbitrary data consisting of floats. - public List Data { get; } - - public Interpolator2DDataPoint() {} - public Interpolator2DDataPoint(in Vector2 coordinates, in List data) { - Coordinates = coordinates; - Data = data; - } - - // Validate and parse the data from strings to floats. - public static (bool, List) ValidateAndParseData(in string[] values) { - List parsedValues = new List(); - for (int i = 0; i < values.Length; ++i) { - if (!float.TryParse(values[i], out float parsedValue)) { - return (false, new List()); - } - parsedValues.Add(parsedValue); - } - return (true, parsedValues); - } -} - -// The 2D interpolator class interpolates values on a 2D grid. -// The first two columns specify the coordinates of each data point that are used for interpolating -// the remaining data values. -public abstract class IInterpolator2D { - // 2D interpolator data points. - protected readonly List _data = new List(); - - public IInterpolator2D(in string[] csvLines) { - foreach (string line in csvLines) { - string[] values = line.Split(','); - (bool success, List parsedValues) = - Interpolator2DDataPoint.ValidateAndParseData(values); - if (success && parsedValues.Count >= 2) { - _data.Add(new Interpolator2DDataPoint(new Vector2(parsedValues[0], parsedValues[1]), - parsedValues.Skip(2).ToList())); - } - } - } - public IInterpolator2D(List data) { - _data = data; - } - - // Interpolate the value. - public abstract Interpolator2DDataPoint Interpolate(float x, float y); - public Interpolator2DDataPoint Interpolate(in Vector2 coordinates) { - return Interpolate(coordinates.x, coordinates.y); - } -} - -// The 2D nearest neighbor interpolator class interpolates values on a 2D grid using nearest -// neighbor interpolation. -public class NearestNeighborInterpolator2D : IInterpolator2D { - // K-D tree for nearest neighbor interpolation. - private KDTree _tree; - - public NearestNeighborInterpolator2D(in string[] csvLines) : base(csvLines) { - _tree = new KDTree( - _data, (Interpolator2DDataPoint point) => point.Coordinates); - } - public NearestNeighborInterpolator2D(List data) : base(data) { - _tree = new KDTree( - _data, (Interpolator2DDataPoint point) => point.Coordinates); - } - - // Interpolate the value using nearest neighbor interpolation. - public override Interpolator2DDataPoint Interpolate(float x, float y) { - Interpolator2DDataPoint closestPoint = _tree.NearestNeighbor(new Vector2(x, y)); - if (closestPoint == null) { - Debug.LogError("No data points available for interpolation."); - return new Interpolator2DDataPoint(new Vector2(x, y), new List()); - } - return closestPoint; - } -} diff --git a/Assets/Scripts/Utils/Interpolator2D.cs.meta b/Assets/Scripts/Utils/Interpolator2D.cs.meta deleted file mode 100644 index f218f859c..000000000 --- a/Assets/Scripts/Utils/Interpolator2D.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 77e73224d83214521b21d18f1dd73a81 \ No newline at end of file diff --git a/Assets/Scripts/Utils/Interpolator2DBase.cs b/Assets/Scripts/Utils/Interpolator2DBase.cs new file mode 100644 index 000000000..63e5d8e64 --- /dev/null +++ b/Assets/Scripts/Utils/Interpolator2DBase.cs @@ -0,0 +1,41 @@ +using System.Collections.Generic; +using System.Linq; +using UnityEngine; + +// Base implementation of a 2D interpolator. +public abstract class Interpolator2DBase : IInterpolator2D { + // 2D interpolator data points. + protected readonly List _data; + + public IReadOnlyList Data => _data; + + public Interpolator2DBase(IEnumerable data) { + _data = data.ToList() ?? new List(); + } + public Interpolator2DBase(string[] csvLines) : this(ParseCsvLines(csvLines)) {} + + // Interpolate the value. + public abstract Interpolator2DDataPoint Interpolate(float x, float y); + public Interpolator2DDataPoint Interpolate(in Vector2 coordinates) { + return Interpolate(coordinates.x, coordinates.y); + } + + // Parse the CSV lines into 2D data points. + private static IEnumerable ParseCsvLines(IEnumerable lines) { + var parsedDataPoints = new List(); + foreach (string line in lines) { + if (string.IsNullOrWhiteSpace(line)) { + continue; + } + var values = line.Split(','); + var (success, parsedValues) = Interpolator2DDataPoint.ValidateAndParseData(values); + if (success && parsedValues.Count >= 2) { + parsedDataPoints.Add(new Interpolator2DDataPoint { + Coordinates = new Vector2(parsedValues[0], parsedValues[1]), + Data = parsedValues.Skip(2).ToList(), + }); + } + } + return parsedDataPoints; + } +} diff --git a/Assets/Scripts/Utils/Interpolator2DBase.cs.meta b/Assets/Scripts/Utils/Interpolator2DBase.cs.meta new file mode 100644 index 000000000..94f13cacb --- /dev/null +++ b/Assets/Scripts/Utils/Interpolator2DBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 9e713790c0f3a425c84b89510e444dd0 \ No newline at end of file diff --git a/Assets/Scripts/Utils/Interpolator2DDataPoint.cs b/Assets/Scripts/Utils/Interpolator2DDataPoint.cs new file mode 100644 index 000000000..18399ff1f --- /dev/null +++ b/Assets/Scripts/Utils/Interpolator2DDataPoint.cs @@ -0,0 +1,23 @@ +using System.Collections.Generic; +using UnityEngine; + +// 2D interpolator data point. +public class Interpolator2DDataPoint { + // 2D coordinates. + public Vector2 Coordinates { get; set; } + + // Arbitrary data consisting of floats. + public List Data { get; set; } + + // Validate and parse the data from strings to floats. + public static (bool, List) ValidateAndParseData(string[] values) { + var parsedValues = new List(); + for (int i = 0; i < values.Length; ++i) { + if (!float.TryParse(values[i], out float parsedValue)) { + return (false, new List()); + } + parsedValues.Add(parsedValue); + } + return (true, parsedValues); + } +} diff --git a/Assets/Scripts/Utils/Interpolator2DDataPoint.cs.meta b/Assets/Scripts/Utils/Interpolator2DDataPoint.cs.meta new file mode 100644 index 000000000..b4bb5c1fc --- /dev/null +++ b/Assets/Scripts/Utils/Interpolator2DDataPoint.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: f9cb236ca18f44d638c5ab53a55eeddb \ No newline at end of file diff --git a/Assets/Scripts/Utils/KDTree.cs b/Assets/Scripts/Utils/KDTree.cs index ee8874f70..415159a42 100644 --- a/Assets/Scripts/Utils/KDTree.cs +++ b/Assets/Scripts/Utils/KDTree.cs @@ -5,83 +5,88 @@ // K-D tree node. public class KDNode { - public T Data; - public KDNode Left = null; - public KDNode Right = null; - - public KDNode(T data) { - Data = data; - } + public T Data { get; set; } + public KDNode Left { get; set; } + public KDNode Right { get; set; } } // K-D tree with 2D coordinates. public class KDTree { + // Root node. private KDNode _root; + + // Function to get the coordinates from the tree elements. private Func _getCoordinates; - public KDTree(in IReadOnlyList points, Func getCoordinates) { + public KDTree(IReadOnlyList points, Func getCoordinates) { _getCoordinates = getCoordinates; _root = BuildTree(points, depth: 0); } - private KDNode BuildTree(in IReadOnlyList points, int depth) { - if (points.Count == 0) - return null; - - int k = 2; - int axis = depth % k; - - // Sort the points by axis and find the median. - List sortedPoints = - points.OrderBy(point => (axis == 0 ? _getCoordinates(point).x : _getCoordinates(point).y)) - .ToList(); - int medianIndex = sortedPoints.Count / 2; - T medianPoint = sortedPoints[medianIndex]; - - KDNode node = new KDNode(medianPoint); - List leftPoints = sortedPoints.GetRange(0, medianIndex); - List rightPoints = - sortedPoints.GetRange(medianIndex + 1, sortedPoints.Count - medianIndex - 1); - - node.Left = BuildTree(leftPoints, depth + 1); - node.Right = BuildTree(rightPoints, depth + 1); - return node; - } - - public T NearestNeighbor(Vector2 target) { - KDNode neighbor = NearestNeighbor(_root, target, depth: 0, best: null); + // Find the nearest neighbor to the target node. + public T NearestNeighbor(in Vector2 target) { + var neighbor = NearestNeighbor(_root, target, depth: 0, bestNode: null); if (neighbor == null) { return default(T); } return neighbor.Data; } - private KDNode NearestNeighbor(KDNode node, Vector2 target, int depth, KDNode best) { - if (node == null) - return best; + // Find the nearest neighbor to the target node. + private KDNode NearestNeighbor(KDNode node, in Vector2 target, int depth, + KDNode bestNode) { + if (node == null) { + return bestNode; + } - float currentDistance = Vector2.Distance(_getCoordinates(node.Data), target); - float bestDistance = - best == null ? float.MaxValue : Vector2.Distance(_getCoordinates(best.Data), target); - if (currentDistance < bestDistance) - best = node; + var nodeCoordinates = _getCoordinates(node.Data); + var currentDistance = Vector2.Distance(nodeCoordinates, target); + var bestDistance = bestNode == null ? float.MaxValue + : Vector2.Distance(_getCoordinates(bestNode.Data), target); + if (currentDistance < bestDistance) { + bestNode = node; + bestDistance = currentDistance; + } - int axis = depth % 2; - KDNode nextBranch = (axis == 0 ? target.x < _getCoordinates(node.Data).x - : target.y < _getCoordinates(node.Data).y) - ? node.Left - : node.Right; - KDNode otherBranch = (nextBranch == node.Left) ? node.Right : node.Left; + var axis = depth % 2; + var targetCoordinatesValue = axis == 0 ? target.x : target.y; + var nodeCoordinatesValue = axis == 0 ? nodeCoordinates.x : nodeCoordinates.y; + var nextBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Left : node.Right; + var otherBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Right : node.Right; - // Check the next branch. - best = NearestNeighbor(nextBranch, target, depth + 1, best); + // Explore the next branch first. + bestNode = NearestNeighbor(nextBranch, target, depth + 1, bestNode); - // Check the other branch. - if ((axis == 0 && Mathf.Abs(target.x - _getCoordinates(node.Data).x) < bestDistance) || - (axis == 1 && Mathf.Abs(target.y - _getCoordinates(node.Data).y) < bestDistance)) { - best = NearestNeighbor(otherBranch, target, depth + 1, best); + // Explore the other branch. + if (Mathf.Abs(targetCoordinatesValue - nodeCoordinatesValue) < bestDistance) { + bestNode = NearestNeighbor(otherBranch, target, depth + 1, bestNode); } + return bestNode; + } + + // Construct a tree from the list of points. + private KDNode BuildTree(IReadOnlyList points, int depth) { + if (points.Count == 0) { + return null; + } + + var k = 2; + var axis = depth % k; - return best; + // Sort the points by axis and find the median. + var sortedPoints = points.Select(p => new { Point = p, Coordinates = _getCoordinates(p) }) + .OrderBy(p => axis == 0 ? p.Coordinates.x : p.Coordinates.y) + .Select(p => p.Point) + .ToList(); + var medianIndex = sortedPoints.Count / 2; + var medianPoint = sortedPoints[medianIndex]; + + var node = new KDNode { Data = medianPoint }; + var leftPoints = sortedPoints.GetRange(0, medianIndex); + var rightPoints = sortedPoints.GetRange(medianIndex + 1, sortedPoints.Count - medianIndex - 1); + + node.Left = BuildTree(leftPoints, depth + 1); + node.Right = BuildTree(rightPoints, depth + 1); + return node; } } diff --git a/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs b/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs new file mode 100644 index 000000000..8c3b24ea0 --- /dev/null +++ b/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs @@ -0,0 +1,29 @@ +using System.Collections.Generic; +using UnityEngine; + +// The 2D nearest neighbor interpolator class interpolates values on a 2D grid using nearest +// neighbor interpolation. +public class NearestNeighborInterpolator2D : Interpolator2DBase { + // K-D tree for nearest neighbor interpolation. + private KDTree _tree; + + public NearestNeighborInterpolator2D(IEnumerable data) : base(data) { + _tree = new KDTree( + _data, (Interpolator2DDataPoint point) => point.Coordinates); + } + public NearestNeighborInterpolator2D(string[] csvLines) : base(csvLines) { + _tree = new KDTree( + _data, (Interpolator2DDataPoint point) => point.Coordinates); + } + + // Interpolate the value using nearest neighbor interpolation. + public override Interpolator2DDataPoint Interpolate(float x, float y) { + var closestPoint = _tree.NearestNeighbor(new Vector2(x, y)); + if (closestPoint == null) { + Debug.LogError("No data points available for interpolation."); + return new Interpolator2DDataPoint { Coordinates = new Vector2(x, y), + Data = new List() }; + } + return closestPoint; + } +} diff --git a/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs.meta b/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs.meta new file mode 100644 index 000000000..cde6a79d9 --- /dev/null +++ b/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 89997441a601e468abd0c5a7f7af58f2 \ No newline at end of file diff --git a/Assets/Scripts/Utils/PriorityQueue.cs b/Assets/Scripts/Utils/PriorityQueue.cs index 8e101b269..d5d563319 100644 --- a/Assets/Scripts/Utils/PriorityQueue.cs +++ b/Assets/Scripts/Utils/PriorityQueue.cs @@ -26,11 +26,11 @@ public T Dequeue() { throw new System.InvalidOperationException("The priority queue is empty."); } - float minKey = _buffer.Keys.Min(); - Queue queue = _buffer[minKey]; - T item = queue.Dequeue(); + var firstPair = _buffer.First(); + var queue = firstPair.Value; + var item = queue.Dequeue(); if (queue.Count == 0) { - _buffer.Remove(minKey); + _buffer.Remove(firstPair.Key); } return item; } @@ -40,9 +40,7 @@ public T Peek() { if (IsEmpty()) { throw new System.InvalidOperationException("The priority queue is empty."); } - - float minKey = _buffer.Keys.Min(); - return _buffer[minKey].Peek(); + return _buffer.First().Value.Peek(); } // Return an enumerator for the priority queue. From f6ec3eaba3f2526c6d2f3831fcdb4300b06ebf62 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 16 Oct 2025 10:42:03 -0700 Subject: [PATCH 44/74] Fix unit tests for interpolator classes --- Assets/Tests/EditMode/Interpolator2DTests.cs | 107 ------------------ .../EditMode/Interpolator2DTests.cs.meta | 2 - Assets/Tests/EditMode/KDTreeTests.cs | 21 ++-- .../NearestNeighborInterpolator2DTests.cs | 69 +++++++++++ ...NearestNeighborInterpolator2DTests.cs.meta | 2 + Assets/Tests/EditMode/PriorityQueueTests.cs | 69 +++++------ Assets/Tests/EditMode/SanityTests.cs | 9 +- 7 files changed, 113 insertions(+), 166 deletions(-) delete mode 100644 Assets/Tests/EditMode/Interpolator2DTests.cs delete mode 100644 Assets/Tests/EditMode/Interpolator2DTests.cs.meta create mode 100644 Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs create mode 100644 Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs.meta diff --git a/Assets/Tests/EditMode/Interpolator2DTests.cs b/Assets/Tests/EditMode/Interpolator2DTests.cs deleted file mode 100644 index a76dfa78c..000000000 --- a/Assets/Tests/EditMode/Interpolator2DTests.cs +++ /dev/null @@ -1,107 +0,0 @@ -using Is = NUnit.Framework.Is; -using NUnit.Framework; -using NUnit.Framework.Constraints; -using System.Collections.Generic; -using UnityEngine; -using UnityEngine.TestTools; - -public class NearestNeighborInterpolator2DTests { - // Helper method to count how many data points the 2D interpolator is holding. - private static int GetNumDataPoints(NearestNeighborInterpolator2D interpolator) { - var fieldInfo = typeof(NearestNeighborInterpolator2D) - .GetField("_data", System.Reflection.BindingFlags.NonPublic | - System.Reflection.BindingFlags.Instance); - - // Get the raw value and use IList.Count. - var dataList = fieldInfo.GetValue(interpolator) as System.Collections.IList; - return dataList?.Count ?? 0; - } - - [Test] - public void TestEmptyAndInvalidLines() { - string[] csvLines = { - "", // Empty. - " , ", // Whitespace. - "abc,123,45,67" // Invalid because "abc" is not float. - }; - NearestNeighborInterpolator2D interpolator = new NearestNeighborInterpolator2D(csvLines); - - // This should result in zero valid data points. - Assert.AreEqual(0, GetNumDataPoints(interpolator), "No valid lines should have been parsed."); - - // Expect the error log before calling the method that produces it. - LogAssert.Expect(LogType.Error, "No data points available for interpolation."); - Interpolator2DDataPoint result = interpolator.Interpolate(1, 1); - Assert.AreEqual(0, result.Data.Count, "Interpolator returned data from an empty dataset."); - } - - [Test] - public void TestSingleDataPoint() { - string[] csvLines = { "0,0,45,2" }; - NearestNeighborInterpolator2D interpolator = new NearestNeighborInterpolator2D(csvLines); - - // Query a random far point. - Interpolator2DDataPoint result = interpolator.Interpolate(100, 200); - Assert.AreEqual(new Vector2(0, 0), result.Coordinates); - Assert.AreEqual(2, result.Data.Count); - Assert.AreEqual(45f, result.Data[0]); - Assert.AreEqual(2f, result.Data[1]); - } - - [Test] - public void TestInsufficientColumns() { - string[] csvLines = { "100.0,200.0" }; - NearestNeighborInterpolator2D interpolator = new NearestNeighborInterpolator2D(csvLines); - - // Interpolate from the single data point. - Interpolator2DDataPoint result = interpolator.Interpolate(100.0f, 200.0f); - Assert.NotNull(result); - Assert.AreEqual(new Vector2(100.0f, 200.0f), result.Coordinates); - Assert.AreEqual(0, result.Data.Count, "Expected zero data columns for this line."); - } - - [Test] - public void TestOutOfRangeQuery() { - // minimal data covering some small region. - string[] csvLines = { "0,0,45,2", "10,10,30,3", "20,5,25,4" }; - NearestNeighborInterpolator2D interpolator = new NearestNeighborInterpolator2D(csvLines); - - // Query something very far from all data points, e.g., (1000, 1000). - Interpolator2DDataPoint result = interpolator.Interpolate(1000, 1000); - - // The nearest neighbor is (20, 5), so we expect (25, 4). - Assert.AreEqual(new Vector2(20, 5), result.Coordinates); - Assert.AreEqual(2, result.Data.Count); - Assert.AreEqual(25f, result.Data[0]); - Assert.AreEqual(4f, result.Data[1]); - } - - [Test] - public void TestInterpolateVector2Success() { - string[] csvLines = { "1,1,10,20", "1,2,30,40", "2,1,50,60", "2,2,70,80" }; - NearestNeighborInterpolator2D interpolator = new NearestNeighborInterpolator2D(csvLines); - - Vector2 testPoint = new Vector2(1.5f, 1.5f); - Interpolator2DDataPoint result = interpolator.Interpolate(testPoint); - Assert.AreEqual(2, result.Data.Count); - - // Check if the result matches any of the possible nearest point pairs. - bool isValidPair = (result.Data[0] == 10f && result.Data[1] == 20f) || - (result.Data[0] == 30f && result.Data[1] == 40f) || - (result.Data[0] == 50f && result.Data[1] == 60f) || - (result.Data[0] == 70f && result.Data[1] == 80f); - Assert.IsTrue(isValidPair, "Result should match one of the nearest point pairs."); - } - - [Test] - public void TestInterpolateVector2Error() { - // Empty dataset. - string[] csvLines = {}; - NearestNeighborInterpolator2D interpolator = new NearestNeighborInterpolator2D(csvLines); - - Vector2 testPoint = new Vector2(1.0f, 1.0f); - LogAssert.Expect(LogType.Error, "No data points available for interpolation."); - Interpolator2DDataPoint result = interpolator.Interpolate(testPoint); - Assert.AreEqual(0, result.Data.Count, "Should return empty list when no data is available."); - } -} diff --git a/Assets/Tests/EditMode/Interpolator2DTests.cs.meta b/Assets/Tests/EditMode/Interpolator2DTests.cs.meta deleted file mode 100644 index b28a020b4..000000000 --- a/Assets/Tests/EditMode/Interpolator2DTests.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 3b9fd3396c272ab4499b5f89736cb318 \ No newline at end of file diff --git a/Assets/Tests/EditMode/KDTreeTests.cs b/Assets/Tests/EditMode/KDTreeTests.cs index 369ece3c9..9d9477852 100644 --- a/Assets/Tests/EditMode/KDTreeTests.cs +++ b/Assets/Tests/EditMode/KDTreeTests.cs @@ -1,21 +1,18 @@ using NUnit.Framework; -using System; using System.Collections.Generic; -using System.Linq; using UnityEngine; -using UnityEngine.TestTools; public class KDTreeTests { [Test] - public void TestEmpty() { - KDTree tree = new KDTree(new List(), (Vector2 point) => point); + public void NearestNeighbor_EmptyTree_ReturnsZero() { + var tree = new KDTree(new List(), (Vector2 point) => point); Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), Vector2.zero); Assert.AreEqual(tree.NearestNeighbor(new Vector2(1, 1)), Vector2.zero); } [Test] - public void TestSingle() { - KDTree tree = + public void NearestNeighbor_SinglePoint_ReturnsSinglePoint() { + var tree = new KDTree(new List { new Vector2(1, 1) }, (Vector2 point) => point); Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), new Vector2(1, 1)); Assert.AreEqual(tree.NearestNeighbor(new Vector2(1, 6)), new Vector2(1, 1)); @@ -24,14 +21,14 @@ public void TestSingle() { } [Test] - public void TestNearestNeighborWithinPoints() { + public void NearestNeighbor_MatchesDataPoint_ReturnsSamePoint() { List points = new List(); for (int i = 0; i <= 10; ++i) { for (int j = 0; j <= 10; ++j) { points.Add(new Vector2(i, j)); } } - KDTree tree = new KDTree(points, (Vector2 point) => point); + var tree = new KDTree(points, (Vector2 point) => point); Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), Vector2.zero); Assert.AreEqual(tree.NearestNeighbor(new Vector2(1, 6)), new Vector2(1, 6)); Assert.AreEqual(tree.NearestNeighbor(new Vector2(3, 2)), new Vector2(3, 2)); @@ -39,14 +36,14 @@ public void TestNearestNeighborWithinPoints() { } [Test] - public void TestNearestNeighborAroundPoints() { - List points = new List(); + public void NearestNeighbor_NearDataPoint_ReturnsClosestPoint() { + var points = new List(); for (int i = 0; i <= 10; ++i) { for (int j = 0; j <= 10; ++j) { points.Add(new Vector2(i, j)); } } - KDTree tree = new KDTree(points, (Vector2 point) => point); + var tree = new KDTree(points, (Vector2 point) => point); Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), Vector2.zero); Assert.AreEqual(tree.NearestNeighbor(new Vector2(1.4f, 1.2f)), new Vector2(1, 1)); Assert.AreEqual(tree.NearestNeighbor(new Vector2(2.7f, 2.2f)), new Vector2(3, 2)); diff --git a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs new file mode 100644 index 000000000..28a4f051c --- /dev/null +++ b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs @@ -0,0 +1,69 @@ +using NUnit.Framework; +using System.Collections.Generic; +using UnityEngine; + +public class NearestNeighborInterpolator2DTests { + private const float _epsilon = 1e-3f; + + [Test] + public void Interpolate_InvalidPoints_ReturnsEmptyResult() { + string[] csvLines = { + "", // Empty. + " , ", // Whitespace. + "abc,123,45,67" // Invalid because "abc" is not float. + }; + var interpolator = new NearestNeighborInterpolator2D(csvLines); + Assert.AreEqual(0, interpolator.Data.Count); + var result = interpolator.Interpolate(1, 1); + Assert.AreEqual(0, result.Data.Count); + } + + [Test] + public void Interpolate_SinglePoint_ReturnsSinglePoint() { + string[] csvLines = { "0,0,45,2" }; + var interpolator = new NearestNeighborInterpolator2D(csvLines); + var result = interpolator.Interpolate(100, 200); + Assert.AreEqual(new Vector2(0, 0), result.Coordinates); + Assert.AreEqual(new List { 45f, 2f }, result.Data, _epsilon); + } + + [Test] + public void Interpolate_InsufficientColumns_ReturnsEmptyResult() { + string[] csvLines = { "100.0,200.0" }; + var interpolator = new NearestNeighborInterpolator2D(csvLines); + var result = interpolator.Interpolate(100f, 200f); + Assert.NotNull(result); + Assert.AreEqual(new Vector2(100f, 200f), result.Coordinates, _epsilon); + Assert.AreEqual(0, result.Data.Count); + } + + [Test] + public void Interpolate_QueryOutOfRange_ReturnsClosestPoint() { + string[] csvLines = { "0,0,45,2", "10,10,30,3", "20,5,25,4" }; + var interpolator = new NearestNeighborInterpolator2D(csvLines); + var result = interpolator.Interpolate(1000, 1000); + Assert.AreEqual(new Vector2(20, 5), result.Coordinates); + Assert.AreEqual(new List { 25f, 4f }, result.Data, _epsilon); + } + + [Test] + public void Interpolate_BetweenDataPoints_ReturnsOnePoint() { + string[] csvLines = { "1,1,10,20", "1,2,30,40", "2,1,50,60", "2,2,70,80" }; + var interpolator = new NearestNeighborInterpolator2D(csvLines); + var testPoint = new Vector2(1.5f, 1.5f); + var result = interpolator.Interpolate(testPoint); + Assert.AreEqual(2, result.Data.Count); + var isValidPair = + (result.Data == new Vector2(10f, 20f) || result.Data == new Vector2(30f, 40f) || + result.Data == new Vector2(50f, 60f) || result.Data == new Vector2(70f, 80f)); + Assert.IsTrue(isValidPair); + } + + [Test] + public void Interpolate_NoDataPoints_ReturnsEmptyResult() { + string[] csvLines = {}; + var interpolator = new NearestNeighborInterpolator2D(csvLines); + Interpolator2DDataPoint result = interpolator.Interpolate(new Vector2(1f, 1f)); + Assert.AreEqual(0, result.Data.Count); + } +} diff --git a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs.meta b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs.meta new file mode 100644 index 000000000..f38c001f9 --- /dev/null +++ b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 7aa7a2a68cdbb411d9d8fd7f519012c1 \ No newline at end of file diff --git a/Assets/Tests/EditMode/PriorityQueueTests.cs b/Assets/Tests/EditMode/PriorityQueueTests.cs index 4d8d20ad5..166df5d02 100644 --- a/Assets/Tests/EditMode/PriorityQueueTests.cs +++ b/Assets/Tests/EditMode/PriorityQueueTests.cs @@ -1,50 +1,45 @@ using NUnit.Framework; -using System; using System.Collections.Generic; -using System.Linq; -using UnityEngine; -using UnityEngine.TestTools; public class PriorityQueueTests { [Test] - public void TestIsEmpty() { - PriorityQueue queue = new PriorityQueue(); + public void IsEmpty_ReturnsCorrectly() { + var queue = new PriorityQueue(); Assert.True(queue.IsEmpty()); - queue.Enqueue(item: 1.0f, priority: 1.0f); + queue.Enqueue(item: 1f, priority: 1f); Assert.False(queue.IsEmpty()); } [Test] - public void TestDequeueWhenEmpty() { - PriorityQueue queue = new PriorityQueue(); + public void Dequeue_WhenEmpty_ThrowsException() { + var queue = new PriorityQueue(); Assert.Throws(() => { queue.Dequeue(); }); } [Test] - public void TestPeekWhenEmpty() { - PriorityQueue queue = new PriorityQueue(); + public void Peek_WhenEmpty_ThrowsException() { + var queue = new PriorityQueue(); Assert.Throws(() => { queue.Peek(); }); } [Test] - public void TestPeek() { - PriorityQueue queue = new PriorityQueue(); - queue.Enqueue(item: "a", priority: 3.0f); - queue.Enqueue(item: "b", priority: 1.0f); - queue.Enqueue(item: "c", priority: 5.0f); + public void Peek_ReturnsItemWithLowestPriority() { + var queue = new PriorityQueue(); + queue.Enqueue(item: "a", priority: 3f); + queue.Enqueue(item: "b", priority: 1f); + queue.Enqueue(item: "c", priority: 5f); queue.Enqueue(item: "d", priority: 3.2f); Assert.AreEqual("b", queue.Peek()); } [Test] - public void TestPriority() { - PriorityQueue queue = new PriorityQueue(); - queue.Enqueue(item: "a", priority: 3.0f); - queue.Enqueue(item: "b", priority: 1.0f); - queue.Enqueue(item: "c", priority: 5.0f); + public void Dequeue_ReturnsItemsInIncreasingPriority() { + var queue = new PriorityQueue(); + queue.Enqueue(item: "a", priority: 3f); + queue.Enqueue(item: "b", priority: 1f); + queue.Enqueue(item: "c", priority: 5f); queue.Enqueue(item: "d", priority: 3.2f); - - List expectedOrder = new List { "b", "a", "d", "c" }; + var expectedOrder = new List { "b", "a", "d", "c" }; int index = 0; while (!queue.IsEmpty()) { Assert.AreEqual(expectedOrder[index], queue.Dequeue()); @@ -53,15 +48,14 @@ public void TestPriority() { } [Test] - public void TestEnumerator() { - PriorityQueue queue = new PriorityQueue(); - queue.Enqueue(item: "a", priority: 3.0f); - queue.Enqueue(item: "b", priority: 1.0f); - queue.Enqueue(item: "c", priority: 5.0f); + public void Enumerator_ReturnsItemsInIncreasingPriority() { + var queue = new PriorityQueue(); + queue.Enqueue(item: "a", priority: 3f); + queue.Enqueue(item: "b", priority: 1f); + queue.Enqueue(item: "c", priority: 5f); queue.Enqueue(item: "d", priority: 3.2f); - - List expectedOrder = new List { "b", "a", "d", "c" }; - IEnumerator enumerator = queue.GetEnumerator(); + var expectedOrder = new List { "b", "a", "d", "c" }; + var enumerator = queue.GetEnumerator(); int index = 0; while (enumerator.MoveNext()) { Assert.AreEqual(expectedOrder[index], enumerator.Current); @@ -70,14 +64,13 @@ public void TestEnumerator() { } [Test] - public void TestIterator() { - PriorityQueue queue = new PriorityQueue(); - queue.Enqueue(item: "a", priority: 3.0f); - queue.Enqueue(item: "b", priority: 1.0f); - queue.Enqueue(item: "c", priority: 5.0f); + public void Iterator_ReturnsItemsInIncreasingPriority() { + var queue = new PriorityQueue(); + queue.Enqueue(item: "a", priority: 3f); + queue.Enqueue(item: "b", priority: 1f); + queue.Enqueue(item: "c", priority: 5f); queue.Enqueue(item: "d", priority: 3.2f); - - List expectedOrder = new List { "b", "a", "d", "c" }; + var expectedOrder = new List { "b", "a", "d", "c" }; int index = 0; foreach (var item in queue) { Assert.AreEqual(expectedOrder[index], item); diff --git a/Assets/Tests/EditMode/SanityTests.cs b/Assets/Tests/EditMode/SanityTests.cs index 6b075233c..1fbfc5998 100644 --- a/Assets/Tests/EditMode/SanityTests.cs +++ b/Assets/Tests/EditMode/SanityTests.cs @@ -1,20 +1,15 @@ using NUnit.Framework; -using UnityEngine; -using UnityEngine.TestTools; using System.Collections; +using UnityEngine.TestTools; public class SanityTests { [Test] - public void SanityTestSimplePasses() { - // Use the Assert class to test conditions + public void Pass() { Assert.Pass("This test passes."); } - // A UnityTest behaves like a coroutine in Play Mode. In Edit Mode you can use - // `yield return null;` to skip a frame. [UnityTest] public IEnumerator SanityTestWithEnumeratorPasses() { - // Use the Assert class to test conditions. // Use yield to skip a frame. yield return null; Assert.Pass("This test passes after skipping a frame."); From 820eb7d6b73ee6467ae3fb43eeb180ad7d30d1bb Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 16 Oct 2025 10:58:17 -0700 Subject: [PATCH 45/74] Add hierarchical as property of predictor clases --- Assets/Scripts/Algorithms/Prediction/IPredictor.cs | 4 +++- .../Algorithms/Prediction/LinearExtrapolator.cs | 10 ++++++---- Assets/Scripts/Algorithms/Prediction/PredictorBase.cs | 8 +++++++- Assets/Tests/EditMode/LinearExtrapolatorTests.cs | 10 +++++----- 4 files changed, 21 insertions(+), 11 deletions(-) diff --git a/Assets/Scripts/Algorithms/Prediction/IPredictor.cs b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs index 7050b79a5..0435df0cc 100644 --- a/Assets/Scripts/Algorithms/Prediction/IPredictor.cs +++ b/Assets/Scripts/Algorithms/Prediction/IPredictor.cs @@ -2,6 +2,8 @@ // // The predictor predicts the trajectories of hierarchical objects. public interface IPredictor { + IHierarchical Hierarchical { get; set; } + // Predict the future state of the hierarchical object. - PredictorState Predict(IHierarchical hierarchical, float time); + PredictorState Predict(float time); } diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs index 47333a250..ae7991eb0 100644 --- a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs +++ b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs @@ -4,14 +4,16 @@ // The linear extrapolator class predicts the trajectory of an agent by linearly extrapolating its // current position and velocity. public class LinearExtrapolator : PredictorBase { + public LinearExtrapolator(IHierarchical hierarchical) : base(hierarchical) {} + // Predict the future state of the hierarchical object by linearly extrapolating its current // position and velocity. - public override PredictorState Predict(IHierarchical hierarchical, float time) { - var position = hierarchical.Position + hierarchical.Velocity * time; + public override PredictorState Predict(float time) { + var position = Hierarchical.Position + Hierarchical.Velocity * time; return new PredictorState { Position = position, - Velocity = hierarchical.Velocity, - Acceleration = hierarchical.Acceleration, + Velocity = Hierarchical.Velocity, + Acceleration = Hierarchical.Acceleration, }; } } diff --git a/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs index 2e7e64530..a8065a208 100644 --- a/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs +++ b/Assets/Scripts/Algorithms/Prediction/PredictorBase.cs @@ -1,5 +1,11 @@ // Base implementation of a predictor. public abstract class PredictorBase : IPredictor { + public IHierarchical Hierarchical { get; set; } + + public PredictorBase(IHierarchical hierarchical) { + Hierarchical = hierarchical; + } + // Predict the future state of the hierarchical object. - public abstract PredictorState Predict(IHierarchical hierarchical, float time); + public abstract PredictorState Predict(float time); } diff --git a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs index a845fbf77..d296a7b43 100644 --- a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs +++ b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs @@ -2,20 +2,20 @@ using UnityEngine; public class LinearExtrapolatorTests { - private LinearExtrapolator _predictor; private FixedHierarchical _hierarchical; + private LinearExtrapolator _predictor; [SetUp] public void SetUp() { - _predictor = new LinearExtrapolator(); _hierarchical = new FixedHierarchical(position: new Vector3(10, 0, 5), velocity: new Vector3(1, -2, 2), acceleration: new Vector3(-1, 1, 2)); + _predictor = new LinearExtrapolator(_hierarchical); } [Test] public void Predict_AtTimeZero_ReturnsCurrentState() { - var predictedState = _predictor.Predict(_hierarchical, time: 0f); + var predictedState = _predictor.Predict(time: 0f); Assert.AreEqual(_hierarchical.Position, predictedState.Position); Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); @@ -23,7 +23,7 @@ public void Predict_AtTimeZero_ReturnsCurrentState() { [Test] public void Predict_InThePast_ReturnsExtrapolatedState() { - var predictedState = _predictor.Predict(_hierarchical, time: -5f); + var predictedState = _predictor.Predict(time: -5f); Assert.AreEqual(new Vector3(5, 10, -5), predictedState.Position); Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); @@ -31,7 +31,7 @@ public void Predict_InThePast_ReturnsExtrapolatedState() { [Test] public void Predict_InTheFuture_ReturnsExtrapolatedState() { - var predictedState = _predictor.Predict(_hierarchical, time: 10f); + var predictedState = _predictor.Predict(time: 10f); Assert.AreEqual(new Vector3(20, -20, 25), predictedState.Position); Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); From 1a875cf3c4d98e789bb29e617e00f217d138ce7a Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 16 Oct 2025 17:47:11 -0700 Subject: [PATCH 46/74] Clean up all predictor and planner classes --- .../Planning/ILaunchAnglePlanner.cs | 16 +++ .../Planning/ILaunchAnglePlanner.cs.meta | 2 + .../Algorithms/Planning/ILaunchPlanner.cs | 10 ++ .../Planning/ILaunchPlanner.cs.meta | 2 + .../Planning/IterativeLaunchPlanner.cs | 84 +++++++++++++ .../Planning/IterativeLaunchPlanner.cs.meta | 2 + .../Planning/IterativeLaunchPlannerLegacy.cs | 4 +- .../Planning/LaunchAngleCsvInterpolator.cs | 46 +++++++ .../LaunchAngleCsvInterpolator.cs.meta | 2 + .../LaunchAngleDataInterpolatorBase.cs | 25 ++++ .../LaunchAngleDataInterpolatorBase.cs.meta | 2 + .../Planning/LaunchAngleDataPoint.cs | 8 ++ .../Planning/LaunchAngleDataPoint.cs.meta | 2 + .../Algorithms/Planning/LaunchAngleInput.cs | 8 ++ .../Planning/LaunchAngleInput.cs.meta | 2 + .../Planning/LaunchAngleInterpolator.cs | 49 +++++--- .../Planning/LaunchAngleInterpolatorBase.cs | 45 +++++++ .../LaunchAngleInterpolatorBase.cs.meta | 2 + .../Algorithms/Planning/LaunchAngleOutput.cs | 8 ++ .../Planning/LaunchAngleOutput.cs.meta | 2 + .../Algorithms/Planning/LaunchAnglePlanner.cs | 63 ---------- .../Planning/LaunchAnglePlanner.cs.meta | 2 - .../Planning/LaunchAnglePlannerBase.cs | 28 +++++ .../Planning/LaunchAnglePlannerBase.cs.meta | 2 + .../Scripts/Algorithms/Planning/LaunchPlan.cs | 11 +- .../Algorithms/Planning/LaunchPlannerBase.cs | 14 +++ .../Planning/LaunchPlannerBase.cs.meta | 2 + .../Planning/LaunchPlannerLegacy.cs | 5 +- .../Scripts/Hierarchical/FixedHierarchical.cs | 19 +-- Assets/Scripts/IADS/IADS.cs | 11 +- Assets/Scripts/Utils/KDTree.cs | 2 +- .../EditMode/IterativeLaunchPlannerTests.cs | 113 +++++++++--------- Assets/Tests/EditMode/KDTreeTests.cs | 31 +++-- .../LaunchAngleCsvInterpolatorTests.cs | 93 ++++++++++++++ .../LaunchAngleCsvInterpolatorTests.cs.meta | 2 + .../LaunchAngleDataInterpolatorBaseTests.cs | 76 ++++++++++++ ...unchAngleDataInterpolatorBaseTests.cs.meta | 2 + .../EditMode/LaunchAngleInterpolatorTests.cs | 87 -------------- .../LaunchAngleInterpolatorTests.cs.meta | 2 - Assets/Tests/EditMode/LaunchPlanTests.cs | 16 +-- .../NearestNeighborInterpolator2DTests.cs | 23 +++- Assets/Tests/EditMode/PriorityQueueTests.cs | 1 + 42 files changed, 651 insertions(+), 275 deletions(-) create mode 100644 Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs create mode 100644 Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs create mode 100644 Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs create mode 100644 Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs.meta delete mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs delete mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs.meta create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs create mode 100644 Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs.meta create mode 100644 Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs create mode 100644 Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs.meta create mode 100644 Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs create mode 100644 Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs.meta delete mode 100644 Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs delete mode 100644 Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs.meta diff --git a/Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs b/Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs new file mode 100644 index 000000000..cb223bea2 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs @@ -0,0 +1,16 @@ +using UnityEngine; + +// Interface for a launch angle planner. +// +// The launch angle planner outputs the optimal launch angle and the time-to-target. +public interface ILaunchAnglePlanner { + // Launcher agent. + IAgent Agent { get; set; } + + // Calculate the optimal launch angle in degrees and the time-to-target in seconds. + LaunchAngleOutput Plan(in LaunchAngleInput input); + LaunchAngleOutput Plan(in Vector3 targetPosition); + + // Return the absolute intercept position given the target position. + Vector3 InterceptPosition(in Vector3 targetPosition); +} diff --git a/Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs.meta b/Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs.meta new file mode 100644 index 000000000..f156b7dd3 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/ILaunchAnglePlanner.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 67763985c50ce47c588c84ae802a7590 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs new file mode 100644 index 000000000..06ebbcba3 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs @@ -0,0 +1,10 @@ +// Interface for a launch planner. +// +// The launch planner finds the convergence point between the launch angle planner and the +// predictor, so that the interceptor reaches the same position as the target at the same time. +public interface ILaunchPlanner { + ILaunchAnglePlanner LaunchAnglePlanner { get; set; } + IPredictor Predictor { get; set; } + + LaunchPlan Plan(); +} diff --git a/Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs.meta b/Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs.meta new file mode 100644 index 000000000..a609cbd32 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/ILaunchPlanner.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: a953f7bcdab3a4541b396da8e31d6571 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs new file mode 100644 index 000000000..d0bbeac3b --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs @@ -0,0 +1,84 @@ +using UnityEngine; + +// The iterative launch planner class is a launch planner that performs an iterative process to +// determine the intercept point. The algorithm continuously performs the following 2-step iterative +// process: +// 1. Time-to-intercept estimation: The algorithm determines the time it takes the interceptor to +// reach the target at the predicted intercept position, which is initialized to the target's +// current position. +// 2. Intercept position estimation: The algorithm predicts the target position at the estimated +// time-to-intercept. +public class IterativeLaunchPlanner : LaunchPlannerBase { + // Maximum number of iterations before declaring failure. In certain cases, the predictor and + // planner do not converge, so we need to limit the number of iterations. + private const int _maxNumIterations = 10; + + // Convergence threshold in meters for the difference vector magnitude. Convergence is declared + // when the intercept position has not changed by more than this threshold between iterations. + private const float _convergenceThreshold = 10f; + + // Maximum intercept position threshold in meters to declare convergence. This threshold is used + // as a final sanity check to ensure that the predicted target position and intercept position do + // not differ by more than this threshold. This threshold should be set depending on the + // granularity of the possible intercept positions. + private const float _interceptPositionThreshold = 1000f; + + public IterativeLaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictor predictor) + : base(launchAnglePlanner, predictor) {} + + // Plan the launch by finding the convergence point between the launch angle planner and the + // predictor. + public override LaunchPlan Plan() { + var initialState = Predictor.Predict(time: 0f); + var targetPosition = initialState.Position; + + var launchAngleOutput = new LaunchAngleOutput(); + var interceptPosition = new Vector3(); + for (int i = 0; i < _maxNumIterations; ++i) { + // Estimate the time-to-intercept. + launchAngleOutput = LaunchAnglePlanner.Plan(targetPosition); + var timeToIntercept = launchAngleOutput.TimeToPosition; + + // Estimate the target position. + var predictedState = Predictor.Predict(timeToIntercept); + targetPosition = predictedState.Position; + + // Check whether the intercept direction has changed, in which case the algorithm has + // converged. + var newInterceptPosition = LaunchAnglePlanner.InterceptPosition(targetPosition); + if ((interceptPosition - newInterceptPosition).magnitude < _convergenceThreshold) { + interceptPosition = newInterceptPosition; + break; + } + interceptPosition = newInterceptPosition; + + // Check that the target is moving towards the intercept position. Otherwise, the interceptor + // should wait to be launched. + var targetToInterceptPosition = interceptPosition - initialState.Position; + var targetToPredictedPosition = targetPosition - initialState.Position; + if (Vector3.Dot(targetToInterceptPosition, targetToPredictedPosition) < 0) { + return LaunchPlan.NoLaunch(); + } + } + + // Check that the interceptor is moving towards the target. If the target is moving too fast, + // the interceptor might be launched backwards because the intercept position and the predicted + // position are behind the asset. In this case, the interceptor should wait to be launched. + var interceptorToInterceptPosition = interceptPosition; + var threatToPredictedPosition = targetPosition - initialState.Position; + if (Vector3.Dot(interceptorToInterceptPosition, threatToPredictedPosition) > 0) { + return LaunchPlan.NoLaunch(); + } + + // Check that the intercept position and the predicted position are within some threshold + // distance of each other. + if (Vector3.Distance(interceptPosition, targetPosition) < _interceptPositionThreshold) { + return new LaunchPlan { + ShouldLaunch = true, + LaunchAngle = launchAngleOutput.LaunchAngle, + InterceptPosition = targetPosition, + }; + } + return LaunchPlan.NoLaunch(); + } +} diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs.meta b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs.meta new file mode 100644 index 000000000..93f9ed30e --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 45d458fd8f68b4bc48f9222f3bba94d5 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs index 318977e8d..85cc82976 100644 --- a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs +++ b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlannerLegacy.cs @@ -23,7 +23,7 @@ public class IterativeLaunchPlannerLegacy : ILaunchPlannerLegacy { // granularity of the possible intercept positions. private const float InterceptPositionThreshold = 1000f; - public IterativeLaunchPlannerLegacy(ILaunchAnglePlanner launchAnglePlanner, + public IterativeLaunchPlannerLegacy(ILaunchAnglePlannerLegacy launchAnglePlanner, IPredictorLegacy predictor) : base(launchAnglePlanner, predictor) {} @@ -76,7 +76,7 @@ public override LaunchPlan Plan() { return new LaunchPlan { ShouldLaunch = true, LaunchAngle = launchAngleOutput.LaunchAngle, - RelativeInterceptPosition = targetPosition, + InterceptPosition = targetPosition, }; } return LaunchPlan.NoLaunch(); diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs new file mode 100644 index 000000000..c6e79e461 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs @@ -0,0 +1,46 @@ +using System; +using System.IO; +using UnityEngine; + +// The launch angle CSV interpolator loads launch angle data from a CSV file and provides +// interpolated values for arbitrary target positions. +public class LaunchAngleCsvInterpolator : LaunchAngleInterpolatorBase { + // Path to the CSV file. + // The first two columns of the CSV file specify the coordinates of each data point. + // The third column denotes the launch angle in degrees, and the fourth column denotes the time to + // reach the target position. + private readonly string _relativePath; + + // Delegate for loading the CSV file. + public delegate string ConfigLoaderDelegate(string path); + private readonly ConfigLoaderDelegate _configLoader; + + public LaunchAngleCsvInterpolator(IAgent agent) + : this(agent, path: Path.Combine("Planning", "hydra70_launch_angle.csv"), + configLoader: ConfigLoader.LoadFromStreamingAssets) {} + public LaunchAngleCsvInterpolator(IAgent agent, string path, ConfigLoaderDelegate configLoader) + : base(agent) { + _relativePath = path; + _configLoader = configLoader; + } + + // Initialize the interpolator. + protected override void InitInterpolator() { + var fileContent = _configLoader(_relativePath); + if (string.IsNullOrEmpty(fileContent)) { + Debug.LogError($"Failed to load CSV file from {_relativePath}."); + throw new InvalidOperationException("Interpolator could not be initialized."); + } + + var csvLines = fileContent.Split('\n'); + if (csvLines.Length < 1) { + throw new InvalidOperationException("No data points available for interpolation."); + } + + try { + _interpolator = new NearestNeighborInterpolator2D(csvLines); + } catch (Exception e) { + throw new InvalidOperationException("Failed to initialize interpolator: " + e.Message); + } + } +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs.meta new file mode 100644 index 000000000..bea81dba7 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4c9ae1065d2764dd3bdeb7d4add0468d \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs new file mode 100644 index 000000000..3911dbd49 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs @@ -0,0 +1,25 @@ +using System.Collections.Generic; +using UnityEngine; + +// Base implementation of a launch angle data interpolator. +// +// The launch angle data interpolator interpolates the values from a static list of values. +public abstract class LaunchAngleDataInterpolatorBase : LaunchAngleInterpolatorBase { + public LaunchAngleDataInterpolatorBase(IAgent agent) : base(agent) {} + + // Initialize the interpolator. + protected override void InitInterpolator() { + var interpolatorDataPoints = new List(); + foreach (var dataPoint in GenerateData()) { + var interpolatorDataPoint = new Interpolator2DDataPoint { + Coordinates = new Vector2(dataPoint.Input.Distance, dataPoint.Input.Altitude), + Data = new List { dataPoint.Output.LaunchAngle, dataPoint.Output.TimeToPosition }, + }; + interpolatorDataPoints.Add(interpolatorDataPoint); + } + _interpolator = new NearestNeighborInterpolator2D(interpolatorDataPoints); + } + + // Generate the launch angle data points to interpolate. + protected abstract IEnumerable GenerateData(); +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs.meta new file mode 100644 index 000000000..ed36f17f2 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataInterpolatorBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 24c29596a37794dfba5a16210a157251 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs new file mode 100644 index 000000000..22c77fc1b --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs @@ -0,0 +1,8 @@ +// Launch angle data point. +public struct LaunchAngleDataPoint { + // Launch angle input. + public LaunchAngleInput Input { get; set; } + + // Launch angle output. + public LaunchAngleOutput Output { get; set; } +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs.meta new file mode 100644 index 000000000..666d3fb0f --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleDataPoint.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: c3e3174b813f24403a410ea5bd9a3cf5 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs new file mode 100644 index 000000000..17e3c8e36 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs @@ -0,0 +1,8 @@ +// Launch angle input. +public struct LaunchAngleInput { + // Horizontal distance in meters. + public float Distance { get; set; } + + // Altitude in meters. + public float Altitude { get; set; } +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs.meta new file mode 100644 index 000000000..4595be773 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleInput.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 8271069e9465448278674d2e8295a776 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolator.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolator.cs index 3cd0126e8..c15c8a306 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolator.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolator.cs @@ -4,14 +4,32 @@ using System.IO; using UnityEngine; +// The launch angle planner class is an interface for a planner that outputs the optimal launch +// angle and the time-to-target. +public interface ILaunchAnglePlannerLegacy { + // Calculate the optimal launch angle in degrees and the time-to-target in seconds. + public LaunchAngleOutput Plan(in LaunchAngleInput input); + public LaunchAngleOutput Plan(Vector3 position) { + Vector2 direction = ConvertToDirection(position); + return Plan(new LaunchAngleInput { Distance = direction[0], Altitude = direction[1] }); + } + + // Get the intercept position. + public Vector3 GetInterceptPosition(Vector3 position); + + // Convert from a 3D vector to a 2D direction that ignores the azimuth. + protected static Vector2 ConvertToDirection(Vector3 position) { + return new Vector2(Vector3.ProjectOnPlane(position, Vector3.up).magnitude, + Vector3.Project(position, Vector3.up).magnitude); + } +} + // The launch angle interpolator class determines the optimal launch angle and the time-to-target // given the horizontal distance and altitude of the target. -public abstract class ILaunchAngleInterpolator : ILaunchAnglePlanner { +public abstract class ILaunchAngleInterpolatorLegacy : ILaunchAnglePlannerLegacy { // Launch angle data interpolator. protected IInterpolator2D _interpolator; - public ILaunchAngleInterpolator() : base() {} - // Initialize the interpolator. protected abstract void InitInterpolator(); @@ -29,13 +47,13 @@ public LaunchAngleOutput Plan(in LaunchAngleInput input) { throw new InvalidOperationException("Interpolator returned invalid data."); } - return new LaunchAngleOutput(launchAngle: interpolatedDataPoint.Data[0], - timeToPosition: interpolatedDataPoint.Data[1]); + return new LaunchAngleOutput { LaunchAngle = interpolatedDataPoint.Data[0], + TimeToPosition = interpolatedDataPoint.Data[1] }; } // Get the intercept position. public Vector3 GetInterceptPosition(Vector3 position) { - Vector2 direction = ILaunchAnglePlanner.ConvertToDirection(position); + Vector2 direction = ILaunchAnglePlannerLegacy.ConvertToDirection(position); Interpolator2DDataPoint interpolatedDataPoint = _interpolator.Interpolate(direction[0], direction[1]); Vector3 cylindricalPosition = Coordinates3.ConvertCartesianToCylindrical(position); @@ -47,7 +65,7 @@ public Vector3 GetInterceptPosition(Vector3 position) { // The launch angle CSV interpolator class loads launch angle data from a CSV file and provides // interpolated values for arbitrary target positions. -public class LaunchAngleCsvInterpolator : ILaunchAngleInterpolator { +public class LaunchAngleCsvInterpolatorLegacy : ILaunchAngleInterpolatorLegacy { // Path to the CSV file. // The first two columns of the CSV file specify the coordinates of each data point. // The third column denotes the launch angle in degrees, and the fourth column denotes the time to @@ -58,8 +76,8 @@ public class LaunchAngleCsvInterpolator : ILaunchAngleInterpolator { public delegate string ConfigLoaderDelegate(string path); private readonly ConfigLoaderDelegate _configLoader; - public LaunchAngleCsvInterpolator(string path = null, ConfigLoaderDelegate configLoader = null) - : base() { + public LaunchAngleCsvInterpolatorLegacy(string path = null, + ConfigLoaderDelegate configLoader = null) { _relativePath = path ?? Path.Combine("Planning", "hydra70_launch_angle.csv"); _configLoader = configLoader ?? ConfigLoader.LoadFromStreamingAssets; } @@ -86,21 +104,20 @@ protected override void InitInterpolator() { } // The launch angle data interpolator class interpolates the values from a static list of values. -public abstract class LaunchAngleDataInterpolator : ILaunchAngleInterpolator { - public LaunchAngleDataInterpolator() : base() {} - +public abstract class LaunchAngleDataInterpolatorLegacy : ILaunchAngleInterpolatorLegacy { // Initialize the interpolator. protected override void InitInterpolator() { List interpolatorDataPoints = new List(); foreach (var dataPoint in GenerateData()) { - Interpolator2DDataPoint interpolatorDataPoint = new Interpolator2DDataPoint( - new Vector2(dataPoint.Input.Distance, dataPoint.Input.Altitude), - new List { dataPoint.Output.LaunchAngle, dataPoint.Output.TimeToPosition }); + Interpolator2DDataPoint interpolatorDataPoint = new Interpolator2DDataPoint { + Coordinates = new Vector2(dataPoint.Input.Distance, dataPoint.Input.Altitude), + Data = new List { dataPoint.Output.LaunchAngle, dataPoint.Output.TimeToPosition } + }; interpolatorDataPoints.Add(interpolatorDataPoint); } _interpolator = new NearestNeighborInterpolator2D(interpolatorDataPoints); } - // Generate the list of launch angle data points to interpolate. + // Generate the launch angle data points to interpolate. protected abstract List GenerateData(); } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs new file mode 100644 index 000000000..f15a8cde5 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs @@ -0,0 +1,45 @@ +using System; +using System.Collections.Generic; +using UnityEngine; + +// Base imple,entation of a launch angle interpolator. +// +// The interpolator determines the optimal launch angle and the time-to-target given the horizontal +// distance and altitude of the target. +public abstract class LaunchAngleInterpolatorBase : LaunchAnglePlannerBase { + // Launch angle data interpolator. + protected IInterpolator2D _interpolator; + + public LaunchAngleInterpolatorBase(IAgent agent) : base(agent) {} + + // Calculate the optimal launch angle in degrees and the time-to-target in seconds. + public override LaunchAngleOutput Plan(in LaunchAngleInput input) { + if (_interpolator == null) { + InitInterpolator(); + } + + var interpolatedDataPoint = _interpolator.Interpolate(input.Distance, input.Altitude); + if (interpolatedDataPoint == null || interpolatedDataPoint.Data == null || + interpolatedDataPoint.Data.Count < 2) { + throw new InvalidOperationException("Interpolator returned invalid data."); + } + + return new LaunchAngleOutput { LaunchAngle = interpolatedDataPoint.Data[0], + TimeToPosition = interpolatedDataPoint.Data[1] }; + } + + // Return the absolute intercept position given the absolute target position. + public override Vector3 InterceptPosition(in Vector3 targetPosition) { + var direction = ConvertToRelativeDirection(targetPosition); + var interpolatedDataPoint = _interpolator.Interpolate(direction[0], direction[1]); + var relativePosition = targetPosition - Agent.Position; + var cylindricalRelativePosition = Coordinates3.ConvertCartesianToCylindrical(relativePosition); + return Coordinates3.ConvertCylindricalToCartesian( + r: interpolatedDataPoint.Coordinates[0], azimuth: cylindricalRelativePosition.y, + height: interpolatedDataPoint.Coordinates[1]) + + Agent.Position; + } + + // Initialize the interpolator. + protected abstract void InitInterpolator(); +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs.meta new file mode 100644 index 000000000..718cebb80 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: af587135b25324a80a5321ba6846d51a \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs new file mode 100644 index 000000000..3785c3560 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs @@ -0,0 +1,8 @@ +// Launch angle output. +public struct LaunchAngleOutput { + // Launch angle in degrees. + public float LaunchAngle { get; set; } + + // Time to reach the target position in seconds. + public float TimeToPosition { get; set; } +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs.meta new file mode 100644 index 000000000..c770f27c1 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleOutput.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4b627d4c40a554e2087f1adca5fe40e9 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs deleted file mode 100644 index 544c07d59..000000000 --- a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs +++ /dev/null @@ -1,63 +0,0 @@ -using UnityEngine; - -// Launch angle input. -public struct LaunchAngleInput { - // Horizontal distance in meters. - public float Distance { get; } - - // Altitude in meters. - public float Altitude { get; } - - public LaunchAngleInput(float distance, float altitude) { - Distance = distance; - Altitude = altitude; - } -} - -// Launch angle output. -public struct LaunchAngleOutput { - // Launch angle in degrees. - public float LaunchAngle { get; } - - // Time to reach the target position in seconds. - public float TimeToPosition { get; } - - public LaunchAngleOutput(float launchAngle, float timeToPosition) { - LaunchAngle = launchAngle; - TimeToPosition = timeToPosition; - } -} - -// Launch angle data point. -public struct LaunchAngleDataPoint { - // Launch angle input. - public LaunchAngleInput Input { get; } - - // Launch angle output. - public LaunchAngleOutput Output { get; } - - public LaunchAngleDataPoint(in LaunchAngleInput input, in LaunchAngleOutput output) { - Input = input; - Output = output; - } -} - -// The launch angle planner class is an interface for a planner that outputs the optimal launch -// angle and the time-to-target. -public interface ILaunchAnglePlanner { - // Calculate the optimal launch angle in degrees and the time-to-target in seconds. - public LaunchAngleOutput Plan(in LaunchAngleInput input); - public LaunchAngleOutput Plan(Vector3 position) { - Vector2 direction = ConvertToDirection(position); - return Plan(new LaunchAngleInput(distance: direction[0], altitude: direction[1])); - } - - // Get the intercept position. - public Vector3 GetInterceptPosition(Vector3 position); - - // Convert from a 3D vector to a 2D direction that ignores the azimuth. - protected static Vector2 ConvertToDirection(Vector3 position) { - return new Vector2(Vector3.ProjectOnPlane(position, Vector3.up).magnitude, - Vector3.Project(position, Vector3.up).magnitude); - } -} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs.meta deleted file mode 100644 index 531f344d9..000000000 --- a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlanner.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 0ff592c12255945c38fb5f6b4b676642 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs new file mode 100644 index 000000000..508b6bf87 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs @@ -0,0 +1,28 @@ +using UnityEngine; + +// Base implementation of a launch angle planner. +public abstract class LaunchAnglePlannerBase : ILaunchAnglePlanner { + public IAgent Agent { get; set; } + + public LaunchAnglePlannerBase(IAgent agent) { + Agent = agent; + } + + // Calculate the optimal launch angle in degrees and the time-to-target in seconds. + public abstract LaunchAngleOutput Plan(in LaunchAngleInput input); + public LaunchAngleOutput Plan(in Vector3 targetPosition) { + Vector2 direction = ConvertToRelativeDirection(targetPosition); + return Plan(new LaunchAngleInput { Distance = direction[0], Altitude = direction[1] }); + } + + // Return the absolute intercept position given the absolute target position. + public abstract Vector3 InterceptPosition(in Vector3 targetPosition); + + // Convert from a 3D vector to a 2D direction that ignores the azimuth and that is relative to the + // agent's position. + protected Vector2 ConvertToRelativeDirection(in Vector3 position) { + var relativePosition = position - Agent.Position; + return new Vector2(Vector3.ProjectOnPlane(relativePosition, Vector3.up).magnitude, + Vector3.Project(relativePosition, Vector3.up).magnitude); + } +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs.meta new file mode 100644 index 000000000..c5f2a28e4 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e36db218a19d241eeac7f7ffde372b6c \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs index bad2a7d2b..45da8f2c6 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs @@ -8,17 +8,18 @@ public struct LaunchPlan { // Launch angle in degrees measured from the horizon. public float LaunchAngle { get; set; } - // Intercept position relative to the launcher's position. - public Vector3 RelativeInterceptPosition { get; set; } + // Absolute intercept position. + public Vector3 InterceptPosition { get; set; } // No-launch launch plan. public static LaunchPlan NoLaunch() { return new LaunchPlan { ShouldLaunch = false }; } - // Get the normalized launch vector. - public Vector3 NormalizedLaunchVector() { - var interceptDirection = Coordinates3.ConvertCartesianToSpherical(RelativeInterceptPosition); + // Get the normalized launch vector given the agent's position. + public Vector3 NormalizedLaunchVector(in Vector3 position) { + var relativeInterceptPosition = InterceptPosition - position; + var interceptDirection = Coordinates3.ConvertCartesianToSpherical(relativeInterceptPosition); return Coordinates3.ConvertSphericalToCartesian(r: 1, azimuth: interceptDirection[1], elevation: LaunchAngle); } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs new file mode 100644 index 000000000..d78a640b9 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs @@ -0,0 +1,14 @@ +// Base implementation of a launch planner. +public abstract class LaunchPlannerBase : ILaunchPlanner { + public ILaunchAnglePlanner LaunchAnglePlanner { get; set; } + public IPredictor Predictor { get; set; } + + public LaunchPlannerBase(ILaunchAnglePlanner launchAnglePlanner, IPredictor predictor) { + LaunchAnglePlanner = launchAnglePlanner; + Predictor = predictor; + } + + // Plan the launch by finding the convergence point between the launch angle planner and the + // predictor. + public abstract LaunchPlan Plan(); +} diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs.meta b/Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs.meta new file mode 100644 index 000000000..6cf5768a5 --- /dev/null +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlannerBase.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: f0c122ed36ae7426687b63c5a8c2c1e4 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs index de12bf7d1..183a8ad53 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlannerLegacy.cs @@ -4,12 +4,13 @@ // intercept a target. public abstract class ILaunchPlannerLegacy { // Launch angle planner. - protected ILaunchAnglePlanner _launchAnglePlanner; + protected ILaunchAnglePlannerLegacy _launchAnglePlanner; // Agent trajectory predictor. protected IPredictorLegacy _predictor; - public ILaunchPlannerLegacy(ILaunchAnglePlanner launchAnglePlanner, IPredictorLegacy predictor) { + public ILaunchPlannerLegacy(ILaunchAnglePlannerLegacy launchAnglePlanner, + IPredictorLegacy predictor) { _launchAnglePlanner = launchAnglePlanner; _predictor = predictor; } diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs b/Assets/Scripts/Hierarchical/FixedHierarchical.cs index cebb575ff..bd8c6b181 100644 --- a/Assets/Scripts/Hierarchical/FixedHierarchical.cs +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs @@ -3,25 +3,26 @@ // The fixed hierarchical object represents a hierarchical object with a fixed position, velocity, // and acceleration and should primarily be used for testing. public class FixedHierarchical : HierarchicalBase { - private Vector3 _position; - private Vector3 _velocity; - private Vector3 _acceleration; + public new Vector3 Position { get; set; } + public new Vector3 Velocity { get; set; } + public new Vector3 Acceleration { get; set; } + public FixedHierarchical() : this(position: Vector3.zero, velocity: Vector3.zero) {} public FixedHierarchical(in Vector3 position, in Vector3 velocity) : this(position, velocity, acceleration: Vector3.zero) {} public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { - _position = position; - _velocity = velocity; - _acceleration = acceleration; + Position = position; + Velocity = velocity; + Acceleration = acceleration; } protected override Vector3 GetPosition() { - return _position; + return Position; } protected override Vector3 GetVelocity() { - return _velocity; + return Velocity; } protected override Vector3 GetAcceleration() { - return _acceleration; + return Acceleration; } } diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index 6ea19c67e..8df9df70d 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -14,8 +14,8 @@ public class IADS : MonoBehaviour { 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 ILaunchAnglePlannerLegacy _launchAnglePlanner = + new LaunchAngleCsvInterpolatorLegacy(Path.Combine("Planning", "hydra70_launch_angle.csv")); private IAssignmentLegacy _assignmentScheme = new MaxSpeedAssignment(); private Coroutine _launchInterceptorsCoroutine; @@ -119,9 +119,9 @@ private void CheckAndLaunchInterceptors() { // Check whether an interceptor should be launched. if (plan.ShouldLaunch) { Debug.Log( - $"Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.RelativeInterceptPosition}."); + $"Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.InterceptPosition}."); UIManager.Instance.LogActionMessage( - $"[IADS] Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.RelativeInterceptPosition}."); + $"[IADS] Launching a carrier interceptor at an elevation of {plan.LaunchAngle} degrees to position {plan.InterceptPosition}."); // Create a new interceptor. Configs.AgentConfig config = @@ -134,7 +134,8 @@ private void CheckAndLaunchInterceptors() { initialState.Position = Coordinates3.ToProto(Vector3.zero); // Set the initial velocity to point along the launch vector. - initialState.Velocity = Coordinates3.ToProto(plan.NormalizedLaunchVector() * 1e-3f); + initialState.Velocity = + Coordinates3.ToProto(plan.NormalizedLaunchVector(position: Vector3.zero) * 1e-3f); Interceptor interceptor = SimManager.Instance.CreateInterceptor(config, initialState); // Assign the interceptor to the cluster. diff --git a/Assets/Scripts/Utils/KDTree.cs b/Assets/Scripts/Utils/KDTree.cs index 415159a42..afd013c13 100644 --- a/Assets/Scripts/Utils/KDTree.cs +++ b/Assets/Scripts/Utils/KDTree.cs @@ -52,7 +52,7 @@ private KDNode NearestNeighbor(KDNode node, in Vector2 target, int depth, var targetCoordinatesValue = axis == 0 ? target.x : target.y; var nodeCoordinatesValue = axis == 0 ? nodeCoordinates.x : nodeCoordinates.y; var nextBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Left : node.Right; - var otherBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Right : node.Right; + var otherBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Right : node.Left; // Explore the next branch first. bestNode = NearestNeighbor(nextBranch, target, depth + 1, bestNode); diff --git a/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs b/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs index 9c9fe023d..fefcce187 100644 --- a/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs +++ b/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs @@ -1,97 +1,102 @@ using NUnit.Framework; -using System.Collections; using System.Collections.Generic; -using System.Linq; using UnityEngine; -using UnityEngine.TestTools; -public class IterativeLaunchPlannerTests { - private class DummyLaunchAngleDataInterpolator : LaunchAngleDataInterpolator { - public DummyLaunchAngleDataInterpolator() : base() {} +public class IterativeLaunchPlannerTests : TestBase { + private class TestLaunchAngleDataInterpolator : LaunchAngleDataInterpolatorBase { + public TestLaunchAngleDataInterpolator(IAgent agent) : base(agent) {} - // Generate the list of launch angle data points to interpolate. - protected override List GenerateData() { + // Generate the launch angle data points to interpolate. + protected override IEnumerable GenerateData() { return new List { - new LaunchAngleDataPoint(new LaunchAngleInput(distance: 1, altitude: 100), - new LaunchAngleOutput(launchAngle: 90, timeToPosition: 10)), - new LaunchAngleDataPoint(new LaunchAngleInput(distance: 60, altitude: 1), - new LaunchAngleOutput(launchAngle: 20, timeToPosition: 13)), - new LaunchAngleDataPoint(new LaunchAngleInput(distance: 80, altitude: 1), - new LaunchAngleOutput(launchAngle: 15, timeToPosition: 16)), - new LaunchAngleDataPoint(new LaunchAngleInput(distance: 100, altitude: 1), - new LaunchAngleOutput(launchAngle: 10, timeToPosition: 20)), + new LaunchAngleDataPoint { + Input = new LaunchAngleInput { Distance = 1, Altitude = 100 }, + Output = new LaunchAngleOutput { LaunchAngle = 90, TimeToPosition = 10 }, + }, + new LaunchAngleDataPoint { + Input = new LaunchAngleInput { Distance = 60, Altitude = 1 }, + Output = new LaunchAngleOutput { LaunchAngle = 20, TimeToPosition = 13 }, + }, + new LaunchAngleDataPoint { + Input = new LaunchAngleInput { Distance = 80, Altitude = 1 }, + Output = new LaunchAngleOutput { LaunchAngle = 15, TimeToPosition = 16 }, + }, + new LaunchAngleDataPoint { + Input = new LaunchAngleInput { Distance = 100, Altitude = 1 }, + Output = new LaunchAngleOutput { LaunchAngle = 10, TimeToPosition = 20 }, + }, }; } } - private static ILaunchAnglePlanner _launchAnglePlanner = new DummyLaunchAngleDataInterpolator(); + private AgentBase _agent; + private FixedHierarchical _target; + private ILaunchAnglePlanner _launchAnglePlanner; + private IPredictor _predictor; + private IterativeLaunchPlanner _planner; - public static Agent GenerateAgent(in Vector3 position, in Vector3 velocity) { - Agent agent = new GameObject().AddComponent(); - Rigidbody rb = agent.gameObject.AddComponent(); - agent.transform.position = position; - rb.linearVelocity = velocity; - return agent; + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _target = new FixedHierarchical(); + _launchAnglePlanner = new TestLaunchAngleDataInterpolator(_agent); + _predictor = new LinearExtrapolator(_target); + _planner = new IterativeLaunchPlanner(_launchAnglePlanner, _predictor); } [Test] - public void TestInterceptAtDataPoint() { - Agent agent = GenerateAgent(position: new Vector3(1, 110, 0), velocity: new Vector3(0, -1, 0)); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - IterativeLaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); - LaunchPlan plan = planner.Plan(); + public void Plan_InterceptAtDataPoint_ReturnsLaunch() { + _target.Position = new Vector3(1, 110, 0); + _target.Velocity = new Vector3(0, -1, 0); + var plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); Assert.AreEqual(90, plan.LaunchAngle); Assert.AreEqual(new Vector3(1, 100, 0), plan.InterceptPosition); } [Test] - public void TestInterceptAroundDataPoint() { - Agent agent = - GenerateAgent(position: new Vector3(1, 110, 0), velocity: new Vector3(0, -1.1f, 0)); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - IterativeLaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); - LaunchPlan plan = planner.Plan(); + public void Plan_InterceptNearDataPoint_ReturnsLaunch() { + _target.Position = new Vector3(1, 110, 0); + _target.Velocity = new Vector3(0, -1.1f, 0); + var plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); Assert.AreEqual(90, plan.LaunchAngle); Assert.AreEqual(new Vector3(1, 99, 0), plan.InterceptPosition); } [Test] - public void TestInterceptAcrossMultipleDataPoints() { - Agent agent = GenerateAgent(position: new Vector3(126, 1, 0), velocity: new Vector3(-5, 0, 0)); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - IterativeLaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); - LaunchPlan plan = planner.Plan(); + public void Plan_InterceptBetweenDataPoints_ReturnsLaunch() { + _target.Position = new Vector3(126, 1, 0); + _target.Velocity = new Vector3(-5, 0, 0); + var plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); Assert.AreEqual(20, plan.LaunchAngle); Assert.AreEqual(new Vector3(61, 1, 0), plan.InterceptPosition); } [Test] - public void TestLaunchDivergingFromOrigin() { - Agent agent = GenerateAgent(position: new Vector3(0, 1, -80), velocity: new Vector3(0, 0, -1)); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - IterativeLaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); - LaunchPlan plan = planner.Plan(); + public void Plan_DivergingFromOrigin_ReturnsNoLaunch() { + _target.Position = new Vector3(0, 1, -80); + _target.Velocity = new Vector3(0, 0, -1); + var plan = _planner.Plan(); Assert.IsFalse(plan.ShouldLaunch); } [Test] - public void TestNoLaunchDivergingFromInterceptPoint() { - Agent agent = GenerateAgent(position: new Vector3(1, 105, 0), velocity: new Vector3(0, 1, 0)); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - IterativeLaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); - LaunchPlan plan = planner.Plan(); + public void Plan_DivergingFromInterceptPoint_ReturnsNoLaunch() { + _target.Position = new Vector3(1, 105, 0); + _target.Velocity = new Vector3(0, 1, 0); + var plan = _planner.Plan(); Assert.IsFalse(plan.ShouldLaunch); } [Test] - public void TestNoLaunchTooFarFromInterceptPoint() { - Agent agent = GenerateAgent(position: new Vector3(1, 2000, 0), velocity: new Vector3(0, -1, 0)); - LinearExtrapolator predictor = new LinearExtrapolator(agent); - IterativeLaunchPlanner planner = new IterativeLaunchPlanner(_launchAnglePlanner, predictor); - LaunchPlan plan = planner.Plan(); + public void Plan_TooFarFromInterceptPoint_ReturnsNoLaunch() { + _target.Position = new Vector3(1, 2000, 0); + _target.Velocity = new Vector3(0, -1, 0); + var plan = _planner.Plan(); Assert.IsFalse(plan.ShouldLaunch); } } diff --git a/Assets/Tests/EditMode/KDTreeTests.cs b/Assets/Tests/EditMode/KDTreeTests.cs index 9d9477852..78e54e76c 100644 --- a/Assets/Tests/EditMode/KDTreeTests.cs +++ b/Assets/Tests/EditMode/KDTreeTests.cs @@ -1,23 +1,24 @@ using NUnit.Framework; using System.Collections.Generic; using UnityEngine; +using UnityEngine.TestTools.Utils; public class KDTreeTests { [Test] public void NearestNeighbor_EmptyTree_ReturnsZero() { var tree = new KDTree(new List(), (Vector2 point) => point); - Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), Vector2.zero); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(1, 1)), Vector2.zero); + Assert.AreEqual(Vector2.zero, tree.NearestNeighbor(Vector2.zero)); + Assert.AreEqual(Vector2.zero, tree.NearestNeighbor(new Vector2(1, 1))); } [Test] public void NearestNeighbor_SinglePoint_ReturnsSinglePoint() { var tree = new KDTree(new List { new Vector2(1, 1) }, (Vector2 point) => point); - Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), new Vector2(1, 1)); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(1, 6)), new Vector2(1, 1)); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(3, 2)), new Vector2(1, 1)); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(9, 9)), new Vector2(1, 1)); + Assert.AreEqual(new Vector2(1, 1), tree.NearestNeighbor(Vector2.zero)); + Assert.AreEqual(new Vector2(1, 1), tree.NearestNeighbor(new Vector2(1, 6))); + Assert.AreEqual(new Vector2(1, 1), tree.NearestNeighbor(new Vector2(3, 2))); + Assert.AreEqual(new Vector2(1, 1), tree.NearestNeighbor(new Vector2(9, 9))); } [Test] @@ -30,9 +31,9 @@ public void NearestNeighbor_MatchesDataPoint_ReturnsSamePoint() { } var tree = new KDTree(points, (Vector2 point) => point); Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), Vector2.zero); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(1, 6)), new Vector2(1, 6)); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(3, 2)), new Vector2(3, 2)); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(9, 9)), new Vector2(9, 9)); + Assert.AreEqual(new Vector2(1, 6), tree.NearestNeighbor(new Vector2(1, 6))); + Assert.AreEqual(new Vector2(3, 2), tree.NearestNeighbor(new Vector2(3, 2))); + Assert.AreEqual(new Vector2(9, 9), tree.NearestNeighbor(new Vector2(9, 9))); } [Test] @@ -44,9 +45,13 @@ public void NearestNeighbor_NearDataPoint_ReturnsClosestPoint() { } } var tree = new KDTree(points, (Vector2 point) => point); - Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), Vector2.zero); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(1.4f, 1.2f)), new Vector2(1, 1)); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(2.7f, 2.2f)), new Vector2(3, 2)); - Assert.AreEqual(tree.NearestNeighbor(new Vector2(9.05f, 8.61f)), new Vector2(9, 9)); + Assert.That(tree.NearestNeighbor(Vector2.zero), + Is.EqualTo(Vector2.zero).Using(Vector2EqualityComparer.Instance)); + Assert.That(tree.NearestNeighbor(new Vector2(1.4f, 1.2f)), + Is.EqualTo(new Vector2(1, 1)).Using(Vector2EqualityComparer.Instance)); + Assert.That(tree.NearestNeighbor(new Vector2(2.7f, 2.2f)), + Is.EqualTo(new Vector2(3, 2)).Using(Vector2EqualityComparer.Instance)); + Assert.That(tree.NearestNeighbor(new Vector2(9.05f, 8.61f)), + Is.EqualTo(new Vector2(9, 9)).Using(Vector2EqualityComparer.Instance)); } } diff --git a/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs b/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs new file mode 100644 index 000000000..9de94a63c --- /dev/null +++ b/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs @@ -0,0 +1,93 @@ +using NUnit.Framework; +using System; +using System.Collections.Generic; +using System.IO; +using UnityEngine; +using UnityEngine.TestTools; +using UnityEngine.TestTools.Utils; + +public class LaunchAngleCsvInterpolatorTests : TestBase { + private const float _epsilon = 0.01f; + + private AgentBase _agent; + private LaunchAngleCsvInterpolator _interpolator; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _interpolator = new LaunchAngleCsvInterpolator( + _agent, path: Path.Combine("Planning", "hydra70_launch_angle.csv"), + configLoader: ConfigLoader.LoadFromStreamingAssets); + } + + [Test] + public void Plan_MatchesDataPoint_ReturnsSamePoint() { + var input = new LaunchAngleInput { Distance = 9074.84f, Altitude = 97.7306f }; + var expectedOutput = new LaunchAngleOutput { LaunchAngle = 22f, TimeToPosition = 21.32f }; + Assert.AreEqual(expectedOutput, _interpolator.Plan(input)); + } + + [Test] + public void Plan_NearDataPoint_ReturnsClosestPoint() { + var input = new LaunchAngleInput { Distance = 9076f, Altitude = 94f }; + var expectedOutput = new LaunchAngleOutput { LaunchAngle = 22f, TimeToPosition = 21.32f }; + Assert.AreEqual(expectedOutput, _interpolator.Plan(input)); + } + + [Test] + public void Plan_FileNotFound_ThrowsException() { + LogAssert.ignoreFailingMessages = true; + + // Inject a mock ConfigLoader that returns an empty string. + LaunchAngleCsvInterpolator.ConfigLoaderDelegate mockLoader = (string path) => ""; + Assert.Throws(() => { + var interpolator = new LaunchAngleCsvInterpolator(_agent, path: "Planning/nonexistent.csv", + configLoader: mockLoader); + interpolator.Plan(new LaunchAngleInput()); + }); + } + + [Test] + public void Plan_InvalidInterpolationData_ThrowsException() { + LogAssert.ignoreFailingMessages = true; + + // Create a mock CSV with invalid data format. + string mockCsv = "invalid,csv,data\n1,2,3"; + LaunchAngleCsvInterpolator.ConfigLoaderDelegate mockLoader = (string path) => mockCsv; + + var interpolator = new LaunchAngleCsvInterpolator(_agent, path: null, configLoader: mockLoader); + var input = new LaunchAngleInput { Distance = 1, Altitude = 2 }; + Assert.Throws(() => { interpolator.Plan(input); }); + } + + [Test] + public void InterceptPosition_MatchesDataPoint_ReturnsSamePoint() { + var targetPosition = new Vector3(9074.84f, 97.7306f, 0f); + _interpolator.Plan(new LaunchAngleInput()); + Assert.That(_interpolator.InterceptPosition(targetPosition), + Is.EqualTo(new Vector3(9074.84f, 97.7306f, 0f)) + .Using(new Vector3EqualityComparer(_epsilon))); + } + + [Test] + public void InterceptPosition_NearDataPoint_ReturnsClosestPoint() { + var targetPosition = new Vector3(9078f, 93f, 0f); + _interpolator.Plan(new LaunchAngleInput()); + Assert.That(_interpolator.InterceptPosition(targetPosition), + Is.EqualTo(new Vector3(9074.84f, 97.7306f, 0f)) + .Using(new Vector3EqualityComparer(_epsilon))); + } + + [Test] + public void InterceptPosition_MovesWithAgentPosition() { + var agentPosition = new Vector3(61.1f, 5055.5f, 874.9f); + _agent.Position = agentPosition; + var targetPosition = agentPosition + new Vector3(9074.84f, 97.7306f, 0f); + _interpolator.Plan(new LaunchAngleInput()); + Assert.That(_interpolator.InterceptPosition(targetPosition), + Is.EqualTo(agentPosition + new Vector3(9074.84f, 97.7306f, 0f)) + .Using(new Vector3EqualityComparer(_epsilon))); + } +} diff --git a/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs.meta b/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs.meta new file mode 100644 index 000000000..005d40f06 --- /dev/null +++ b/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 1604d9137bf704495a7152ea5345706c \ No newline at end of file diff --git a/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs new file mode 100644 index 000000000..94c3e4384 --- /dev/null +++ b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs @@ -0,0 +1,76 @@ +using NUnit.Framework; +using System.Collections.Generic; +using UnityEngine; +using UnityEngine.TestTools.Utils; + +public class LaunchAngleDataInterpolatorBaseTests : TestBase { + private class TestLaunchAngleDataInterpolator : LaunchAngleDataInterpolatorBase { + public TestLaunchAngleDataInterpolator(IAgent agent) : base(agent) {} + + // Generate the launch angle data points to interpolate. + protected override IEnumerable GenerateData() { + return new List { + new LaunchAngleDataPoint { + Input = new LaunchAngleInput { Distance = 1, Altitude = 100 }, + Output = new LaunchAngleOutput { LaunchAngle = 90, TimeToPosition = 10 }, + }, + new LaunchAngleDataPoint { + Input = new LaunchAngleInput { Distance = 100, Altitude = 1 }, + Output = new LaunchAngleOutput { LaunchAngle = 10, TimeToPosition = 20 }, + }, + }; + } + } + + private AgentBase _agent; + private TestLaunchAngleDataInterpolator _interpolator; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + Rigidbody agentRb = _agent.gameObject.AddComponent(); + InvokePrivateMethod(_agent, "Awake"); + _interpolator = new TestLaunchAngleDataInterpolator(_agent); + } + + [Test] + public void Plan_MatchesDataPoint_ReturnsSamePoint() { + var input = new LaunchAngleInput { Distance = 1, Altitude = 100 }; + var expectedOutput = new LaunchAngleOutput { LaunchAngle = 90, TimeToPosition = 10 }; + Assert.AreEqual(expectedOutput, _interpolator.Plan(input)); + } + + [Test] + public void Plan_NearDataPoint_ReturnsClosestPoint() { + var input = new LaunchAngleInput { Distance = 1, Altitude = 200 }; + var expectedOutput = new LaunchAngleOutput { LaunchAngle = 90, TimeToPosition = 10 }; + Assert.AreEqual(expectedOutput, _interpolator.Plan(input)); + } + + [Test] + public void InterceptPosition_MatchesDataPoint_ReturnsSamePoint() { + var targetPosition = new Vector3(100f, 1f, 0f); + _interpolator.Plan(new LaunchAngleInput()); + Assert.That(_interpolator.InterceptPosition(targetPosition), + Is.EqualTo(new Vector3(100f, 1f, 0f)).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void InterceptPosition_NearDataPoint_ReturnsClosestPoint() { + var targetPosition = new Vector3(101f, 2f, 0f); + _interpolator.Plan(new LaunchAngleInput()); + Assert.That(_interpolator.InterceptPosition(targetPosition), + Is.EqualTo(new Vector3(100f, 1f, 0f)).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void InterceptPosition_MovesWithAgentPosition() { + var agentPosition = new Vector3(61, 5055, 874); + _agent.Position = agentPosition; + var targetPosition = agentPosition + new Vector3(100f, 1f, 0f); + _interpolator.Plan(new LaunchAngleInput()); + Assert.That(_interpolator.InterceptPosition(targetPosition), + Is.EqualTo(agentPosition + new Vector3(100f, 1f, 0f)) + .Using(Vector3EqualityComparer.Instance)); + } +} diff --git a/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs.meta b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs.meta new file mode 100644 index 000000000..dd9239e1b --- /dev/null +++ b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e68af3c4e16684ffc882b7512fcd1fd9 \ No newline at end of file diff --git a/Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs b/Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs deleted file mode 100644 index 189c618f1..000000000 --- a/Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs +++ /dev/null @@ -1,87 +0,0 @@ -using NUnit.Framework; -using System.Collections; -using System.Collections.Generic; -using UnityEngine; -using UnityEngine.TestTools; -using System; - -public class LaunchAngleCsvInterpolatorTests { - [Test] - public void TestDataPoint() { - LaunchAngleInput input = new LaunchAngleInput(distance: 9074.84f, altitude: 97.7306f); - LaunchAngleOutput expectedOutput = - new LaunchAngleOutput(launchAngle: 22f, timeToPosition: 21.32f); - - LaunchAngleCsvInterpolator interpolator = new LaunchAngleCsvInterpolator(); - Assert.AreEqual(expectedOutput, interpolator.Plan(input)); - } - - [Test] - public void TestNearestNeighbor() { - LaunchAngleInput input = new LaunchAngleInput(distance: 9076f, altitude: 94f); - LaunchAngleOutput expectedOutput = - new LaunchAngleOutput(launchAngle: 22f, timeToPosition: 21.32f); - - LaunchAngleCsvInterpolator interpolator = new LaunchAngleCsvInterpolator(); - Assert.AreEqual(expectedOutput, interpolator.Plan(input)); - } - - [Test] - public void TestFileNotFound() { - // Inject a mock ConfigLoader that returns an empty string. - LaunchAngleCsvInterpolator.ConfigLoaderDelegate mockLoader = (string path) => ""; - - LogAssert.Expect(LogType.Error, "Failed to load CSV file from Planning/nonexistent.csv."); - Assert.Throws(() => { - var interpolator = new LaunchAngleCsvInterpolator("Planning/nonexistent.csv", mockLoader); - interpolator.Plan(new LaunchAngleInput()); - }); - } - - [Test] - public void TestInvalidInterpolationData() { - // Create a mock CSV with invalid data format. - string mockCsv = "invalid,csv,data\n1,2,3"; - LaunchAngleCsvInterpolator.ConfigLoaderDelegate mockLoader = (string path) => mockCsv; - - var interpolator = new LaunchAngleCsvInterpolator(null, mockLoader); - var input = new LaunchAngleInput(distance: 1, altitude: 2); - - var ex = Assert.Throws(() => { interpolator.Plan(input); }); - Assert.That(ex.Message, Is.EqualTo("Interpolator returned invalid data.")); - } -} - -public class LaunchAngleDataInterpolatorTests { - private class DummyLaunchAngleDataInterpolator : LaunchAngleDataInterpolator { - public DummyLaunchAngleDataInterpolator() : base() {} - - // Generate the list of launch angle data points to interpolate. - protected override List GenerateData() { - return new List { - new LaunchAngleDataPoint(new LaunchAngleInput(distance: 1, altitude: 100), - new LaunchAngleOutput(launchAngle: 90, timeToPosition: 10)), - new LaunchAngleDataPoint(new LaunchAngleInput(distance: 100, altitude: 1), - new LaunchAngleOutput(launchAngle: 10, timeToPosition: 20)), - }; - } - } - - [Test] - public void TestDataPoint() { - LaunchAngleInput input = new LaunchAngleInput(distance: 1, altitude: 100); - LaunchAngleOutput expectedOutput = new LaunchAngleOutput(launchAngle: 90, timeToPosition: 10); - - DummyLaunchAngleDataInterpolator interpolator = new DummyLaunchAngleDataInterpolator(); - Assert.AreEqual(expectedOutput, interpolator.Plan(input)); - } - - [Test] - public void TestNearestNeighbor() { - LaunchAngleInput input = new LaunchAngleInput(distance: 1, altitude: 200); - LaunchAngleOutput expectedOutput = new LaunchAngleOutput(launchAngle: 90, timeToPosition: 10); - - DummyLaunchAngleDataInterpolator interpolator = new DummyLaunchAngleDataInterpolator(); - Assert.AreEqual(expectedOutput, interpolator.Plan(input)); - } -} diff --git a/Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs.meta b/Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs.meta deleted file mode 100644 index cbe4267c2..000000000 --- a/Assets/Tests/EditMode/LaunchAngleInterpolatorTests.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: fd8efcbfd3ac34a6d8b5a70c88a7e757 \ No newline at end of file diff --git a/Assets/Tests/EditMode/LaunchPlanTests.cs b/Assets/Tests/EditMode/LaunchPlanTests.cs index a9db0db03..394db614b 100644 --- a/Assets/Tests/EditMode/LaunchPlanTests.cs +++ b/Assets/Tests/EditMode/LaunchPlanTests.cs @@ -16,9 +16,9 @@ public void NormalizedLaunchVector_IsNormalized() { var launchPlan = new LaunchPlan { ShouldLaunch = true, LaunchAngle = 45f, - RelativeInterceptPosition = new Vector3(1, 1, 0), + InterceptPosition = new Vector3(1, 1, 0), }; - var launchVector = launchPlan.NormalizedLaunchVector(); + var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.AreEqual(1f, launchVector.magnitude, _epsilon); } @@ -27,9 +27,9 @@ public void NormalizedLaunchVector_PointsVertically() { var launchPlan = new LaunchPlan { ShouldLaunch = true, LaunchAngle = 90f, - RelativeInterceptPosition = new Vector3(1, 1, 0), + InterceptPosition = new Vector3(1, 1, 0), }; - var launchVector = launchPlan.NormalizedLaunchVector(); + var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.That(launchVector, Is.EqualTo(new Vector3(0, 1f, 0)).Using(Vector3EqualityComparer.Instance)); } @@ -39,9 +39,9 @@ public void NormalizedLaunchVector_PointsHorizontally() { var launchPlan = new LaunchPlan { ShouldLaunch = true, LaunchAngle = 0f, - RelativeInterceptPosition = new Vector3(1, 1, 0), + InterceptPosition = new Vector3(1, 1, 0), }; - var launchVector = launchPlan.NormalizedLaunchVector(); + var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.That(launchVector, Is.EqualTo(new Vector3(1f, 0, 0)).Using(Vector3EqualityComparer.Instance)); } @@ -51,9 +51,9 @@ public void NormalizedLaunchVector_PointsDiagonally() { var launchPlan = new LaunchPlan { ShouldLaunch = true, LaunchAngle = 45f, - RelativeInterceptPosition = new Vector3(1, 0, 1), + InterceptPosition = new Vector3(1, 0, 1), }; - var launchVector = launchPlan.NormalizedLaunchVector(); + var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.That(launchVector, Is.EqualTo(new Vector3(1, Mathf.Sqrt(2f), 1).normalized) .Using(Vector3EqualityComparer.Instance)); } diff --git a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs index 28a4f051c..651c47034 100644 --- a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs +++ b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs @@ -1,12 +1,16 @@ using NUnit.Framework; using System.Collections.Generic; +using System.Linq; using UnityEngine; +using UnityEngine.TestTools; +using UnityEngine.TestTools.Utils; public class NearestNeighborInterpolator2DTests { private const float _epsilon = 1e-3f; [Test] public void Interpolate_InvalidPoints_ReturnsEmptyResult() { + LogAssert.ignoreFailingMessages = true; string[] csvLines = { "", // Empty. " , ", // Whitespace. @@ -24,7 +28,9 @@ public void Interpolate_SinglePoint_ReturnsSinglePoint() { var interpolator = new NearestNeighborInterpolator2D(csvLines); var result = interpolator.Interpolate(100, 200); Assert.AreEqual(new Vector2(0, 0), result.Coordinates); - Assert.AreEqual(new List { 45f, 2f }, result.Data, _epsilon); + Assert.AreEqual(2, result.Data.Count); + Assert.AreEqual(45f, result.Data[0], _epsilon); + Assert.AreEqual(2f, result.Data[1], _epsilon); } [Test] @@ -33,7 +39,8 @@ public void Interpolate_InsufficientColumns_ReturnsEmptyResult() { var interpolator = new NearestNeighborInterpolator2D(csvLines); var result = interpolator.Interpolate(100f, 200f); Assert.NotNull(result); - Assert.AreEqual(new Vector2(100f, 200f), result.Coordinates, _epsilon); + Assert.That(result.Coordinates, + Is.EqualTo(new Vector2(100f, 200f)).Using(Vector2EqualityComparer.Instance)); Assert.AreEqual(0, result.Data.Count); } @@ -43,7 +50,9 @@ public void Interpolate_QueryOutOfRange_ReturnsClosestPoint() { var interpolator = new NearestNeighborInterpolator2D(csvLines); var result = interpolator.Interpolate(1000, 1000); Assert.AreEqual(new Vector2(20, 5), result.Coordinates); - Assert.AreEqual(new List { 25f, 4f }, result.Data, _epsilon); + Assert.AreEqual(2, result.Data.Count); + Assert.AreEqual(25f, result.Data[0], _epsilon); + Assert.AreEqual(4f, result.Data[1], _epsilon); } [Test] @@ -53,14 +62,16 @@ public void Interpolate_BetweenDataPoints_ReturnsOnePoint() { var testPoint = new Vector2(1.5f, 1.5f); var result = interpolator.Interpolate(testPoint); Assert.AreEqual(2, result.Data.Count); - var isValidPair = - (result.Data == new Vector2(10f, 20f) || result.Data == new Vector2(30f, 40f) || - result.Data == new Vector2(50f, 60f) || result.Data == new Vector2(70f, 80f)); + var validData = + new List> { new List { 10f, 20f }, new List { 30f, 40f }, + new List { 50f, 60f }, new List { 70f, 80f } }; + var isValidPair = validData.Any(v => v.SequenceEqual(result.Data)); Assert.IsTrue(isValidPair); } [Test] public void Interpolate_NoDataPoints_ReturnsEmptyResult() { + LogAssert.ignoreFailingMessages = true; string[] csvLines = {}; var interpolator = new NearestNeighborInterpolator2D(csvLines); Interpolator2DDataPoint result = interpolator.Interpolate(new Vector2(1f, 1f)); diff --git a/Assets/Tests/EditMode/PriorityQueueTests.cs b/Assets/Tests/EditMode/PriorityQueueTests.cs index 166df5d02..e514839d8 100644 --- a/Assets/Tests/EditMode/PriorityQueueTests.cs +++ b/Assets/Tests/EditMode/PriorityQueueTests.cs @@ -1,4 +1,5 @@ using NUnit.Framework; +using System; using System.Collections.Generic; public class PriorityQueueTests { From b69ca74c6d085e65c317944a9e2c810bf8664fb2 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 16 Oct 2025 18:09:30 -0700 Subject: [PATCH 47/74] Address comments --- .../Algorithms/Planning/LaunchAngleCsvInterpolator.cs | 2 +- .../Algorithms/Planning/LaunchAngleInterpolatorBase.cs | 2 +- Assets/Scripts/Utils/KDTree.cs | 7 +++---- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs index c6e79e461..ecf3ed451 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs @@ -32,7 +32,7 @@ protected override void InitInterpolator() { throw new InvalidOperationException("Interpolator could not be initialized."); } - var csvLines = fileContent.Split('\n'); + var csvLines = fileContent.Split(new[] { "\r\n", "\r", "\n" }, StringSplitOptions.None); if (csvLines.Length < 1) { throw new InvalidOperationException("No data points available for interpolation."); } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs index f15a8cde5..9786b0648 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs @@ -2,7 +2,7 @@ using System.Collections.Generic; using UnityEngine; -// Base imple,entation of a launch angle interpolator. +// Base implementation of a launch angle interpolator. // // The interpolator determines the optimal launch angle and the time-to-target given the horizontal // distance and altitude of the target. diff --git a/Assets/Scripts/Utils/KDTree.cs b/Assets/Scripts/Utils/KDTree.cs index afd013c13..50a52aa72 100644 --- a/Assets/Scripts/Utils/KDTree.cs +++ b/Assets/Scripts/Utils/KDTree.cs @@ -74,10 +74,9 @@ private KDNode BuildTree(IReadOnlyList points, int depth) { var axis = depth % k; // Sort the points by axis and find the median. - var sortedPoints = points.Select(p => new { Point = p, Coordinates = _getCoordinates(p) }) - .OrderBy(p => axis == 0 ? p.Coordinates.x : p.Coordinates.y) - .Select(p => p.Point) - .ToList(); + var sortedPoints = + points.OrderBy(point => (axis == 0 ? _getCoordinates(point).x : _getCoordinates(point).y)) + .ToList(); var medianIndex = sortedPoints.Count / 2; var medianPoint = sortedPoints[medianIndex]; From 78b20e125fefb21c70e01b47523e8df37f90f887 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 16 Oct 2025 18:39:43 -0700 Subject: [PATCH 48/74] Address comments --- Assets/Scripts/Hierarchical/HierarchicalAgent.cs | 2 +- Assets/Scripts/Hierarchical/HierarchicalBase.cs | 2 +- Assets/Scripts/Sensors/SensorBase.cs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs index 7468cfc23..aacec9764 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalAgent.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalAgent.cs @@ -5,7 +5,7 @@ // Since agents cannot inherit from both HierarchicalBase and MonoBehaviour, agents use composition // to know their position within the hierarchical strategy. public class HierarchicalAgent : HierarchicalBase { - // Agent to which the hierarchical node belongs. + // Agent to which this hierarchical node belongs. public IAgent Agent { get; set; } protected override Vector3 GetPosition() { diff --git a/Assets/Scripts/Hierarchical/HierarchicalBase.cs b/Assets/Scripts/Hierarchical/HierarchicalBase.cs index 4934f196f..e53f95bf1 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalBase.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalBase.cs @@ -9,7 +9,7 @@ public class HierarchicalBase : IHierarchical { // List of hierarchical objects in the hierarchy level below. private List _subHierarchicals = new List(); - // List of hierarchical objects pursuing the hierarchical object. + // List of hierarchical objects pursuing this hierarchical object. private List _pursuers = new List(); public IReadOnlyList SubHierarchicals => _subHierarchicals.AsReadOnly(); diff --git a/Assets/Scripts/Sensors/SensorBase.cs b/Assets/Scripts/Sensors/SensorBase.cs index b2b8662d0..394328aac 100644 --- a/Assets/Scripts/Sensors/SensorBase.cs +++ b/Assets/Scripts/Sensors/SensorBase.cs @@ -2,7 +2,7 @@ // Base implementation of a sensor. public abstract class SensorBase : ISensor { - // Agent to which the sensor belongs. + // Agent to which this sensor belongs. public IAgent Agent { get; set; } public SensorBase(IAgent agent) { From 0097541c0e0ce8896530d476e2499981ecc154d6 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 16 Oct 2025 23:31:56 -0700 Subject: [PATCH 49/74] Add Legacy suffix to clusterer class names --- ...rer.cs => AgglomerativeClustererLegacy.cs} | 6 +++--- ...a => AgglomerativeClustererLegacy.cs.meta} | 0 ...sClusterer.cs => KMeansClustererLegacy.cs} | 15 +++++++------- ....cs.meta => KMeansClustererLegacy.cs.meta} | 0 Assets/Scripts/IADS/IADS.cs | 2 +- .../EditMode/AgglomerativeClustererTests.cs | 14 ++++++------- Assets/Tests/EditMode/KMeansClustererTests.cs | 20 +++++++++---------- 7 files changed, 28 insertions(+), 29 deletions(-) rename Assets/Scripts/Algorithms/Clustering/{AgglomerativeClusterer.cs => AgglomerativeClustererLegacy.cs} (94%) rename Assets/Scripts/Algorithms/Clustering/{AgglomerativeClusterer.cs.meta => AgglomerativeClustererLegacy.cs.meta} (100%) rename Assets/Scripts/Algorithms/Clustering/{KMeansClusterer.cs => KMeansClustererLegacy.cs} (86%) rename Assets/Scripts/Algorithms/Clustering/{KMeansClusterer.cs.meta => KMeansClustererLegacy.cs.meta} (100%) diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClustererLegacy.cs similarity index 94% rename from Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs rename to Assets/Scripts/Algorithms/Clustering/AgglomerativeClustererLegacy.cs index cc70295c4..17cf30e8f 100644 --- a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClustererLegacy.cs @@ -4,10 +4,10 @@ // The agglomerative clusterer class performs agglomerative clustering with the stopping condition // given by the size and radius constraints. -public class AgglomerativeClusterer : ISizeAndRadiusConstrainedClusterer { - public AgglomerativeClusterer(List objects, int maxSize, float maxRadius) +public class AgglomerativeClustererLegacy : ISizeAndRadiusConstrainedClusterer { + public AgglomerativeClustererLegacy(List objects, int maxSize, float maxRadius) : base(objects, maxSize, maxRadius) {} - public AgglomerativeClusterer(List agents, int maxSize, float maxRadius) + public AgglomerativeClustererLegacy(List agents, int maxSize, float maxRadius) : base(agents, maxSize, maxRadius) {} // Cluster the game objects. diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs.meta b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClustererLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs.meta rename to Assets/Scripts/Algorithms/Clustering/AgglomerativeClustererLegacy.cs.meta diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/KMeansClustererLegacy.cs similarity index 86% rename from Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs rename to Assets/Scripts/Algorithms/Clustering/KMeansClustererLegacy.cs index 9f6ad0ff6..ee69dd9a8 100644 --- a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/KMeansClustererLegacy.cs @@ -4,7 +4,7 @@ using UnityEngine; // The k-means clusterer class performs k-means clustering. -public class KMeansClusterer : IClustererLegacy { +public class KMeansClustererLegacy : IClustererLegacy { public const float Epsilon = 1e-3f; // Number of clusters. @@ -13,11 +13,12 @@ public class KMeansClusterer : IClustererLegacy { // Maximum number of iterations. private int _maxIterations = 20; - public KMeansClusterer(List objects, int k, int maxIterations = 20) : base(objects) { + public KMeansClustererLegacy(List objects, int k, int maxIterations = 20) + : base(objects) { _k = k; _maxIterations = maxIterations; } - public KMeansClusterer(List agents, int k, int maxIterations = 20) : base(agents) { + public KMeansClustererLegacy(List agents, int k, int maxIterations = 20) : base(agents) { _k = k; _maxIterations = maxIterations; } @@ -86,16 +87,16 @@ private void AssignObjectsToCluster() { // The constrained k-means clusterer class performs k-means clustering under size and radius // constraints. -public class ConstrainedKMeansClusterer : ISizeAndRadiusConstrainedClusterer { - public ConstrainedKMeansClusterer(List objects, int maxSize, float maxRadius) +public class ConstrainedKMeansClustererLegacy : ISizeAndRadiusConstrainedClusterer { + public ConstrainedKMeansClustererLegacy(List objects, int maxSize, float maxRadius) : base(objects, maxSize, maxRadius) {} // Cluster the game objects. public override void Cluster() { int numClusters = (int)Mathf.Ceil(_objects.Count / _maxSize); - KMeansClusterer clusterer; + KMeansClustererLegacy clusterer; while (true) { - clusterer = new KMeansClusterer(_objects, numClusters); + clusterer = new KMeansClustererLegacy(_objects, numClusters); clusterer.Cluster(); // Count the number of over-populated and over-sized clusters. diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs.meta b/Assets/Scripts/Algorithms/Clustering/KMeansClustererLegacy.cs.meta similarity index 100% rename from Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs.meta rename to Assets/Scripts/Algorithms/Clustering/KMeansClustererLegacy.cs.meta diff --git a/Assets/Scripts/IADS/IADS.cs b/Assets/Scripts/IADS/IADS.cs index 8df9df70d..63973def0 100644 --- a/Assets/Scripts/IADS/IADS.cs +++ b/Assets/Scripts/IADS/IADS.cs @@ -345,7 +345,7 @@ private void ClusterThreats() { // Cluster threats. IClustererLegacy clusterer = - new AgglomerativeClusterer(new List(threats), MaxSize, MaxRadius); + new AgglomerativeClustererLegacy(new List(threats), MaxSize, MaxRadius); clusterer.Cluster(); var clusters = clusterer.Clusters; Debug.Log($"Clustered {threats.Count} threats into {clusters.Count} clusters."); diff --git a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs index 0608e2e57..03135f3a8 100644 --- a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs +++ b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs @@ -21,8 +21,8 @@ public static GameObject GenerateObject(in Vector3 position) { [Test] public void TestSingleCluster() { - AgglomerativeClusterer clusterer = - new AgglomerativeClusterer(Objects, maxSize: Objects.Count, maxRadius: Mathf.Infinity); + var clusterer = new AgglomerativeClustererLegacy(Objects, maxSize: Objects.Count, + maxRadius: Mathf.Infinity); clusterer.Cluster(); Assert.AreEqual(1, clusterer.Clusters.Count); ClusterLegacy cluster = clusterer.Clusters[0]; @@ -32,8 +32,8 @@ public void TestSingleCluster() { [Test] public void TestMaxSizeOne() { - AgglomerativeClusterer clusterer = - new AgglomerativeClusterer(Objects, maxSize: 1, maxRadius: Mathf.Infinity); + var clusterer = + new AgglomerativeClustererLegacy(Objects, maxSize: 1, maxRadius: Mathf.Infinity); clusterer.Cluster(); Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); foreach (var cluster in clusterer.Clusters) { @@ -43,8 +43,7 @@ public void TestMaxSizeOne() { [Test] public void TestZeroRadius() { - AgglomerativeClusterer clusterer = - new AgglomerativeClusterer(Objects, maxSize: Objects.Count, maxRadius: 0); + var clusterer = new AgglomerativeClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 0); clusterer.Cluster(); Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); foreach (var cluster in clusterer.Clusters) { @@ -54,8 +53,7 @@ public void TestZeroRadius() { [Test] public void TestSmallRadius() { - AgglomerativeClusterer clusterer = - new AgglomerativeClusterer(Objects, maxSize: Objects.Count, maxRadius: 1); + var clusterer = new AgglomerativeClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 1); clusterer.Cluster(); Assert.AreEqual(3, clusterer.Clusters.Count); List clusters = diff --git a/Assets/Tests/EditMode/KMeansClustererTests.cs b/Assets/Tests/EditMode/KMeansClustererTests.cs index 7cf68b41f..c90618f2d 100644 --- a/Assets/Tests/EditMode/KMeansClustererTests.cs +++ b/Assets/Tests/EditMode/KMeansClustererTests.cs @@ -21,7 +21,7 @@ public static GameObject GenerateObject(in Vector3 position) { [Test] public void TestSingleCluster() { - KMeansClusterer clusterer = new KMeansClusterer(Objects, k: 1); + var clusterer = new KMeansClustererLegacy(Objects, k: 1); clusterer.Cluster(); ClusterLegacy cluster = clusterer.Clusters[0]; Assert.AreEqual(Objects.Count, cluster.Size()); @@ -54,7 +54,7 @@ public void TestTwoDistinctClustersWithResetNeeded() { objects.AddRange(groupB); // Create clusterer with k = 2. - KMeansClusterer clusterer = new KMeansClusterer(objects, k: 2); + var clusterer = new KMeansClustererLegacy(objects, k: 2); clusterer.Cluster(); // We expect exactly 2 clusters. @@ -109,8 +109,8 @@ public static GameObject GenerateObject(in Vector3 position) { [Test] public void TestSingleCluster() { - ConstrainedKMeansClusterer clusterer = - new ConstrainedKMeansClusterer(Objects, maxSize: Objects.Count, maxRadius: Mathf.Infinity); + var clusterer = new ConstrainedKMeansClustererLegacy(Objects, maxSize: Objects.Count, + maxRadius: Mathf.Infinity); clusterer.Cluster(); Assert.AreEqual(1, clusterer.Clusters.Count); ClusterLegacy cluster = clusterer.Clusters[0]; @@ -120,8 +120,8 @@ public void TestSingleCluster() { [Test] public void TestMaxSizeOne() { - ConstrainedKMeansClusterer clusterer = - new ConstrainedKMeansClusterer(Objects, maxSize: 1, maxRadius: Mathf.Infinity); + var clusterer = + new ConstrainedKMeansClustererLegacy(Objects, maxSize: 1, maxRadius: Mathf.Infinity); clusterer.Cluster(); Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); foreach (var cluster in clusterer.Clusters) { @@ -131,8 +131,8 @@ public void TestMaxSizeOne() { [Test] public void TestZeroRadius() { - ConstrainedKMeansClusterer clusterer = - new ConstrainedKMeansClusterer(Objects, maxSize: Objects.Count, maxRadius: 0); + var clusterer = + new ConstrainedKMeansClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 0); clusterer.Cluster(); Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); foreach (var cluster in clusterer.Clusters) { @@ -142,8 +142,8 @@ public void TestZeroRadius() { [Test] public void TestSmallRadius() { - ConstrainedKMeansClusterer clusterer = - new ConstrainedKMeansClusterer(Objects, maxSize: Objects.Count, maxRadius: 1); + var clusterer = + new ConstrainedKMeansClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 1); clusterer.Cluster(); Assert.AreEqual(2, clusterer.Clusters.Count); } From 8aa9a1bcd67dbf28f46e6ab43a364478d5bcd237 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 17 Oct 2025 00:47:45 -0700 Subject: [PATCH 50/74] Fix all clustering algorithm implementations --- .../Clustering/AgglomerativeClusterer.cs | 90 +++++++++++++++++++ .../Clustering/AgglomerativeClusterer.cs.meta | 2 + .../Algorithms/Clustering/ClustererBase.cs | 2 +- .../Clustering/ConstrainedKMeansClusterer.cs | 40 +++++++++ .../ConstrainedKMeansClusterer.cs.meta | 2 + .../Algorithms/Clustering/IClusterer.cs | 2 +- .../Algorithms/Clustering/KMeansClusterer.cs | 78 ++++++++++++++++ .../Clustering/KMeansClusterer.cs.meta | 2 + .../Scripts/Hierarchical/FixedHierarchical.cs | 3 +- 9 files changed, 218 insertions(+), 3 deletions(-) create mode 100644 Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs create mode 100644 Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs.meta create mode 100644 Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs create mode 100644 Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs.meta create mode 100644 Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs create mode 100644 Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs.meta diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs new file mode 100644 index 000000000..2b0992ade --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs @@ -0,0 +1,90 @@ +using System.Collections.Generic; +using UnityEngine; + +// The agglomerative clusterer performs agglomerative clustering with the stopping condition given +// by the size and radius constraints. +public class AgglomerativeClusterer : SizeAndRadiusConstrainedClustererBase { + public AgglomerativeClusterer(int maxSize, float maxRadius) : base(maxSize, maxRadius) {} + + // Generate the clusters from the list of hierarchical objects. + public override List Cluster(IEnumerable hierarchicals) { + var clusters = new List(); + foreach (var hierarchical in hierarchicals) { + var cluster = new Cluster { Centroid = hierarchical.Position }; + cluster.AddSubHierarchical(hierarchical); + clusters.Add(cluster); + } + + // Create a set containing all valid cluster indices. + var validClusterIndices = new HashSet(); + for (int i = 0; i < clusters.Count; ++i) { + validClusterIndices.Add(i); + } + + // Find the pairwise distances between all clusters. + // Only the lower triangular part of the distances matrix is used, i.e., row index > column + // index. + var distances = new float[clusters.Count, clusters.Count]; + for (int i = 0; i < clusters.Count; ++i) { + for (int j = 0; j < i; ++j) { + distances[i, j] = Vector3.Distance(clusters[i].Centroid, clusters[j].Centroid); + } + } + + while (true) { + // Find the minimum distance between two clusters. + float minDistance = Mathf.Infinity; + int clusterIndex1 = -1; + int clusterIndex2 = -1; + // Invariant: clusterIndex1 > clusterIndex2. + foreach (int i in validClusterIndices) { + for (int j = 0; j < i; ++j) { + if (validClusterIndices.Contains(j) && distances[i, j] < minDistance) { + minDistance = distances[i, j]; + clusterIndex1 = i; + clusterIndex2 = j; + } + } + } + + // Check whether the minimum distance exceeds the maximum cluster radius, in which case the + // algorithm has converged. This produces a conservative solution because the radius of a + // merged cluster is less than or equal to the sum of the original cluster radii. + if (minDistance == Mathf.Infinity || minDistance > _maxRadius) { + break; + } + + // Check whether merging the two clusters would violate the size constraint. + if (clusters[clusterIndex1].Size + clusters[clusterIndex2].Size > _maxSize) { + distances[clusterIndex1, clusterIndex2] = Mathf.Infinity; + continue; + } + + // Merge the two clusters together and keep the cluster with the smaller index, i.e., + // clusterIndex2. + clusters[clusterIndex2].Merge(clusters[clusterIndex1]); + clusters[clusterIndex2].Recenter(); + validClusterIndices.Remove(clusterIndex1); + + // Update the distances matrix using the distance between the cluster centroids. + // TODO(titan): Change the distance metric to use average or maximum linkage. + foreach (int i in validClusterIndices) { + if (i < clusterIndex2) { + distances[clusterIndex2, i] = + Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); + } else if (i > clusterIndex2) { + distances[i, clusterIndex2] = + Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); + } + } + } + + // Select only the valid clusters. + for (int i = clusters.Count - 1; i >= 0; --i) { + if (!validClusterIndices.Contains(i)) { + clusters.RemoveAt(i); + } + } + return clusters; + } +} diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs.meta b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs.meta new file mode 100644 index 000000000..1868730e2 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: c520cf15716da41eaa25a730894d69d8 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs index 4debbf9f7..30b9b706b 100644 --- a/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs +++ b/Assets/Scripts/Algorithms/Clustering/ClustererBase.cs @@ -3,5 +3,5 @@ // Base implementation of a clustering algorithm. public abstract class ClustererBase : IClusterer { // Generate the clusters from the list of hierarchical objects. - public abstract IEnumerable Cluster(IEnumerable hierarchicals); + public abstract List Cluster(IEnumerable hierarchicals); } diff --git a/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs new file mode 100644 index 000000000..906992f16 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs @@ -0,0 +1,40 @@ +using System.Collections.Generic; +using System.Linq; +using UnityEngine; + +// The constrained k-means clusterer performs k-means clustering under size and radius constraints. +public class ConstrainedKMeansClusterer : SizeAndRadiusConstrainedClustererBase { + public ConstrainedKMeansClusterer(int maxSize, float maxRadius) : base(maxSize, maxRadius) {} + + // Generate the clusters from the list of hierarchical objects. + public override List Cluster(IEnumerable hierarchicals) { + int numClusters = (int)Mathf.Ceil(hierarchicals.Count() / _maxSize); + KMeansClusterer clusterer; + List clusters; + while (true) { + clusterer = new KMeansClusterer(numClusters); + clusters = clusterer.Cluster(hierarchicals); + + // Count the number of over-populated and over-sized clusters. + int numOverPopulatedClusters = 0; + int numOverSizedClusters = 0; + foreach (var cluster in clusters) { + if (cluster.Size > _maxSize) { + ++numOverPopulatedClusters; + } + if (cluster.Radius() > _maxRadius) { + ++numOverSizedClusters; + } + } + + // If all clusters satisfy the size and radius constraints, the algorithm has converged. + if (numOverPopulatedClusters == 0 && numOverSizedClusters == 0) { + break; + } + + numClusters += + (int)Mathf.Ceil(Mathf.Max(numOverPopulatedClusters, numOverSizedClusters) / 2f); + } + return clusters; + } +} diff --git a/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs.meta b/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs.meta new file mode 100644 index 000000000..ff8b1abae --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 2e6d3d55b22784bd2ac0bc571bd5d1c3 \ No newline at end of file diff --git a/Assets/Scripts/Algorithms/Clustering/IClusterer.cs b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs index 21c398e3e..7fb6ebf1d 100644 --- a/Assets/Scripts/Algorithms/Clustering/IClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/IClusterer.cs @@ -5,5 +5,5 @@ // The clustering algorithm generaters a list of clusters given a list of hierarchical objects. public interface IClusterer { // Generate the clusters from the list of hierarchical objects. - IEnumerable Cluster(IEnumerable hierarchicals); + List Cluster(IEnumerable hierarchicals); } diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs new file mode 100644 index 000000000..343f7e169 --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs @@ -0,0 +1,78 @@ +using System.Collections.Generic; +using System.Linq; +using UnityEngine; + +// The k-means clusterer class performs k-means clustering. +public class KMeansClusterer : ClustererBase { + // Convergence threshold. + protected const float _epsilon = 1e-3f; + + // Number of clusters. + private readonly int _k; + + // Maximum number of iterations. + private readonly int _maxIterations; + + public KMeansClusterer(int k) : this(k, maxIterations: 20) {} + public KMeansClusterer(int k, int maxIterations) { + _k = k; + _maxIterations = maxIterations; + } + + // Generate the clusters from the list of hierarchical objects. + public override List Cluster(IEnumerable hierarchicals) { + // Initialize the clusters with centroids located at the positions of k random hierarchical + // objects. Perform Fisher-Yates shuffling to find k random hierarchical objects. + var hierarchicalsList = hierarchicals.ToList(); + var clusters = new List(); + for (int i = 0; i < _k; ++i) { + int j = Random.Range(i, hierarchicalsList.Count); + (hierarchicalsList[i], hierarchicalsList[j]) = (hierarchicalsList[j], hierarchicalsList[i]); + + clusters.Add(new Cluster { Centroid = hierarchicalsList[i].Position }); + } + + bool converged = false; + int iteration = 0; + while (!converged && iteration < _maxIterations) { + AssignToClusters(clusters, hierarchicals); + + // Calculate the new clusters as the mean of all assigned hierarchical objects. + converged = true; + for (int clusterIndex = 0; clusterIndex < clusters.Count; ++clusterIndex) { + var oldClusterPosition = clusters[clusterIndex].Centroid; + if (clusters[clusterIndex].IsEmpty) { + int hierarchicalIndex = Random.Range(0, hierarchicalsList.Count); + clusters[clusterIndex].Centroid = hierarchicalsList[hierarchicalIndex].Position; + } else { + clusters[clusterIndex].Recenter(); + } + + // Check whether the algorithm has converged by checking whether the cluster has moved. + if (Vector3.Distance(oldClusterPosition, clusters[clusterIndex].Position) > _epsilon) { + converged = false; + } + } + ++iteration; + } + AssignToClusters(clusters, hierarchicals); + return clusters; + } + + private static void AssignToClusters(IReadOnlyList clusters, + IEnumerable hierarchicals) { + // Determine the closest centroid to each hierarchical object. + foreach (var hierarchical in hierarchicals) { + float minDistance = Mathf.Infinity; + int minIndex = -1; + for (int clusterIndex = 0; clusterIndex < clusters.Count; ++clusterIndex) { + float distance = Vector3.Distance(clusters[clusterIndex].Centroid, hierarchical.Position); + if (distance < minDistance) { + minDistance = distance; + minIndex = clusterIndex; + } + } + clusters[minIndex].AddSubHierarchical(hierarchical); + } + } +} diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs.meta b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs.meta new file mode 100644 index 000000000..eb8775b7f --- /dev/null +++ b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: db08893c69df34819abd47d86d2601e9 \ No newline at end of file diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs b/Assets/Scripts/Hierarchical/FixedHierarchical.cs index bd8c6b181..7bfc68d65 100644 --- a/Assets/Scripts/Hierarchical/FixedHierarchical.cs +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs @@ -7,7 +7,8 @@ public class FixedHierarchical : HierarchicalBase { public new Vector3 Velocity { get; set; } public new Vector3 Acceleration { get; set; } - public FixedHierarchical() : this(position: Vector3.zero, velocity: Vector3.zero) {} + public FixedHierarchical() : this(position: Vector3.zero) {} + public FixedHierarchical(in Vector3 position) : this(position, velocity: Vector3.zero) {} public FixedHierarchical(in Vector3 position, in Vector3 velocity) : this(position, velocity, acceleration: Vector3.zero) {} public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { From 70c3f1303c020186b0374bd75d23f0ad96517c22 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 17 Oct 2025 00:48:07 -0700 Subject: [PATCH 51/74] Add unit tests for clustering algorithms --- .../EditMode/AgglomerativeClustererTests.cs | 109 ++++++----- Assets/Tests/EditMode/ClusterTests.cs | 58 +++--- .../ConstrainedKMeansClustererTests.cs | 58 ++++++ .../ConstrainedKMeansClustererTests.cs.meta | 2 + Assets/Tests/EditMode/KMeansClustererTests.cs | 181 +++++------------- 5 files changed, 201 insertions(+), 207 deletions(-) create mode 100644 Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs create mode 100644 Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs.meta diff --git a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs index 03135f3a8..d59baabe0 100644 --- a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs +++ b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs @@ -1,68 +1,85 @@ using NUnit.Framework; -using System.Collections; using System.Collections.Generic; using System.Linq; using UnityEngine; -using UnityEngine.TestTools; public class AgglomerativeClustererTests { - public static GameObject GenerateObject(in Vector3 position) { - GameObject obj = new GameObject(); - obj.transform.position = position; - return obj; - } - - public static readonly List Objects = new List { - GenerateObject(new Vector3(0, 0, 0)), - GenerateObject(new Vector3(0, 1, 0)), - GenerateObject(new Vector3(0, 1.5f, 0)), - GenerateObject(new Vector3(0, 2.5f, 0)), + private static readonly List _hierarchicals = new List { + new FixedHierarchical(position: new Vector3(0, 0, 0)), + new FixedHierarchical(position: new Vector3(0, 1, 0)), + new FixedHierarchical(position: new Vector3(0, 1.5f, 0)), + new FixedHierarchical(position: new Vector3(0, 2.5f, 0)), }; + private AgglomerativeClusterer _clusterer; + [Test] - public void TestSingleCluster() { - var clusterer = new AgglomerativeClustererLegacy(Objects, maxSize: Objects.Count, - maxRadius: Mathf.Infinity); - clusterer.Cluster(); - Assert.AreEqual(1, clusterer.Clusters.Count); - ClusterLegacy cluster = clusterer.Clusters[0]; - Assert.AreEqual(Objects.Count, cluster.Size()); - Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid()); + public void Cluster_NoConstraints_ReturnsSingleCluster() { + _clusterer = + new AgglomerativeClusterer(maxSize: _hierarchicals.Count, maxRadius: Mathf.Infinity); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(1, clusters.Count); + var cluster = clusters[0]; + Assert.AreEqual(_hierarchicals.Count, cluster.Size); + Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid); + Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Position); } [Test] - public void TestMaxSizeOne() { - var clusterer = - new AgglomerativeClustererLegacy(Objects, maxSize: 1, maxRadius: Mathf.Infinity); - clusterer.Cluster(); - Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); - foreach (var cluster in clusterer.Clusters) { - Assert.AreEqual(1, cluster.Size()); + public void Cluster_MaxSizeOne_ReturnsSingletonClusters() { + _clusterer = new AgglomerativeClusterer(maxSize: 1, maxRadius: Mathf.Infinity); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(_hierarchicals.Count, clusters.Count); + foreach (var cluster in clusters) { + Assert.AreEqual(1, cluster.Size); } } [Test] - public void TestZeroRadius() { - var clusterer = new AgglomerativeClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 0); - clusterer.Cluster(); - Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); - foreach (var cluster in clusterer.Clusters) { - Assert.AreEqual(1, cluster.Size()); + public void Cluster_ZeroRadius_ReturnsSingletonClusters() { + _clusterer = new AgglomerativeClusterer(maxSize: _hierarchicals.Count, maxRadius: 0); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(_hierarchicals.Count, clusters.Count); + foreach (var cluster in clusters) { + Assert.AreEqual(1, cluster.Size); } } [Test] - public void TestSmallRadius() { - var clusterer = new AgglomerativeClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 1); - clusterer.Cluster(); - Assert.AreEqual(3, clusterer.Clusters.Count); - List clusters = - clusterer.Clusters.OrderBy(cluster => cluster.Coordinates[1]).ToList(); - Assert.AreEqual(1, clusters[0].Size()); - Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Coordinates); - Assert.AreEqual(2, clusters[1].Size()); - Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Coordinates); - Assert.AreEqual(1, clusters[2].Size()); - Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Coordinates); + public void Cluster_SmallRadius_ReturnsMultipleClusters() { + _clusterer = new AgglomerativeClusterer(maxSize: _hierarchicals.Count, maxRadius: 0.5f); + var clusters = _clusterer.Cluster(_hierarchicals); + Debug.Log( + $"{clusters[0].Centroid} {clusters[1].Centroid} {clusters[0].Radius()} {clusters[1].Radius()}"); + Assert.AreEqual(3, clusters.Count); + var sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); + Assert.AreEqual(1, sortedClusters[0].Size); + Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Centroid); + Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Position); + Assert.AreEqual(2, sortedClusters[1].Size); + Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Centroid); + Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Position); + Assert.AreEqual(1, sortedClusters[2].Size); + Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Centroid); + Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Position); + } + + [Test] + public void Cluster_SmallSize_ReturnsMultipleClusters() { + _clusterer = new AgglomerativeClusterer(maxSize: 2, maxRadius: 1); + var clusters = _clusterer.Cluster(_hierarchicals); + Debug.Log( + $"{clusters[0].Centroid} {clusters[1].Centroid} {clusters[0].Size} {clusters[1].Size}"); + Assert.AreEqual(3, clusters.Count); + var sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); + Assert.AreEqual(1, sortedClusters[0].Size); + Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Centroid); + Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Position); + Assert.AreEqual(2, sortedClusters[1].Size); + Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Centroid); + Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Position); + Assert.AreEqual(1, sortedClusters[2].Size); + Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Centroid); + Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Position); } } diff --git a/Assets/Tests/EditMode/ClusterTests.cs b/Assets/Tests/EditMode/ClusterTests.cs index cd814d0fc..969e173cc 100644 --- a/Assets/Tests/EditMode/ClusterTests.cs +++ b/Assets/Tests/EditMode/ClusterTests.cs @@ -5,14 +5,10 @@ public class ClusterTests { private const float _epsilon = 1e-3f; - private static FixedHierarchical GenerateObject(in Vector3 position) { - return new FixedHierarchical(position, velocity: Vector3.zero, acceleration: Vector3.zero); - } - - private static Cluster GenerateCluster(IReadOnlyList objects) { - Cluster cluster = new Cluster(); - foreach (var obj in objects) { - cluster.AddSubHierarchical(obj); + private static Cluster GenerateCluster(IReadOnlyList hierarchicals) { + var cluster = new Cluster(); + foreach (var hierarchical in hierarchicals) { + cluster.AddSubHierarchical(hierarchical); } cluster.Recenter(); return cluster; @@ -29,11 +25,11 @@ public void Centroid_SetAndGets_WorksCorrectly() { [Test] public void Size_ReturnsCorrectly() { const int size = 10; - var objects = new List(); + var hierarchicals = new List(); for (int i = 0; i < size; ++i) { - objects.Add(GenerateObject(new Vector3(0, i, 0))); + hierarchicals.Add(new FixedHierarchical(new Vector3(0, i, 0))); } - Cluster cluster = GenerateCluster(objects); + Cluster cluster = GenerateCluster(hierarchicals); Assert.AreEqual(size, cluster.Size); } @@ -41,18 +37,18 @@ public void Size_ReturnsCorrectly() { public void IsEmpty_ReturnsCorrectly() { var cluster = new Cluster(); Assert.IsTrue(cluster.IsEmpty); - cluster.AddSubHierarchical(GenerateObject(Vector3.zero)); + cluster.AddSubHierarchical(new FixedHierarchical(Vector3.zero)); Assert.IsFalse(cluster.IsEmpty); } [Test] public void Radius_ReturnsDistanceFromCentroidToFarthest() { const int size = 10; - var objects = new List(); + var hierarchicals = new List(); for (int i = 0; i < size; ++i) { - objects.Add(GenerateObject(new Vector3(0, i, 0))); + hierarchicals.Add(new FixedHierarchical(new Vector3(0, i, 0))); } - Cluster cluster = GenerateCluster(objects); + Cluster cluster = GenerateCluster(hierarchicals); var centroid = size / 2 - 2; cluster.Centroid = new Vector3(0, centroid, 0); Assert.AreEqual(size - 1 - centroid, cluster.Radius(), _epsilon); @@ -66,14 +62,14 @@ public void Radius_With_NoSubHierarchicals_ReturnsZero() { [Test] public void Recenter_SetsCentroidToMeanOfSubHierarchicalPositions() { - var objects = new List(); + var hierarchicals = new List(); for (int i = -1; i <= 1; ++i) { for (int j = -1; j <= 1; ++j) { - objects.Add(GenerateObject(new Vector3(i, j, 0))); + hierarchicals.Add(new FixedHierarchical(new Vector3(i, j, 0))); } } - var cluster = GenerateCluster(objects); - cluster.AddSubHierarchical(GenerateObject(new Vector3(10, -10, 0))); + var cluster = GenerateCluster(hierarchicals); + cluster.AddSubHierarchical(new FixedHierarchical(new Vector3(10, -10, 0))); Assert.AreNotEqual(new Vector3(1, -1, 0), cluster.Centroid); cluster.Recenter(); Assert.AreEqual(new Vector3(1, -1, 0), cluster.Centroid); @@ -82,14 +78,14 @@ public void Recenter_SetsCentroidToMeanOfSubHierarchicalPositions() { [Test] public void Merge_CombinesAllSubHierarchicals() { const int size = 10; - var objects1 = new List(); - var objects2 = new List(); + var hierarchicals1 = new List(); + var hierarchicals2 = new List(); for (int i = 0; i < size; ++i) { - objects1.Add(GenerateObject(new Vector3(0, i, 0))); - objects2.Add(GenerateObject(new Vector3(i, 0, 0))); + hierarchicals1.Add(new FixedHierarchical(new Vector3(0, i, 0))); + hierarchicals2.Add(new FixedHierarchical(new Vector3(i, 0, 0))); } - var cluster1 = GenerateCluster(objects1); - var cluster2 = GenerateCluster(objects2); + var cluster1 = GenerateCluster(hierarchicals1); + var cluster2 = GenerateCluster(hierarchicals2); int size1 = cluster1.Size; int size2 = cluster2.Size; cluster1.Merge(cluster2); @@ -99,14 +95,14 @@ public void Merge_CombinesAllSubHierarchicals() { [Test] public void Merge_DoesNotUpdateCentroid() { const int size = 10; - var objects1 = new List(); - var objects2 = new List(); + var hierarchicals1 = new List(); + var hierarchicals2 = new List(); for (int i = 0; i < size; ++i) { - objects1.Add(GenerateObject(new Vector3(0, i, 0))); - objects2.Add(GenerateObject(new Vector3(i, 0, 0))); + hierarchicals1.Add(new FixedHierarchical(new Vector3(0, i, 0))); + hierarchicals2.Add(new FixedHierarchical(new Vector3(i, 0, 0))); } - var cluster1 = GenerateCluster(objects1); - var cluster2 = GenerateCluster(objects2); + var cluster1 = GenerateCluster(hierarchicals1); + var cluster2 = GenerateCluster(hierarchicals2); var centroid1 = cluster1.Centroid; var centroid2 = cluster2.Centroid; cluster1.Merge(cluster2); diff --git a/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs new file mode 100644 index 000000000..f2cf374b6 --- /dev/null +++ b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs @@ -0,0 +1,58 @@ +using NUnit.Framework; +using System.Collections.Generic; +using UnityEngine; + +public class ConstrainedKMeansClustererTests { + private static readonly List _hierarchicals = new List { + new FixedHierarchical(position: new Vector3(0, 0, 0)), + new FixedHierarchical(position: new Vector3(0, 1, 0)), + new FixedHierarchical(position: new Vector3(0, 1.5f, 0)), + new FixedHierarchical(position: new Vector3(0, 2.5f, 0)), + }; + + private ConstrainedKMeansClusterer _clusterer; + + [SetUp] + public void SetUp() { + Random.InitState(5); + } + + [Test] + public void Cluster_NoConstraints_ReturnsSingleCluster() { + _clusterer = + new ConstrainedKMeansClusterer(maxSize: _hierarchicals.Count, maxRadius: Mathf.Infinity); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(1, clusters.Count); + var cluster = clusters[0]; + Assert.AreEqual(_hierarchicals.Count, cluster.Size); + Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid); + Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Position); + } + + [Test] + public void Cluster_MaxSizeOne_ReturnsSingletonClusters() { + _clusterer = new ConstrainedKMeansClusterer(maxSize: 1, maxRadius: Mathf.Infinity); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(_hierarchicals.Count, clusters.Count); + foreach (var cluster in clusters) { + Assert.AreEqual(1, cluster.Size); + } + } + + [Test] + public void Cluster_ZeroRadius_ReturnsSingletonClusters() { + _clusterer = new ConstrainedKMeansClusterer(maxSize: _hierarchicals.Count, maxRadius: 0); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(_hierarchicals.Count, clusters.Count); + foreach (var cluster in clusters) { + Assert.AreEqual(1, cluster.Size); + } + } + + [Test] + public void Cluster_SmallRadius_ReturnsMultipleClusters() { + _clusterer = new ConstrainedKMeansClusterer(maxSize: _hierarchicals.Count, maxRadius: 1); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(2, clusters.Count); + } +} diff --git a/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs.meta b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs.meta new file mode 100644 index 000000000..d3c014bd6 --- /dev/null +++ b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: b1daace9df19447e9b6f85dbc5032742 \ No newline at end of file diff --git a/Assets/Tests/EditMode/KMeansClustererTests.cs b/Assets/Tests/EditMode/KMeansClustererTests.cs index c90618f2d..8db538e80 100644 --- a/Assets/Tests/EditMode/KMeansClustererTests.cs +++ b/Assets/Tests/EditMode/KMeansClustererTests.cs @@ -1,150 +1,71 @@ using NUnit.Framework; -using System.Collections; using System.Collections.Generic; using System.Linq; using UnityEngine; -using UnityEngine.TestTools; public class KMeansClustererTests { - public static readonly List Objects = new List { - GenerateObject(new Vector3(0, 0, 0)), - GenerateObject(new Vector3(0, 1, 0)), - GenerateObject(new Vector3(0, 1.5f, 0)), - GenerateObject(new Vector3(0, 2.5f, 0)), + private static readonly List _hierarchicals = new List { + new FixedHierarchical(position: new Vector3(0, 0, 0)), + new FixedHierarchical(position: new Vector3(0, 1, 0)), + new FixedHierarchical(position: new Vector3(0, 1.5f, 0)), + new FixedHierarchical(position: new Vector3(0, 2.5f, 0)), }; - public static GameObject GenerateObject(in Vector3 position) { - GameObject obj = new GameObject(); - obj.transform.position = position; - return obj; + private KMeansClusterer _clusterer; + + [SetUp] + public void SetUp() { + Random.InitState(5); } [Test] - public void TestSingleCluster() { - var clusterer = new KMeansClustererLegacy(Objects, k: 1); - clusterer.Cluster(); - ClusterLegacy cluster = clusterer.Clusters[0]; - Assert.AreEqual(Objects.Count, cluster.Size()); - Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Coordinates); - Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid()); + public void Cluster_SingleCluster() { + _clusterer = new KMeansClusterer(k: 1); + var clusters = _clusterer.Cluster(_hierarchicals); + Assert.AreEqual(1, clusters.Count); + var cluster = clusters[0]; + Assert.AreEqual(_hierarchicals.Count, cluster.Size); + Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid); + Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Position); } - // Test to reveal improper clearing of cluster memberships. [Test] - public void TestTwoDistinctClustersWithResetNeeded() { - // Group A: points near (0, 0, 0). - var groupA = new List { - GenerateObject(new Vector3(0, 0, 0)), - GenerateObject(new Vector3(1, 0, 0)), - GenerateObject(new Vector3(0, 1, 0)), - GenerateObject(new Vector3(1, 1, 0)), + public void Cluster_TwoDistinctClusters() { + // Hierarchical objects near (0, 0, 0); + var hierarchicalsA = new List { + new FixedHierarchical(position: new Vector3(0, 0, 0)), + new FixedHierarchical(position: new Vector3(1, 0, 0)), + new FixedHierarchical(position: new Vector3(0, 1, 0)), + new FixedHierarchical(position: new Vector3(1, 1, 0)), }; - // Group B: points near (10, 10, 10). - var groupB = new List { - GenerateObject(new Vector3(10, 10, 10)), - GenerateObject(new Vector3(11, 10, 10)), - GenerateObject(new Vector3(10, 11, 10)), - GenerateObject(new Vector3(11, 11, 10)), + // Hierarchical objects near (10, 10, 10). + var hierarchicalsB = new List { + new FixedHierarchical(position: new Vector3(10, 10, 10)), + new FixedHierarchical(position: new Vector3(11, 10, 10)), + new FixedHierarchical(position: new Vector3(10, 11, 10)), + new FixedHierarchical(position: new Vector3(11, 11, 10)), }; - // Combine them. - var objects = new List(); - objects.AddRange(groupA); - objects.AddRange(groupB); - - // Create clusterer with k = 2. - var clusterer = new KMeansClustererLegacy(objects, k: 2); - clusterer.Cluster(); - - // We expect exactly 2 clusters. - Assert.AreEqual(2, clusterer.Clusters.Count); - - // Retrieve the clusters. - ClusterLegacy c0 = clusterer.Clusters[0]; - ClusterLegacy c1 = clusterer.Clusters[1]; - - // Because the clusters are well-separated, each cluster should contain all points from one - // group or the other, not a mixture. Check via centroids. - var centroid0 = c0.Centroid(); - var centroid1 = c1.Centroid(); - - // One centroid should be near (0.5, 0.5, 0), the other near (10.5, 10.5, 10). - var expectedCentroid0 = new Vector3(0.5f, 0.5f, 0); - var expectedCentroid1 = new Vector3(10.5f, 10.5f, 10); - bool correctPlacement = (centroid0 == expectedCentroid0 && centroid1 == expectedCentroid1) || - (centroid0 == expectedCentroid1 && centroid1 == expectedCentroid0); - Assert.IsTrue( - correctPlacement, - "Centroids not close to the expected group centers. Possible leftover membership from a previous iteration if clusters not cleared."); - - // Additionally, we can count membership to confirm that each cluster got exactly four points - // for a more direct check. - int cluster0Count = c0.Size(); - int cluster1Count = c1.Size(); - Assert.AreEqual(8, cluster0Count + cluster1Count, - "Total membership across clusters does not match the total number of objects."); - - // Even if the clusters swapped roles, each cluster should have 4 points if membership was - // properly reset and re-assigned. - bool clusterCountsValid = cluster0Count == 4 && cluster1Count == 4; - Assert.IsTrue(clusterCountsValid, - $"Cluster sizes not as expected. c0={cluster0Count}, c1={cluster1Count}."); - } -} - -public class ConstrainedKMeansClustererTests { - public static readonly List Objects = new List { - GenerateObject(new Vector3(0, 0, 0)), - GenerateObject(new Vector3(0, 1, 0)), - GenerateObject(new Vector3(0, 1.5f, 0)), - GenerateObject(new Vector3(0, 2.5f, 0)), - }; - - public static GameObject GenerateObject(in Vector3 position) { - GameObject obj = new GameObject(); - obj.transform.position = position; - return obj; - } - - [Test] - public void TestSingleCluster() { - var clusterer = new ConstrainedKMeansClustererLegacy(Objects, maxSize: Objects.Count, - maxRadius: Mathf.Infinity); - clusterer.Cluster(); - Assert.AreEqual(1, clusterer.Clusters.Count); - ClusterLegacy cluster = clusterer.Clusters[0]; - Assert.AreEqual(Objects.Count, cluster.Size()); - Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid()); - } - - [Test] - public void TestMaxSizeOne() { - var clusterer = - new ConstrainedKMeansClustererLegacy(Objects, maxSize: 1, maxRadius: Mathf.Infinity); - clusterer.Cluster(); - Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); - foreach (var cluster in clusterer.Clusters) { - Assert.AreEqual(1, cluster.Size()); - } - } - - [Test] - public void TestZeroRadius() { - var clusterer = - new ConstrainedKMeansClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 0); - clusterer.Cluster(); - Assert.AreEqual(Objects.Count, clusterer.Clusters.Count); - foreach (var cluster in clusterer.Clusters) { - Assert.AreEqual(1, cluster.Size()); - } - } - - [Test] - public void TestSmallRadius() { - var clusterer = - new ConstrainedKMeansClustererLegacy(Objects, maxSize: Objects.Count, maxRadius: 1); - clusterer.Cluster(); - Assert.AreEqual(2, clusterer.Clusters.Count); + // Combine all hierarchical objects. + var allHierarchicals = new List(); + allHierarchicals.AddRange(hierarchicalsA); + allHierarchicals.AddRange(hierarchicalsB); + + // Create a k-means clusterer with k = 2 clusters. + _clusterer = new KMeansClusterer(k: 2); + var clusters = _clusterer.Cluster(allHierarchicals); + Assert.AreEqual(2, clusters.Count); + + // Sort the clusters by the x-coordinate of their centroids. + var sortedClusters = clusters.OrderBy(cluster => cluster.Centroid.x).ToList(); + var clusterA = sortedClusters[0]; + var clusterB = sortedClusters[1]; + Assert.AreEqual(4, clusterA.Size); + Assert.AreEqual(4, clusterB.Size); + Assert.AreEqual(new Vector3(0.5f, 0.5f, 0), clusterA.Centroid); + Assert.AreEqual(new Vector3(0.5f, 0.5f, 0), clusterA.Position); + Assert.AreEqual(new Vector3(10.5f, 10.5f, 10), clusterB.Centroid); + Assert.AreEqual(new Vector3(10.5f, 10.5f, 10), clusterB.Position); } } From 3693e95b01f10a6f1bc2dd7a3ab7981be5485468 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 17 Oct 2025 13:08:53 -0700 Subject: [PATCH 52/74] Address comments --- .../Algorithms/Clustering/AgglomerativeClusterer.cs | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs index 2b0992ade..6b3bb4317 100644 --- a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs @@ -69,12 +69,11 @@ public override List Cluster(IEnumerable hierarchicals) // Update the distances matrix using the distance between the cluster centroids. // TODO(titan): Change the distance metric to use average or maximum linkage. foreach (int i in validClusterIndices) { + var distance = Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); if (i < clusterIndex2) { - distances[clusterIndex2, i] = - Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); + distances[clusterIndex2, i] = distance; } else if (i > clusterIndex2) { - distances[i, clusterIndex2] = - Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); + distances[i, clusterIndex2] = distance; } } } From db49203a21bde8f812d0e5405730854c74ecd861 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 17 Oct 2025 13:26:02 -0700 Subject: [PATCH 53/74] Clean up coordinates classes and tests --- Assets/Scripts/Utils/Coordinates.cs.meta | 2 - Assets/Scripts/Utils/Coordinates2.cs | 26 +++ Assets/Scripts/Utils/Coordinates2.cs.meta | 2 + .../Utils/{Coordinates.cs => Coordinates3.cs} | 25 --- Assets/Scripts/Utils/Coordinates3.cs.meta | 2 + Assets/Tests/EditMode/Coordinates2Tests.cs | 77 +++++++++ .../Tests/EditMode/Coordinates2Tests.cs.meta | 2 + Assets/Tests/EditMode/Coordinates3Tests.cs | 74 ++++++++ .../Tests/EditMode/Coordinates3Tests.cs.meta | 2 + Assets/Tests/EditMode/CoordinatesTests.cs | 158 ------------------ .../Tests/EditMode/CoordinatesTests.cs.meta | 2 - 11 files changed, 185 insertions(+), 187 deletions(-) delete mode 100644 Assets/Scripts/Utils/Coordinates.cs.meta create mode 100644 Assets/Scripts/Utils/Coordinates2.cs create mode 100644 Assets/Scripts/Utils/Coordinates2.cs.meta rename Assets/Scripts/Utils/{Coordinates.cs => Coordinates3.cs} (74%) create mode 100644 Assets/Scripts/Utils/Coordinates3.cs.meta create mode 100644 Assets/Tests/EditMode/Coordinates2Tests.cs create mode 100644 Assets/Tests/EditMode/Coordinates2Tests.cs.meta create mode 100644 Assets/Tests/EditMode/Coordinates3Tests.cs create mode 100644 Assets/Tests/EditMode/Coordinates3Tests.cs.meta delete mode 100644 Assets/Tests/EditMode/CoordinatesTests.cs delete mode 100644 Assets/Tests/EditMode/CoordinatesTests.cs.meta diff --git a/Assets/Scripts/Utils/Coordinates.cs.meta b/Assets/Scripts/Utils/Coordinates.cs.meta deleted file mode 100644 index 82e047050..000000000 --- a/Assets/Scripts/Utils/Coordinates.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: a1cd8152fb4df49a1a08774ef6200f3b \ No newline at end of file diff --git a/Assets/Scripts/Utils/Coordinates2.cs b/Assets/Scripts/Utils/Coordinates2.cs new file mode 100644 index 000000000..06c6571a0 --- /dev/null +++ b/Assets/Scripts/Utils/Coordinates2.cs @@ -0,0 +1,26 @@ +using UnityEngine; + +// Utility functions for 2D coordinates. +// In Cartesian coordinates, the x-axis points right, and the y-axis points up. The coordinates are +// given by (x, y). +// In polar coordinates, the angle is measured in degrees from the x-axis counterclockwise to the +// y-axis. The coordinates are given by (r, theta). +public static class Coordinates2 { + public static Vector2 ConvertCartesianToPolar(in Vector2 cartesian) { + float r = cartesian.magnitude; + float theta = Mathf.Atan2(cartesian.y, cartesian.x) * Mathf.Rad2Deg; + return new Vector2(r, theta); + } + public static Vector2 ConvertCartesianToPolar(float x, float y) { + return ConvertCartesianToPolar(new Vector2(x, y)); + } + + public static Vector2 ConvertPolarToCartesian(in Vector2 polar) { + float x = polar.x * Mathf.Cos(polar.y * Mathf.Deg2Rad); + float y = polar.x * Mathf.Sin(polar.y * Mathf.Deg2Rad); + return new Vector2(x, y); + } + public static Vector2 ConvertPolarToCartesian(float r, float theta) { + return ConvertPolarToCartesian(new Vector2(r, theta)); + } +} diff --git a/Assets/Scripts/Utils/Coordinates2.cs.meta b/Assets/Scripts/Utils/Coordinates2.cs.meta new file mode 100644 index 000000000..dfaf2ebab --- /dev/null +++ b/Assets/Scripts/Utils/Coordinates2.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 4071ee6ad6d9f4c97b0eabd62a04777b \ No newline at end of file diff --git a/Assets/Scripts/Utils/Coordinates.cs b/Assets/Scripts/Utils/Coordinates3.cs similarity index 74% rename from Assets/Scripts/Utils/Coordinates.cs rename to Assets/Scripts/Utils/Coordinates3.cs index b50fc8b02..b581e33db 100644 --- a/Assets/Scripts/Utils/Coordinates.cs +++ b/Assets/Scripts/Utils/Coordinates3.cs @@ -1,30 +1,5 @@ using UnityEngine; -// Utility functions for 2D coordinates. -// In Cartesian coordinates, the x-axis points right, and the y-axis points up. The coordinates are -// given by (x, y). -// In polar coordinates, the angle is measured in degrees from the x-axis counterclockwise to the -// y-axis. The coordinates are given by (r, theta). -public static class Coordinates2 { - public static Vector2 ConvertCartesianToPolar(in Vector2 cartesian) { - float r = cartesian.magnitude; - float theta = Mathf.Atan2(cartesian.y, cartesian.x) * Mathf.Rad2Deg; - return new Vector2(r, theta); - } - public static Vector2 ConvertCartesianToPolar(float x, float y) { - return ConvertCartesianToPolar(new Vector2(x, y)); - } - - public static Vector2 ConvertPolarToCartesian(in Vector2 polar) { - float x = polar.x * Mathf.Cos(polar.y * Mathf.Deg2Rad); - float y = polar.x * Mathf.Sin(polar.y * Mathf.Deg2Rad); - return new Vector2(x, y); - } - public static Vector2 ConvertPolarToCartesian(float r, float theta) { - return ConvertPolarToCartesian(new Vector2(r, theta)); - } -} - // Utility functions for 3D coordinates. // In Cartesian coordinates, the x-axis points right, the y-axis points up, and the z-axis points // forward. The coordinates are given by (x, y, z). diff --git a/Assets/Scripts/Utils/Coordinates3.cs.meta b/Assets/Scripts/Utils/Coordinates3.cs.meta new file mode 100644 index 000000000..1018e88ed --- /dev/null +++ b/Assets/Scripts/Utils/Coordinates3.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 3bde15800558a47009e406cf476c8830 \ No newline at end of file diff --git a/Assets/Tests/EditMode/Coordinates2Tests.cs b/Assets/Tests/EditMode/Coordinates2Tests.cs new file mode 100644 index 000000000..a87b610c8 --- /dev/null +++ b/Assets/Tests/EditMode/Coordinates2Tests.cs @@ -0,0 +1,77 @@ +using NUnit.Framework; +using UnityEngine; +using UnityEngine.TestTools.Utils; + +public class Coordinates2Tests { + private const float _epsilon = 1e-4f; + + [Test] + public void ConvertCartesianToPolar_Origin_ReturnsZeroDistance() { + var cartesian = Vector2.zero; + Assert.AreEqual(0, Coordinates2.ConvertCartesianToPolar(cartesian).x, _epsilon); + } + + [Test] + public void ConvertCartesianToPolar_OnXAxis_ReturnsZeroAzimuth() { + var cartesian = new Vector2(5, 0); + var expectedPolar = new Vector2(5.0f, 0.0f); + var actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); + Assert.That(actualPolar, Is.EqualTo(expectedPolar).Using(Vector2EqualityComparer.Instance)); + } + + [Test] + public void ConvertCartesianToPolar_OnYAxis_ReturnsNinetyAzimuth() { + var cartesian = new Vector2(0, 5); + var expectedPolar = new Vector2(5.0f, 90.0f); + var actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); + Assert.That(actualPolar, Is.EqualTo(expectedPolar).Using(Vector2EqualityComparer.Instance)); + } + + [Test] + public void ConvertPolarToCartesian_ZeroDistance_ReturnsOrigin() { + var expectedCartesian = Vector2.zero; + 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) { + var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); + } + } + + [Test] + public void ConvertPolarToCartesian_ZeroAzimuth_ReturnsOnXAxis() { + var polar = new Vector2(10, 0); + var expectedCartesian = new Vector2(10.0f, 0.0f); + var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); + } + + [Test] + public void ConvertPolarToCartesian_NinetyAzimuth_ReturnsOnYAxis() { + Vector2 polar = new Vector2(10, 90); + Vector2 expectedCartesian = new Vector2(0.0f, 10.0f); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); + } + + [Test] + public void ConvertPolarToCartesian_ThirtyAzimuth() { + var polar = new Vector2(20, 30); + var expectedCartesian = new Vector2(10 * Mathf.Sqrt(3), 10); + var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); + } + + [Test] + public void ConvertPolarToCartesian_FortyFiveAzimuth() { + var polar = new Vector2(10 * Mathf.Sqrt(2), 45); + var expectedCartesian = new Vector2(10.0f, 10.0f); + var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); + } +} diff --git a/Assets/Tests/EditMode/Coordinates2Tests.cs.meta b/Assets/Tests/EditMode/Coordinates2Tests.cs.meta new file mode 100644 index 000000000..f85717638 --- /dev/null +++ b/Assets/Tests/EditMode/Coordinates2Tests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 548adc25bf05d4d19b3bef208f46ed41 \ No newline at end of file diff --git a/Assets/Tests/EditMode/Coordinates3Tests.cs b/Assets/Tests/EditMode/Coordinates3Tests.cs new file mode 100644 index 000000000..d090e1788 --- /dev/null +++ b/Assets/Tests/EditMode/Coordinates3Tests.cs @@ -0,0 +1,74 @@ +using NUnit.Framework; +using UnityEngine; +using UnityEngine.TestTools.Utils; + +public class Coordinates3Tests { + private const float _epsilon = 1e-4f; + + [Test] + public void ConvertCartesianToSpherical_Origin_ReturnsZeroDistance() { + var cartesian = Vector3.zero; + Assert.AreEqual(0, Coordinates3.ConvertCartesianToSpherical(cartesian).x, _epsilon); + } + + [Test] + public void ConvertCartesianToSpherical_ReturnsCorrectly() { + var cartesian = new Vector3(2, -1, 3); + var expectedSpherical = new Vector3(Mathf.Sqrt(14), 33.690068f, -15.501360f); + var actualSpherical = Coordinates3.ConvertCartesianToSpherical(cartesian); + Assert.That(actualSpherical, + Is.EqualTo(expectedSpherical).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void ConvertSphericalToCartesian_ZeroDistance_ReturnsOrigin() { + var spherical = new Vector3(0, 45, 90); + var expectedCartesian = Vector3.zero; + var actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void ConvertSphericalToCartesian_ReturnsCorrectly() { + var spherical = new Vector3(10, 60, 30); + var expectedCartesian = new Vector3(7.5f, 5.0f, 4.3301270189221945f); + var actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void ConvertCartesianToCylindrical_Origin_ReturnsZeroDistanceZeroHeight() { + Vector3 cartesian = Vector3.zero; + Assert.AreEqual(0, Coordinates3.ConvertCartesianToCylindrical(cartesian).x, _epsilon); + Assert.AreEqual(0, Coordinates3.ConvertCartesianToCylindrical(cartesian).z, _epsilon); + } + + [Test] + public void ConvertCartesianToCylindrical_ReturnsCorrectly() { + var cartesian = new Vector3(2, -1, 3); + var expectedCylindrical = new Vector3(Mathf.Sqrt(13), 33.6900675f, -1); + var actualCylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); + Assert.That(actualCylindrical, + Is.EqualTo(actualCylindrical).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void ConvertCylindricalToCartesian_ZeroDistance_ZeroHeight_ReturnsOrigin() { + var cylindrical = new Vector3(0, 45, 0); + var expectedCartesian = Vector3.zero; + var actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); + } + + [Test] + public void ConvertCylindricalToCartesian_ReturnsCorrectly() { + var cylindrical = new Vector3(10, 60, 2); + var expectedCartesian = new Vector3(8.660254f, 2.0f, 5.0f); + var actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); + } +} diff --git a/Assets/Tests/EditMode/Coordinates3Tests.cs.meta b/Assets/Tests/EditMode/Coordinates3Tests.cs.meta new file mode 100644 index 000000000..e5866f249 --- /dev/null +++ b/Assets/Tests/EditMode/Coordinates3Tests.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 36a079b9baa954873a602cbbc915ba5d \ No newline at end of file diff --git a/Assets/Tests/EditMode/CoordinatesTests.cs b/Assets/Tests/EditMode/CoordinatesTests.cs deleted file mode 100644 index 1fd2eda69..000000000 --- a/Assets/Tests/EditMode/CoordinatesTests.cs +++ /dev/null @@ -1,158 +0,0 @@ -using NUnit.Framework; -using UnityEngine; -using UnityEngine.TestTools; - -public class Coordinates2Tests { - private const float Epsilon = 0.0001f; - - [Test] - public void TestCartesianToPolarOrigin() { - Vector2 cartesian = Vector2.zero; - 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); - 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); - 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[] 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); - 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); - 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); - 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); - 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 Coordinates3Tests { - private const float Epsilon = 0.0001f; - - [Test] - public void TestCartesianToSphericalOrigin() { - Vector3 cartesian = Vector3.zero; - 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); - 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; - 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); - 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 TestCylindricalToCartesianOrigin() { - Vector3 cylindrical = new Vector3(0, 45, 0); - Vector3 expectedCartesian = Vector3.zero; - Vector3 actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); - 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 TestCylindricalToCartesian() { - Vector3 cylindrical = new Vector3(10, 60, 2); - Vector3 expectedCartesian = new Vector3(8.660254f, 2.0f, 5.0f); - Vector3 actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); - 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 TestCartesianToCylindricalOrigin() { - Vector3 cartesian = Vector3.zero; - Assert.That(Coordinates3.ConvertCartesianToCylindrical(cartesian).x, - Is.EqualTo(0).Within(Epsilon)); - } - - [Test] - public void TestCartesianToCylindrical() { - Vector3 cartesian = new Vector3(2, -1, 3); - Vector3 expectedCylindrical = new Vector3(Mathf.Sqrt(13), 33.6900675f, -1); - Vector3 actualCylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); - Assert.That(actualCylindrical.x, Is.EqualTo(expectedCylindrical.x).Within(Epsilon)); - Assert.That(actualCylindrical.y, Is.EqualTo(expectedCylindrical.y).Within(Epsilon)); - Assert.That(actualCylindrical.z, Is.EqualTo(expectedCylindrical.z).Within(Epsilon)); - } -} diff --git a/Assets/Tests/EditMode/CoordinatesTests.cs.meta b/Assets/Tests/EditMode/CoordinatesTests.cs.meta deleted file mode 100644 index 61bfb9ec1..000000000 --- a/Assets/Tests/EditMode/CoordinatesTests.cs.meta +++ /dev/null @@ -1,2 +0,0 @@ -fileFormatVersion: 2 -guid: 63e53175ac9ce4a05a109cb5a939b6ec \ No newline at end of file From 2d858fa78d1369c9fe071e851704baa86455fff4 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 17 Oct 2025 13:55:22 -0700 Subject: [PATCH 54/74] Address comments --- Assets/Scripts/Utils/Coordinates3.cs | 6 +++- Assets/Tests/EditMode/Coordinates2Tests.cs | 22 ++++++++----- Assets/Tests/EditMode/Coordinates3Tests.cs | 38 +++++++++++++++++----- 3 files changed, 47 insertions(+), 19 deletions(-) diff --git a/Assets/Scripts/Utils/Coordinates3.cs b/Assets/Scripts/Utils/Coordinates3.cs index b581e33db..306438a9e 100644 --- a/Assets/Scripts/Utils/Coordinates3.cs +++ b/Assets/Scripts/Utils/Coordinates3.cs @@ -12,11 +12,12 @@ public static class Coordinates3 { public static Vector3 ConvertCartesianToSpherical(in Vector3 cartesian) { float r = cartesian.magnitude; float azimuth = Mathf.Atan2(cartesian.x, cartesian.z) * Mathf.Rad2Deg; - float elevation = Mathf.Atan(cartesian.y / Mathf.Sqrt(cartesian.x * cartesian.x + + float elevation = Mathf.Atan2(cartesian.y, Mathf.Sqrt(cartesian.x * cartesian.x + cartesian.z * cartesian.z)) * Mathf.Rad2Deg; return new Vector3(r, azimuth, elevation); } + public static Vector3 ConvertCartesianToSpherical(float x, float y, float z) { return ConvertCartesianToSpherical(new Vector3(x, y, z)); } @@ -29,6 +30,7 @@ public static Vector3 ConvertSphericalToCartesian(in Vector3 spherical) { Mathf.Cos(spherical.y * Mathf.Deg2Rad); return new Vector3(x, y, z); } + public static Vector3 ConvertSphericalToCartesian(float r, float azimuth, float elevation) { return ConvertSphericalToCartesian(new Vector3(r, azimuth, elevation)); } @@ -39,6 +41,7 @@ public static Vector3 ConvertCartesianToCylindrical(in Vector3 cartesian) { float height = cartesian.y; return new Vector3(r, azimuth, height); } + public static Vector3 ConvertCartesianToCylindrical(float x, float y, float z) { return ConvertCartesianToCylindrical(new Vector3(x, y, z)); } @@ -49,6 +52,7 @@ public static Vector3 ConvertCylindricalToCartesian(in Vector3 cylindrical) { float z = cylindrical.x * Mathf.Cos(cylindrical.y * Mathf.Deg2Rad); return new Vector3(x, y, z); } + public static Vector3 ConvertCylindricalToCartesian(float r, float azimuth, float height) { return ConvertCylindricalToCartesian(new Vector3(r, azimuth, height)); } diff --git a/Assets/Tests/EditMode/Coordinates2Tests.cs b/Assets/Tests/EditMode/Coordinates2Tests.cs index a87b610c8..cacf0a4d3 100644 --- a/Assets/Tests/EditMode/Coordinates2Tests.cs +++ b/Assets/Tests/EditMode/Coordinates2Tests.cs @@ -8,7 +8,9 @@ public class Coordinates2Tests { [Test] public void ConvertCartesianToPolar_Origin_ReturnsZeroDistance() { var cartesian = Vector2.zero; - Assert.AreEqual(0, Coordinates2.ConvertCartesianToPolar(cartesian).x, _epsilon); + var polar = Coordinates2.ConvertCartesianToPolar(cartesian); + Assert.AreEqual(0, polar.x, _epsilon); + Assert.IsFalse(float.IsNaN(polar.y)); } [Test] @@ -28,15 +30,17 @@ public void ConvertCartesianToPolar_OnYAxis_ReturnsNinetyAzimuth() { } [Test] - public void ConvertPolarToCartesian_ZeroDistance_ReturnsOrigin() { + [TestCase(0f)] + [TestCase(45f)] + [TestCase(90f)] + [TestCase(180f)] + [TestCase(360f)] + public void ConvertPolarToCartesian_ZeroDistance_ReturnsOrigin(float theta) { + var polar = new Vector2(0, theta); var expectedCartesian = Vector2.zero; - 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) { - var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); - Assert.That(actualCartesian, - Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); - } + var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(actualCartesian, + Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); } [Test] diff --git a/Assets/Tests/EditMode/Coordinates3Tests.cs b/Assets/Tests/EditMode/Coordinates3Tests.cs index d090e1788..8b6410748 100644 --- a/Assets/Tests/EditMode/Coordinates3Tests.cs +++ b/Assets/Tests/EditMode/Coordinates3Tests.cs @@ -8,7 +8,10 @@ public class Coordinates3Tests { [Test] public void ConvertCartesianToSpherical_Origin_ReturnsZeroDistance() { var cartesian = Vector3.zero; - Assert.AreEqual(0, Coordinates3.ConvertCartesianToSpherical(cartesian).x, _epsilon); + var spherical = Coordinates3.ConvertCartesianToSpherical(cartesian); + Assert.AreEqual(0, spherical.x, _epsilon); + Assert.IsFalse(float.IsNaN(spherical.y)); + Assert.IsFalse(float.IsNaN(spherical.z)); } [Test] @@ -21,8 +24,18 @@ public void ConvertCartesianToSpherical_ReturnsCorrectly() { } [Test] - public void ConvertSphericalToCartesian_ZeroDistance_ReturnsOrigin() { - var spherical = new Vector3(0, 45, 90); + [TestCase(0f, 0f)] + [TestCase(45f, 0f)] + [TestCase(90f, 0f)] + [TestCase(180f, 0f)] + [TestCase(360f, 0f)] + [TestCase(45f, 0f)] + [TestCase(45f, 45f)] + [TestCase(45f, 90f)] + [TestCase(45f, 180f)] + [TestCase(45f, 360f)] + public void ConvertSphericalToCartesian_ZeroDistance_ReturnsOrigin(float theta, float phi) { + var spherical = new Vector3(0, theta, phi); var expectedCartesian = Vector3.zero; var actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); Assert.That(actualCartesian, @@ -40,9 +53,11 @@ public void ConvertSphericalToCartesian_ReturnsCorrectly() { [Test] public void ConvertCartesianToCylindrical_Origin_ReturnsZeroDistanceZeroHeight() { - Vector3 cartesian = Vector3.zero; - Assert.AreEqual(0, Coordinates3.ConvertCartesianToCylindrical(cartesian).x, _epsilon); - Assert.AreEqual(0, Coordinates3.ConvertCartesianToCylindrical(cartesian).z, _epsilon); + var cartesian = Vector3.zero; + var cylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); + Assert.AreEqual(0, cylindrical.x, _epsilon); + Assert.IsFalse(float.IsNaN(cylindrical.y)); + Assert.AreEqual(0, cylindrical.z, _epsilon); } [Test] @@ -51,12 +66,17 @@ public void ConvertCartesianToCylindrical_ReturnsCorrectly() { var expectedCylindrical = new Vector3(Mathf.Sqrt(13), 33.6900675f, -1); var actualCylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); Assert.That(actualCylindrical, - Is.EqualTo(actualCylindrical).Using(Vector3EqualityComparer.Instance)); + Is.EqualTo(expectedCylindrical).Using(Vector3EqualityComparer.Instance)); } [Test] - public void ConvertCylindricalToCartesian_ZeroDistance_ZeroHeight_ReturnsOrigin() { - var cylindrical = new Vector3(0, 45, 0); + [TestCase(0f)] + [TestCase(45f)] + [TestCase(90f)] + [TestCase(180f)] + [TestCase(360f)] + public void ConvertCylindricalToCartesian_ZeroDistance_ZeroHeight_ReturnsOrigin(float theta) { + var cylindrical = new Vector3(0, theta, 0); var expectedCartesian = Vector3.zero; var actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); Assert.That(actualCartesian, From fa34391b97736f9c55aec9351bcee8f3e004a130 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 11:44:11 -0700 Subject: [PATCH 55/74] Address comments --- Assets/Scripts/Movements/AerialMovement.cs | 18 +- Assets/Scripts/Movements/GroundMovement.cs | 2 +- .../{RocketMovement.cs => MissileMovement.cs} | 32 ++-- ...vement.cs.meta => MissileMovement.cs.meta} | 0 Assets/Scripts/Movements/MovementBase.cs | 8 +- Assets/Scripts/Utils/Constants.cs | 2 +- Assets/Tests/EditMode/GroundMovementTests.cs | 4 +- Assets/Tests/EditMode/IdealMovementTests.cs | 12 +- Assets/Tests/EditMode/MissileMovementTests.cs | 156 ++++++++++++++++++ ...s.cs.meta => MissileMovementTests.cs.meta} | 0 Assets/Tests/EditMode/RocketMovementTests.cs | 107 ------------ 11 files changed, 197 insertions(+), 144 deletions(-) rename Assets/Scripts/Movements/{RocketMovement.cs => MissileMovement.cs} (70%) rename Assets/Scripts/Movements/{RocketMovement.cs.meta => MissileMovement.cs.meta} (100%) create mode 100644 Assets/Tests/EditMode/MissileMovementTests.cs rename Assets/Tests/EditMode/{RocketMovementTests.cs.meta => MissileMovementTests.cs.meta} (100%) delete mode 100644 Assets/Tests/EditMode/RocketMovementTests.cs diff --git a/Assets/Scripts/Movements/AerialMovement.cs b/Assets/Scripts/Movements/AerialMovement.cs index 69f860d41..2f2196acf 100644 --- a/Assets/Scripts/Movements/AerialMovement.cs +++ b/Assets/Scripts/Movements/AerialMovement.cs @@ -9,19 +9,19 @@ public AerialMovement(IAgent agent) : base(agent) {} // Calculate the dynamic pressure on the agent. protected float GetDynamicPressure() { - var airDensity = Constants.CalculateAirDensityAtAltitude(Agent.Position.y); - var flowSpeed = Agent.Speed; + float airDensity = Constants.CalculateAirDensityAtAltitude(Agent.Position.y); + float flowSpeed = Agent.Speed; return 0.5f * airDensity * (flowSpeed * flowSpeed); } // Calculate the air drag acting on the agent. protected float CalculateDrag() { var staticConfig = Agent.StaticConfig; - var dragCoefficient = staticConfig.LiftDragConfig?.DragCoefficient ?? 0; - var crossSectionalArea = staticConfig.BodyConfig?.CrossSectionalArea ?? 0; - var mass = staticConfig.BodyConfig?.Mass ?? 1; - var dynamicPressure = GetDynamicPressure(); - var dragForce = dragCoefficient * dynamicPressure * crossSectionalArea; + float dragCoefficient = staticConfig.LiftDragConfig?.DragCoefficient ?? 0; + float crossSectionalArea = staticConfig.BodyConfig?.CrossSectionalArea ?? 0; + float mass = staticConfig.BodyConfig?.Mass ?? 1; + float dynamicPressure = GetDynamicPressure(); + float dragForce = dragCoefficient * dynamicPressure * crossSectionalArea; return dragForce / mass; } @@ -29,9 +29,9 @@ protected float CalculateDrag() { // acceleration normal to the velocity vector is considered "lift". protected float CalculateLiftInducedDrag(in Vector3 accelerationInput) { var staticConfig = Agent.StaticConfig; - var liftAcceleration = + float liftAcceleration = Vector3.ProjectOnPlane(accelerationInput, Agent.transform.forward).magnitude; - var liftDragRatio = staticConfig.LiftDragConfig?.LiftDragRatio ?? 1; + float liftDragRatio = staticConfig.LiftDragConfig?.LiftDragRatio ?? 1; return Mathf.Abs(liftAcceleration / liftDragRatio); } } diff --git a/Assets/Scripts/Movements/GroundMovement.cs b/Assets/Scripts/Movements/GroundMovement.cs index 4fb86b805..54724bdc3 100644 --- a/Assets/Scripts/Movements/GroundMovement.cs +++ b/Assets/Scripts/Movements/GroundMovement.cs @@ -11,7 +11,7 @@ public GroundMovement(IAgent agent) : base(agent) {} // ignore drag and gravity along the ground. public override Vector3 Act(in Vector3 accelerationInput) { // Ensure that there is no acceleration out of the x-z plane. - var constrainedAccelerationInput = accelerationInput; + Vector3 constrainedAccelerationInput = accelerationInput; constrainedAccelerationInput.y = 0; return LimitAccelerationInput(constrainedAccelerationInput); } diff --git a/Assets/Scripts/Movements/RocketMovement.cs b/Assets/Scripts/Movements/MissileMovement.cs similarity index 70% rename from Assets/Scripts/Movements/RocketMovement.cs rename to Assets/Scripts/Movements/MissileMovement.cs index 8133db38e..131fbc3e1 100644 --- a/Assets/Scripts/Movements/RocketMovement.cs +++ b/Assets/Scripts/Movements/MissileMovement.cs @@ -1,14 +1,18 @@ using UnityEngine; -// Rocket movement. +// Missile movement. // -// A rocket is defined by having three modes during its flight: ready mode (before it is launched), -// boost mode (when its burner is on), and midcourse mode (after the burner has completed burning). -public class RocketMovement : AerialMovement { +// A missile is defined by having multiple modes during its flight: ready mode (before it is +// launched), boost mode (when its burner is on), midcourse mode (after the burner has completed +// burning), and terminal mode (when the agent is homing in on the target). +// +// We define two additional states for our simulator: initialized state (when the missile has been +// created) and terminated state (when the missile has finished its flight). +public class MissileMovement : AerialMovement { // Flight phase of the agent. public Simulation.FlightPhase FlightPhase { get; set; } - public RocketMovement(IAgent agent) : base(agent) { + public MissileMovement(IAgent agent) : base(agent) { FlightPhase = Simulation.FlightPhase.Initialized; } @@ -44,18 +48,18 @@ public override Vector3 Act(in Vector3 accelerationInput) { // In the ready phase, the agent is subject to drag and gravity but has not boosted yet. private Vector3 ActReady(in Vector3 accelerationInput) { - var limitedAccelerationInput = LimitAccelerationInput(accelerationInput); + Vector3 limitedAccelerationInput = LimitAccelerationInput(accelerationInput); return CalculateNetAccelerationInput(limitedAccelerationInput); } // In the boost phase, the boost acceleration is added to the control acceleration input. private Vector3 ActBoost(in Vector3 accelerationInput) { - var limitedAccelerationInput = LimitAccelerationInput(accelerationInput); + Vector3 limitedAccelerationInput = LimitAccelerationInput(accelerationInput); // Determine the boost acceleration. - var boostAcceleration = + float boostAcceleration = (Agent.StaticConfig?.BoostConfig?.BoostAcceleration ?? 0) * Constants.kGravity; - var totalAccelerationInput = + Vector3 totalAccelerationInput = boostAcceleration * Agent.transform.forward + limitedAccelerationInput; return CalculateNetAccelerationInput(totalAccelerationInput); } @@ -63,7 +67,7 @@ private Vector3 ActBoost(in Vector3 accelerationInput) { // In the midcourse phase, the agent accelerates according to the control acceleration input but // is subject to drag and gravity. private Vector3 ActMidCourse(in Vector3 accelerationInput) { - var limitedAccelerationInput = LimitAccelerationInput(accelerationInput); + Vector3 limitedAccelerationInput = LimitAccelerationInput(accelerationInput); return CalculateNetAccelerationInput(limitedAccelerationInput); } @@ -76,10 +80,10 @@ private Vector3 ActTerminal(in Vector3 accelerationInput) { // Calculate the acceleration input with drag and gravity. private Vector3 CalculateNetAccelerationInput(in Vector3 accelerationInput) { - var gravity = Physics.gravity; - var airDrag = CalculateDrag(); - var liftInducedDrag = CalculateLiftInducedDrag(accelerationInput + Physics.gravity); - var dragAcceleration = -(airDrag + liftInducedDrag); + Vector3 gravity = Physics.gravity; + float airDrag = CalculateDrag(); + float liftInducedDrag = CalculateLiftInducedDrag(accelerationInput + Physics.gravity); + float dragAcceleration = -(airDrag + liftInducedDrag); return accelerationInput + gravity + dragAcceleration * Agent.transform.forward; } } diff --git a/Assets/Scripts/Movements/RocketMovement.cs.meta b/Assets/Scripts/Movements/MissileMovement.cs.meta similarity index 100% rename from Assets/Scripts/Movements/RocketMovement.cs.meta rename to Assets/Scripts/Movements/MissileMovement.cs.meta diff --git a/Assets/Scripts/Movements/MovementBase.cs b/Assets/Scripts/Movements/MovementBase.cs index 67d9dc1c1..d0d49ceb7 100644 --- a/Assets/Scripts/Movements/MovementBase.cs +++ b/Assets/Scripts/Movements/MovementBase.cs @@ -26,17 +26,17 @@ public float MaxForwardAcceleration() { // Get the maximum normal acceleration from the agent's static configuration. public float MaxNormalAcceleration() { - var maxReferenceNormalAcceleration = + float maxReferenceNormalAcceleration = (Agent.StaticConfig?.AccelerationConfig?.MaxReferenceNormalAcceleration ?? 0) * Constants.kGravity; - var referenceSpeed = Agent.StaticConfig?.AccelerationConfig?.ReferenceSpeed ?? 1; + float referenceSpeed = Agent.StaticConfig?.AccelerationConfig?.ReferenceSpeed ?? 1; return Mathf.Pow(Agent.Speed / referenceSpeed, 2) * maxReferenceNormalAcceleration; } // Limit acceleration input to the agent's maximum forward and normal accelerations. protected Vector3 LimitAccelerationInput(in Vector3 accelerationInput) { - var forwardAccelerationInput = Vector3.Project(accelerationInput, Agent.transform.forward); - var normalAccelerationInput = accelerationInput - forwardAccelerationInput; + Vector3 forwardAccelerationInput = Vector3.Project(accelerationInput, Agent.transform.forward); + Vector3 normalAccelerationInput = accelerationInput - forwardAccelerationInput; // Limit the forward and the normal acceleration magnitude. forwardAccelerationInput = diff --git a/Assets/Scripts/Utils/Constants.cs b/Assets/Scripts/Utils/Constants.cs index 8df717117..5477c1c14 100644 --- a/Assets/Scripts/Utils/Constants.cs +++ b/Assets/Scripts/Utils/Constants.cs @@ -1,8 +1,8 @@ using System; using UnityEngine; +// Physical constants. public static class Constants { - // Constants (these should be defined with appropriate values). public const float kAirDensity = 1.204f; // Sea level air density in kg/m^3. public const float kAirDensityScaleHeight = 10.4f; // Scale height in km. public static readonly float kGravity = -Physics.gravity.y; // Standard gravity in m/s^2. diff --git a/Assets/Tests/EditMode/GroundMovementTests.cs b/Assets/Tests/EditMode/GroundMovementTests.cs index 423bccf1d..22cc0b980 100644 --- a/Assets/Tests/EditMode/GroundMovementTests.cs +++ b/Assets/Tests/EditMode/GroundMovementTests.cs @@ -29,8 +29,8 @@ public void SetUp() { [Test] public void Act_KeepsAgentWithinXZPlane() { - var accelerationInput = new Vector3(1, -2, 3); - var appliedAccelerationInput = _movement.Act(accelerationInput); + Vector3 accelerationInput = new Vector3(1, -2, 3); + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); Assert.AreEqual(new Vector3(1, 0, 3), appliedAccelerationInput); } } diff --git a/Assets/Tests/EditMode/IdealMovementTests.cs b/Assets/Tests/EditMode/IdealMovementTests.cs index b17a2b829..7cfb6abf6 100644 --- a/Assets/Tests/EditMode/IdealMovementTests.cs +++ b/Assets/Tests/EditMode/IdealMovementTests.cs @@ -32,22 +32,22 @@ public void SetUp() { [Test] public void Act_LimitsForwardAccelerationInput() { - var accelerationInput = new Vector3(0, 0, 100); - var appliedAccelerationInput = _movement.Act(accelerationInput); + Vector3 accelerationInput = new Vector3(0, 0, 100); + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); Assert.AreEqual(new Vector3(0, 0, 10), appliedAccelerationInput); } [Test] public void Act_LimitsForwardAccelerationInput_NegativeAcceleration() { - var accelerationInput = new Vector3(0, 0, -20); - var appliedAccelerationInput = _movement.Act(accelerationInput); + Vector3 accelerationInput = new Vector3(0, 0, -20); + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); Assert.AreEqual(new Vector3(0, 0, -10), appliedAccelerationInput); } [Test] public void Act_LimitsNormalAccelerationInput() { - var accelerationInput = new Vector3(0, 50, 0); - var appliedAccelerationInput = _movement.Act(accelerationInput); + Vector3 accelerationInput = new Vector3(0, 50, 0); + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); Assert.That(appliedAccelerationInput, Is.EqualTo(new Vector3(0, 20f, 0)).Using(Vector3EqualityComparer.Instance)); } diff --git a/Assets/Tests/EditMode/MissileMovementTests.cs b/Assets/Tests/EditMode/MissileMovementTests.cs new file mode 100644 index 000000000..9d1084ed0 --- /dev/null +++ b/Assets/Tests/EditMode/MissileMovementTests.cs @@ -0,0 +1,156 @@ +using NUnit.Framework; +using UnityEngine; + +public class MissileMovementTests : TestBase { + private const float _epsilon = 1e-3f; + + private AgentBase _agent; + private MissileMovement _movement; + + [SetUp] + public void SetUp() { + _agent = new GameObject("Agent").AddComponent(); + _agent.StaticConfig = new Configs.StaticConfig() { + AccelerationConfig = + new Configs.AccelerationConfig() { + MaxForwardAcceleration = 10, + MaxReferenceNormalAcceleration = 5 / Constants.kGravity, + ReferenceSpeed = 100, + }, + BoostConfig = + new Configs.BoostConfig() { + BoostTime = 1, + BoostAcceleration = 100 / Constants.kGravity, + }, + LiftDragConfig = + new Configs.LiftDragConfig() { + DragCoefficient = 1, + LiftDragRatio = 1, + }, + BodyConfig = + new Configs.BodyConfig() { + CrossSectionalArea = 1, + Mass = 1, + }, + }; + Rigidbody agentRb = _agent.gameObject.AddComponent(); + _agent.transform.rotation = Quaternion.LookRotation(new Vector3(0, 0, 1)); + InvokePrivateMethod(_agent, "Awake"); + _movement = new MissileMovement(_agent); + } + + [Test] + public void Act_Initialized_ReturnsZero() { + _movement.FlightPhase = Simulation.FlightPhase.Initialized; + Vector3 accelerationInput = new Vector3(1, -2, 3); + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Vector3.zero, appliedAccelerationInput); + } + + [Test] + public void Act_Ready_AddsAirDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Ready; + _agent.Velocity = new Vector3(0, 0, 100); + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + + // Calculate the expected air drag. + // Velocity = 100 m/s, Cd = 1, area = 1 m^2, mass = 1 kg, altitude = 0 m. + float airDensity = Constants.CalculateAirDensityAtAltitude(altitude: 0); + float dynamicPressure = 0.5f * airDensity * 100f * 100f; + float dragForce = 1f * dynamicPressure * 1f; + float expectedDrag = dragForce / 1f; + Assert.AreEqual(-expectedDrag - Constants.kGravity, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Ready_AddsLiftInducedDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Ready; + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Ready_AddsGravity() { + _movement.FlightPhase = Simulation.FlightPhase.Ready; + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.y, _epsilon); + } + + [Test] + public void Act_Boost_AddsAirDragToBoostAcceleration() { + _movement.FlightPhase = Simulation.FlightPhase.Boost; + _agent.Velocity = new Vector3(0, 0, 100); + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + float boostAcceleration = 100f; + + // Calculate the expected air drag. + // Velocity = 100 m/s, Cd = 1, area = 1 m^2, mass = 1 kg, altitude = 0 m. + float airDensity = Constants.CalculateAirDensityAtAltitude(altitude: 0); + float dynamicPressure = 0.5f * airDensity * 100f * 100f; + float dragForce = 1f * dynamicPressure * 1f; + float expectedDrag = dragForce / 1f; + Assert.AreEqual(-expectedDrag - Constants.kGravity + boostAcceleration, + appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Boost_AddsLiftInducedDragToBoostAcceleration() { + _movement.FlightPhase = Simulation.FlightPhase.Boost; + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + float boostAcceleration = 100f; + Assert.AreEqual(-Constants.kGravity + boostAcceleration, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Boost_AddsGravity() { + _movement.FlightPhase = Simulation.FlightPhase.Midcourse; + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.y, _epsilon); + } + + [Test] + public void Act_Midcourse_AddsAirDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Midcourse; + _agent.Velocity = new Vector3(0, 0, 100); + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + + // Calculate the expected air drag. + // Velocity = 100 m/s, Cd = 1, area = 1 m^2, mass = 1 kg, altitude = 0 m. + float airDensity = Constants.CalculateAirDensityAtAltitude(altitude: 0); + float dynamicPressure = 0.5f * airDensity * 100f * 100f; + float dragForce = 1f * dynamicPressure * 1f; + float expectedDrag = dragForce / 1f; + Assert.AreEqual(-expectedDrag - Constants.kGravity, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_Midcourse_AddsLiftInducedDrag() { + _movement.FlightPhase = Simulation.FlightPhase.Midcourse; + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.z, _epsilon); + } + + [Test] + public void Act_MidCourse_AddsGravity() { + _movement.FlightPhase = Simulation.FlightPhase.Midcourse; + Vector3 accelerationInput = Vector3.zero; + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.y, _epsilon); + } + + [Test] + public void Act_Terminated_ReturnsZero() { + _movement.FlightPhase = Simulation.FlightPhase.Terminated; + Vector3 accelerationInput = new Vector3(1, -2, 3); + Vector3 appliedAccelerationInput = _movement.Act(accelerationInput); + Assert.AreEqual(Vector3.zero, appliedAccelerationInput); + } +} diff --git a/Assets/Tests/EditMode/RocketMovementTests.cs.meta b/Assets/Tests/EditMode/MissileMovementTests.cs.meta similarity index 100% rename from Assets/Tests/EditMode/RocketMovementTests.cs.meta rename to Assets/Tests/EditMode/MissileMovementTests.cs.meta diff --git a/Assets/Tests/EditMode/RocketMovementTests.cs b/Assets/Tests/EditMode/RocketMovementTests.cs deleted file mode 100644 index 50a05ad12..000000000 --- a/Assets/Tests/EditMode/RocketMovementTests.cs +++ /dev/null @@ -1,107 +0,0 @@ -using NUnit.Framework; -using UnityEngine; - -public class RocketMovementTests : TestBase { - private const float _epsilon = 1e-3f; - - private AgentBase _agent; - private RocketMovement _movement; - - [SetUp] - public void SetUp() { - _agent = new GameObject("Agent").AddComponent(); - _agent.StaticConfig = new Configs.StaticConfig() { - AccelerationConfig = - new Configs.AccelerationConfig() { - MaxForwardAcceleration = 10, - MaxReferenceNormalAcceleration = 5 / Constants.kGravity, - ReferenceSpeed = 100, - }, - BoostConfig = - new Configs.BoostConfig() { - BoostTime = 1, - BoostAcceleration = 100, - }, - LiftDragConfig = - new Configs.LiftDragConfig() { - DragCoefficient = 1, - LiftDragRatio = 1, - }, - BodyConfig = - new Configs.BodyConfig() { - CrossSectionalArea = 1, - Mass = 1, - }, - }; - Rigidbody agentRb = _agent.gameObject.AddComponent(); - _agent.transform.rotation = Quaternion.LookRotation(new Vector3(0, 0, 1)); - InvokePrivateMethod(_agent, "Awake"); - _movement = new RocketMovement(_agent); - } - - [Test] - public void Act_Initialized_ReturnsZero() { - _movement.FlightPhase = Simulation.FlightPhase.Initialized; - var accelerationInput = new Vector3(1, -2, 3); - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Vector3.zero, appliedAccelerationInput); - } - - [Test] - public void Act_Ready_AddsAirDrag() { - _movement.FlightPhase = Simulation.FlightPhase.Ready; - _agent.Velocity = new Vector3(0, 0, 100); - var accelerationInput = Vector3.zero; - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(-6020f - Constants.kGravity, appliedAccelerationInput.z, _epsilon); - } - - [Test] - public void Act_Ready_AddsLiftInducedDrag() { - _movement.FlightPhase = Simulation.FlightPhase.Ready; - var accelerationInput = Vector3.zero; - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.z, _epsilon); - } - - [Test] - public void Act_Ready_AddsGravity() { - _movement.FlightPhase = Simulation.FlightPhase.Ready; - var accelerationInput = Vector3.zero; - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.y, _epsilon); - } - - [Test] - public void Act_Midcourse_AddsAirDrag() { - _movement.FlightPhase = Simulation.FlightPhase.Midcourse; - _agent.Velocity = new Vector3(0, 0, 100); - var accelerationInput = Vector3.zero; - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(-6020f - Constants.kGravity, appliedAccelerationInput.z, _epsilon); - } - - [Test] - public void Act_Midcourse_AddsLiftInducedDrag() { - _movement.FlightPhase = Simulation.FlightPhase.Midcourse; - var accelerationInput = Vector3.zero; - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.z, _epsilon); - } - - [Test] - public void Act_MidCourse_AddsGravity() { - _movement.FlightPhase = Simulation.FlightPhase.Midcourse; - var accelerationInput = Vector3.zero; - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(-Constants.kGravity, appliedAccelerationInput.y, _epsilon); - } - - [Test] - public void Act_Terminated_ReturnsZero() { - _movement.FlightPhase = Simulation.FlightPhase.Terminated; - var accelerationInput = new Vector3(1, -2, 3); - var appliedAccelerationInput = _movement.Act(accelerationInput); - Assert.AreEqual(Vector3.zero, appliedAccelerationInput); - } -} From 27309b616067bfe8b5d30b01d38983e709954a17 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 11:46:31 -0700 Subject: [PATCH 56/74] Use explicit Vector3 type instead of var --- Assets/Scripts/Agents/AgentBase.cs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Assets/Scripts/Agents/AgentBase.cs b/Assets/Scripts/Agents/AgentBase.cs index 7c5c85c49..b0052a989 100644 --- a/Assets/Scripts/Agents/AgentBase.cs +++ b/Assets/Scripts/Agents/AgentBase.cs @@ -100,7 +100,7 @@ private Transformation GetRelativeTransformation(in Vector3 position, in Vector3 var transformation = new Transformation(); // Get the relative position transformation. - var relativePosition = position - Position; + Vector3 relativePosition = position - Position; transformation.position = GetRelativePositionTransformation(relativePosition); // Get the relative velocity transformation. @@ -123,8 +123,8 @@ private PositionTransformation GetRelativePositionTransformation(in Vector3 rela // Calculate the distance (range) to the target. positionTransformation.range = relativePosition.magnitude; - var flatRelativePosition = Vector3.ProjectOnPlane(relativePosition, transform.up); - var verticalRelativePosition = relativePosition - flatRelativePosition; + Vector3 flatRelativePosition = Vector3.ProjectOnPlane(relativePosition, transform.up); + Vector3 verticalRelativePosition = relativePosition - flatRelativePosition; // Calculate the elevation (vertical angle relative to forward). positionTransformation.elevation = @@ -160,14 +160,14 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela velocityTransformation.range = Vector3.Dot(relativeVelocity, relativePosition.normalized); // Project relative velocity onto the sphere passing through the target. - var tangentialVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); + Vector3 tangentialVelocity = Vector3.ProjectOnPlane(relativeVelocity, relativePosition); // The target azimuth vector is orthogonal to the relative position vector and // points to the starboard of the target along the azimuth-elevation sphere. - var targetAzimuth = Vector3.Cross(transform.up, relativePosition); + Vector3 targetAzimuth = Vector3.Cross(transform.up, relativePosition); // The target elevation vector is orthogonal to the relative position vector // and points upwards from the target along the azimuth-elevation sphere. - var targetElevation = Vector3.Cross(relativePosition, transform.right); + Vector3 targetElevation = Vector3.Cross(relativePosition, transform.right); // If the relative position vector is parallel to the yaw or pitch axis, the // target azimuth vector or the target elevation vector will be undefined. if (targetAzimuth.sqrMagnitude < _epsilon) { @@ -178,7 +178,7 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela // Project the relative velocity vector on the azimuth-elevation sphere onto // the target azimuth vector. - var tangentialVelocityOnAzimuth = Vector3.Project(tangentialVelocity, targetAzimuth); + Vector3 tangentialVelocityOnAzimuth = Vector3.Project(tangentialVelocity, targetAzimuth); // Calculate the time derivative of the azimuth to the target. velocityTransformation.azimuth = @@ -189,7 +189,7 @@ private VelocityTransformation GetRelativeVelocityTransformation(in Vector3 rela // Project the velocity vector on the azimuth-elevation sphere onto the target // elevation vector. - var tangentialVelocityOnElevation = Vector3.Project(tangentialVelocity, targetElevation); + Vector3 tangentialVelocityOnElevation = Vector3.Project(tangentialVelocity, targetElevation); // Calculate the time derivative of the elevation to the target. velocityTransformation.elevation = From 2b310cbf2e6772a59495e3146cda2d76c876a937 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 11:51:54 -0700 Subject: [PATCH 57/74] Use explicit types instead of var --- Assets/Tests/EditMode/ClusterTests.cs | 58 +++++++++++++-------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/Assets/Tests/EditMode/ClusterTests.cs b/Assets/Tests/EditMode/ClusterTests.cs index cd814d0fc..10995bf19 100644 --- a/Assets/Tests/EditMode/ClusterTests.cs +++ b/Assets/Tests/EditMode/ClusterTests.cs @@ -5,13 +5,13 @@ public class ClusterTests { private const float _epsilon = 1e-3f; - private static FixedHierarchical GenerateObject(in Vector3 position) { + private static FixedHierarchical GenerateHierarchical(in Vector3 position) { return new FixedHierarchical(position, velocity: Vector3.zero, acceleration: Vector3.zero); } - private static Cluster GenerateCluster(IReadOnlyList objects) { + private static Cluster GenerateCluster(IReadOnlyList hierarchicals) { Cluster cluster = new Cluster(); - foreach (var obj in objects) { + foreach (var obj in hierarchicals) { cluster.AddSubHierarchical(obj); } cluster.Recenter(); @@ -29,11 +29,11 @@ public void Centroid_SetAndGets_WorksCorrectly() { [Test] public void Size_ReturnsCorrectly() { const int size = 10; - var objects = new List(); + var hierarchicals = new List(); for (int i = 0; i < size; ++i) { - objects.Add(GenerateObject(new Vector3(0, i, 0))); + hierarchicals.Add(GenerateHierarchical(new Vector3(0, i, 0))); } - Cluster cluster = GenerateCluster(objects); + Cluster cluster = GenerateCluster(hierarchicals); Assert.AreEqual(size, cluster.Size); } @@ -41,19 +41,19 @@ public void Size_ReturnsCorrectly() { public void IsEmpty_ReturnsCorrectly() { var cluster = new Cluster(); Assert.IsTrue(cluster.IsEmpty); - cluster.AddSubHierarchical(GenerateObject(Vector3.zero)); + cluster.AddSubHierarchical(GenerateHierarchical(Vector3.zero)); Assert.IsFalse(cluster.IsEmpty); } [Test] public void Radius_ReturnsDistanceFromCentroidToFarthest() { const int size = 10; - var objects = new List(); + var hierarchicals = new List(); for (int i = 0; i < size; ++i) { - objects.Add(GenerateObject(new Vector3(0, i, 0))); + hierarchicals.Add(GenerateHierarchical(new Vector3(0, i, 0))); } - Cluster cluster = GenerateCluster(objects); - var centroid = size / 2 - 2; + Cluster cluster = GenerateCluster(hierarchicals); + float centroid = size / 2 - 2; cluster.Centroid = new Vector3(0, centroid, 0); Assert.AreEqual(size - 1 - centroid, cluster.Radius(), _epsilon); } @@ -66,14 +66,14 @@ public void Radius_With_NoSubHierarchicals_ReturnsZero() { [Test] public void Recenter_SetsCentroidToMeanOfSubHierarchicalPositions() { - var objects = new List(); + var hierarchicals = new List(); for (int i = -1; i <= 1; ++i) { for (int j = -1; j <= 1; ++j) { - objects.Add(GenerateObject(new Vector3(i, j, 0))); + hierarchicals.Add(GenerateHierarchical(new Vector3(i, j, 0))); } } - var cluster = GenerateCluster(objects); - cluster.AddSubHierarchical(GenerateObject(new Vector3(10, -10, 0))); + Cluster cluster = GenerateCluster(hierarchicals); + cluster.AddSubHierarchical(GenerateHierarchical(new Vector3(10, -10, 0))); Assert.AreNotEqual(new Vector3(1, -1, 0), cluster.Centroid); cluster.Recenter(); Assert.AreEqual(new Vector3(1, -1, 0), cluster.Centroid); @@ -82,14 +82,14 @@ public void Recenter_SetsCentroidToMeanOfSubHierarchicalPositions() { [Test] public void Merge_CombinesAllSubHierarchicals() { const int size = 10; - var objects1 = new List(); - var objects2 = new List(); + var hierarchicals1 = new List(); + var hierarchicals2 = new List(); for (int i = 0; i < size; ++i) { - objects1.Add(GenerateObject(new Vector3(0, i, 0))); - objects2.Add(GenerateObject(new Vector3(i, 0, 0))); + hierarchicals1.Add(GenerateHierarchical(new Vector3(0, i, 0))); + hierarchicals2.Add(GenerateHierarchical(new Vector3(i, 0, 0))); } - var cluster1 = GenerateCluster(objects1); - var cluster2 = GenerateCluster(objects2); + Cluster cluster1 = GenerateCluster(hierarchicals1); + Cluster cluster2 = GenerateCluster(hierarchicals2); int size1 = cluster1.Size; int size2 = cluster2.Size; cluster1.Merge(cluster2); @@ -99,16 +99,16 @@ public void Merge_CombinesAllSubHierarchicals() { [Test] public void Merge_DoesNotUpdateCentroid() { const int size = 10; - var objects1 = new List(); - var objects2 = new List(); + var hierarchicals1 = new List(); + var hierarchicals2 = new List(); for (int i = 0; i < size; ++i) { - objects1.Add(GenerateObject(new Vector3(0, i, 0))); - objects2.Add(GenerateObject(new Vector3(i, 0, 0))); + hierarchicals1.Add(GenerateHierarchical(new Vector3(0, i, 0))); + hierarchicals2.Add(GenerateHierarchical(new Vector3(i, 0, 0))); } - var cluster1 = GenerateCluster(objects1); - var cluster2 = GenerateCluster(objects2); - var centroid1 = cluster1.Centroid; - var centroid2 = cluster2.Centroid; + Cluster cluster1 = GenerateCluster(hierarchicals1); + Cluster cluster2 = GenerateCluster(hierarchicals2); + Vector3 centroid1 = cluster1.Centroid; + Vector3 centroid2 = cluster2.Centroid; cluster1.Merge(cluster2); Assert.AreNotEqual((centroid1 + centroid2) / 2, cluster1.Centroid); cluster1.Recenter(); From cabe6afd639740935edcb43e7a401a4d3f1e7fc3 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 12:24:10 -0700 Subject: [PATCH 58/74] Use explicit types instead of var --- Assets/Scripts/Evasion/OrthogonalEvasion.cs | 34 ++++++++++----------- Assets/Scripts/Sensors/IdealSensor.cs | 2 +- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/Assets/Scripts/Evasion/OrthogonalEvasion.cs b/Assets/Scripts/Evasion/OrthogonalEvasion.cs index 9747f06d4..4643722e2 100644 --- a/Assets/Scripts/Evasion/OrthogonalEvasion.cs +++ b/Assets/Scripts/Evasion/OrthogonalEvasion.cs @@ -17,10 +17,10 @@ public override bool ShouldEvade(IAgent pursuer) { return false; } - var evasionRangeThreshold = + float evasionRangeThreshold = Agent.AgentConfig?.DynamicConfig?.FlightConfig?.EvasionConfig?.RangeThreshold ?? defaultEvasionRangeThreshold; - var sensorOutput = Agent.Sensor.Sense(pursuer); + SensorOutput sensorOutput = Agent.Sensor.Sense(pursuer); return sensorOutput.Position.Range <= evasionRangeThreshold && sensorOutput.Velocity.Range < 0; } @@ -30,23 +30,23 @@ public override Vector3 Evade(IAgent pursuer) { const float groundProximityThresholdFactor = 5f; const float groundAvoidanceUpFactor = 5f; - var agentPosition = Agent.Position; - var agentVelocity = Agent.Velocity; - var pursuerPosition = pursuer.Position; - var pursuerVelocity = pursuer.Velocity; + Vector3 agentPosition = Agent.Position; + Vector3 agentVelocity = Agent.Velocity; + Vector3 pursuerPosition = pursuer.Position; + Vector3 pursuerVelocity = pursuer.Velocity; // Evade the pursuer by turning the velocity to be orthogonal to the pursuer's velocity. - var normalVelocity = Vector3.ProjectOnPlane(agentVelocity, pursuerVelocity); + Vector3 normalVelocity = Vector3.ProjectOnPlane(agentVelocity, pursuerVelocity); if (normalVelocity.sqrMagnitude < epsilon) { // If the agent's velocity is aligned with the pursuer's velocity, choose a random normal // direction in which to evade. normalVelocity = pursuer.transform.right; } - var normalAccelerationDirection = + Vector3 normalAccelerationDirection = Vector3.ProjectOnPlane(normalVelocity, agentVelocity).normalized; // Turn away from the pursuer. - var relativePosition = pursuerPosition - agentPosition; + Vector3 relativePosition = pursuerPosition - agentPosition; if (Vector3.Dot(relativePosition, normalAccelerationDirection) > 0) { normalAccelerationDirection *= -1; } @@ -56,14 +56,14 @@ public override Vector3 Evade(IAgent pursuer) { float groundProximityThreshold = Mathf.Abs(agentVelocity.y) * groundProximityThresholdFactor; if (agentVelocity.y < 0 && altitude < groundProximityThreshold) { // Determine the evasion direction based on the angle to pursuer. - var angle = Vector3.SignedAngle(Agent.transform.forward, relativePosition, Vector3.up); + float angle = Vector3.SignedAngle(Agent.transform.forward, relativePosition, Vector3.up); // Choose the direction that leads away from the pursuer. - var rightDirection = Agent.transform.right; - var bestHorizontalDirection = angle > 0 ? -rightDirection : rightDirection; + Vector3 rightDirection = Agent.transform.right; + Vector3 bestHorizontalDirection = angle > 0 ? -rightDirection : rightDirection; // Blend between horizontal evasion and slight upward movement. - var blendFactor = 1 - (altitude / groundProximityThreshold); + float blendFactor = 1 - (altitude / groundProximityThreshold); normalAccelerationDirection = Vector3 .Lerp(normalAccelerationDirection, @@ -71,14 +71,14 @@ public override Vector3 Evade(IAgent pursuer) { blendFactor) .normalized; } - var normalAcceleration = normalAccelerationDirection * Agent.MaxNormalAcceleration(); + Vector3 normalAcceleration = normalAccelerationDirection * Agent.MaxNormalAcceleration(); // Set the forward acceleration. - var forwardAcceleration = Vector3.zero; + Vector3 forwardAcceleration = Vector3.zero; if (Agent is IThreat threat) { // Set the forward acceleration to reach the target speed within one time step. - var targetSpeed = threat.LookupPowerTable(Configs.Power.Max); - var speedError = targetSpeed - Agent.Speed; + float targetSpeed = threat.LookupPowerTable(Configs.Power.Max); + float speedError = targetSpeed - Agent.Speed; forwardAcceleration = speedError / Time.fixedDeltaTime * Agent.transform.forward; } return normalAcceleration + forwardAcceleration; diff --git a/Assets/Scripts/Sensors/IdealSensor.cs b/Assets/Scripts/Sensors/IdealSensor.cs index fa7185796..4373b5836 100644 --- a/Assets/Scripts/Sensors/IdealSensor.cs +++ b/Assets/Scripts/Sensors/IdealSensor.cs @@ -16,7 +16,7 @@ public override SensorOutput Sense(IHierarchical hierarchical) { // Sense the target agent. public override SensorOutput Sense(IAgent agent) { - var relativeTransformation = Agent.GetRelativeTransformation(agent); + Transformation relativeTransformation = Agent.GetRelativeTransformation(agent); return GenerateSensorOutput(relativeTransformation); } From ebb88f2e5c95f1d64f8ba490d84f6bdaeeac0eee Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 12:30:20 -0700 Subject: [PATCH 59/74] Use explicit types instead of var --- Assets/Scripts/Behavior/DirectAttackBehavior.cs | 10 +++++----- .../Scripts/Behavior/DirectAttackBehaviorLegacy.cs | 2 +- Assets/Tests/EditMode/DirectAttackBehaviorTests.cs | 14 ++++++++------ 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/Assets/Scripts/Behavior/DirectAttackBehavior.cs b/Assets/Scripts/Behavior/DirectAttackBehavior.cs index b7810ca10..a69d28deb 100644 --- a/Assets/Scripts/Behavior/DirectAttackBehavior.cs +++ b/Assets/Scripts/Behavior/DirectAttackBehavior.cs @@ -16,8 +16,8 @@ public override (Vector3 waypointPosition, Configs.Power power) return (targetPosition, Configs.Power.Max); } - var directionToTarget = targetPosition - Agent.Position; - var distanceToTarget = directionToTarget.magnitude; + Vector3 directionToTarget = targetPosition - Agent.Position; + float distanceToTarget = directionToTarget.magnitude; // Find the index of the first waypoint whose position is closer to the target than the current // position. @@ -28,8 +28,8 @@ public override (Vector3 waypointPosition, Configs.Power power) } } - var waypointPosition = targetPosition; - var power = Configs.Power.Idle; + Vector3 waypointPosition = targetPosition; + Configs.Power power = Configs.Power.Idle; if (waypointIndex == FlightPlan.Waypoints.Count) { // This is the last waypoint, so target the final position with the last waypoint's power // setting. @@ -38,7 +38,7 @@ public override (Vector3 waypointPosition, Configs.Power power) return (waypointPosition, power); } // There is a next waypoint. - var waypoint = FlightPlan.Waypoints[waypointIndex]; + Configs.FlightPlanWaypoint waypoint = FlightPlan.Waypoints[waypointIndex]; waypointPosition = targetPosition - directionToTarget.normalized * waypoint.Distance; waypointPosition.y = waypoint.Altitude; power = waypoint.Power; diff --git a/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs b/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs index 66d07ebb5..2fbbb896f 100644 --- a/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs +++ b/Assets/Scripts/Behavior/DirectAttackBehaviorLegacy.cs @@ -34,7 +34,7 @@ public override (Vector3 waypointPosition, Configs.Power power) return (waypointPosition, power); } // There is a next waypoint. - var waypoint = FlightPlan.Waypoints[waypointIndex]; + Configs.FlightPlanWaypoint waypoint = FlightPlan.Waypoints[waypointIndex]; waypointPosition = targetPosition - directionToTarget.normalized * waypoint.Distance; waypointPosition.y = waypoint.Altitude; power = waypoint.Power; diff --git a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs index 1c2e1e0f2..97c90e7f4 100644 --- a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs +++ b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs @@ -44,7 +44,7 @@ public void SetUp() { [Test] public void GetNextWaypoint_BeyondFarthestWaypoint() { _agent.Position = new Vector3(-100, 0, 0); - (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); Assert.AreEqual(new Vector3(0, 100, 0), waypoint); Assert.AreEqual(Configs.Power.Cruise, power); } @@ -52,7 +52,7 @@ public void GetNextWaypoint_BeyondFarthestWaypoint() { [Test] public void GetNextWaypoint_AtFarthestWaypoint() { _agent.Position = new Vector3(0, 0, 0); - (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); Assert.AreEqual(new Vector3(500, 50, 0), waypoint); Assert.AreEqual(Configs.Power.Mil, power); } @@ -60,7 +60,7 @@ public void GetNextWaypoint_AtFarthestWaypoint() { [Test] public void GetNextWaypoint_BetweenWaypoints() { _agent.Position = new Vector3(600, 0, 0); - (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); Assert.AreEqual(new Vector3(900, 25, 0), waypoint); Assert.AreEqual(Configs.Power.Max, power); } @@ -68,7 +68,7 @@ public void GetNextWaypoint_BetweenWaypoints() { [Test] public void GetNextWaypoint_PastClosestWaypoint() { _agent.Position = new Vector3(920, 0, 0); - (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPosition); + (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); Assert.AreEqual(new Vector3(1000, 0, 0), waypoint); Assert.AreEqual(Configs.Power.Max, power); } @@ -76,7 +76,8 @@ public void GetNextWaypoint_PastClosestWaypoint() { [Test] public void GetNextWaypoint_AtFarthestWaypoint_WithNonzeroZ() { _agent.Position = new Vector3(0, 0, 0); - (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); + (Vector3 waypoint, Configs.Power power) = + _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); Assert.AreEqual(new Vector3(400, 50, 300), waypoint); Assert.AreEqual(Configs.Power.Mil, power); } @@ -84,7 +85,8 @@ public void GetNextWaypoint_AtFarthestWaypoint_WithNonzeroZ() { [Test] public void GetNextWaypoint_BetweenWaypoints_WithNonzeroZ() { _agent.Position = new Vector3(440, 0, 330); - (var waypoint, var power) = _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); + (Vector3 waypoint, Configs.Power power) = + _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); Assert.AreEqual(new Vector3(720, 25, 540), waypoint); Assert.AreEqual(Configs.Power.Max, power); } From ac2c6560e05538df3072b53c4ca0564ea0ccb704 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 12:34:10 -0700 Subject: [PATCH 60/74] Use explicit types instead of var --- Assets/Scripts/Controller/ApnController.cs | 6 +++--- Assets/Scripts/Controller/PnController.cs | 20 +++++++++---------- .../Scripts/Controller/WaypointController.cs | 10 +++++----- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/Assets/Scripts/Controller/ApnController.cs b/Assets/Scripts/Controller/ApnController.cs index b49f1eea1..5dfbd69e0 100644 --- a/Assets/Scripts/Controller/ApnController.cs +++ b/Assets/Scripts/Controller/ApnController.cs @@ -7,11 +7,11 @@ public ApnController(IAgent agent, float gain) : base(agent, gain) {} // Controller-dependent implementation of the control law. protected override Vector3 Plan(in Transformation relativeTransformation) { - var accelerationInput = base.Plan(relativeTransformation); + Vector3 accelerationInput = base.Plan(relativeTransformation); // Add a feedforward term proportional to the target's acceleration. - var targetAcceleration = relativeTransformation.Acceleration.Cartesian; - var normalTargetAcceleration = + Vector3 targetAcceleration = relativeTransformation.Acceleration.Cartesian; + Vector3 normalTargetAcceleration = Vector3.ProjectOnPlane(targetAcceleration, Agent.transform.forward); return accelerationInput + Gain / 2 * normalTargetAcceleration; } diff --git a/Assets/Scripts/Controller/PnController.cs b/Assets/Scripts/Controller/PnController.cs index 25ff821f6..5e86f694e 100644 --- a/Assets/Scripts/Controller/PnController.cs +++ b/Assets/Scripts/Controller/PnController.cs @@ -22,19 +22,19 @@ public PnController(IAgent agent, float gain) : base(agent) { protected override Vector3 Plan(in Transformation relativeTransformation) { // Cache the transform, velocity, and speed. var agentTransform = Agent.transform; - var right = agentTransform.right; - var up = agentTransform.up; + Vector3 right = agentTransform.right; + Vector3 up = agentTransform.up; // Extract the bearing and closing velocity from the relative transformation. - var losAz = relativeTransformation.Position.Azimuth; - var losEl = relativeTransformation.Position.Elevation; - var losRateAz = relativeTransformation.Velocity.Azimuth; - var losRateEl = relativeTransformation.Velocity.Elevation; + float losAz = relativeTransformation.Position.Azimuth; + float losEl = relativeTransformation.Position.Elevation; + float losRateAz = relativeTransformation.Velocity.Azimuth; + float losRateEl = relativeTransformation.Velocity.Elevation; // The closing velocity is negative because the closing velocity is opposite to the range rate. - var closingVelocity = -relativeTransformation.Velocity.Range; + float closingVelocity = -relativeTransformation.Velocity.Range; // Set the turn factor, which is equal to the closing velocity by default. - var turnFactor = closingVelocity; + float turnFactor = closingVelocity; // Handle a negative closing velocity. In this case, since the target is moving away from the // agent, apply a stronger turn. if (closingVelocity < 0) { @@ -52,8 +52,8 @@ protected override Vector3 Plan(in Transformation relativeTransformation) { turnFactor = Mathf.Abs(closingVelocity) * _negativeClosingVelocityTurnFactor; } - var accelerationAz = Gain * turnFactor * losRateAz; - var accelerationEl = Gain * turnFactor * losRateEl; + float accelerationAz = Gain * turnFactor * losRateAz; + float accelerationEl = Gain * turnFactor * losRateEl; return right * accelerationAz + up * accelerationEl; } } diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointController.cs index 1233e64b0..2e2163936 100644 --- a/Assets/Scripts/Controller/WaypointController.cs +++ b/Assets/Scripts/Controller/WaypointController.cs @@ -10,18 +10,18 @@ public WaypointController(IAgent agent) : base(agent) {} // Controller-dependent implementation of the control law. protected override Vector3 Plan(in Transformation relativeTransformation) { - var relativePosition = relativeTransformation.Position.Cartesian; + Vector3 relativePosition = relativeTransformation.Position.Cartesian; if (relativeTransformation.Position.Range < AccelerationCutoffDistanceSqr) { return Vector3.zero; } // To reach the waypoint as fast as possible, use the maximum forward acceleration. - var forward = Agent.transform.forward; - var forwardAccelerationInput = forward * Agent.MaxForwardAcceleration(); + Vector3 forward = Agent.transform.forward; + Vector3 forwardAccelerationInput = forward * Agent.MaxForwardAcceleration(); // Steer as hard as possible towards the waypoint. - var normalAcceleration = Vector3.ProjectOnPlane(relativePosition, forward).normalized; - var normalAccelerationInput = normalAcceleration * Agent.MaxNormalAcceleration(); + Vector3 normalAcceleration = Vector3.ProjectOnPlane(relativePosition, forward).normalized; + Vector3 normalAccelerationInput = normalAcceleration * Agent.MaxNormalAcceleration(); return forwardAccelerationInput + normalAccelerationInput; } } From 4fb28bff7c67191f92d4e35e03a95587ffe60168 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 13:10:41 -0700 Subject: [PATCH 61/74] Address comments --- .../Planning/IterativeLaunchPlanner.cs | 22 +++++----- .../Planning/LaunchAngleCsvInterpolator.cs | 10 +++-- .../Planning/LaunchAngleInterpolatorBase.cs | 22 +++++++--- .../Planning/LaunchAnglePlannerBase.cs | 14 ++++--- .../Scripts/Algorithms/Planning/LaunchPlan.cs | 5 ++- .../Prediction/LinearExtrapolator.cs | 2 +- Assets/Scripts/Utils/Direction.cs | 6 +++ Assets/Scripts/Utils/Direction.cs.meta | 2 + Assets/Scripts/Utils/Interpolator2DBase.cs | 7 ++-- .../Scripts/Utils/Interpolator2DDataPoint.cs | 2 +- Assets/Scripts/Utils/KDTree.cs | 42 ++++++++++--------- .../Utils/NearestNeighborInterpolator2D.cs | 2 +- Assets/Scripts/Utils/PriorityQueue.cs | 4 +- .../EditMode/IterativeLaunchPlannerTests.cs | 12 +++--- Assets/Tests/EditMode/KDTreeTests.cs | 2 +- .../LaunchAngleDataInterpolatorBaseTests.cs | 3 -- Assets/Tests/EditMode/LaunchPlanTests.cs | 8 ++-- .../Tests/EditMode/LinearExtrapolatorTests.cs | 6 +-- .../NearestNeighborInterpolator2DTests.cs | 13 +++--- 19 files changed, 104 insertions(+), 80 deletions(-) create mode 100644 Assets/Scripts/Utils/Direction.cs create mode 100644 Assets/Scripts/Utils/Direction.cs.meta diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs index d0bbeac3b..775d05686 100644 --- a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs +++ b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs @@ -29,23 +29,23 @@ public IterativeLaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictor // Plan the launch by finding the convergence point between the launch angle planner and the // predictor. public override LaunchPlan Plan() { - var initialState = Predictor.Predict(time: 0f); - var targetPosition = initialState.Position; + PredictorState initialState = Predictor.Predict(time: 0f); + Vector3 targetPosition = initialState.Position; - var launchAngleOutput = new LaunchAngleOutput(); - var interceptPosition = new Vector3(); + LaunchAngleOutput launchAngleOutput = new LaunchAngleOutput(); + Vector3 interceptPosition = new Vector3(); for (int i = 0; i < _maxNumIterations; ++i) { // Estimate the time-to-intercept. launchAngleOutput = LaunchAnglePlanner.Plan(targetPosition); - var timeToIntercept = launchAngleOutput.TimeToPosition; + float timeToIntercept = launchAngleOutput.TimeToPosition; // Estimate the target position. - var predictedState = Predictor.Predict(timeToIntercept); + PredictorState predictedState = Predictor.Predict(timeToIntercept); targetPosition = predictedState.Position; // Check whether the intercept direction has changed, in which case the algorithm has // converged. - var newInterceptPosition = LaunchAnglePlanner.InterceptPosition(targetPosition); + Vector3 newInterceptPosition = LaunchAnglePlanner.InterceptPosition(targetPosition); if ((interceptPosition - newInterceptPosition).magnitude < _convergenceThreshold) { interceptPosition = newInterceptPosition; break; @@ -54,8 +54,8 @@ public override LaunchPlan Plan() { // Check that the target is moving towards the intercept position. Otherwise, the interceptor // should wait to be launched. - var targetToInterceptPosition = interceptPosition - initialState.Position; - var targetToPredictedPosition = targetPosition - initialState.Position; + Vector3 targetToInterceptPosition = interceptPosition - initialState.Position; + Vector3 targetToPredictedPosition = targetPosition - initialState.Position; if (Vector3.Dot(targetToInterceptPosition, targetToPredictedPosition) < 0) { return LaunchPlan.NoLaunch(); } @@ -64,8 +64,8 @@ public override LaunchPlan Plan() { // Check that the interceptor is moving towards the target. If the target is moving too fast, // the interceptor might be launched backwards because the intercept position and the predicted // position are behind the asset. In this case, the interceptor should wait to be launched. - var interceptorToInterceptPosition = interceptPosition; - var threatToPredictedPosition = targetPosition - initialState.Position; + Vector3 interceptorToInterceptPosition = interceptPosition; + Vector3 threatToPredictedPosition = targetPosition - initialState.Position; if (Vector3.Dot(interceptorToInterceptPosition, threatToPredictedPosition) > 0) { return LaunchPlan.NoLaunch(); } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs index ecf3ed451..ebcd078d4 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs @@ -1,5 +1,6 @@ using System; using System.IO; +using System.Linq; using UnityEngine; // The launch angle CSV interpolator loads launch angle data from a CSV file and provides @@ -26,19 +27,20 @@ public LaunchAngleCsvInterpolator(IAgent agent, string path, ConfigLoaderDelegat // Initialize the interpolator. protected override void InitInterpolator() { - var fileContent = _configLoader(_relativePath); + string fileContent = _configLoader(_relativePath); if (string.IsNullOrEmpty(fileContent)) { Debug.LogError($"Failed to load CSV file from {_relativePath}."); throw new InvalidOperationException("Interpolator could not be initialized."); } - var csvLines = fileContent.Split(new[] { "\r\n", "\r", "\n" }, StringSplitOptions.None); - if (csvLines.Length < 1) { + string[] csvLines = fileContent.Split(new[] { "\r\n", "\r", "\n" }, StringSplitOptions.None); + string[] dataLines = csvLines.Where(line => !string.IsNullOrWhiteSpace(line)).ToArray(); + if (dataLines.Length < 1) { throw new InvalidOperationException("No data points available for interpolation."); } try { - _interpolator = new NearestNeighborInterpolator2D(csvLines); + _interpolator = new NearestNeighborInterpolator2D(dataLines); } catch (Exception e) { throw new InvalidOperationException("Failed to initialize interpolator: " + e.Message); } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs index 9786b0648..331adc2ed 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleInterpolatorBase.cs @@ -18,7 +18,8 @@ public override LaunchAngleOutput Plan(in LaunchAngleInput input) { InitInterpolator(); } - var interpolatedDataPoint = _interpolator.Interpolate(input.Distance, input.Altitude); + Interpolator2DDataPoint interpolatedDataPoint = + _interpolator.Interpolate(input.Distance, input.Altitude); if (interpolatedDataPoint == null || interpolatedDataPoint.Data == null || interpolatedDataPoint.Data.Count < 2) { throw new InvalidOperationException("Interpolator returned invalid data."); @@ -30,10 +31,21 @@ public override LaunchAngleOutput Plan(in LaunchAngleInput input) { // Return the absolute intercept position given the absolute target position. public override Vector3 InterceptPosition(in Vector3 targetPosition) { - var direction = ConvertToRelativeDirection(targetPosition); - var interpolatedDataPoint = _interpolator.Interpolate(direction[0], direction[1]); - var relativePosition = targetPosition - Agent.Position; - var cylindricalRelativePosition = Coordinates3.ConvertCartesianToCylindrical(relativePosition); + if (_interpolator == null) { + InitInterpolator(); + } + + Direction direction = ConvertToRelativeDirection(targetPosition); + Interpolator2DDataPoint interpolatedDataPoint = + _interpolator.Interpolate(direction.Distance, direction.Altitude); + if (interpolatedDataPoint == null || interpolatedDataPoint.Data == null || + interpolatedDataPoint.Data.Count < 2) { + throw new InvalidOperationException("Interpolator returned invalid data."); + } + + Vector3 relativePosition = targetPosition - Agent.Position; + Vector3 cylindricalRelativePosition = + Coordinates3.ConvertCartesianToCylindrical(relativePosition); return Coordinates3.ConvertCylindricalToCartesian( r: interpolatedDataPoint.Coordinates[0], azimuth: cylindricalRelativePosition.y, height: interpolatedDataPoint.Coordinates[1]) + diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs index 508b6bf87..50ae9e141 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs @@ -11,8 +11,9 @@ public LaunchAnglePlannerBase(IAgent agent) { // Calculate the optimal launch angle in degrees and the time-to-target in seconds. public abstract LaunchAngleOutput Plan(in LaunchAngleInput input); public LaunchAngleOutput Plan(in Vector3 targetPosition) { - Vector2 direction = ConvertToRelativeDirection(targetPosition); - return Plan(new LaunchAngleInput { Distance = direction[0], Altitude = direction[1] }); + Direction direction = ConvertToRelativeDirection(targetPosition); + return Plan( + new LaunchAngleInput { Distance = direction.Distance, Altitude = direction.Altitude }); } // Return the absolute intercept position given the absolute target position. @@ -20,9 +21,10 @@ public LaunchAngleOutput Plan(in Vector3 targetPosition) { // Convert from a 3D vector to a 2D direction that ignores the azimuth and that is relative to the // agent's position. - protected Vector2 ConvertToRelativeDirection(in Vector3 position) { - var relativePosition = position - Agent.Position; - return new Vector2(Vector3.ProjectOnPlane(relativePosition, Vector3.up).magnitude, - Vector3.Project(relativePosition, Vector3.up).magnitude); + protected Direction ConvertToRelativeDirection(in Vector3 position) { + Vector3 relativePosition = position - Agent.Position; + return new Direction { Distance = + Vector3.ProjectOnPlane(relativePosition, Vector3.up).magnitude, + Altitude = Vector3.Project(relativePosition, Vector3.up).magnitude }; } } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs index 45da8f2c6..ef36320c8 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchPlan.cs @@ -18,8 +18,9 @@ public static LaunchPlan NoLaunch() { // Get the normalized launch vector given the agent's position. public Vector3 NormalizedLaunchVector(in Vector3 position) { - var relativeInterceptPosition = InterceptPosition - position; - var interceptDirection = Coordinates3.ConvertCartesianToSpherical(relativeInterceptPosition); + Vector3 relativeInterceptPosition = InterceptPosition - position; + Vector3 interceptDirection = + Coordinates3.ConvertCartesianToSpherical(relativeInterceptPosition); return Coordinates3.ConvertSphericalToCartesian(r: 1, azimuth: interceptDirection[1], elevation: LaunchAngle); } diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs index ae7991eb0..7a869c07b 100644 --- a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs +++ b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs @@ -9,7 +9,7 @@ public LinearExtrapolator(IHierarchical hierarchical) : base(hierarchical) {} // Predict the future state of the hierarchical object by linearly extrapolating its current // position and velocity. public override PredictorState Predict(float time) { - var position = Hierarchical.Position + Hierarchical.Velocity * time; + Vector3 position = Hierarchical.Position + Hierarchical.Velocity * time; return new PredictorState { Position = position, Velocity = Hierarchical.Velocity, diff --git a/Assets/Scripts/Utils/Direction.cs b/Assets/Scripts/Utils/Direction.cs new file mode 100644 index 000000000..597b86a82 --- /dev/null +++ b/Assets/Scripts/Utils/Direction.cs @@ -0,0 +1,6 @@ +// The direction specifies the distance and altitude with no azimuth information. +public struct Direction { + public float Distance { get; set; } + + public float Altitude { get; set; } +} diff --git a/Assets/Scripts/Utils/Direction.cs.meta b/Assets/Scripts/Utils/Direction.cs.meta new file mode 100644 index 000000000..9a32de0b8 --- /dev/null +++ b/Assets/Scripts/Utils/Direction.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 1b805484818754ca5ba3d4a7fb142641 \ No newline at end of file diff --git a/Assets/Scripts/Utils/Interpolator2DBase.cs b/Assets/Scripts/Utils/Interpolator2DBase.cs index 63e5d8e64..22b13b539 100644 --- a/Assets/Scripts/Utils/Interpolator2DBase.cs +++ b/Assets/Scripts/Utils/Interpolator2DBase.cs @@ -10,7 +10,7 @@ public abstract class Interpolator2DBase : IInterpolator2D { public IReadOnlyList Data => _data; public Interpolator2DBase(IEnumerable data) { - _data = data.ToList() ?? new List(); + _data = data?.ToList() ?? new List(); } public Interpolator2DBase(string[] csvLines) : this(ParseCsvLines(csvLines)) {} @@ -27,8 +27,9 @@ private static IEnumerable ParseCsvLines(IEnumerable parsedValues) = + Interpolator2DDataPoint.ValidateAndParseData(values); if (success && parsedValues.Count >= 2) { parsedDataPoints.Add(new Interpolator2DDataPoint { Coordinates = new Vector2(parsedValues[0], parsedValues[1]), diff --git a/Assets/Scripts/Utils/Interpolator2DDataPoint.cs b/Assets/Scripts/Utils/Interpolator2DDataPoint.cs index 18399ff1f..9e6d19190 100644 --- a/Assets/Scripts/Utils/Interpolator2DDataPoint.cs +++ b/Assets/Scripts/Utils/Interpolator2DDataPoint.cs @@ -7,7 +7,7 @@ public class Interpolator2DDataPoint { public Vector2 Coordinates { get; set; } // Arbitrary data consisting of floats. - public List Data { get; set; } + public IReadOnlyList Data { get; set; } // Validate and parse the data from strings to floats. public static (bool, List) ValidateAndParseData(string[] values) { diff --git a/Assets/Scripts/Utils/KDTree.cs b/Assets/Scripts/Utils/KDTree.cs index 50a52aa72..89806cb31 100644 --- a/Assets/Scripts/Utils/KDTree.cs +++ b/Assets/Scripts/Utils/KDTree.cs @@ -5,9 +5,9 @@ // K-D tree node. public class KDNode { - public T Data { get; set; } - public KDNode Left { get; set; } - public KDNode Right { get; set; } + public T Data { get; internal set; } + public KDNode Left { get; internal set; } + public KDNode Right { get; internal set; } } // K-D tree with 2D coordinates. @@ -25,7 +25,7 @@ public KDTree(IReadOnlyList points, Func getCoordinates) { // Find the nearest neighbor to the target node. public T NearestNeighbor(in Vector2 target) { - var neighbor = NearestNeighbor(_root, target, depth: 0, bestNode: null); + KDNode neighbor = NearestNeighbor(_root, target, depth: 0, bestNode: null); if (neighbor == null) { return default(T); } @@ -39,20 +39,21 @@ private KDNode NearestNeighbor(KDNode node, in Vector2 target, int depth, return bestNode; } - var nodeCoordinates = _getCoordinates(node.Data); - var currentDistance = Vector2.Distance(nodeCoordinates, target); - var bestDistance = bestNode == null ? float.MaxValue - : Vector2.Distance(_getCoordinates(bestNode.Data), target); + Vector2 nodeCoordinates = _getCoordinates(node.Data); + float currentDistance = Vector2.Distance(nodeCoordinates, target); + float bestDistance = bestNode == null + ? float.MaxValue + : Vector2.Distance(_getCoordinates(bestNode.Data), target); if (currentDistance < bestDistance) { bestNode = node; bestDistance = currentDistance; } - var axis = depth % 2; - var targetCoordinatesValue = axis == 0 ? target.x : target.y; - var nodeCoordinatesValue = axis == 0 ? nodeCoordinates.x : nodeCoordinates.y; - var nextBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Left : node.Right; - var otherBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Right : node.Left; + int axis = depth % 2; + float targetCoordinatesValue = axis == 0 ? target.x : target.y; + float nodeCoordinatesValue = axis == 0 ? nodeCoordinates.x : nodeCoordinates.y; + KDNode nextBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Left : node.Right; + KDNode otherBranch = targetCoordinatesValue < nodeCoordinatesValue ? node.Right : node.Left; // Explore the next branch first. bestNode = NearestNeighbor(nextBranch, target, depth + 1, bestNode); @@ -70,19 +71,20 @@ private KDNode BuildTree(IReadOnlyList points, int depth) { return null; } - var k = 2; - var axis = depth % k; + int k = 2; + int axis = depth % k; // Sort the points by axis and find the median. - var sortedPoints = + List sortedPoints = points.OrderBy(point => (axis == 0 ? _getCoordinates(point).x : _getCoordinates(point).y)) .ToList(); - var medianIndex = sortedPoints.Count / 2; - var medianPoint = sortedPoints[medianIndex]; + int medianIndex = sortedPoints.Count / 2; + T medianPoint = sortedPoints[medianIndex]; var node = new KDNode { Data = medianPoint }; - var leftPoints = sortedPoints.GetRange(0, medianIndex); - var rightPoints = sortedPoints.GetRange(medianIndex + 1, sortedPoints.Count - medianIndex - 1); + List leftPoints = sortedPoints.GetRange(0, medianIndex); + List rightPoints = + sortedPoints.GetRange(medianIndex + 1, sortedPoints.Count - medianIndex - 1); node.Left = BuildTree(leftPoints, depth + 1); node.Right = BuildTree(rightPoints, depth + 1); diff --git a/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs b/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs index 8c3b24ea0..4b30f592f 100644 --- a/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs +++ b/Assets/Scripts/Utils/NearestNeighborInterpolator2D.cs @@ -18,7 +18,7 @@ public NearestNeighborInterpolator2D(string[] csvLines) : base(csvLines) { // Interpolate the value using nearest neighbor interpolation. public override Interpolator2DDataPoint Interpolate(float x, float y) { - var closestPoint = _tree.NearestNeighbor(new Vector2(x, y)); + Interpolator2DDataPoint closestPoint = _tree.NearestNeighbor(new Vector2(x, y)); if (closestPoint == null) { Debug.LogError("No data points available for interpolation."); return new Interpolator2DDataPoint { Coordinates = new Vector2(x, y), diff --git a/Assets/Scripts/Utils/PriorityQueue.cs b/Assets/Scripts/Utils/PriorityQueue.cs index d5d563319..76f65ee9a 100644 --- a/Assets/Scripts/Utils/PriorityQueue.cs +++ b/Assets/Scripts/Utils/PriorityQueue.cs @@ -27,8 +27,8 @@ public T Dequeue() { } var firstPair = _buffer.First(); - var queue = firstPair.Value; - var item = queue.Dequeue(); + Queue queue = firstPair.Value; + T item = queue.Dequeue(); if (queue.Count == 0) { _buffer.Remove(firstPair.Key); } diff --git a/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs b/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs index fefcce187..544c04e92 100644 --- a/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs +++ b/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs @@ -50,7 +50,7 @@ public void SetUp() { public void Plan_InterceptAtDataPoint_ReturnsLaunch() { _target.Position = new Vector3(1, 110, 0); _target.Velocity = new Vector3(0, -1, 0); - var plan = _planner.Plan(); + LaunchPlan plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); Assert.AreEqual(90, plan.LaunchAngle); Assert.AreEqual(new Vector3(1, 100, 0), plan.InterceptPosition); @@ -60,7 +60,7 @@ public void Plan_InterceptAtDataPoint_ReturnsLaunch() { public void Plan_InterceptNearDataPoint_ReturnsLaunch() { _target.Position = new Vector3(1, 110, 0); _target.Velocity = new Vector3(0, -1.1f, 0); - var plan = _planner.Plan(); + LaunchPlan plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); Assert.AreEqual(90, plan.LaunchAngle); Assert.AreEqual(new Vector3(1, 99, 0), plan.InterceptPosition); @@ -70,7 +70,7 @@ public void Plan_InterceptNearDataPoint_ReturnsLaunch() { public void Plan_InterceptBetweenDataPoints_ReturnsLaunch() { _target.Position = new Vector3(126, 1, 0); _target.Velocity = new Vector3(-5, 0, 0); - var plan = _planner.Plan(); + LaunchPlan plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); Assert.AreEqual(20, plan.LaunchAngle); Assert.AreEqual(new Vector3(61, 1, 0), plan.InterceptPosition); @@ -80,7 +80,7 @@ public void Plan_InterceptBetweenDataPoints_ReturnsLaunch() { public void Plan_DivergingFromOrigin_ReturnsNoLaunch() { _target.Position = new Vector3(0, 1, -80); _target.Velocity = new Vector3(0, 0, -1); - var plan = _planner.Plan(); + LaunchPlan plan = _planner.Plan(); Assert.IsFalse(plan.ShouldLaunch); } @@ -88,7 +88,7 @@ public void Plan_DivergingFromOrigin_ReturnsNoLaunch() { public void Plan_DivergingFromInterceptPoint_ReturnsNoLaunch() { _target.Position = new Vector3(1, 105, 0); _target.Velocity = new Vector3(0, 1, 0); - var plan = _planner.Plan(); + LaunchPlan plan = _planner.Plan(); Assert.IsFalse(plan.ShouldLaunch); } @@ -96,7 +96,7 @@ public void Plan_DivergingFromInterceptPoint_ReturnsNoLaunch() { public void Plan_TooFarFromInterceptPoint_ReturnsNoLaunch() { _target.Position = new Vector3(1, 2000, 0); _target.Velocity = new Vector3(0, -1, 0); - var plan = _planner.Plan(); + LaunchPlan plan = _planner.Plan(); Assert.IsFalse(plan.ShouldLaunch); } } diff --git a/Assets/Tests/EditMode/KDTreeTests.cs b/Assets/Tests/EditMode/KDTreeTests.cs index 78e54e76c..32453efb5 100644 --- a/Assets/Tests/EditMode/KDTreeTests.cs +++ b/Assets/Tests/EditMode/KDTreeTests.cs @@ -30,7 +30,7 @@ public void NearestNeighbor_MatchesDataPoint_ReturnsSamePoint() { } } var tree = new KDTree(points, (Vector2 point) => point); - Assert.AreEqual(tree.NearestNeighbor(Vector2.zero), Vector2.zero); + Assert.AreEqual(Vector2.zero, tree.NearestNeighbor(Vector2.zero)); Assert.AreEqual(new Vector2(1, 6), tree.NearestNeighbor(new Vector2(1, 6))); Assert.AreEqual(new Vector2(3, 2), tree.NearestNeighbor(new Vector2(3, 2))); Assert.AreEqual(new Vector2(9, 9), tree.NearestNeighbor(new Vector2(9, 9))); diff --git a/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs index 94c3e4384..72f57aa6f 100644 --- a/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs +++ b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs @@ -50,7 +50,6 @@ public void Plan_NearDataPoint_ReturnsClosestPoint() { [Test] public void InterceptPosition_MatchesDataPoint_ReturnsSamePoint() { var targetPosition = new Vector3(100f, 1f, 0f); - _interpolator.Plan(new LaunchAngleInput()); Assert.That(_interpolator.InterceptPosition(targetPosition), Is.EqualTo(new Vector3(100f, 1f, 0f)).Using(Vector3EqualityComparer.Instance)); } @@ -58,7 +57,6 @@ public void InterceptPosition_MatchesDataPoint_ReturnsSamePoint() { [Test] public void InterceptPosition_NearDataPoint_ReturnsClosestPoint() { var targetPosition = new Vector3(101f, 2f, 0f); - _interpolator.Plan(new LaunchAngleInput()); Assert.That(_interpolator.InterceptPosition(targetPosition), Is.EqualTo(new Vector3(100f, 1f, 0f)).Using(Vector3EqualityComparer.Instance)); } @@ -68,7 +66,6 @@ public void InterceptPosition_MovesWithAgentPosition() { var agentPosition = new Vector3(61, 5055, 874); _agent.Position = agentPosition; var targetPosition = agentPosition + new Vector3(100f, 1f, 0f); - _interpolator.Plan(new LaunchAngleInput()); Assert.That(_interpolator.InterceptPosition(targetPosition), Is.EqualTo(agentPosition + new Vector3(100f, 1f, 0f)) .Using(Vector3EqualityComparer.Instance)); diff --git a/Assets/Tests/EditMode/LaunchPlanTests.cs b/Assets/Tests/EditMode/LaunchPlanTests.cs index 394db614b..06c82a220 100644 --- a/Assets/Tests/EditMode/LaunchPlanTests.cs +++ b/Assets/Tests/EditMode/LaunchPlanTests.cs @@ -18,7 +18,7 @@ public void NormalizedLaunchVector_IsNormalized() { LaunchAngle = 45f, InterceptPosition = new Vector3(1, 1, 0), }; - var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); + Vector3 launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.AreEqual(1f, launchVector.magnitude, _epsilon); } @@ -29,7 +29,7 @@ public void NormalizedLaunchVector_PointsVertically() { LaunchAngle = 90f, InterceptPosition = new Vector3(1, 1, 0), }; - var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); + Vector3 launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.That(launchVector, Is.EqualTo(new Vector3(0, 1f, 0)).Using(Vector3EqualityComparer.Instance)); } @@ -41,7 +41,7 @@ public void NormalizedLaunchVector_PointsHorizontally() { LaunchAngle = 0f, InterceptPosition = new Vector3(1, 1, 0), }; - var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); + Vector3 launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.That(launchVector, Is.EqualTo(new Vector3(1f, 0, 0)).Using(Vector3EqualityComparer.Instance)); } @@ -53,7 +53,7 @@ public void NormalizedLaunchVector_PointsDiagonally() { LaunchAngle = 45f, InterceptPosition = new Vector3(1, 0, 1), }; - var launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); + Vector3 launchVector = launchPlan.NormalizedLaunchVector(position: Vector3.zero); Assert.That(launchVector, Is.EqualTo(new Vector3(1, Mathf.Sqrt(2f), 1).normalized) .Using(Vector3EqualityComparer.Instance)); } diff --git a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs index d296a7b43..88f314488 100644 --- a/Assets/Tests/EditMode/LinearExtrapolatorTests.cs +++ b/Assets/Tests/EditMode/LinearExtrapolatorTests.cs @@ -15,7 +15,7 @@ public void SetUp() { [Test] public void Predict_AtTimeZero_ReturnsCurrentState() { - var predictedState = _predictor.Predict(time: 0f); + PredictorState predictedState = _predictor.Predict(time: 0f); Assert.AreEqual(_hierarchical.Position, predictedState.Position); Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); @@ -23,7 +23,7 @@ public void Predict_AtTimeZero_ReturnsCurrentState() { [Test] public void Predict_InThePast_ReturnsExtrapolatedState() { - var predictedState = _predictor.Predict(time: -5f); + PredictorState predictedState = _predictor.Predict(time: -5f); Assert.AreEqual(new Vector3(5, 10, -5), predictedState.Position); Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); @@ -31,7 +31,7 @@ public void Predict_InThePast_ReturnsExtrapolatedState() { [Test] public void Predict_InTheFuture_ReturnsExtrapolatedState() { - var predictedState = _predictor.Predict(time: 10f); + PredictorState predictedState = _predictor.Predict(time: 10f); Assert.AreEqual(new Vector3(20, -20, 25), predictedState.Position); Assert.AreEqual(_hierarchical.Velocity, predictedState.Velocity); Assert.AreEqual(_hierarchical.Acceleration, predictedState.Acceleration); diff --git a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs index 651c47034..6fc860403 100644 --- a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs +++ b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs @@ -18,7 +18,7 @@ public void Interpolate_InvalidPoints_ReturnsEmptyResult() { }; var interpolator = new NearestNeighborInterpolator2D(csvLines); Assert.AreEqual(0, interpolator.Data.Count); - var result = interpolator.Interpolate(1, 1); + Interpolator2DDataPoint result = interpolator.Interpolate(1, 1); Assert.AreEqual(0, result.Data.Count); } @@ -26,7 +26,7 @@ public void Interpolate_InvalidPoints_ReturnsEmptyResult() { public void Interpolate_SinglePoint_ReturnsSinglePoint() { string[] csvLines = { "0,0,45,2" }; var interpolator = new NearestNeighborInterpolator2D(csvLines); - var result = interpolator.Interpolate(100, 200); + Interpolator2DDataPoint result = interpolator.Interpolate(100, 200); Assert.AreEqual(new Vector2(0, 0), result.Coordinates); Assert.AreEqual(2, result.Data.Count); Assert.AreEqual(45f, result.Data[0], _epsilon); @@ -37,7 +37,7 @@ public void Interpolate_SinglePoint_ReturnsSinglePoint() { public void Interpolate_InsufficientColumns_ReturnsEmptyResult() { string[] csvLines = { "100.0,200.0" }; var interpolator = new NearestNeighborInterpolator2D(csvLines); - var result = interpolator.Interpolate(100f, 200f); + Interpolator2DDataPoint result = interpolator.Interpolate(100, 200); Assert.NotNull(result); Assert.That(result.Coordinates, Is.EqualTo(new Vector2(100f, 200f)).Using(Vector2EqualityComparer.Instance)); @@ -48,7 +48,7 @@ public void Interpolate_InsufficientColumns_ReturnsEmptyResult() { public void Interpolate_QueryOutOfRange_ReturnsClosestPoint() { string[] csvLines = { "0,0,45,2", "10,10,30,3", "20,5,25,4" }; var interpolator = new NearestNeighborInterpolator2D(csvLines); - var result = interpolator.Interpolate(1000, 1000); + Interpolator2DDataPoint result = interpolator.Interpolate(1000, 1000); Assert.AreEqual(new Vector2(20, 5), result.Coordinates); Assert.AreEqual(2, result.Data.Count); Assert.AreEqual(25f, result.Data[0], _epsilon); @@ -59,8 +59,7 @@ public void Interpolate_QueryOutOfRange_ReturnsClosestPoint() { public void Interpolate_BetweenDataPoints_ReturnsOnePoint() { string[] csvLines = { "1,1,10,20", "1,2,30,40", "2,1,50,60", "2,2,70,80" }; var interpolator = new NearestNeighborInterpolator2D(csvLines); - var testPoint = new Vector2(1.5f, 1.5f); - var result = interpolator.Interpolate(testPoint); + Interpolator2DDataPoint result = interpolator.Interpolate(1.5f, 1.5f); Assert.AreEqual(2, result.Data.Count); var validData = new List> { new List { 10f, 20f }, new List { 30f, 40f }, @@ -74,7 +73,7 @@ public void Interpolate_NoDataPoints_ReturnsEmptyResult() { LogAssert.ignoreFailingMessages = true; string[] csvLines = {}; var interpolator = new NearestNeighborInterpolator2D(csvLines); - Interpolator2DDataPoint result = interpolator.Interpolate(new Vector2(1f, 1f)); + Interpolator2DDataPoint result = interpolator.Interpolate(1, 1); Assert.AreEqual(0, result.Data.Count); } } From 67eba5908fda643c9bf85cf51b24910704b83fc7 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 13:16:59 -0700 Subject: [PATCH 62/74] Use explicit types instead of var --- .../Clustering/AgglomerativeClusterer.cs | 2 +- .../Algorithms/Clustering/KMeansClusterer.cs | 4 ++-- .../EditMode/AgglomerativeClustererTests.cs | 16 ++++++++-------- .../EditMode/ConstrainedKMeansClustererTests.cs | 10 +++++----- Assets/Tests/EditMode/KMeansClustererTests.cs | 12 ++++++------ 5 files changed, 22 insertions(+), 22 deletions(-) diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs index 6b3bb4317..e912eb699 100644 --- a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs @@ -69,7 +69,7 @@ public override List Cluster(IEnumerable hierarchicals) // Update the distances matrix using the distance between the cluster centroids. // TODO(titan): Change the distance metric to use average or maximum linkage. foreach (int i in validClusterIndices) { - var distance = Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); + float distance = Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); if (i < clusterIndex2) { distances[clusterIndex2, i] = distance; } else if (i > clusterIndex2) { diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs index 343f7e169..63d797207 100644 --- a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs @@ -23,7 +23,7 @@ public KMeansClusterer(int k, int maxIterations) { public override List Cluster(IEnumerable hierarchicals) { // Initialize the clusters with centroids located at the positions of k random hierarchical // objects. Perform Fisher-Yates shuffling to find k random hierarchical objects. - var hierarchicalsList = hierarchicals.ToList(); + List hierarchicalsList = hierarchicals.ToList(); var clusters = new List(); for (int i = 0; i < _k; ++i) { int j = Random.Range(i, hierarchicalsList.Count); @@ -40,7 +40,7 @@ public override List Cluster(IEnumerable hierarchicals) // Calculate the new clusters as the mean of all assigned hierarchical objects. converged = true; for (int clusterIndex = 0; clusterIndex < clusters.Count; ++clusterIndex) { - var oldClusterPosition = clusters[clusterIndex].Centroid; + Vector3 oldClusterPosition = clusters[clusterIndex].Centroid; if (clusters[clusterIndex].IsEmpty) { int hierarchicalIndex = Random.Range(0, hierarchicalsList.Count); clusters[clusterIndex].Centroid = hierarchicalsList[hierarchicalIndex].Position; diff --git a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs index d59baabe0..05fa1de48 100644 --- a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs +++ b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs @@ -17,9 +17,9 @@ public class AgglomerativeClustererTests { public void Cluster_NoConstraints_ReturnsSingleCluster() { _clusterer = new AgglomerativeClusterer(maxSize: _hierarchicals.Count, maxRadius: Mathf.Infinity); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(1, clusters.Count); - var cluster = clusters[0]; + Cluster cluster = clusters[0]; Assert.AreEqual(_hierarchicals.Count, cluster.Size); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Position); @@ -28,7 +28,7 @@ public void Cluster_NoConstraints_ReturnsSingleCluster() { [Test] public void Cluster_MaxSizeOne_ReturnsSingletonClusters() { _clusterer = new AgglomerativeClusterer(maxSize: 1, maxRadius: Mathf.Infinity); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(_hierarchicals.Count, clusters.Count); foreach (var cluster in clusters) { Assert.AreEqual(1, cluster.Size); @@ -38,7 +38,7 @@ public void Cluster_MaxSizeOne_ReturnsSingletonClusters() { [Test] public void Cluster_ZeroRadius_ReturnsSingletonClusters() { _clusterer = new AgglomerativeClusterer(maxSize: _hierarchicals.Count, maxRadius: 0); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(_hierarchicals.Count, clusters.Count); foreach (var cluster in clusters) { Assert.AreEqual(1, cluster.Size); @@ -48,11 +48,11 @@ public void Cluster_ZeroRadius_ReturnsSingletonClusters() { [Test] public void Cluster_SmallRadius_ReturnsMultipleClusters() { _clusterer = new AgglomerativeClusterer(maxSize: _hierarchicals.Count, maxRadius: 0.5f); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Debug.Log( $"{clusters[0].Centroid} {clusters[1].Centroid} {clusters[0].Radius()} {clusters[1].Radius()}"); Assert.AreEqual(3, clusters.Count); - var sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); + List sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); Assert.AreEqual(1, sortedClusters[0].Size); Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Centroid); Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Position); @@ -67,11 +67,11 @@ public void Cluster_SmallRadius_ReturnsMultipleClusters() { [Test] public void Cluster_SmallSize_ReturnsMultipleClusters() { _clusterer = new AgglomerativeClusterer(maxSize: 2, maxRadius: 1); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Debug.Log( $"{clusters[0].Centroid} {clusters[1].Centroid} {clusters[0].Size} {clusters[1].Size}"); Assert.AreEqual(3, clusters.Count); - var sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); + List sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); Assert.AreEqual(1, sortedClusters[0].Size); Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Centroid); Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Position); diff --git a/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs index f2cf374b6..0d80133a6 100644 --- a/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs +++ b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs @@ -21,9 +21,9 @@ public void SetUp() { public void Cluster_NoConstraints_ReturnsSingleCluster() { _clusterer = new ConstrainedKMeansClusterer(maxSize: _hierarchicals.Count, maxRadius: Mathf.Infinity); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(1, clusters.Count); - var cluster = clusters[0]; + Cluster cluster = clusters[0]; Assert.AreEqual(_hierarchicals.Count, cluster.Size); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Position); @@ -32,7 +32,7 @@ public void Cluster_NoConstraints_ReturnsSingleCluster() { [Test] public void Cluster_MaxSizeOne_ReturnsSingletonClusters() { _clusterer = new ConstrainedKMeansClusterer(maxSize: 1, maxRadius: Mathf.Infinity); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(_hierarchicals.Count, clusters.Count); foreach (var cluster in clusters) { Assert.AreEqual(1, cluster.Size); @@ -42,7 +42,7 @@ public void Cluster_MaxSizeOne_ReturnsSingletonClusters() { [Test] public void Cluster_ZeroRadius_ReturnsSingletonClusters() { _clusterer = new ConstrainedKMeansClusterer(maxSize: _hierarchicals.Count, maxRadius: 0); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(_hierarchicals.Count, clusters.Count); foreach (var cluster in clusters) { Assert.AreEqual(1, cluster.Size); @@ -52,7 +52,7 @@ public void Cluster_ZeroRadius_ReturnsSingletonClusters() { [Test] public void Cluster_SmallRadius_ReturnsMultipleClusters() { _clusterer = new ConstrainedKMeansClusterer(maxSize: _hierarchicals.Count, maxRadius: 1); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(2, clusters.Count); } } diff --git a/Assets/Tests/EditMode/KMeansClustererTests.cs b/Assets/Tests/EditMode/KMeansClustererTests.cs index 8db538e80..0484280a3 100644 --- a/Assets/Tests/EditMode/KMeansClustererTests.cs +++ b/Assets/Tests/EditMode/KMeansClustererTests.cs @@ -21,9 +21,9 @@ public void SetUp() { [Test] public void Cluster_SingleCluster() { _clusterer = new KMeansClusterer(k: 1); - var clusters = _clusterer.Cluster(_hierarchicals); + List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(1, clusters.Count); - var cluster = clusters[0]; + Cluster cluster = clusters[0]; Assert.AreEqual(_hierarchicals.Count, cluster.Size); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Centroid); Assert.AreEqual(new Vector3(0, 1.25f, 0), cluster.Position); @@ -54,13 +54,13 @@ public void Cluster_TwoDistinctClusters() { // Create a k-means clusterer with k = 2 clusters. _clusterer = new KMeansClusterer(k: 2); - var clusters = _clusterer.Cluster(allHierarchicals); + List clusters = _clusterer.Cluster(allHierarchicals); Assert.AreEqual(2, clusters.Count); // Sort the clusters by the x-coordinate of their centroids. - var sortedClusters = clusters.OrderBy(cluster => cluster.Centroid.x).ToList(); - var clusterA = sortedClusters[0]; - var clusterB = sortedClusters[1]; + List sortedClusters = clusters.OrderBy(cluster => cluster.Centroid.x).ToList(); + Cluster clusterA = sortedClusters[0]; + Cluster clusterB = sortedClusters[1]; Assert.AreEqual(4, clusterA.Size); Assert.AreEqual(4, clusterB.Size); Assert.AreEqual(new Vector3(0.5f, 0.5f, 0), clusterA.Centroid); From 9e37530722e1be3fd2fad71006bac966b61a6921 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 13:19:11 -0700 Subject: [PATCH 63/74] Use explicit types instead of var --- Assets/Tests/EditMode/Coordinates2Tests.cs | 14 +++++++------- Assets/Tests/EditMode/Coordinates3Tests.cs | 16 ++++++++-------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/Assets/Tests/EditMode/Coordinates2Tests.cs b/Assets/Tests/EditMode/Coordinates2Tests.cs index cacf0a4d3..49c4a3233 100644 --- a/Assets/Tests/EditMode/Coordinates2Tests.cs +++ b/Assets/Tests/EditMode/Coordinates2Tests.cs @@ -8,7 +8,7 @@ public class Coordinates2Tests { [Test] public void ConvertCartesianToPolar_Origin_ReturnsZeroDistance() { var cartesian = Vector2.zero; - var polar = Coordinates2.ConvertCartesianToPolar(cartesian); + Vector2 polar = Coordinates2.ConvertCartesianToPolar(cartesian); Assert.AreEqual(0, polar.x, _epsilon); Assert.IsFalse(float.IsNaN(polar.y)); } @@ -17,7 +17,7 @@ public void ConvertCartesianToPolar_Origin_ReturnsZeroDistance() { public void ConvertCartesianToPolar_OnXAxis_ReturnsZeroAzimuth() { var cartesian = new Vector2(5, 0); var expectedPolar = new Vector2(5.0f, 0.0f); - var actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); + Vector2 actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); Assert.That(actualPolar, Is.EqualTo(expectedPolar).Using(Vector2EqualityComparer.Instance)); } @@ -25,7 +25,7 @@ public void ConvertCartesianToPolar_OnXAxis_ReturnsZeroAzimuth() { public void ConvertCartesianToPolar_OnYAxis_ReturnsNinetyAzimuth() { var cartesian = new Vector2(0, 5); var expectedPolar = new Vector2(5.0f, 90.0f); - var actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); + Vector2 actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); Assert.That(actualPolar, Is.EqualTo(expectedPolar).Using(Vector2EqualityComparer.Instance)); } @@ -38,7 +38,7 @@ public void ConvertCartesianToPolar_OnYAxis_ReturnsNinetyAzimuth() { public void ConvertPolarToCartesian_ZeroDistance_ReturnsOrigin(float theta) { var polar = new Vector2(0, theta); var expectedCartesian = Vector2.zero; - var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); } @@ -47,7 +47,7 @@ public void ConvertPolarToCartesian_ZeroDistance_ReturnsOrigin(float theta) { public void ConvertPolarToCartesian_ZeroAzimuth_ReturnsOnXAxis() { var polar = new Vector2(10, 0); var expectedCartesian = new Vector2(10.0f, 0.0f); - var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); } @@ -65,7 +65,7 @@ public void ConvertPolarToCartesian_NinetyAzimuth_ReturnsOnYAxis() { public void ConvertPolarToCartesian_ThirtyAzimuth() { var polar = new Vector2(20, 30); var expectedCartesian = new Vector2(10 * Mathf.Sqrt(3), 10); - var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); } @@ -74,7 +74,7 @@ public void ConvertPolarToCartesian_ThirtyAzimuth() { public void ConvertPolarToCartesian_FortyFiveAzimuth() { var polar = new Vector2(10 * Mathf.Sqrt(2), 45); var expectedCartesian = new Vector2(10.0f, 10.0f); - var actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); } diff --git a/Assets/Tests/EditMode/Coordinates3Tests.cs b/Assets/Tests/EditMode/Coordinates3Tests.cs index 8b6410748..34b390c14 100644 --- a/Assets/Tests/EditMode/Coordinates3Tests.cs +++ b/Assets/Tests/EditMode/Coordinates3Tests.cs @@ -8,7 +8,7 @@ public class Coordinates3Tests { [Test] public void ConvertCartesianToSpherical_Origin_ReturnsZeroDistance() { var cartesian = Vector3.zero; - var spherical = Coordinates3.ConvertCartesianToSpherical(cartesian); + Vector3 spherical = Coordinates3.ConvertCartesianToSpherical(cartesian); Assert.AreEqual(0, spherical.x, _epsilon); Assert.IsFalse(float.IsNaN(spherical.y)); Assert.IsFalse(float.IsNaN(spherical.z)); @@ -18,7 +18,7 @@ public void ConvertCartesianToSpherical_Origin_ReturnsZeroDistance() { public void ConvertCartesianToSpherical_ReturnsCorrectly() { var cartesian = new Vector3(2, -1, 3); var expectedSpherical = new Vector3(Mathf.Sqrt(14), 33.690068f, -15.501360f); - var actualSpherical = Coordinates3.ConvertCartesianToSpherical(cartesian); + Vector3 actualSpherical = Coordinates3.ConvertCartesianToSpherical(cartesian); Assert.That(actualSpherical, Is.EqualTo(expectedSpherical).Using(Vector3EqualityComparer.Instance)); } @@ -37,7 +37,7 @@ public void ConvertCartesianToSpherical_ReturnsCorrectly() { public void ConvertSphericalToCartesian_ZeroDistance_ReturnsOrigin(float theta, float phi) { var spherical = new Vector3(0, theta, phi); var expectedCartesian = Vector3.zero; - var actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); + Vector3 actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); } @@ -46,7 +46,7 @@ public void ConvertSphericalToCartesian_ZeroDistance_ReturnsOrigin(float theta, public void ConvertSphericalToCartesian_ReturnsCorrectly() { var spherical = new Vector3(10, 60, 30); var expectedCartesian = new Vector3(7.5f, 5.0f, 4.3301270189221945f); - var actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); + Vector3 actualCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); } @@ -54,7 +54,7 @@ public void ConvertSphericalToCartesian_ReturnsCorrectly() { [Test] public void ConvertCartesianToCylindrical_Origin_ReturnsZeroDistanceZeroHeight() { var cartesian = Vector3.zero; - var cylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); + Vector3 cylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); Assert.AreEqual(0, cylindrical.x, _epsilon); Assert.IsFalse(float.IsNaN(cylindrical.y)); Assert.AreEqual(0, cylindrical.z, _epsilon); @@ -64,7 +64,7 @@ public void ConvertCartesianToCylindrical_Origin_ReturnsZeroDistanceZeroHeight() public void ConvertCartesianToCylindrical_ReturnsCorrectly() { var cartesian = new Vector3(2, -1, 3); var expectedCylindrical = new Vector3(Mathf.Sqrt(13), 33.6900675f, -1); - var actualCylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); + Vector3 actualCylindrical = Coordinates3.ConvertCartesianToCylindrical(cartesian); Assert.That(actualCylindrical, Is.EqualTo(expectedCylindrical).Using(Vector3EqualityComparer.Instance)); } @@ -78,7 +78,7 @@ public void ConvertCartesianToCylindrical_ReturnsCorrectly() { public void ConvertCylindricalToCartesian_ZeroDistance_ZeroHeight_ReturnsOrigin(float theta) { var cylindrical = new Vector3(0, theta, 0); var expectedCartesian = Vector3.zero; - var actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); + Vector3 actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); } @@ -87,7 +87,7 @@ public void ConvertCylindricalToCartesian_ZeroDistance_ZeroHeight_ReturnsOrigin( public void ConvertCylindricalToCartesian_ReturnsCorrectly() { var cylindrical = new Vector3(10, 60, 2); var expectedCartesian = new Vector3(8.660254f, 2.0f, 5.0f); - var actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); + Vector3 actualCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); } From 7fbb74554be3f9cf20d21d70a096e45ce837e150 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 13:24:50 -0700 Subject: [PATCH 64/74] Add argument annotations to FixedHierarchical constructor --- Assets/Scripts/Hierarchical/FixedHierarchical.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Hierarchical/FixedHierarchical.cs b/Assets/Scripts/Hierarchical/FixedHierarchical.cs index 7bfc68d65..6ef5aa47b 100644 --- a/Assets/Scripts/Hierarchical/FixedHierarchical.cs +++ b/Assets/Scripts/Hierarchical/FixedHierarchical.cs @@ -8,9 +8,10 @@ public class FixedHierarchical : HierarchicalBase { public new Vector3 Acceleration { get; set; } public FixedHierarchical() : this(position: Vector3.zero) {} - public FixedHierarchical(in Vector3 position) : this(position, velocity: Vector3.zero) {} + public FixedHierarchical(in Vector3 position) + : this(position: position, velocity: Vector3.zero) {} public FixedHierarchical(in Vector3 position, in Vector3 velocity) - : this(position, velocity, acceleration: Vector3.zero) {} + : this(position: position, velocity: velocity, acceleration: Vector3.zero) {} public FixedHierarchical(in Vector3 position, in Vector3 velocity, in Vector3 acceleration) { Position = position; Velocity = velocity; From a1c34d60583e6657b9a62890e6817cd13a36d534 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 14:08:44 -0700 Subject: [PATCH 65/74] Fix private const capitalization --- Assets/Scripts/Controller/WaypointController.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointController.cs index 2e2163936..baf1c0784 100644 --- a/Assets/Scripts/Controller/WaypointController.cs +++ b/Assets/Scripts/Controller/WaypointController.cs @@ -4,14 +4,14 @@ public class WaypointController : ControllerBase { // To prevent overshooting near the waypoint, if the squared distance to the waypoint is less than // the threshold, do not apply any acceleration. - private const float AccelerationCutoffDistanceSqr = 1.0f; + private const float _accelerationCutoffDistanceSqr = 1.0f; public WaypointController(IAgent agent) : base(agent) {} // Controller-dependent implementation of the control law. protected override Vector3 Plan(in Transformation relativeTransformation) { Vector3 relativePosition = relativeTransformation.Position.Cartesian; - if (relativeTransformation.Position.Range < AccelerationCutoffDistanceSqr) { + if (relativeTransformation.Position.Range < _accelerationCutoffDistanceSqr) { return Vector3.zero; } From 51e8d7657bf05798782ecb21744109cea310b591 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Sat, 18 Oct 2025 14:16:24 -0700 Subject: [PATCH 66/74] Clean up _maxNumIterations in k-means clusterer --- .../Algorithms/Clustering/KMeansClusterer.cs | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs index 63d797207..34047841e 100644 --- a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs @@ -4,6 +4,9 @@ // The k-means clusterer class performs k-means clustering. public class KMeansClusterer : ClustererBase { + // Default maximum number of iterations. + protected const int _defaultMaxNumIterations = 20; + // Convergence threshold. protected const float _epsilon = 1e-3f; @@ -11,12 +14,12 @@ public class KMeansClusterer : ClustererBase { private readonly int _k; // Maximum number of iterations. - private readonly int _maxIterations; + private readonly int _maxNumIterations; - public KMeansClusterer(int k) : this(k, maxIterations: 20) {} - public KMeansClusterer(int k, int maxIterations) { + public KMeansClusterer(int k) : this(k, _defaultMaxNumIterations) {} + public KMeansClusterer(int k, int maxNumIterations) { _k = k; - _maxIterations = maxIterations; + _maxNumIterations = maxNumIterations; } // Generate the clusters from the list of hierarchical objects. @@ -33,8 +36,8 @@ public override List Cluster(IEnumerable hierarchicals) } bool converged = false; - int iteration = 0; - while (!converged && iteration < _maxIterations) { + int numIteration = 0; + while (!converged && numIteration < _maxNumIterations) { AssignToClusters(clusters, hierarchicals); // Calculate the new clusters as the mean of all assigned hierarchical objects. @@ -53,7 +56,7 @@ public override List Cluster(IEnumerable hierarchicals) converged = false; } } - ++iteration; + ++numIteration; } AssignToClusters(clusters, hierarchicals); return clusters; From e702f14149cb2e1d8523cf8ebc3c71df6abf16f5 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Thu, 23 Oct 2025 22:43:14 -0700 Subject: [PATCH 67/74] Address comments --- Assets/Proto/Configs/dynamic_config.proto | 3 +- Assets/Scripts/Evasion/OrthogonalEvasion.cs | 23 +++--- .../Scripts/Generated/Proto/DynamicConfig.cs | 3 +- .../Tests/EditMode/OrthogonalEvasionTests.cs | 71 +++++++++++++------ 4 files changed, 63 insertions(+), 37 deletions(-) diff --git a/Assets/Proto/Configs/dynamic_config.proto b/Assets/Proto/Configs/dynamic_config.proto index eb088d61e..51296f17d 100644 --- a/Assets/Proto/Configs/dynamic_config.proto +++ b/Assets/Proto/Configs/dynamic_config.proto @@ -15,7 +15,8 @@ message EvasionConfig { // If true, evasion is enabled. bool enabled = 1; - // Range threshold in m at which the agent should evade. + // Range threshold in meters. The agent will evade when a pursuer is within + // this range threshold with a positive closing velocity. float range_threshold = 2; } diff --git a/Assets/Scripts/Evasion/OrthogonalEvasion.cs b/Assets/Scripts/Evasion/OrthogonalEvasion.cs index 4643722e2..6f4610db5 100644 --- a/Assets/Scripts/Evasion/OrthogonalEvasion.cs +++ b/Assets/Scripts/Evasion/OrthogonalEvasion.cs @@ -10,23 +10,19 @@ public OrthogonalEvasion(IAgent agent) : base(agent) {} // Determine whether to perform any evasive maneuvers. public override bool ShouldEvade(IAgent pursuer) { - // Default evasion range threshold in meters. - const float defaultEvasionRangeThreshold = 1000f; - if (!(Agent.AgentConfig?.DynamicConfig?.FlightConfig?.EvasionConfig?.Enabled ?? false)) { return false; } - float evasionRangeThreshold = - Agent.AgentConfig?.DynamicConfig?.FlightConfig?.EvasionConfig?.RangeThreshold ?? - defaultEvasionRangeThreshold; SensorOutput sensorOutput = Agent.Sensor.Sense(pursuer); + float evasionRangeThreshold = + Agent.AgentConfig.DynamicConfig.FlightConfig.EvasionConfig.RangeThreshold; return sensorOutput.Position.Range <= evasionRangeThreshold && sensorOutput.Velocity.Range < 0; } // Calculate the acceleration input to evade the pursuer. public override Vector3 Evade(IAgent pursuer) { - const float epsilon = 1e-12f; + const float epsilon = 1e-6f; const float groundProximityThresholdFactor = 5f; const float groundAvoidanceUpFactor = 5f; @@ -42,6 +38,9 @@ public override Vector3 Evade(IAgent pursuer) { // direction in which to evade. normalVelocity = pursuer.transform.right; } + // If the agent's velocity is aligned with the normal velocity, i.e., orthogonal to the + // pursuer's velocity, then the normal acceleration should be zero as the agent should continue + // in the same direction. Vector3 normalAccelerationDirection = Vector3.ProjectOnPlane(normalVelocity, agentVelocity).normalized; @@ -73,14 +72,8 @@ public override Vector3 Evade(IAgent pursuer) { } Vector3 normalAcceleration = normalAccelerationDirection * Agent.MaxNormalAcceleration(); - // Set the forward acceleration. - Vector3 forwardAcceleration = Vector3.zero; - if (Agent is IThreat threat) { - // Set the forward acceleration to reach the target speed within one time step. - float targetSpeed = threat.LookupPowerTable(Configs.Power.Max); - float speedError = targetSpeed - Agent.Speed; - forwardAcceleration = speedError / Time.fixedDeltaTime * Agent.transform.forward; - } + // Apply the maximum forward acceleration. + Vector3 forwardAcceleration = Agent.transform.forward * Agent.MaxForwardAcceleration(); return normalAcceleration + forwardAcceleration; } } diff --git a/Assets/Scripts/Generated/Proto/DynamicConfig.cs b/Assets/Scripts/Generated/Proto/DynamicConfig.cs index b85656ead..4b116fbb6 100644 --- a/Assets/Scripts/Generated/Proto/DynamicConfig.cs +++ b/Assets/Scripts/Generated/Proto/DynamicConfig.cs @@ -125,7 +125,8 @@ public bool Enabled { public const int RangeThresholdFieldNumber = 2; private float rangeThreshold_; /// - /// Range threshold in m at which the agent should evade. + /// Range threshold in meters. The agent will evade when a pursuer is within + /// this range threshold with a positive closing velocity. /// [global::System.Diagnostics.DebuggerNonUserCodeAttribute] [global::System.CodeDom.Compiler.GeneratedCode("protoc", null)] diff --git a/Assets/Tests/EditMode/OrthogonalEvasionTests.cs b/Assets/Tests/EditMode/OrthogonalEvasionTests.cs index e7734a0a9..f7d04d818 100644 --- a/Assets/Tests/EditMode/OrthogonalEvasionTests.cs +++ b/Assets/Tests/EditMode/OrthogonalEvasionTests.cs @@ -16,7 +16,7 @@ public void SetUp() { new Configs.AccelerationConfig() { MaxForwardAcceleration = 10, MaxReferenceNormalAcceleration = 5 / Constants.kGravity, - ReferenceSpeed = 100, + ReferenceSpeed = 1, }, }; _agent.AgentConfig = new Configs.AgentConfig() { @@ -30,6 +30,7 @@ public void SetUp() { }, }; Rigidbody agentRb = _agent.gameObject.AddComponent(); + _agent.transform.rotation = Quaternion.LookRotation(new Vector3(0, 0, 1)); InvokePrivateMethod(_agent, "Awake"); _agent.Sensor = new IdealSensor(_agent); _pursuer = new GameObject("Agent").AddComponent(); @@ -41,9 +42,9 @@ public void SetUp() { [Test] public void ShouldEvade_EvasionDisabled_ReturnsFalse() { _agent.Position = new Vector3(0, 0, 0); - _agent.Velocity = new Vector3(0, 2, 0); - _pursuer.Position = new Vector3(0, 100, 0); - _pursuer.Velocity = new Vector3(0, -1, 0); + _agent.Velocity = new Vector3(0, 0, 2); + _pursuer.Position = new Vector3(0, 0, 100); + _pursuer.Velocity = new Vector3(0, 0, -1); _agent.AgentConfig.DynamicConfig.FlightConfig.EvasionConfig.Enabled = false; Assert.IsFalse(_evasion.ShouldEvade(_pursuer)); } @@ -51,36 +52,66 @@ public void ShouldEvade_EvasionDisabled_ReturnsFalse() { [Test] public void ShouldEvade_OutsideRange_ReturnsFalse() { _agent.Position = new Vector3(0, 0, 0); - _agent.Velocity = new Vector3(0, 2, 0); - _pursuer.Position = new Vector3(0, 2000, 0); - _pursuer.Velocity = new Vector3(0, -1, 0); + _agent.Velocity = new Vector3(0, 0, 2); + _pursuer.Position = new Vector3(0, 0, 2000); + _pursuer.Velocity = new Vector3(0, 0, -1); Assert.IsFalse(_evasion.ShouldEvade(_pursuer)); } [Test] public void ShouldEvade_MovingAway_ReturnsFalse() { _agent.Position = new Vector3(0, 0, 0); - _agent.Velocity = new Vector3(0, 1, 0); - _pursuer.Position = new Vector3(0, 500, 0); - _pursuer.Velocity = new Vector3(0, 2, 0); + _agent.Velocity = new Vector3(0, 0, 1); + _pursuer.Position = new Vector3(0, 0, 500); + _pursuer.Velocity = new Vector3(0, 0, 2); Assert.IsFalse(_evasion.ShouldEvade(_pursuer)); } [Test] - public void Evade_NonZeroNormalAcceleration() { + public void Evade_AppliesMaxNormalAcceleration() { _agent.Position = new Vector3(0, 0, 0); - _agent.Velocity = new Vector3(1, 2, 0); - _pursuer.Position = new Vector3(0, 100, 0); - _pursuer.Velocity = new Vector3(0, -1, 0); - Assert.AreEqual(0, Vector3.Dot(_evasion.Evade(_pursuer), _agent.Velocity)); + _agent.Velocity = new Vector3(0, 0, 2); + _pursuer.Position = new Vector3(0, 0, 100); + _pursuer.Velocity = new Vector3(1, 0, -1); + Vector3 accelerationInput = _evasion.Evade(_pursuer); + Vector3 normalAccelerationInput = + Vector3.ProjectOnPlane(accelerationInput, _agent.transform.forward); + Assert.AreEqual(_agent.MaxNormalAcceleration(), normalAccelerationInput.magnitude, _epsilon); } [Test] - public void Evade_OrthogonalToPursuerVelocity() { + public void Evade_AppliesMaxForwardAcceleration() { _agent.Position = new Vector3(0, 0, 0); - _agent.Velocity = new Vector3(0, 2, 0); - _pursuer.Position = new Vector3(0, 100, 0); - _pursuer.Velocity = new Vector3(0, -1, 0); - Assert.AreEqual(0, Vector3.Dot(_evasion.Evade(_pursuer), _pursuer.Velocity)); + _agent.Velocity = new Vector3(0, 0, 2); + _pursuer.Position = new Vector3(0, 0, 100); + _pursuer.Velocity = new Vector3(1, 0, -1); + Vector3 accelerationInput = _evasion.Evade(_pursuer); + Vector3 forwardAccelerationInput = Vector3.Project(accelerationInput, _agent.transform.forward); + Assert.AreEqual(_agent.MaxForwardAcceleration(), forwardAccelerationInput.magnitude, _epsilon); + } + + [Test] + public void Evade_AgentAndPursuerAlignedVelocities_OrthogonalToPursuerVelocity() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 0, 100); + _pursuer.Position = new Vector3(0, 0, 100); + _pursuer.Velocity = _agent.Velocity; + Vector3 accelerationInput = _evasion.Evade(_pursuer); + Vector3 normalAccelerationInput = + Vector3.ProjectOnPlane(accelerationInput, _agent.transform.forward); + Assert.AreNotEqual(Vector3.zero, normalAccelerationInput); + Assert.AreEqual(0, Vector3.Dot(normalAccelerationInput, _pursuer.Velocity)); + } + + [Test] + public void Evade_AgentAndPursuerOrthogonalVelocities_AppliesNoNormalAcceleration() { + _agent.Position = new Vector3(0, 0, 0); + _agent.Velocity = new Vector3(0, 0, 100); + _pursuer.Position = new Vector3(100, 0, 0); + _pursuer.Velocity = new Vector3(-100, 0, 0); + Vector3 accelerationInput = _evasion.Evade(_pursuer); + Vector3 normalAccelerationInput = + Vector3.ProjectOnPlane(accelerationInput, _agent.transform.forward); + Assert.AreEqual(Vector3.zero, normalAccelerationInput); } } From 59cab920416b934ba6f8ee19aed991aa7b1a86a5 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 24 Oct 2025 09:55:07 -0700 Subject: [PATCH 68/74] Address comments --- Assets/Scripts/Behavior/AttackBehaviorBase.cs | 7 ++++-- .../Scripts/Behavior/DirectAttackBehavior.cs | 1 - Assets/Scripts/Behavior/FlightPlan.cs | 15 +++++++++++-- .../EditMode/DirectAttackBehaviorTests.cs | 22 +++++++++++++++++++ Assets/Tests/EditMode/FlightPlanTests.cs | 20 +++++++++++++++++ 5 files changed, 60 insertions(+), 5 deletions(-) diff --git a/Assets/Scripts/Behavior/AttackBehaviorBase.cs b/Assets/Scripts/Behavior/AttackBehaviorBase.cs index 7c9a5769a..1b1a37d27 100644 --- a/Assets/Scripts/Behavior/AttackBehaviorBase.cs +++ b/Assets/Scripts/Behavior/AttackBehaviorBase.cs @@ -14,13 +14,16 @@ public abstract class AttackBehaviorBase : IAttackBehavior { public Configs.AttackBehaviorConfig Config { get => _config; - set => _config = value; + set { + _config = value; + _flightPlan = null; + } } public FlightPlan FlightPlan { get { if (_flightPlan == null) { - _flightPlan = new FlightPlan(Config.FlightPlan); + _flightPlan = new FlightPlan(Config?.FlightPlan); } return _flightPlan; } diff --git a/Assets/Scripts/Behavior/DirectAttackBehavior.cs b/Assets/Scripts/Behavior/DirectAttackBehavior.cs index a69d28deb..149f309d0 100644 --- a/Assets/Scripts/Behavior/DirectAttackBehavior.cs +++ b/Assets/Scripts/Behavior/DirectAttackBehavior.cs @@ -33,7 +33,6 @@ public override (Vector3 waypointPosition, Configs.Power power) if (waypointIndex == FlightPlan.Waypoints.Count) { // This is the last waypoint, so target the final position with the last waypoint's power // setting. - waypointPosition = targetPosition; power = FlightPlan.Waypoints[FlightPlan.Waypoints.Count - 1].Power; return (waypointPosition, power); } diff --git a/Assets/Scripts/Behavior/FlightPlan.cs b/Assets/Scripts/Behavior/FlightPlan.cs index 29bb79d55..6532f9c74 100644 --- a/Assets/Scripts/Behavior/FlightPlan.cs +++ b/Assets/Scripts/Behavior/FlightPlan.cs @@ -5,14 +5,25 @@ // // The flight plan contains a sorted list of waypoints for the agent on the way to the asset. public class FlightPlan { + // Flight plan configuration. + private Configs.FlightPlan _config; + // List of waypoints sorted by distance in descending order. - List _waypoints; + private List _waypoints; - public Configs.FlightPlan Config { get; set; } + public Configs.FlightPlan Config { + get => _config; + set { + _config = value; + _waypoints = null; + } + } public IReadOnlyList Waypoints { get { if (_waypoints == null) { + // Sort the waypoints by distance in descending order to allow attack behaviors to iterate + // from the farthest waypoint to the closest one. _waypoints = Config.Waypoints.OrderByDescending(waypoint => waypoint.Distance).ToList(); } return _waypoints.AsReadOnly(); diff --git a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs index 97c90e7f4..80efe1c38 100644 --- a/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs +++ b/Assets/Tests/EditMode/DirectAttackBehaviorTests.cs @@ -43,31 +43,44 @@ public void SetUp() { [Test] public void GetNextWaypoint_BeyondFarthestWaypoint() { + // Distance to target is 1100. _agent.Position = new Vector3(-100, 0, 0); + // The farthest waypoint is (distance = 1000, altitude = 100). (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); + // Waypoint position is at target - direction * 1000 = 1000 - 1 * 1000 = 0 at altitude 100. Assert.AreEqual(new Vector3(0, 100, 0), waypoint); Assert.AreEqual(Configs.Power.Cruise, power); } [Test] public void GetNextWaypoint_AtFarthestWaypoint() { + // Distance to target is 1000. _agent.Position = new Vector3(0, 0, 0); + // There is a waypoint at distance = 1000, so the attack behavior chooses the next waypoint at + // (distance = 500, altitude = 50). (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); + // Waypoint position is at target - direction * 500 = 1000 - 1 * 500 = 500 at altitude 50. Assert.AreEqual(new Vector3(500, 50, 0), waypoint); Assert.AreEqual(Configs.Power.Mil, power); } [Test] public void GetNextWaypoint_BetweenWaypoints() { + // Distance to target is 400. _agent.Position = new Vector3(600, 0, 0); + // The next waypoint is at (distance = 100, altitude = 25). (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); + // Waypoint position is at target - direction * 100 = 1000 - 1 * 100 = 900 at altitude 25. Assert.AreEqual(new Vector3(900, 25, 0), waypoint); Assert.AreEqual(Configs.Power.Max, power); } [Test] public void GetNextWaypoint_PastClosestWaypoint() { + // Distance to target is 80. _agent.Position = new Vector3(920, 0, 0); + // The closest waypoint is at (distance = 100, altitude = 25), so the attack behavior chooses + // the target position as the next waypoint. (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPosition); Assert.AreEqual(new Vector3(1000, 0, 0), waypoint); Assert.AreEqual(Configs.Power.Max, power); @@ -75,18 +88,27 @@ public void GetNextWaypoint_PastClosestWaypoint() { [Test] public void GetNextWaypoint_AtFarthestWaypoint_WithNonzeroZ() { + // Distance to target is sqrt(800^2 + 600^2) = 1000. _agent.Position = new Vector3(0, 0, 0); + // There is a waypoint at distance = 1000, so the attack behavior chooses the next waypoint at + // (distance = 500, altitude = 50). (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); + // Waypoint position is at target - direction * 500 = (800, 600) - (400, 300) = (400, 300) at + // altitude 50. Assert.AreEqual(new Vector3(400, 50, 300), waypoint); Assert.AreEqual(Configs.Power.Mil, power); } [Test] public void GetNextWaypoint_BetweenWaypoints_WithNonzeroZ() { + // Distance to target is sqrt(360^2 + 270^2) = 450. _agent.Position = new Vector3(440, 0, 330); + // The next waypoint is at (distance = 100, altitude = 25). (Vector3 waypoint, Configs.Power power) = _attackBehavior.GetNextWaypoint(_targetPositionWithNonzeroZ); + // Waypoint position is at target - direction * 100 = (800, 600) - (80, 60) = (720, 540) at + // altitude 25. Assert.AreEqual(new Vector3(720, 25, 540), waypoint); Assert.AreEqual(Configs.Power.Max, power); } diff --git a/Assets/Tests/EditMode/FlightPlanTests.cs b/Assets/Tests/EditMode/FlightPlanTests.cs index c469215cf..a5934b20a 100644 --- a/Assets/Tests/EditMode/FlightPlanTests.cs +++ b/Assets/Tests/EditMode/FlightPlanTests.cs @@ -43,4 +43,24 @@ public void Waypoints_HandlesNullConfig() { var flightPlan = new FlightPlan(config: null); Assert.AreEqual(0, flightPlan.Waypoints.Count); } + + [Test] + public void Waypoints_IsResetAfterSettingConfig() { + var flightPlan = new FlightPlan(config: null); + Assert.AreEqual(0, flightPlan.Waypoints.Count); + flightPlan.Config = new Configs.FlightPlan() { + Type = Configs.FlightPlanType.DistanceToTarget, + Waypoints = + { + new Configs.FlightPlanWaypoint() { + Distance = 100, + Altitude = 25, + Power = Configs.Power.Max, + }, + }, + }; + Assert.AreEqual(1, flightPlan.Waypoints.Count); + Assert.AreEqual(100, flightPlan.Waypoints[0].Distance); + Assert.AreEqual(25, flightPlan.Waypoints[0].Altitude); + } } From 040cd523a1431dfbb0d7390a4f0e38d47bc116d2 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 24 Oct 2025 11:38:38 -0700 Subject: [PATCH 69/74] Address comments --- Assets/Scripts/Controller/PnController.cs | 6 +- .../Scripts/Controller/WaypointController.cs | 11 ++-- Assets/Tests/EditMode/ApnControllerTests.cs | 45 ++++++++++++++ Assets/Tests/EditMode/PnControllerTests.cs | 60 ++++++++++++++++++- .../Tests/EditMode/WaypointControllerTests.cs | 26 ++++++++ 5 files changed, 140 insertions(+), 8 deletions(-) diff --git a/Assets/Scripts/Controller/PnController.cs b/Assets/Scripts/Controller/PnController.cs index 5e86f694e..783831018 100644 --- a/Assets/Scripts/Controller/PnController.cs +++ b/Assets/Scripts/Controller/PnController.cs @@ -21,7 +21,7 @@ public PnController(IAgent agent, float gain) : base(agent) { // Controller-dependent implementation of the control law. protected override Vector3 Plan(in Transformation relativeTransformation) { // Cache the transform, velocity, and speed. - var agentTransform = Agent.transform; + Transform agentTransform = Agent.transform; Vector3 right = agentTransform.right; Vector3 up = agentTransform.up; @@ -35,8 +35,8 @@ protected override Vector3 Plan(in Transformation relativeTransformation) { // Set the turn factor, which is equal to the closing velocity by default. float turnFactor = closingVelocity; - // Handle a negative closing velocity. In this case, since the target is moving away from the - // agent, apply a stronger turn. + // Handle a negative closing velocity. If the target is moving away from the agent, apply a + // stronger turn as the agent most likely passed the target already and should turn around. if (closingVelocity < 0) { turnFactor = Mathf.Max(1f, Mathf.Abs(closingVelocity) * _negativeClosingVelocityTurnFactor); } diff --git a/Assets/Scripts/Controller/WaypointController.cs b/Assets/Scripts/Controller/WaypointController.cs index baf1c0784..9f89eceff 100644 --- a/Assets/Scripts/Controller/WaypointController.cs +++ b/Assets/Scripts/Controller/WaypointController.cs @@ -2,22 +2,25 @@ // The waypoint controller steers the agent to the target using direct linear guidance. public class WaypointController : ControllerBase { - // To prevent overshooting near the waypoint, if the squared distance to the waypoint is less than - // the threshold, do not apply any acceleration. - private const float _accelerationCutoffDistanceSqr = 1.0f; + // To prevent overshooting near the waypoint, if the distance to the waypoint is less than the + // threshold, do not apply any acceleration. + private const float _accelerationCutoffDistance = 1.0f; public WaypointController(IAgent agent) : base(agent) {} // Controller-dependent implementation of the control law. protected override Vector3 Plan(in Transformation relativeTransformation) { Vector3 relativePosition = relativeTransformation.Position.Cartesian; - if (relativeTransformation.Position.Range < _accelerationCutoffDistanceSqr) { + if (relativeTransformation.Position.Range < _accelerationCutoffDistance) { return Vector3.zero; } // To reach the waypoint as fast as possible, use the maximum forward acceleration. Vector3 forward = Agent.transform.forward; Vector3 forwardAccelerationInput = forward * Agent.MaxForwardAcceleration(); + if (Vector3.Dot(relativePosition, Agent.Velocity) < 0) { + forwardAccelerationInput *= -1; + } // Steer as hard as possible towards the waypoint. Vector3 normalAcceleration = Vector3.ProjectOnPlane(relativePosition, forward).normalized; diff --git a/Assets/Tests/EditMode/ApnControllerTests.cs b/Assets/Tests/EditMode/ApnControllerTests.cs index 495b4c6ed..1a88e77fb 100644 --- a/Assets/Tests/EditMode/ApnControllerTests.cs +++ b/Assets/Tests/EditMode/ApnControllerTests.cs @@ -20,14 +20,59 @@ public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingUpwards() { _agent.HierarchicalAgent.TargetModel = new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, -1), acceleration: new Vector3(0, 1, 0)); + // Vertical PN acceleration = gain * closing velocity * elevation line-of-sight rate = 1 * 1 * 1 + // = 1. Vertical APN feedforward acceleration = 0.5 * gain * y-acceleration = 1/2 * 1 * 1 = 0.5. Assert.AreEqual(new Vector3(0, 1.5f, 0), _controller.Plan()); } + [Test] + public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingUpwards_WithNoAcceleration() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, -1)); + // Vertical PN acceleration = gain * closing velocity * elevation line-of-sight rate = 1 * 1 * 1 + // = 1. + Assert.AreEqual(new Vector3(0, 1, 0), _controller.Plan()); + } + + [Test] + public void + Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingUpwards_WithAccelerationAlongBoresight() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 0, -5)); + // Vertical PN acceleration = gain * closing velocity * elevation line-of-sight rate = 1 * 1 * 1 + // = 1. + Assert.AreEqual(new Vector3(0, 1, 0), _controller.Plan()); + } + [Test] public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingLeft() { _agent.HierarchicalAgent.TargetModel = new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(-1, 0, -1), acceleration: new Vector3(0, 1, 0)); + // Horizontal PN acceleration = gain * closing velocity * azimuth line-of-sight rate = 1 * 1 * + // -1 = -1. Vertical APN feedforward acceleration = 0.5 * gain * y-acceleration = 1/2 * 1 * 1 = + // 0.5. Assert.AreEqual(new Vector3(-1, 0.5f, 0), _controller.Plan()); } + + [Test] + public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingLeft_WithNoAcceleration() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(-1, 0, -1)); + // Horizontal PN acceleration = gain * closing velocity * azimuth line-of-sight rate = 1 * 1 * + // -1 = -1. + Assert.AreEqual(new Vector3(-1, 0, 0), _controller.Plan()); + } + + [Test] + public void + Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingLeft_WithAccelerationAlongBoresight() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(-1, 0, -1), + acceleration: new Vector3(0, 0, -5)); + // Horizontal PN acceleration = gain * closing velocity * azimuth line-of-sight rate = 1 * 1 * + // -1 = -1. + Assert.AreEqual(new Vector3(-1, 0, 0), _controller.Plan()); + } } diff --git a/Assets/Tests/EditMode/PnControllerTests.cs b/Assets/Tests/EditMode/PnControllerTests.cs index 9de277515..7188475a5 100644 --- a/Assets/Tests/EditMode/PnControllerTests.cs +++ b/Assets/Tests/EditMode/PnControllerTests.cs @@ -32,13 +32,23 @@ public void Plan_TargetAtBoresight_ZeroClosingVelocity_ReturnsZero() { _agent.HierarchicalAgent.TargetModel = new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, 0)); Assert.AreEqual(Vector3.zero, _controller.Plan()); - Assert.AreEqual(Vector3.zero, _controller.Plan()); + } + + [Test] + public void Plan_TargetAtBoresight_NegativeClosingVelocity_ReturnsZero() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, 1)); + // Vertical acceleration = gain * closing velocity * elevation line-of-sight rate = 1 * 1 * 1 + // = 1. The acceleration is multiplied by a turn factor of 100 to force a quicker turn. + Assert.AreEqual(new Vector3(0, 1, 0) * 100, _controller.Plan()); } [Test] public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingUpwards() { _agent.HierarchicalAgent.TargetModel = new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(0, 1, -1)); + // Vertical acceleration = gain * closing velocity * elevation line-of-sight rate = 1 * 1 * 1 + // = 1. Assert.AreEqual(new Vector3(0, 1, 0), _controller.Plan()); } @@ -46,6 +56,54 @@ public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingUpwards() { public void Plan_TargetAtBoresight_NonzeroClosingVelocity_MovingLeft() { _agent.HierarchicalAgent.TargetModel = new FixedHierarchical(position: new Vector3(0, 0, 1), velocity: new Vector3(-1, 0, -1)); + // Horizontal acceleration = gain * closing velocity * azimuth line-of-sight rate = 1 * 1 * -1 = + // -1. Assert.AreEqual(new Vector3(-1, 0, 0), _controller.Plan()); } + + [Test] + public void Plan_TargetAtBroadside_MovingInParallel() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(1, 0, 0), velocity: new Vector3(-1, 0, 1)); + // Horizontal acceleration = gain * closing velocity * azimuth line-of-sight rate = 1 * 1 * -1 = + // -1. Vertical acceleration is clamped, so gain * closing velocity * elevation line-of-sight + // rate = 1 * 1 * 0.2 = 0.2. The acceleration is multiplied by a turn factor of 100 to force a + // quicker turn. + Assert.AreEqual(new Vector3(-1, 0.2f, 0) * 100, _controller.Plan()); + } + + [Test] + public void Plan_TargetAtBroadside_MovingUpwards() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(1, 0, 0), velocity: new Vector3(-1, 1, 0)); + // Horizontal acceleration is clamped, so gain * closing velocity * azimuth line-of-sight rate = + // 1 * 1 * 0.2 = 0.2. Vertical acceleration = gain * closing velocity * elevation line-of-sight + // rate = 1 * 1 * 1 = 1. The acceleration is multiplied by a turn factor of 100 to force a + // quicker turn. + Assert.AreEqual(new Vector3(0.2f, 1, 0) * 100, _controller.Plan()); + } + + [Test] + public void Plan_TargetOverhead_MovingInParallel() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 1, 0), velocity: new Vector3(0, -1, 1)); + // Horizontal acceleration is clamped, so gain * closing velocity * azimuth line-of-sight rate = + // 1 * 1 * 0.2 = 0.2. + // Vertical acceleration = gain * closing velocity * elevation line-of-sight rate = 1 * 1 * -1 = + // -1. + // The acceleration is multiplied by a turn factor of 100 to force a quicker turn. + Assert.AreEqual(new Vector3(0.2f, -1, 0) * 100, _controller.Plan()); + } + + [Test] + public void Plan_TargetOverhead_MovingRight() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 1, 0), velocity: new Vector3(1, -1, 0)); + // Horizontal acceleration = gain * closing velocity * azimuth line-of-sight rate = 1 * 1 * 1 + // = 1. + // Vertical acceleration is clamped, so gain * closing velocity * elevation line-of-sight rate = + // 1 * 1 * 0.2 = 0.2. The acceleration is multiplied by a turn factor of 100 to force a quicker + // turn. + Assert.AreEqual(new Vector3(1, 0.2f, 0) * 100, _controller.Plan()); + } } diff --git a/Assets/Tests/EditMode/WaypointControllerTests.cs b/Assets/Tests/EditMode/WaypointControllerTests.cs index 401788eab..cf8d77936 100644 --- a/Assets/Tests/EditMode/WaypointControllerTests.cs +++ b/Assets/Tests/EditMode/WaypointControllerTests.cs @@ -47,4 +47,30 @@ public void Plan_UsesMaximumNormalAcceleration() { acceleration: new Vector3(0, 1, 0)); Assert.AreEqual(5, _controller.Plan().y); } + + [Test] + public void Plan_WaypointBehind_UsesMaximumNegativeForwardAcceleration() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 0, -1), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(-10, _controller.Plan().z); + } + + [Test] + public void Plan_WaypointAtRightBoresight_AcceleratesToTheRight() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(1, 0, 0), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(5, _controller.Plan().x); + Assert.AreEqual(0, _controller.Plan().y); + } + + [Test] + public void Plan_WaypointOverhead_AcceleratesUpwards() { + _agent.HierarchicalAgent.TargetModel = + new FixedHierarchical(position: new Vector3(0, 1, 0), velocity: new Vector3(0, 1, -1), + acceleration: new Vector3(0, 1, 0)); + Assert.AreEqual(0, _controller.Plan().x); + Assert.AreEqual(5, _controller.Plan().y); + } } From 2b20b5d6147453159620d1e1cc7638c3b970bbd9 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 24 Oct 2025 12:12:13 -0700 Subject: [PATCH 70/74] Address comments --- .../Algorithms/Planning/IterativeLaunchPlanner.cs | 6 +++--- .../Planning/LaunchAngleCsvInterpolator.cs | 9 +++++---- .../Algorithms/Planning/LaunchAnglePlannerBase.cs | 2 +- .../Algorithms/Prediction/LinearExtrapolator.cs | 1 - Assets/Scripts/Utils/KDTree.cs | 4 ++++ Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs | 9 +++++++++ .../EditMode/LaunchAngleCsvInterpolatorTests.cs | 4 ---- .../EditMode/LaunchAngleDataInterpolatorBaseTests.cs | 12 ++++++++++++ .../EditMode/NearestNeighborInterpolator2DTests.cs | 2 ++ 9 files changed, 36 insertions(+), 13 deletions(-) diff --git a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs index 775d05686..c50595ef6 100644 --- a/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs +++ b/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs @@ -64,9 +64,9 @@ public override LaunchPlan Plan() { // Check that the interceptor is moving towards the target. If the target is moving too fast, // the interceptor might be launched backwards because the intercept position and the predicted // position are behind the asset. In this case, the interceptor should wait to be launched. - Vector3 interceptorToInterceptPosition = interceptPosition; - Vector3 threatToPredictedPosition = targetPosition - initialState.Position; - if (Vector3.Dot(interceptorToInterceptPosition, threatToPredictedPosition) > 0) { + Vector3 interceptorToInterceptPosition = interceptPosition - LaunchAnglePlanner.Agent.Position; + Vector3 targetToPredictedTargetPosition = targetPosition - initialState.Position; + if (Vector3.Dot(interceptorToInterceptPosition, targetToPredictedTargetPosition) > 0) { return LaunchPlan.NoLaunch(); } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs index ebcd078d4..8e0e33f25 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAngleCsvInterpolator.cs @@ -29,20 +29,21 @@ public LaunchAngleCsvInterpolator(IAgent agent, string path, ConfigLoaderDelegat protected override void InitInterpolator() { string fileContent = _configLoader(_relativePath); if (string.IsNullOrEmpty(fileContent)) { - Debug.LogError($"Failed to load CSV file from {_relativePath}."); - throw new InvalidOperationException("Interpolator could not be initialized."); + throw new InvalidOperationException($"CSV file {_relativePath} is either null or empty."); } string[] csvLines = fileContent.Split(new[] { "\r\n", "\r", "\n" }, StringSplitOptions.None); string[] dataLines = csvLines.Where(line => !string.IsNullOrWhiteSpace(line)).ToArray(); if (dataLines.Length < 1) { - throw new InvalidOperationException("No data points available for interpolation."); + throw new InvalidOperationException( + $"No data points available for interpolation in {_relativePath}."); } try { _interpolator = new NearestNeighborInterpolator2D(dataLines); } catch (Exception e) { - throw new InvalidOperationException("Failed to initialize interpolator: " + e.Message); + throw new InvalidOperationException( + $"Failed to initialize interpolator from {_relativePath}.", e); } } } diff --git a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs index 50ae9e141..747eac169 100644 --- a/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs +++ b/Assets/Scripts/Algorithms/Planning/LaunchAnglePlannerBase.cs @@ -25,6 +25,6 @@ protected Direction ConvertToRelativeDirection(in Vector3 position) { Vector3 relativePosition = position - Agent.Position; return new Direction { Distance = Vector3.ProjectOnPlane(relativePosition, Vector3.up).magnitude, - Altitude = Vector3.Project(relativePosition, Vector3.up).magnitude }; + Altitude = Vector3.Project(relativePosition, Vector3.up).y }; } } diff --git a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs index 7a869c07b..8da1ee821 100644 --- a/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs +++ b/Assets/Scripts/Algorithms/Prediction/LinearExtrapolator.cs @@ -1,4 +1,3 @@ -using System.Linq; using UnityEngine; // The linear extrapolator class predicts the trajectory of an agent by linearly extrapolating its diff --git a/Assets/Scripts/Utils/KDTree.cs b/Assets/Scripts/Utils/KDTree.cs index 89806cb31..444876097 100644 --- a/Assets/Scripts/Utils/KDTree.cs +++ b/Assets/Scripts/Utils/KDTree.cs @@ -57,6 +57,10 @@ private KDNode NearestNeighbor(KDNode node, in Vector2 target, int depth, // Explore the next branch first. bestNode = NearestNeighbor(nextBranch, target, depth + 1, bestNode); + if (bestNode != null) { + bestDistance = Vector2.Distance(_getCoordinates(bestNode.Data), target); + ; + } // Explore the other branch. if (Mathf.Abs(targetCoordinatesValue - nodeCoordinatesValue) < bestDistance) { diff --git a/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs b/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs index 544c04e92..c8c2ddd6a 100644 --- a/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs +++ b/Assets/Tests/EditMode/IterativeLaunchPlannerTests.cs @@ -52,6 +52,9 @@ public void Plan_InterceptAtDataPoint_ReturnsLaunch() { _target.Velocity = new Vector3(0, -1, 0); LaunchPlan plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); + // Target moves from (1, 110, 0) with a velocity of (0, -1, 0) for 10 seconds to reach (1, 100, + // 0). + // The interceptor can reach (1, 100, 0) after 10 seconds with a launch angle of 90 degrees. Assert.AreEqual(90, plan.LaunchAngle); Assert.AreEqual(new Vector3(1, 100, 0), plan.InterceptPosition); } @@ -62,6 +65,9 @@ public void Plan_InterceptNearDataPoint_ReturnsLaunch() { _target.Velocity = new Vector3(0, -1.1f, 0); LaunchPlan plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); + // Target moves from (1, 110, 0) with a velocity of (0, -1.1, 0) for 10 seconds to reach (1, 99, + // 0). + // The interceptor can reach (1, 100, 0) after 10 seconds with a launch angle of 90 degrees. Assert.AreEqual(90, plan.LaunchAngle); Assert.AreEqual(new Vector3(1, 99, 0), plan.InterceptPosition); } @@ -72,6 +78,9 @@ public void Plan_InterceptBetweenDataPoints_ReturnsLaunch() { _target.Velocity = new Vector3(-5, 0, 0); LaunchPlan plan = _planner.Plan(); Assert.IsTrue(plan.ShouldLaunch); + // Target moves from (126, 1, 0) with a velocity of (-5, 0, 0) for 13 seconds to reach (61, 1, + // 0). + // The interceptor can reach (60, 1, 0) after 13 seconds with a launch angle of 20 degrees. Assert.AreEqual(20, plan.LaunchAngle); Assert.AreEqual(new Vector3(61, 1, 0), plan.InterceptPosition); } diff --git a/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs b/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs index 9de94a63c..c21d72d6b 100644 --- a/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs +++ b/Assets/Tests/EditMode/LaunchAngleCsvInterpolatorTests.cs @@ -38,8 +38,6 @@ public void Plan_NearDataPoint_ReturnsClosestPoint() { [Test] public void Plan_FileNotFound_ThrowsException() { - LogAssert.ignoreFailingMessages = true; - // Inject a mock ConfigLoader that returns an empty string. LaunchAngleCsvInterpolator.ConfigLoaderDelegate mockLoader = (string path) => ""; Assert.Throws(() => { @@ -51,8 +49,6 @@ public void Plan_FileNotFound_ThrowsException() { [Test] public void Plan_InvalidInterpolationData_ThrowsException() { - LogAssert.ignoreFailingMessages = true; - // Create a mock CSV with invalid data format. string mockCsv = "invalid,csv,data\n1,2,3"; LaunchAngleCsvInterpolator.ConfigLoaderDelegate mockLoader = (string path) => mockCsv; diff --git a/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs index 72f57aa6f..9dbddd179 100644 --- a/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs +++ b/Assets/Tests/EditMode/LaunchAngleDataInterpolatorBaseTests.cs @@ -61,6 +61,18 @@ public void InterceptPosition_NearDataPoint_ReturnsClosestPoint() { Is.EqualTo(new Vector3(100f, 1f, 0f)).Using(Vector3EqualityComparer.Instance)); } + [Test] + public void InterceptPosition_PreservesAzimuth() { + var targetPosition = new Vector3(70.7f, 1f, 70.7f); // 45 degree azimuth + Vector3 interceptPosition = _interpolator.InterceptPosition(targetPosition); + + // Should preserve the 45 degree azimuth while snapping to nearest data point at (distance = + // 100, altitude = 1). + float expectedAzimuth = Mathf.Atan2(70.7f, 70.7f) * Mathf.Rad2Deg; + float actualAzimuth = Mathf.Atan2(interceptPosition.z, interceptPosition.x) * Mathf.Rad2Deg; + Assert.AreEqual(expectedAzimuth, actualAzimuth, 0.1f); + } + [Test] public void InterceptPosition_MovesWithAgentPosition() { var agentPosition = new Vector3(61, 5055, 874); diff --git a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs index 6fc860403..8871eb916 100644 --- a/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs +++ b/Assets/Tests/EditMode/NearestNeighborInterpolator2DTests.cs @@ -20,6 +20,7 @@ public void Interpolate_InvalidPoints_ReturnsEmptyResult() { Assert.AreEqual(0, interpolator.Data.Count); Interpolator2DDataPoint result = interpolator.Interpolate(1, 1); Assert.AreEqual(0, result.Data.Count); + LogAssert.ignoreFailingMessages = false; } [Test] @@ -75,5 +76,6 @@ public void Interpolate_NoDataPoints_ReturnsEmptyResult() { var interpolator = new NearestNeighborInterpolator2D(csvLines); Interpolator2DDataPoint result = interpolator.Interpolate(1, 1); Assert.AreEqual(0, result.Data.Count); + LogAssert.ignoreFailingMessages = false; } } From d786635947884518e9df8e10a5dc9428201696f9 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 24 Oct 2025 12:19:47 -0700 Subject: [PATCH 71/74] Address comments --- Assets/Tests/EditMode/Coordinates2Tests.cs | 21 +++++++++++----- Assets/Tests/EditMode/Coordinates3Tests.cs | 28 +++++++++++++++++++++- 2 files changed, 42 insertions(+), 7 deletions(-) diff --git a/Assets/Tests/EditMode/Coordinates2Tests.cs b/Assets/Tests/EditMode/Coordinates2Tests.cs index 49c4a3233..79a69f21e 100644 --- a/Assets/Tests/EditMode/Coordinates2Tests.cs +++ b/Assets/Tests/EditMode/Coordinates2Tests.cs @@ -14,7 +14,7 @@ public void ConvertCartesianToPolar_Origin_ReturnsZeroDistance() { } [Test] - public void ConvertCartesianToPolar_OnXAxis_ReturnsZeroAzimuth() { + public void ConvertCartesianToPolar_OnXAxis_ReturnsZeroTheta() { var cartesian = new Vector2(5, 0); var expectedPolar = new Vector2(5.0f, 0.0f); Vector2 actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); @@ -22,7 +22,7 @@ public void ConvertCartesianToPolar_OnXAxis_ReturnsZeroAzimuth() { } [Test] - public void ConvertCartesianToPolar_OnYAxis_ReturnsNinetyAzimuth() { + public void ConvertCartesianToPolar_OnYAxis_ReturnsNinetyTheta() { var cartesian = new Vector2(0, 5); var expectedPolar = new Vector2(5.0f, 90.0f); Vector2 actualPolar = Coordinates2.ConvertCartesianToPolar(cartesian); @@ -44,7 +44,7 @@ public void ConvertPolarToCartesian_ZeroDistance_ReturnsOrigin(float theta) { } [Test] - public void ConvertPolarToCartesian_ZeroAzimuth_ReturnsOnXAxis() { + public void ConvertPolarToCartesian_ZeroTheta_ReturnsOnXAxis() { var polar = new Vector2(10, 0); var expectedCartesian = new Vector2(10.0f, 0.0f); Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); @@ -53,7 +53,7 @@ public void ConvertPolarToCartesian_ZeroAzimuth_ReturnsOnXAxis() { } [Test] - public void ConvertPolarToCartesian_NinetyAzimuth_ReturnsOnYAxis() { + public void ConvertPolarToCartesian_NinetyTheta_ReturnsOnYAxis() { Vector2 polar = new Vector2(10, 90); Vector2 expectedCartesian = new Vector2(0.0f, 10.0f); Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); @@ -62,7 +62,7 @@ public void ConvertPolarToCartesian_NinetyAzimuth_ReturnsOnYAxis() { } [Test] - public void ConvertPolarToCartesian_ThirtyAzimuth() { + public void ConvertPolarToCartesian_ThirtyTheta() { var polar = new Vector2(20, 30); var expectedCartesian = new Vector2(10 * Mathf.Sqrt(3), 10); Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); @@ -71,11 +71,20 @@ public void ConvertPolarToCartesian_ThirtyAzimuth() { } [Test] - public void ConvertPolarToCartesian_FortyFiveAzimuth() { + public void ConvertPolarToCartesian_FortyFiveTheta() { var polar = new Vector2(10 * Mathf.Sqrt(2), 45); var expectedCartesian = new Vector2(10.0f, 10.0f); Vector2 actualCartesian = Coordinates2.ConvertPolarToCartesian(polar); Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector2EqualityComparer.Instance)); } + + [Test] + public void RoundTrip_CartesianToPolarToCartesian_ReturnsOriginal() { + var originalCartesian = new Vector2(3.5f, 7.2f); + Vector2 polar = Coordinates2.ConvertCartesianToPolar(originalCartesian); + Vector2 resultCartesian = Coordinates2.ConvertPolarToCartesian(polar); + Assert.That(resultCartesian, + Is.EqualTo(originalCartesian).Using(Vector2EqualityComparer.Instance)); + } } diff --git a/Assets/Tests/EditMode/Coordinates3Tests.cs b/Assets/Tests/EditMode/Coordinates3Tests.cs index 34b390c14..b2fd8f34f 100644 --- a/Assets/Tests/EditMode/Coordinates3Tests.cs +++ b/Assets/Tests/EditMode/Coordinates3Tests.cs @@ -29,7 +29,6 @@ public void ConvertCartesianToSpherical_ReturnsCorrectly() { [TestCase(90f, 0f)] [TestCase(180f, 0f)] [TestCase(360f, 0f)] - [TestCase(45f, 0f)] [TestCase(45f, 45f)] [TestCase(45f, 90f)] [TestCase(45f, 180f)] @@ -51,6 +50,19 @@ public void ConvertSphericalToCartesian_ReturnsCorrectly() { Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); } + [TestCase(1.5f, 2.5f, 3.5f)] + [TestCase(-1, 2, 3)] + [TestCase(-12, 0, 0)] + [TestCase(0, 5, 0)] + [TestCase(0, 0, -10)] + public void RoundTrip_CartesianToSphericalToCartesian_ReturnsOriginal(float x, float y, float z) { + var originalCartesian = new Vector3(x, y, z); + Vector3 spherical = Coordinates3.ConvertCartesianToSpherical(originalCartesian); + Vector3 resultCartesian = Coordinates3.ConvertSphericalToCartesian(spherical); + Assert.That(resultCartesian, + Is.EqualTo(originalCartesian).Using(Vector3EqualityComparer.Instance)); + } + [Test] public void ConvertCartesianToCylindrical_Origin_ReturnsZeroDistanceZeroHeight() { var cartesian = Vector3.zero; @@ -91,4 +103,18 @@ public void ConvertCylindricalToCartesian_ReturnsCorrectly() { Assert.That(actualCartesian, Is.EqualTo(expectedCartesian).Using(Vector3EqualityComparer.Instance)); } + + [TestCase(1.5f, 2.5f, 3.5f)] + [TestCase(-1, 2, 3)] + [TestCase(-12, 0, 0)] + [TestCase(0, 5, 0)] + [TestCase(0, 0, -10)] + public void RoundTrip_CartesianToCylindricalToCartesian_ReturnsOriginal(float x, float y, + float z) { + var originalCartesian = new Vector3(x, y, z); + Vector3 cylindrical = Coordinates3.ConvertCartesianToCylindrical(originalCartesian); + Vector3 resultCartesian = Coordinates3.ConvertCylindricalToCartesian(cylindrical); + Assert.That(resultCartesian, + Is.EqualTo(originalCartesian).Using(Vector3EqualityComparer.Instance)); + } } From 1327336514d1253ee4121b2c6fb1e13106b30d94 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Fri, 24 Oct 2025 12:20:10 -0700 Subject: [PATCH 72/74] Remove debug log statements from unit tests --- Assets/Tests/EditMode/AgglomerativeClustererTests.cs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs index 05fa1de48..40119ada6 100644 --- a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs +++ b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs @@ -49,8 +49,6 @@ public void Cluster_ZeroRadius_ReturnsSingletonClusters() { public void Cluster_SmallRadius_ReturnsMultipleClusters() { _clusterer = new AgglomerativeClusterer(maxSize: _hierarchicals.Count, maxRadius: 0.5f); List clusters = _clusterer.Cluster(_hierarchicals); - Debug.Log( - $"{clusters[0].Centroid} {clusters[1].Centroid} {clusters[0].Radius()} {clusters[1].Radius()}"); Assert.AreEqual(3, clusters.Count); List sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); Assert.AreEqual(1, sortedClusters[0].Size); @@ -68,8 +66,6 @@ public void Cluster_SmallRadius_ReturnsMultipleClusters() { public void Cluster_SmallSize_ReturnsMultipleClusters() { _clusterer = new AgglomerativeClusterer(maxSize: 2, maxRadius: 1); List clusters = _clusterer.Cluster(_hierarchicals); - Debug.Log( - $"{clusters[0].Centroid} {clusters[1].Centroid} {clusters[0].Size} {clusters[1].Size}"); Assert.AreEqual(3, clusters.Count); List sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); Assert.AreEqual(1, sortedClusters[0].Size); From 1a377adb951323b0de882cc998e0cf0a65eddb7e Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Mon, 27 Oct 2025 23:11:46 -0700 Subject: [PATCH 73/74] Address comments --- .../Clustering/AgglomerativeClusterer.cs | 8 ++++ .../Clustering/ConstrainedKMeansClusterer.cs | 4 ++ .../Algorithms/Clustering/KMeansClusterer.cs | 21 +++++++++- .../SizeAndRadiusConstrainedClustererBase.cs | 9 +++++ .../Scripts/Hierarchical/HierarchicalBase.cs | 6 ++- .../EditMode/AgglomerativeClustererTests.cs | 38 +++++++++++++------ .../ConstrainedKMeansClustererTests.cs | 14 +++++++ .../Tests/EditMode/HierarchicalBaseTests.cs | 13 +++++++ Assets/Tests/EditMode/KMeansClustererTests.cs | 25 +++++++++++- 9 files changed, 122 insertions(+), 16 deletions(-) diff --git a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs index e912eb699..b33b7b9f2 100644 --- a/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/AgglomerativeClusterer.cs @@ -1,4 +1,5 @@ using System.Collections.Generic; +using System.Linq; using UnityEngine; // The agglomerative clusterer performs agglomerative clustering with the stopping condition given @@ -9,6 +10,10 @@ public AgglomerativeClusterer(int maxSize, float maxRadius) : base(maxSize, maxR // Generate the clusters from the list of hierarchical objects. public override List Cluster(IEnumerable hierarchicals) { var clusters = new List(); + if (hierarchicals == null || !hierarchicals.Any()) { + return clusters; + } + foreach (var hierarchical in hierarchicals) { var cluster = new Cluster { Centroid = hierarchical.Position }; cluster.AddSubHierarchical(hierarchical); @@ -69,6 +74,9 @@ public override List Cluster(IEnumerable hierarchicals) // Update the distances matrix using the distance between the cluster centroids. // TODO(titan): Change the distance metric to use average or maximum linkage. foreach (int i in validClusterIndices) { + if (i == clusterIndex2) { + continue; + } float distance = Vector3.Distance(clusters[clusterIndex2].Centroid, clusters[i].Centroid); if (i < clusterIndex2) { distances[clusterIndex2, i] = distance; diff --git a/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs index 906992f16..afb16df5f 100644 --- a/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/ConstrainedKMeansClusterer.cs @@ -8,6 +8,10 @@ public ConstrainedKMeansClusterer(int maxSize, float maxRadius) : base(maxSize, // Generate the clusters from the list of hierarchical objects. public override List Cluster(IEnumerable hierarchicals) { + if (hierarchicals == null || !hierarchicals.Any()) { + return new List(); + } + int numClusters = (int)Mathf.Ceil(hierarchicals.Count() / _maxSize); KMeansClusterer clusterer; List clusters; diff --git a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs index 34047841e..2ec82e233 100644 --- a/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs +++ b/Assets/Scripts/Algorithms/Clustering/KMeansClusterer.cs @@ -1,3 +1,4 @@ +using System; using System.Collections.Generic; using System.Linq; using UnityEngine; @@ -24,12 +25,23 @@ public KMeansClusterer(int k, int maxNumIterations) { // Generate the clusters from the list of hierarchical objects. public override List Cluster(IEnumerable hierarchicals) { + if (hierarchicals == null || !hierarchicals.Any()) { + return new List(); + } + // Initialize the clusters with centroids located at the positions of k random hierarchical // objects. Perform Fisher-Yates shuffling to find k random hierarchical objects. List hierarchicalsList = hierarchicals.ToList(); + + // Validate that k is not greater than the number of hierarchical objects. + if (_k > hierarchicalsList.Count) { + throw new InvalidOperationException( + $"Cannot create {_k} clusters from {hierarchicalsList.Count} hierarchical objects."); + } + var clusters = new List(); for (int i = 0; i < _k; ++i) { - int j = Random.Range(i, hierarchicalsList.Count); + int j = UnityEngine.Random.Range(i, hierarchicalsList.Count); (hierarchicalsList[i], hierarchicalsList[j]) = (hierarchicalsList[j], hierarchicalsList[i]); clusters.Add(new Cluster { Centroid = hierarchicalsList[i].Position }); @@ -45,7 +57,7 @@ public override List Cluster(IEnumerable hierarchicals) for (int clusterIndex = 0; clusterIndex < clusters.Count; ++clusterIndex) { Vector3 oldClusterPosition = clusters[clusterIndex].Centroid; if (clusters[clusterIndex].IsEmpty) { - int hierarchicalIndex = Random.Range(0, hierarchicalsList.Count); + int hierarchicalIndex = UnityEngine.Random.Range(0, hierarchicalsList.Count); clusters[clusterIndex].Centroid = hierarchicalsList[hierarchicalIndex].Position; } else { clusters[clusterIndex].Recenter(); @@ -64,6 +76,11 @@ public override List Cluster(IEnumerable hierarchicals) private static void AssignToClusters(IReadOnlyList clusters, IEnumerable hierarchicals) { + // Clear all clusters. + foreach (var cluster in clusters) { + cluster.ClearSubHierarchicals(); + } + // Determine the closest centroid to each hierarchical object. foreach (var hierarchical in hierarchicals) { float minDistance = Mathf.Infinity; diff --git a/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs index a24119e92..0643f456e 100644 --- a/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs +++ b/Assets/Scripts/Algorithms/Clustering/SizeAndRadiusConstrainedClustererBase.cs @@ -1,3 +1,5 @@ +using System; + // Base implementation of a size and radius-constrained clustering algorithm. // // The size is defined as the maximum number of hierarchical objects within a cluster, and the @@ -11,6 +13,13 @@ public abstract class SizeAndRadiusConstrainedClustererBase : ClustererBase { protected readonly float _maxRadius = 0; public SizeAndRadiusConstrainedClustererBase(int maxSize, float maxRadius) { + if (maxSize <= 0) { + throw new ArgumentOutOfRangeException("Maximum size must be positive.", nameof(maxSize)); + } + if (maxRadius < 0) { + throw new ArgumentOutOfRangeException("Maximum radius must be non-negative.", + nameof(maxRadius)); + } _maxSize = maxSize; _maxRadius = maxRadius; } diff --git a/Assets/Scripts/Hierarchical/HierarchicalBase.cs b/Assets/Scripts/Hierarchical/HierarchicalBase.cs index e53f95bf1..1019dfc09 100644 --- a/Assets/Scripts/Hierarchical/HierarchicalBase.cs +++ b/Assets/Scripts/Hierarchical/HierarchicalBase.cs @@ -7,7 +7,7 @@ // velocities of the sub-hierarchical objects. public class HierarchicalBase : IHierarchical { // List of hierarchical objects in the hierarchy level below. - private List _subHierarchicals = new List(); + protected List _subHierarchicals = new List(); // List of hierarchical objects pursuing this hierarchical object. private List _pursuers = new List(); @@ -38,6 +38,10 @@ public void RemoveSubHierarchical(IHierarchical subHierarchical) { _subHierarchicals.Remove(subHierarchical); } + public void ClearSubHierarchicals() { + _subHierarchicals.Clear(); + } + public void AddPursuer(IHierarchical pursuer) { if (!_pursuers.Contains(pursuer)) { _pursuers.Add(pursuer); diff --git a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs index 40119ada6..f5ad230e4 100644 --- a/Assets/Tests/EditMode/AgglomerativeClustererTests.cs +++ b/Assets/Tests/EditMode/AgglomerativeClustererTests.cs @@ -52,14 +52,14 @@ public void Cluster_SmallRadius_ReturnsMultipleClusters() { Assert.AreEqual(3, clusters.Count); List sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); Assert.AreEqual(1, sortedClusters[0].Size); - Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Centroid); - Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Position); + Assert.AreEqual(new Vector3(0, 0, 0), sortedClusters[0].Centroid); + Assert.AreEqual(new Vector3(0, 0, 0), sortedClusters[0].Position); Assert.AreEqual(2, sortedClusters[1].Size); - Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Centroid); - Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Position); + Assert.AreEqual(new Vector3(0, 1.25f, 0), sortedClusters[1].Centroid); + Assert.AreEqual(new Vector3(0, 1.25f, 0), sortedClusters[1].Position); Assert.AreEqual(1, sortedClusters[2].Size); - Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Centroid); - Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Position); + Assert.AreEqual(new Vector3(0, 2.5f, 0), sortedClusters[2].Centroid); + Assert.AreEqual(new Vector3(0, 2.5f, 0), sortedClusters[2].Position); } [Test] @@ -69,13 +69,27 @@ public void Cluster_SmallSize_ReturnsMultipleClusters() { Assert.AreEqual(3, clusters.Count); List sortedClusters = clusters.OrderBy(cluster => cluster.Centroid[1]).ToList(); Assert.AreEqual(1, sortedClusters[0].Size); - Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Centroid); - Assert.AreEqual(new Vector3(0, 0, 0), clusters[0].Position); + Assert.AreEqual(new Vector3(0, 0, 0), sortedClusters[0].Centroid); + Assert.AreEqual(new Vector3(0, 0, 0), sortedClusters[0].Position); Assert.AreEqual(2, sortedClusters[1].Size); - Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Centroid); - Assert.AreEqual(new Vector3(0, 1.25f, 0), clusters[1].Position); + Assert.AreEqual(new Vector3(0, 1.25f, 0), sortedClusters[1].Centroid); + Assert.AreEqual(new Vector3(0, 1.25f, 0), sortedClusters[1].Position); Assert.AreEqual(1, sortedClusters[2].Size); - Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Centroid); - Assert.AreEqual(new Vector3(0, 2.5f, 0), clusters[2].Position); + Assert.AreEqual(new Vector3(0, 2.5f, 0), sortedClusters[2].Centroid); + Assert.AreEqual(new Vector3(0, 2.5f, 0), sortedClusters[2].Position); + } + + [Test] + public void Cluster_Null_ReturnsNoClusters() { + _clusterer = new AgglomerativeClusterer(maxSize: 2, maxRadius: 1); + List clusters = _clusterer.Cluster(hierarchicals: null); + Assert.AreEqual(0, clusters.Count); + } + + [Test] + public void Cluster_EmptyList_ReturnsNoClusters() { + _clusterer = new AgglomerativeClusterer(maxSize: 2, maxRadius: 1); + List clusters = _clusterer.Cluster(hierarchicals: new List()); + Assert.AreEqual(0, clusters.Count); } } diff --git a/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs index 0d80133a6..cbb734f80 100644 --- a/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs +++ b/Assets/Tests/EditMode/ConstrainedKMeansClustererTests.cs @@ -55,4 +55,18 @@ public void Cluster_SmallRadius_ReturnsMultipleClusters() { List clusters = _clusterer.Cluster(_hierarchicals); Assert.AreEqual(2, clusters.Count); } + + [Test] + public void Cluster_Null_ReturnsNoClusters() { + _clusterer = new ConstrainedKMeansClusterer(maxSize: 2, maxRadius: 1); + List clusters = _clusterer.Cluster(hierarchicals: null); + Assert.AreEqual(0, clusters.Count); + } + + [Test] + public void Cluster_EmptyList_ReturnsNoClusters() { + _clusterer = new ConstrainedKMeansClusterer(maxSize: 2, maxRadius: 1); + List clusters = _clusterer.Cluster(hierarchicals: new List()); + Assert.AreEqual(0, clusters.Count); + } } diff --git a/Assets/Tests/EditMode/HierarchicalBaseTests.cs b/Assets/Tests/EditMode/HierarchicalBaseTests.cs index ee881abda..8eabce4fe 100644 --- a/Assets/Tests/EditMode/HierarchicalBaseTests.cs +++ b/Assets/Tests/EditMode/HierarchicalBaseTests.cs @@ -135,6 +135,19 @@ public void RemoveSubHierarchical_DoesNotRemoveNonExisting() { Assert.AreSame(child1, parent.SubHierarchicals[0]); } + [Test] + public void ClearSubHierarchicals_RemovesAllSubHierarchicals() { + var parent = new HierarchicalBase(); + var child1 = new HierarchicalBase(); + var child2 = new HierarchicalBase(); + parent.AddSubHierarchical(child1); + parent.AddSubHierarchical(child2); + + Assert.AreEqual(2, parent.SubHierarchicals.Count); + parent.ClearSubHierarchicals(); + Assert.AreEqual(0, parent.SubHierarchicals.Count); + } + [Test] public void AddPursuer_AddsCorrectly() { var parent = new HierarchicalBase(); diff --git a/Assets/Tests/EditMode/KMeansClustererTests.cs b/Assets/Tests/EditMode/KMeansClustererTests.cs index 0484280a3..ea2d5c774 100644 --- a/Assets/Tests/EditMode/KMeansClustererTests.cs +++ b/Assets/Tests/EditMode/KMeansClustererTests.cs @@ -1,4 +1,5 @@ using NUnit.Framework; +using System; using System.Collections.Generic; using System.Linq; using UnityEngine; @@ -15,7 +16,7 @@ public class KMeansClustererTests { [SetUp] public void SetUp() { - Random.InitState(5); + UnityEngine.Random.InitState(5); } [Test] @@ -68,4 +69,26 @@ public void Cluster_TwoDistinctClusters() { Assert.AreEqual(new Vector3(10.5f, 10.5f, 10), clusterB.Centroid); Assert.AreEqual(new Vector3(10.5f, 10.5f, 10), clusterB.Position); } + + [Test] + public void Cluster_KGreaterThanNumberOfSubHierarchicals_ThrowsException() { + _clusterer = new KMeansClusterer(k: _hierarchicals.Count + 1); + + Assert.Throws( + () => { List clusters = _clusterer.Cluster(_hierarchicals); }); + } + + [Test] + public void Cluster_Null_ReturnsNoClusters() { + _clusterer = new KMeansClusterer(k: 2); + List clusters = _clusterer.Cluster(hierarchicals: null); + Assert.AreEqual(0, clusters.Count); + } + + [Test] + public void Cluster_EmptyList_ReturnsNoClusters() { + _clusterer = new KMeansClusterer(k: 2); + List clusters = _clusterer.Cluster(hierarchicals: new List()); + Assert.AreEqual(0, clusters.Count); + } } From 7108370b78f76dbcaa90356b6b17f05ea7dd33d0 Mon Sep 17 00:00:00 2001 From: Titan Yuan Date: Tue, 28 Oct 2025 16:23:15 -0700 Subject: [PATCH 74/74] Remove unnecessary newline --- Assets/Tests/EditMode/KMeansClustererTests.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/Assets/Tests/EditMode/KMeansClustererTests.cs b/Assets/Tests/EditMode/KMeansClustererTests.cs index ea2d5c774..259f8fbf8 100644 --- a/Assets/Tests/EditMode/KMeansClustererTests.cs +++ b/Assets/Tests/EditMode/KMeansClustererTests.cs @@ -73,7 +73,6 @@ public void Cluster_TwoDistinctClusters() { [Test] public void Cluster_KGreaterThanNumberOfSubHierarchicals_ThrowsException() { _clusterer = new KMeansClusterer(k: _hierarchicals.Count + 1); - Assert.Throws( () => { List clusters = _clusterer.Cluster(_hierarchicals); }); }