| | 1 | | using System.Collections; |
| | 2 | | using System.Collections.Generic; |
| | 3 | | using UnityEngine; |
| | 4 | |
|
| | 5 | | public abstract class Threat : Agent { |
| | 6 | | protected AttackBehavior _attackBehavior; |
| | 7 | | [SerializeField] |
| | 8 | | protected Vector3 _currentWaypoint; |
| | 9 | | [SerializeField] |
| | 10 | | protected PowerSetting _currentPowerSetting; |
| | 11 | |
|
| | 12 | | protected SensorOutput _sensorOutput; |
| | 13 | | protected Sensor _sensor; |
| | 14 | |
|
| 7 | 15 | | public void SetAttackBehavior(AttackBehavior attackBehavior) { |
| 7 | 16 | | _attackBehavior = attackBehavior; |
| 7 | 17 | | _target = SimManager.Instance.CreateDummyAgent(attackBehavior.targetPosition, |
| | 18 | | attackBehavior.targetVelocity); |
| 7 | 19 | | } |
| | 20 | |
|
| 357 | 21 | | protected float PowerTableLookup(PowerSetting powerSetting) { |
| 357 | 22 | | switch (powerSetting) { |
| | 23 | | case PowerSetting.IDLE: |
| 0 | 24 | | return staticAgentConfig.powerTable.IDLE; |
| | 25 | | case PowerSetting.LOW: |
| 0 | 26 | | return staticAgentConfig.powerTable.LOW; |
| | 27 | | case PowerSetting.CRUISE: |
| 0 | 28 | | return staticAgentConfig.powerTable.CRUISE; |
| | 29 | | case PowerSetting.MIL: |
| 357 | 30 | | return staticAgentConfig.powerTable.MIL; |
| | 31 | | case PowerSetting.MAX: |
| 0 | 32 | | return staticAgentConfig.powerTable.MAX; |
| | 33 | | default: |
| 0 | 34 | | Debug.LogError("Invalid power setting"); |
| 0 | 35 | | return 0f; |
| | 36 | | } |
| 357 | 37 | | } |
| | 38 | |
|
| 0 | 39 | | public override bool IsAssignable() { |
| 0 | 40 | | return false; |
| 0 | 41 | | } |
| | 42 | |
|
| 7 | 43 | | protected override void Start() { |
| 7 | 44 | | base.Start(); |
| 7 | 45 | | _sensor = GetComponent<Sensor>(); |
| 7 | 46 | | } |
| | 47 | |
|
| 357 | 48 | | protected override void FixedUpdate() { |
| 357 | 49 | | base.FixedUpdate(); |
| 357 | 50 | | } |
| | 51 | |
|
| 0 | 52 | | protected Vector3 CalculateForwardAcceleration() { |
| 0 | 53 | | Vector3 transformVelocity = GetVelocity(); |
| | 54 | |
|
| | 55 | | // Get the target speed for the current power setting |
| 0 | 56 | | float targetSpeed = PowerTableLookup(_currentPowerSetting); |
| | 57 | |
|
| | 58 | | // Calculate the current speed |
| 0 | 59 | | float currentSpeed = transformVelocity.magnitude; |
| | 60 | |
|
| | 61 | | // Speed error |
| 0 | 62 | | float speedError = targetSpeed - currentSpeed; |
| | 63 | |
|
| | 64 | | // Proportional gain for speed control |
| 0 | 65 | | float speedControlGain = 10.0f; // Adjust this gain as necessary |
| | 66 | |
|
| | 67 | | // Desired acceleration to adjust speed |
| 0 | 68 | | float desiredAccelerationMagnitude = speedControlGain * speedError; |
| | 69 | |
|
| | 70 | | // Limit the desired acceleration |
| 0 | 71 | | float maxForwardAcceleration = CalculateMaxForwardAcceleration(); |
| 0 | 72 | | desiredAccelerationMagnitude = |
| | 73 | | Mathf.Clamp(desiredAccelerationMagnitude, -maxForwardAcceleration, maxForwardAcceleration); |
| | 74 | |
|
| | 75 | | // Acceleration direction (along current velocity direction) |
| 0 | 76 | | Vector3 accelerationDirection = transformVelocity.normalized; |
| | 77 | |
|
| | 78 | | // Handle zero velocity case |
| 0 | 79 | | if (accelerationDirection.magnitude < 0.1f) { |
| 0 | 80 | | accelerationDirection = transform.forward; // Default direction |
| 0 | 81 | | } |
| | 82 | |
|
| | 83 | | // Calculate acceleration vector |
| 0 | 84 | | Vector3 speedAdjustmentAcceleration = accelerationDirection * desiredAccelerationMagnitude; |
| 0 | 85 | | return speedAdjustmentAcceleration; |
| 0 | 86 | | } |
| | 87 | |
|
| 357 | 88 | | protected Agent GetClosestInterceptor() { |
| 714 | 89 | | if (_interceptors.Count == 0) { |
| 357 | 90 | | return null; |
| | 91 | | } |
| | 92 | |
|
| 0 | 93 | | Agent closestInterceptor = null; |
| 0 | 94 | | float minDistance = float.MaxValue; |
| 0 | 95 | | foreach (var interceptor in _interceptors) { |
| 0 | 96 | | if (!interceptor.HasTerminated()) { |
| 0 | 97 | | SensorOutput sensorOutput = _sensor.Sense(interceptor); |
| 0 | 98 | | if (sensorOutput.position.range < minDistance) { |
| 0 | 99 | | closestInterceptor = interceptor; |
| 0 | 100 | | minDistance = sensorOutput.position.range; |
| 0 | 101 | | } |
| 0 | 102 | | } |
| 0 | 103 | | } |
| 0 | 104 | | return closestInterceptor; |
| 357 | 105 | | } |
| | 106 | |
|
| 357 | 107 | | protected bool ShouldEvade() { |
| 357 | 108 | | if (!dynamicAgentConfig.dynamic_config.flight_config.evasionEnabled) { |
| 0 | 109 | | return false; |
| | 110 | | } |
| | 111 | |
|
| 357 | 112 | | Agent closestInterceptor = GetClosestInterceptor(); |
| 714 | 113 | | if (closestInterceptor == null) { |
| 357 | 114 | | return false; |
| | 115 | | } |
| | 116 | |
|
| 0 | 117 | | float evasionRangeThreshold = |
| | 118 | | dynamicAgentConfig.dynamic_config.flight_config.evasionRangeThreshold; |
| 0 | 119 | | SensorOutput sensorOutput = _sensor.Sense(closestInterceptor); |
| 0 | 120 | | return sensorOutput.position.range <= evasionRangeThreshold && sensorOutput.velocity.range < 0; |
| 357 | 121 | | } |
| | 122 | |
|
| 0 | 123 | | protected Vector3 EvadeInterceptor(Agent interceptor) { |
| 0 | 124 | | Vector3 transformVelocity = GetVelocity(); |
| 0 | 125 | | Vector3 interceptorVelocity = interceptor.GetVelocity(); |
| 0 | 126 | | Vector3 transformPosition = GetPosition(); |
| 0 | 127 | | Vector3 interceptorPosition = interceptor.GetPosition(); |
| | 128 | |
|
| | 129 | | // Set power setting to maximum |
| 0 | 130 | | _currentPowerSetting = PowerSetting.MAX; |
| | 131 | |
|
| | 132 | | // Evade the interceptor by changing the velocity to be normal to the interceptor's velocity |
| 0 | 133 | | Vector3 normalVelocity = Vector3.ProjectOnPlane(transformVelocity, interceptorVelocity); |
| 0 | 134 | | Vector3 normalAccelerationDirection = |
| | 135 | | Vector3.ProjectOnPlane(normalVelocity, transformVelocity).normalized; |
| | 136 | |
|
| | 137 | | // Turn away from the interceptor |
| 0 | 138 | | Vector3 relativePosition = interceptorPosition - transformPosition; |
| 0 | 139 | | if (Vector3.Dot(relativePosition, normalAccelerationDirection) > 0) { |
| 0 | 140 | | normalAccelerationDirection *= -1; |
| 0 | 141 | | } |
| | 142 | |
|
| | 143 | | // Avoid evading straight down when near the ground |
| 0 | 144 | | float altitude = transformPosition.y; |
| 0 | 145 | | float groundProximityThreshold = |
| | 146 | | Mathf.Abs(transformVelocity.y) * 5f; // Adjust threshold as necessary |
| 0 | 147 | | if (transformVelocity.y < 0 && altitude < groundProximityThreshold) { |
| | 148 | | // Determine evasion direction based on angle to interceptor |
| 0 | 149 | | Vector3 toInterceptor = interceptorPosition - transformPosition; |
| 0 | 150 | | Vector3 rightDirection = Vector3.Cross(Vector3.up, transform.forward); |
| 0 | 151 | | float angle = Vector3.SignedAngle(transform.forward, toInterceptor, Vector3.up); |
| | 152 | |
|
| | 153 | | // Choose the direction that leads away from the interceptor |
| 0 | 154 | | Vector3 bestHorizontalDirection = angle > 0 ? -rightDirection : rightDirection; |
| | 155 | |
|
| | 156 | | // Blend between horizontal evasion and slight upward movement |
| 0 | 157 | | float blendFactor = 1 - (altitude / groundProximityThreshold); |
| 0 | 158 | | normalAccelerationDirection = |
| | 159 | | Vector3 |
| | 160 | | .Lerp(normalAccelerationDirection, bestHorizontalDirection + transform.up * 5f, |
| | 161 | | blendFactor) |
| | 162 | | .normalized; |
| 0 | 163 | | } |
| 0 | 164 | | Vector3 normalAcceleration = normalAccelerationDirection * CalculateMaxNormalAcceleration(); |
| | 165 | |
|
| | 166 | | // Adjust forward speed |
| 0 | 167 | | Vector3 forwardAcceleration = CalculateForwardAcceleration(); |
| | 168 | |
|
| 0 | 169 | | return normalAcceleration + forwardAcceleration; |
| 0 | 170 | | } |
| | 171 | |
|
| 0 | 172 | | private void OnTriggerEnter(Collider other) { |
| 0 | 173 | | if (other.gameObject.name == "Floor") { |
| 0 | 174 | | this.HandleThreatMiss(); |
| 0 | 175 | | } |
| | 176 | | // Check if the collision is with another Agent |
| 0 | 177 | | DummyAgent otherAgent = other.gameObject.GetComponentInParent<DummyAgent>(); |
| 0 | 178 | | if (otherAgent != null && _target == otherAgent) { |
| 0 | 179 | | this.HandleThreatHit(); |
| 0 | 180 | | } |
| 0 | 181 | | } |
| | 182 | | } |