| | | 1 | | using UnityEngine; |
| | | 2 | | |
| | | 3 | | public static class FractionalSpeed { |
| | | 4 | | // Calculate the estimated fractional speed for the given hierarchical object to reach the given |
| | | 5 | | // target position. |
| | 0 | 6 | | public static float Calculate(IAgent agent, in Vector3 targetPosition) { |
| | | 7 | | // The speed decays exponentially with the traveled distance and with the bearing change. |
| | 0 | 8 | | float distanceTimeConstant = 2 * (agent.StaticConfig.BodyConfig?.Mass ?? 1) / |
| | | 9 | | (Constants.CalculateAirDensityAtAltitude(agent.Position.y) * |
| | | 10 | | (agent.StaticConfig.LiftDragConfig?.DragCoefficient ?? 0) * |
| | | 11 | | (agent.StaticConfig.BodyConfig?.CrossSectionalArea ?? 0)); |
| | 0 | 12 | | float angleTimeConstant = |
| | | 13 | | agent.StaticConfig.LiftDragConfig?.LiftDragRatio ?? float.PositiveInfinity; |
| | | 14 | | // During the turn, the minimum radius dictates the minimum distance needed to make the turn. |
| | 0 | 15 | | float minTurningRadius = agent.Velocity.sqrMagnitude / agent.MaxNormalAcceleration(); |
| | | 16 | | |
| | 0 | 17 | | Vector3 directionToTarget = targetPosition - agent.Position; |
| | 0 | 18 | | float distanceToTarget = directionToTarget.magnitude; |
| | 0 | 19 | | float angleToTarget = Vector3.Angle(agent.Velocity, directionToTarget) * Mathf.Deg2Rad; |
| | | 20 | | // The fractional speed is the product of the fractional speed after traveling the distance and |
| | | 21 | | // of the fractional speed after turning. |
| | 0 | 22 | | float fractionalSpeed = |
| | | 23 | | Mathf.Exp(-((distanceToTarget + angleToTarget * minTurningRadius) / distanceTimeConstant + |
| | | 24 | | angleToTarget / angleTimeConstant)); |
| | 0 | 25 | | return fractionalSpeed; |
| | 0 | 26 | | } |
| | | 27 | | } |