| | | 1 | | using UnityEngine; |
| | | 2 | | |
| | | 3 | | // The agent will try to evade its pursuer by turning its velocity vector to be orthogonal to the |
| | | 4 | | // pursuer's velocity vector. |
| | | 5 | | // |
| | | 6 | | // Turning orthogonal to the pursuer means that the pursuer needs to apply a normal acceleration to |
| | | 7 | | // intercept the agent, incurring the maximum possible lift-induced drag. |
| | | 8 | | public class OrthogonalEvasion : EvasionBase { |
| | 2865 | 9 | | public OrthogonalEvasion(IAgent agent) : base(agent) {} |
| | | 10 | | |
| | | 11 | | // Determine whether to perform any evasive maneuvers. |
| | 0 | 12 | | public override bool ShouldEvade(IAgent pursuer) { |
| | 0 | 13 | | if (!(Agent.AgentConfig?.DynamicConfig?.FlightConfig?.EvasionConfig?.Enabled ?? false)) { |
| | 0 | 14 | | return false; |
| | | 15 | | } |
| | | 16 | | |
| | 0 | 17 | | SensorOutput sensorOutput = Agent.Sensor.Sense(pursuer); |
| | 0 | 18 | | float evasionRangeThreshold = |
| | | 19 | | Agent.AgentConfig.DynamicConfig.FlightConfig.EvasionConfig.RangeThreshold; |
| | 0 | 20 | | return sensorOutput.Position.Range <= evasionRangeThreshold && sensorOutput.Velocity.Range < 0; |
| | 0 | 21 | | } |
| | | 22 | | |
| | | 23 | | // Calculate the acceleration input to evade the pursuer. |
| | 0 | 24 | | public override Vector3 Evade(IAgent pursuer) { |
| | | 25 | | const float epsilon = 1e-6f; |
| | | 26 | | const float groundProximityThresholdFactor = 5f; |
| | | 27 | | const float groundAvoidanceUpFactor = 5f; |
| | | 28 | | |
| | 0 | 29 | | Vector3 agentPosition = Agent.Position; |
| | 0 | 30 | | Vector3 agentVelocity = Agent.Velocity; |
| | 0 | 31 | | Vector3 pursuerPosition = pursuer.Position; |
| | 0 | 32 | | Vector3 pursuerVelocity = pursuer.Velocity; |
| | | 33 | | |
| | | 34 | | // Evade the pursuer by turning the velocity to be orthogonal to the pursuer's velocity. |
| | 0 | 35 | | Vector3 normalVelocity = Vector3.ProjectOnPlane(agentVelocity, pursuerVelocity); |
| | 0 | 36 | | if (normalVelocity.sqrMagnitude < epsilon) { |
| | | 37 | | // If the agent's velocity is aligned with the pursuer's velocity, choose a random normal |
| | | 38 | | // direction in which to evade. |
| | 0 | 39 | | normalVelocity = pursuer.Right; |
| | 0 | 40 | | } |
| | | 41 | | // If the agent's velocity is aligned with the normal velocity, i.e., orthogonal to the |
| | | 42 | | // pursuer's velocity, then the normal acceleration should be zero as the agent should continue |
| | | 43 | | // in the same direction. |
| | 0 | 44 | | Vector3 normalAccelerationDirection = |
| | | 45 | | Vector3.ProjectOnPlane(normalVelocity, agentVelocity).normalized; |
| | | 46 | | |
| | | 47 | | // Turn away from the pursuer. |
| | 0 | 48 | | Vector3 relativePosition = pursuerPosition - agentPosition; |
| | 0 | 49 | | if (Vector3.Dot(relativePosition, normalAccelerationDirection) > 0) { |
| | 0 | 50 | | normalAccelerationDirection *= -1; |
| | 0 | 51 | | } |
| | | 52 | | |
| | | 53 | | // Avoid evading straight down when near the ground. |
| | 0 | 54 | | float altitude = agentPosition.y; |
| | 0 | 55 | | float groundProximityThreshold = Mathf.Abs(agentVelocity.y) * groundProximityThresholdFactor; |
| | 0 | 56 | | if (agentVelocity.y < 0 && altitude < groundProximityThreshold) { |
| | | 57 | | // Determine the evasion direction based on the angle to pursuer. |
| | 0 | 58 | | float angle = Vector3.SignedAngle(Agent.Forward, relativePosition, Vector3.up); |
| | | 59 | | |
| | | 60 | | // Choose the direction that leads away from the pursuer. |
| | 0 | 61 | | Vector3 rightDirection = Agent.Right; |
| | 0 | 62 | | Vector3 bestHorizontalDirection = angle > 0 ? -rightDirection : rightDirection; |
| | | 63 | | |
| | | 64 | | // Blend between horizontal evasion and slight upward movement. |
| | 0 | 65 | | float blendFactor = 1 - (altitude / groundProximityThreshold); |
| | 0 | 66 | | normalAccelerationDirection = |
| | | 67 | | Vector3 |
| | | 68 | | .Lerp(normalAccelerationDirection, |
| | | 69 | | bestHorizontalDirection + Agent.Up * groundAvoidanceUpFactor, blendFactor) |
| | | 70 | | .normalized; |
| | 0 | 71 | | } |
| | 0 | 72 | | Vector3 normalAcceleration = normalAccelerationDirection * Agent.MaxNormalAcceleration(); |
| | | 73 | | |
| | | 74 | | // Apply the maximum forward acceleration. |
| | 0 | 75 | | Vector3 forwardAcceleration = Agent.Forward * Agent.MaxForwardAcceleration(); |
| | 0 | 76 | | return normalAcceleration + forwardAcceleration; |
| | 0 | 77 | | } |
| | | 78 | | } |