| | | 1 | | using System; |
| | | 2 | | using UnityEngine; |
| | | 3 | | |
| | | 4 | | // The augmented proportional navigation controller applies augmented proportional navigation to |
| | | 5 | | // steer the agent towards its target. |
| | | 6 | | public class ApnController : PnController { |
| | 0 | 7 | | public ApnController(Agent agent, float navigationGain) : base(agent, navigationGain) {} |
| | | 8 | | |
| | 0 | 9 | | protected override Vector3 PlanImpl(in Transformation relativeTransformation) { |
| | 0 | 10 | | Vector3 pnAccelerationInput = base.PlanImpl(relativeTransformation); |
| | | 11 | | |
| | | 12 | | // Add a feedforward term proportional to the target's acceleration. |
| | 0 | 13 | | Vector3 targetAcceleration = relativeTransformation.acceleration.cartesian; |
| | 0 | 14 | | Vector3 normalTargetAcceleration = |
| | | 15 | | Vector3.ProjectOnPlane(targetAcceleration, _agent.transform.forward); |
| | 0 | 16 | | Vector3 accelerationInput = |
| | | 17 | | pnAccelerationInput + _navigationGain / 2 * normalTargetAcceleration; |
| | 0 | 18 | | return accelerationInput; |
| | 0 | 19 | | } |
| | | 20 | | } |