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