| | | 1 | | using UnityEngine; |
| | | 2 | | |
| | | 3 | | // The waypoint controller steers the agent to the target using direct linear guidance. |
| | | 4 | | public class WaypointController : ControllerBase { |
| | | 5 | | // To prevent overshooting near the waypoint, if the distance to the waypoint is less than the |
| | | 6 | | // threshold, do not apply any acceleration. |
| | | 7 | | private const float _accelerationCutoffDistance = 1.0f; |
| | | 8 | | |
| | 18 | 9 | | public WaypointController(IAgent agent) : base(agent) {} |
| | | 10 | | |
| | | 11 | | // Controller-dependent implementation of the control law. |
| | 8 | 12 | | protected override Vector3 Plan(in Transformation relativeTransformation) { |
| | 8 | 13 | | Vector3 relativePosition = relativeTransformation.Position.Cartesian; |
| | 9 | 14 | | if (relativeTransformation.Position.Range < _accelerationCutoffDistance) { |
| | 1 | 15 | | return Vector3.zero; |
| | | 16 | | } |
| | | 17 | | |
| | | 18 | | // To reach the waypoint as fast as possible, use the maximum forward acceleration. |
| | 7 | 19 | | Vector3 forward = Agent.Forward; |
| | 7 | 20 | | Vector3 forwardAccelerationInput = forward * Agent.MaxForwardAcceleration(); |
| | 8 | 21 | | if (Vector3.Dot(relativePosition, Agent.Velocity) < 0) { |
| | 1 | 22 | | forwardAccelerationInput *= -1; |
| | 1 | 23 | | } |
| | | 24 | | |
| | | 25 | | // Steer as hard as possible towards the waypoint. |
| | 7 | 26 | | Vector3 normalAcceleration = Vector3.ProjectOnPlane(relativePosition, forward).normalized; |
| | 7 | 27 | | Vector3 normalAccelerationInput = normalAcceleration * Agent.MaxNormalAcceleration(); |
| | 7 | 28 | | return forwardAccelerationInput + normalAccelerationInput; |
| | 8 | 29 | | } |
| | | 30 | | } |