| | | 1 | | using System; |
| | | 2 | | using UnityEngine; |
| | | 3 | | |
| | | 4 | | // Base implementation of an agent. |
| | | 5 | | // |
| | | 6 | | // See the agent interface for property and method documentation. |
| | | 7 | | public class AgentBase : MonoBehaviour, IAgent, ICommsEndpoint { |
| | | 8 | | public event Action<IAgent> OnTerminated; |
| | | 9 | | |
| | | 10 | | private const float _epsilon = 1e-12f; |
| | | 11 | | |
| | | 12 | | // Rigid body component. |
| | | 13 | | protected Rigidbody _rigidbody; |
| | | 14 | | |
| | | 15 | | // The position field is cached and is updated before every fixed update. |
| | | 16 | | [SerializeField] |
| | | 17 | | private Vector3 _position; |
| | | 18 | | |
| | | 19 | | // The acceleration field is not part of the rigid body component, so it is tracked separately. |
| | | 20 | | // The acceleration is applied as a force during each frame update. |
| | | 21 | | [SerializeField] |
| | | 22 | | private Vector3 _acceleration; |
| | | 23 | | |
| | | 24 | | // The acceleration input is calculated by the controller and provided to the movement behavior. |
| | | 25 | | [SerializeField] |
| | | 26 | | private Vector3 _accelerationInput; |
| | | 27 | | |
| | | 28 | | // The agent's position within the hierarchical strategy is given by the hierarchical agent. |
| | | 29 | | [SerializeReference] |
| | | 30 | | private HierarchicalAgent _hierarchicalAgent; |
| | | 31 | | |
| | | 32 | | // Static configuration of the agent, including agent type, unit cost, acceleration configuration, |
| | | 33 | | // aerodynamics parameters, power table, and visualization configuration. |
| | | 34 | | private Configs.StaticConfig _staticConfig; |
| | | 35 | | |
| | | 36 | | // Agent configuration, including initial state, attack behavior configuration (for threats), |
| | | 37 | | // dynamic configuration, and sub-agent configuration (for interceptors). |
| | | 38 | | private Configs.AgentConfig _agentConfig; |
| | | 39 | | |
| | | 40 | | // Last sensing time. |
| | | 41 | | [SerializeField] |
| | 173 | 42 | | private float _lastSensingTime = Mathf.NegativeInfinity; |
| | | 43 | | |
| | | 44 | | public HierarchicalAgent HierarchicalAgent { |
| | 0 | 45 | | get => _hierarchicalAgent; |
| | 0 | 46 | | set => _hierarchicalAgent = value; |
| | | 47 | | } |
| | | 48 | | public Configs.StaticConfig StaticConfig { |
| | 405 | 49 | | get => _staticConfig; |
| | 64 | 50 | | set { |
| | 64 | 51 | | _staticConfig = value; |
| | 64 | 52 | | _rigidbody.mass = StaticConfig.BodyConfig?.Mass ?? 1; |
| | 64 | 53 | | } |
| | | 54 | | } |
| | | 55 | | public Configs.AgentConfig AgentConfig { |
| | 13 | 56 | | get => _agentConfig; |
| | 7 | 57 | | set { |
| | 7 | 58 | | _agentConfig = value; |
| | 7 | 59 | | UpdateAgentConfig(); |
| | 7 | 60 | | } |
| | | 61 | | } |
| | | 62 | | |
| | 0 | 63 | | public IMovement Movement { get; set; } |
| | 0 | 64 | | public IController Controller { get; set; } |
| | 16 | 65 | | public ISensor Sensor { get; set; } |
| | 70 | 66 | | public IAgent TargetModel { get; set; } |
| | | 67 | | |
| | | 68 | | public Vector3 Position { |
| | 274 | 69 | | get => _position; |
| | 90 | 70 | | set { |
| | 90 | 71 | | Transform.position = value; |
| | 90 | 72 | | _position = value; |
| | 90 | 73 | | } |
| | | 74 | | } |
| | | 75 | | public Vector3 Velocity { |
| | 340 | 76 | | get => _rigidbody.linearVelocity; |
| | 122 | 77 | | set => _rigidbody.linearVelocity = value; |
| | | 78 | | } |
| | 117 | 79 | | public float Speed => Velocity.magnitude; |
| | | 80 | | public Vector3 Acceleration { |
| | 25 | 81 | | get => _acceleration; |
| | 10 | 82 | | set => _acceleration = value; |
| | | 83 | | } |
| | | 84 | | public Vector3 AccelerationInput { |
| | 0 | 85 | | get => _accelerationInput; |
| | 0 | 86 | | set => _accelerationInput = value; |
| | | 87 | | } |
| | | 88 | | |
| | | 89 | | // If true, the agent is able to pursue targets. |
| | 0 | 90 | | public virtual bool IsPursuer => true; |
| | | 91 | | |
| | | 92 | | // Elapsed time since the creation of the agent. |
| | 181 | 93 | | public float ElapsedTime { get; private set; } = 0f; |
| | | 94 | | |
| | | 95 | | // If true, the agent is terminated. |
| | 173 | 96 | | public bool IsTerminated { get; private set; } = false; |
| | | 97 | | |
| | | 98 | | // The agent transform is cached. |
| | 1183 | 99 | | public Transform Transform { get; private set; } |
| | | 100 | | |
| | | 101 | | // The up direction is cached and updated before every fixed update. |
| | 173 | 102 | | public Vector3 Up { get; private set; } |
| | | 103 | | |
| | | 104 | | // The forward direction is cached and updated before every fixed update. |
| | 244 | 105 | | public Vector3 Forward { get; private set; } |
| | | 106 | | |
| | | 107 | | // The right direction is cached and updated before every fixed update. |
| | 174 | 108 | | public Vector3 Right { get; private set; } |
| | | 109 | | |
| | | 110 | | // The inverse rotation is cached and updated before every fixed update. |
| | 291 | 111 | | public Quaternion InverseRotation { get; private set; } |
| | | 112 | | |
| | | 113 | | // Communication node managed by the communication manager. |
| | 0 | 114 | | public CommsNode CommsNode { get; set; } |
| | | 115 | | |
| | 26 | 116 | | public float MaxForwardAcceleration() { |
| | 26 | 117 | | return StaticConfig.AccelerationConfig?.MaxForwardAcceleration ?? 0; |
| | 26 | 118 | | } |
| | | 119 | | |
| | 65 | 120 | | public float MaxNormalAcceleration() { |
| | 65 | 121 | | float maxReferenceNormalAcceleration = |
| | | 122 | | (StaticConfig.AccelerationConfig?.MaxReferenceNormalAcceleration ?? |
| | | 123 | | float.PositiveInfinity) * |
| | | 124 | | Constants.kGravity; |
| | 65 | 125 | | float referenceSpeed = StaticConfig.AccelerationConfig?.ReferenceSpeed ?? 1; |
| | 65 | 126 | | return Mathf.Pow(Speed / referenceSpeed, 2) * maxReferenceNormalAcceleration; |
| | 65 | 127 | | } |
| | | 128 | | |
| | 0 | 129 | | public void CreateTargetModel(IHierarchical target) { |
| | 0 | 130 | | TargetModel = SimManager.Instance.CreateDummyAgent(target.Position, target.Velocity); |
| | 0 | 131 | | } |
| | | 132 | | |
| | 0 | 133 | | public void DestroyTargetModel() { |
| | 0 | 134 | | if (TargetModel != null) { |
| | 0 | 135 | | SimManager.Instance.DestroyDummyAgent(TargetModel); |
| | 0 | 136 | | TargetModel = null; |
| | 0 | 137 | | } |
| | 0 | 138 | | } |
| | | 139 | | |
| | 0 | 140 | | public void UpdateTargetModel() { |
| | 0 | 141 | | if (HierarchicalAgent == null || HierarchicalAgent.Target == null || Sensor == null) { |
| | 0 | 142 | | return; |
| | | 143 | | } |
| | 0 | 144 | | if (HierarchicalAgent.Target.IsTerminated) { |
| | 0 | 145 | | HierarchicalAgent.Target = null; |
| | 0 | 146 | | return; |
| | | 147 | | } |
| | | 148 | | |
| | | 149 | | // Check whether the sensing period has elapsed. |
| | 0 | 150 | | float sensingFrequency = AgentConfig?.DynamicConfig?.SensorConfig?.Frequency ?? Mathf.Infinity; |
| | 0 | 151 | | float sensingPeriod = 1f / sensingFrequency; |
| | 0 | 152 | | if (ElapsedTime - _lastSensingTime >= sensingPeriod) { |
| | | 153 | | // Sense the target. |
| | 0 | 154 | | SensorOutput sensorOutput = Sensor.Sense(HierarchicalAgent.Target); |
| | 0 | 155 | | TargetModel.Position = Position + sensorOutput.Position.Cartesian; |
| | 0 | 156 | | TargetModel.Velocity = Velocity + sensorOutput.Velocity.Cartesian; |
| | 0 | 157 | | TargetModel.Acceleration = Acceleration + sensorOutput.Acceleration.Cartesian; |
| | 0 | 158 | | _lastSensingTime = ElapsedTime; |
| | 0 | 159 | | } |
| | 0 | 160 | | } |
| | | 161 | | |
| | 25 | 162 | | public Transformation GetRelativeTransformation(IAgent target) { |
| | 25 | 163 | | return GetRelativeTransformation(target.Position, target.Velocity, target.Acceleration); |
| | 25 | 164 | | } |
| | | 165 | | |
| | 32 | 166 | | public Transformation GetRelativeTransformation(IHierarchical target) { |
| | 32 | 167 | | return GetRelativeTransformation(target.Position, target.Velocity, target.Acceleration); |
| | 32 | 168 | | } |
| | | 169 | | |
| | 2 | 170 | | public Transformation GetRelativeTransformation(in Vector3 waypoint) { |
| | 2 | 171 | | return GetRelativeTransformation(waypoint, velocity: Vector3.zero, acceleration: Vector3.zero); |
| | 2 | 172 | | } |
| | | 173 | | |
| | 0 | 174 | | public void Terminate() { |
| | 0 | 175 | | if (HierarchicalAgent != null) { |
| | 0 | 176 | | HierarchicalAgent.Target = null; |
| | 0 | 177 | | } |
| | 0 | 178 | | if (Movement is MissileMovement movement) { |
| | 0 | 179 | | movement.FlightPhase = Simulation.FlightPhase.Terminated; |
| | 0 | 180 | | } |
| | 0 | 181 | | IsTerminated = true; |
| | 0 | 182 | | OnTerminated?.Invoke(this); |
| | 0 | 183 | | Destroy(gameObject); |
| | 0 | 184 | | } |
| | | 185 | | |
| | | 186 | | // Awake is called before Start and right after a prefab is instantiated. |
| | 173 | 187 | | protected virtual void Awake() { |
| | 173 | 188 | | Transform = transform; |
| | 173 | 189 | | _rigidbody = GetComponent<Rigidbody>(); |
| | | 190 | | |
| | 173 | 191 | | UpdateTransformData(); |
| | 173 | 192 | | if (EarlyFixedUpdateManager.Instance != null) { |
| | 0 | 193 | | EarlyFixedUpdateManager.Instance.OnEarlyFixedUpdate += UpdateTransformData; |
| | 0 | 194 | | } |
| | 173 | 195 | | } |
| | | 196 | | |
| | | 197 | | // Start is called before the first frame update. |
| | 0 | 198 | | protected virtual void Start() {} |
| | | 199 | | |
| | | 200 | | // FixedUpdate is called multiple times per frame. All physics calculations and updates occur |
| | | 201 | | // immediately after FixedUpdate, and all movement values are multiplied by Time.deltaTime. |
| | 0 | 202 | | protected virtual void FixedUpdate() { |
| | 0 | 203 | | ElapsedTime += Time.fixedDeltaTime; |
| | | 204 | | |
| | 0 | 205 | | UpdateTargetModel(); |
| | 0 | 206 | | AlignWithVelocity(); |
| | 0 | 207 | | } |
| | | 208 | | |
| | | 209 | | // Update is called every frame. |
| | 0 | 210 | | protected virtual void Update() {} |
| | | 211 | | |
| | | 212 | | // LateUpdate is called every frame after all Update functions have been called. |
| | 0 | 213 | | protected virtual void LateUpdate() {} |
| | | 214 | | |
| | | 215 | | // OnDestroy is called when the object is being destroyed. |
| | 0 | 216 | | protected virtual void OnDestroy() { |
| | 0 | 217 | | if (EarlyFixedUpdateManager.Instance != null) { |
| | 0 | 218 | | EarlyFixedUpdateManager.Instance.OnEarlyFixedUpdate -= UpdateTransformData; |
| | 0 | 219 | | } |
| | 0 | 220 | | } |
| | | 221 | | |
| | | 222 | | // UpdateAgentConfig is called whenever the agent configuration is changed. |
| | 7 | 223 | | protected virtual void UpdateAgentConfig() { |
| | | 224 | | // Set the sensor. |
| | 7 | 225 | | switch (AgentConfig.DynamicConfig?.SensorConfig?.Type) { |
| | 7 | 226 | | case Simulation.SensorType.Ideal: { |
| | 7 | 227 | | Sensor = new IdealSensor(this); |
| | 7 | 228 | | break; |
| | | 229 | | } |
| | 0 | 230 | | default: { |
| | 0 | 231 | | Debug.LogWarning($"Sensor type {AgentConfig.DynamicConfig?.SensorConfig?.Type} not found."); |
| | 0 | 232 | | break; |
| | | 233 | | } |
| | | 234 | | } |
| | 7 | 235 | | } |
| | | 236 | | |
| | 0 | 237 | | protected virtual void OnDrawGizmos() { |
| | 0 | 238 | | if (Application.isPlaying) { |
| | 0 | 239 | | Gizmos.color = Color.green; |
| | 0 | 240 | | Gizmos.DrawRay(Position, _accelerationInput); |
| | 0 | 241 | | } |
| | 0 | 242 | | } |
| | | 243 | | |
| | 0 | 244 | | protected bool CheckGroundCollision(Collider other) { |
| | | 245 | | // Check if the agent hit the ground with a negative vertical speed. |
| | 0 | 246 | | return other.gameObject.name == "Floor" && Vector3.Dot(Velocity, Vector3.up) < 0; |
| | 0 | 247 | | } |
| | | 248 | | |
| | 0 | 249 | | protected bool ShouldIgnoreCollision(IAgent otherAgent) { |
| | | 250 | | // Dummy agents are virtual targets and should not trigger collisions. |
| | 0 | 251 | | return otherAgent == null || otherAgent is DummyAgent || otherAgent.IsTerminated; |
| | 0 | 252 | | } |
| | | 253 | | |
| | 173 | 254 | | private void UpdateTransformData() { |
| | 173 | 255 | | _position = Transform.position; |
| | 173 | 256 | | Up = Transform.up; |
| | 173 | 257 | | Forward = Transform.forward; |
| | 173 | 258 | | Right = Transform.right; |
| | 173 | 259 | | InverseRotation = Quaternion.Inverse(Transform.rotation); |
| | 173 | 260 | | } |
| | | 261 | | |
| | 0 | 262 | | private void AlignWithVelocity() { |
| | | 263 | | const float speedThreshold = 0.1f; |
| | | 264 | | const float rotationSpeedDegreesPerSecond = 10000f; |
| | | 265 | | |
| | | 266 | | // Only align if the velocity is significant. |
| | 0 | 267 | | if (Speed > speedThreshold) { |
| | | 268 | | // Create a rotation with the forward direction along the velocity vector and the up direction |
| | | 269 | | // along world up. |
| | 0 | 270 | | Quaternion targetRotation = Quaternion.LookRotation(Velocity, Vector3.up); |
| | | 271 | | |
| | | 272 | | // Smoothly rotate towards the target rotation. |
| | 0 | 273 | | Transform.rotation = Quaternion.RotateTowards( |
| | | 274 | | Transform.rotation, targetRotation, |
| | | 275 | | maxDegreesDelta: rotationSpeedDegreesPerSecond * Time.fixedDeltaTime); |
| | 0 | 276 | | } |
| | 0 | 277 | | } |
| | | 278 | | |
| | | 279 | | private Transformation GetRelativeTransformation(in Vector3 position, in Vector3 velocity, |
| | 59 | 280 | | in Vector3 acceleration) { |
| | 59 | 281 | | Vector3 relativePosition = position - Position; |
| | 59 | 282 | | Vector3 relativeLocalPosition = InverseRotation * relativePosition; |
| | 59 | 283 | | Vector3 relativeVelocity = velocity - Velocity; |
| | 59 | 284 | | Vector3 relativeLocalVelocity = InverseRotation * relativeVelocity; |
| | | 285 | | |
| | 59 | 286 | | float x = relativeLocalPosition.x; |
| | 59 | 287 | | float y = relativeLocalPosition.y; |
| | 59 | 288 | | float z = relativeLocalPosition.z; |
| | | 289 | | |
| | 59 | 290 | | float horizontalSqr = x * x + z * z; |
| | 59 | 291 | | float horizontal = Mathf.Sqrt(horizontalSqr); |
| | 59 | 292 | | float rangeSqr = horizontalSqr + y * y; |
| | 59 | 293 | | float range = Mathf.Sqrt(rangeSqr); |
| | | 294 | | |
| | 59 | 295 | | float azimuth = Mathf.Atan2(x, z); |
| | 59 | 296 | | float elevation = Mathf.Atan2(y, horizontal); |
| | 59 | 297 | | var positionTransformation = new PositionTransformation { |
| | | 298 | | Cartesian = relativePosition, |
| | | 299 | | Range = range, |
| | | 300 | | Azimuth = azimuth, |
| | | 301 | | Elevation = elevation, |
| | | 302 | | }; |
| | | 303 | | |
| | 59 | 304 | | float rangeRate = |
| | | 305 | | range > _epsilon ? Vector3.Dot(relativeLocalVelocity, relativeLocalPosition) / range : 0f; |
| | 59 | 306 | | float azimuthRate = 0f; |
| | 59 | 307 | | float elevationRate = 0f; |
| | 105 | 308 | | if (horizontal > _epsilon) { |
| | 46 | 309 | | azimuthRate = -(x * relativeLocalVelocity.z - z * relativeLocalVelocity.x) / horizontalSqr; |
| | 46 | 310 | | elevationRate = |
| | | 311 | | (relativeLocalVelocity.y * horizontal - |
| | | 312 | | y * (x * relativeLocalVelocity.x + z * relativeLocalVelocity.z) / horizontal) / |
| | | 313 | | rangeSqr; |
| | 59 | 314 | | } else { |
| | | 315 | | // The other agent is exactly above or below. |
| | 13 | 316 | | azimuthRate = 0f; |
| | 13 | 317 | | float horizontalSpeed = Mathf.Sqrt(relativeLocalVelocity.x * relativeLocalVelocity.x + |
| | | 318 | | relativeLocalVelocity.z * relativeLocalVelocity.z); |
| | 13 | 319 | | elevationRate = -horizontalSpeed / (Mathf.Abs(y) > _epsilon ? y : Mathf.Sign(y) * _epsilon); |
| | 13 | 320 | | } |
| | 59 | 321 | | var velocityTransformation = new VelocityTransformation { |
| | | 322 | | Cartesian = relativeVelocity, |
| | | 323 | | Range = rangeRate, |
| | | 324 | | Azimuth = azimuthRate, |
| | | 325 | | Elevation = elevationRate, |
| | | 326 | | }; |
| | | 327 | | |
| | 59 | 328 | | var accelerationTransformation = new AccelerationTransformation { |
| | | 329 | | Cartesian = acceleration, |
| | | 330 | | }; |
| | 59 | 331 | | return new Transformation { |
| | | 332 | | Position = positionTransformation, |
| | | 333 | | Velocity = velocityTransformation, |
| | | 334 | | Acceleration = accelerationTransformation, |
| | | 335 | | }; |
| | 59 | 336 | | } |
| | | 337 | | } |