| | | 1 | | using System; |
| | | 2 | | using UnityEngine; |
| | | 3 | | |
| | | 4 | | // The proportional navigation controller applies proportional navigation to steer the agent towards |
| | | 5 | | // its target. |
| | | 6 | | public class PnController : IController { |
| | | 7 | | // Negative closing velocity turn factor. |
| | | 8 | | protected const float negativeClosingVelocityTurnFactor = 100f; |
| | | 9 | | |
| | | 10 | | // Minimum line-of-sight rate. |
| | | 11 | | protected const float minimumLosRate = 0.2f; |
| | | 12 | | |
| | | 13 | | // Proportional navigation gain. |
| | | 14 | | protected float _navigationGain; |
| | | 15 | | |
| | 4 | 16 | | public PnController(Agent agent, float navigationGain) : base(agent) { |
| | 2 | 17 | | _navigationGain = navigationGain; |
| | 2 | 18 | | } |
| | | 19 | | |
| | 2 | 20 | | protected override Vector3 PlanImpl(in Transformation relativeTransformation) { |
| | | 21 | | // Cache the transform and velocity. |
| | 2 | 22 | | Transform agentTransform = _agent.transform; |
| | 2 | 23 | | Vector3 right = agentTransform.right; |
| | 2 | 24 | | Vector3 up = agentTransform.up; |
| | 2 | 25 | | Vector3 forward = agentTransform.forward; |
| | 2 | 26 | | Vector3 position = agentTransform.position; |
| | | 27 | | |
| | 2 | 28 | | Vector3 velocity = _agent.GetVelocity(); |
| | 2 | 29 | | float speed = velocity.magnitude; |
| | | 30 | | |
| | | 31 | | // Extract the bearing and closing velocity from the relative transformation. |
| | 2 | 32 | | float losAz = relativeTransformation.position.azimuth; |
| | 2 | 33 | | float losEl = relativeTransformation.position.elevation; |
| | 2 | 34 | | float losRateAz = relativeTransformation.velocity.azimuth; |
| | 2 | 35 | | float losRateEl = relativeTransformation.velocity.elevation; |
| | | 36 | | // The closing velocity is negative because the closing velocity is opposite to the range rate. |
| | 2 | 37 | | float closingVelocity = -relativeTransformation.velocity.range; |
| | | 38 | | |
| | | 39 | | // Set the turn factor, which is equal to the closing velocity by default. |
| | 2 | 40 | | float turnFactor = closingVelocity; |
| | | 41 | | // Handle a negative closing velocity. In this case, since the target is moving away from the |
| | | 42 | | // agent, apply a stronger turn. |
| | 2 | 43 | | if (closingVelocity < 0) { |
| | 0 | 44 | | turnFactor = Mathf.Max(1f, Mathf.Abs(closingVelocity) * negativeClosingVelocityTurnFactor); |
| | 0 | 45 | | } |
| | | 46 | | |
| | | 47 | | // Handle the spiral behavior if the target is at a bearing of 90 degrees +- 10 degrees. |
| | 2 | 48 | | if (Mathf.Abs(Mathf.Abs(losAz) - 90f * Mathf.Deg2Rad) < 10f * Mathf.Deg2Rad || |
| | 0 | 49 | | Mathf.Abs(Mathf.Abs(losEl) - 90f * Mathf.Deg2Rad) < 10f * Mathf.Deg2Rad) { |
| | | 50 | | // Check that the agent is not moving in a spiral by clamping the LOS rate. |
| | 0 | 51 | | losRateAz = Mathf.Sign(losRateAz) * Mathf.Max(Mathf.Abs(losRateAz), minimumLosRate); |
| | 0 | 52 | | losRateEl = Mathf.Sign(losRateEl) * Mathf.Max(Mathf.Abs(losRateEl), minimumLosRate); |
| | 0 | 53 | | turnFactor = Mathf.Abs(closingVelocity) * negativeClosingVelocityTurnFactor; |
| | 0 | 54 | | } |
| | | 55 | | |
| | 2 | 56 | | float accelerationAz = _navigationGain * turnFactor * losRateAz; |
| | 2 | 57 | | float accelerationEl = _navigationGain * turnFactor * losRateEl; |
| | 2 | 58 | | Vector3 accelerationInput = right * accelerationAz + up * accelerationEl; |
| | 2 | 59 | | return accelerationInput; |
| | 2 | 60 | | } |
| | | 61 | | } |