/** Clamps the velocity to the max speed and optionally the forwards direction.
* \param velocity Desired velocity of the character. In world units per second.
* \param maxSpeed Max speed of the character. In world units per second.
* \param slowdownFactor Value between 0 and 1 which determines how much slower the character should move than normal.
* Normally 1 but should go to 0 when the character approaches the end of the path.
* \param slowWhenNotFacingTarget Prevent the velocity from being too far away from the forward direction of the character
* and slow the character down if the desired velocity is not in the same direction as the forward vector.
* \param forward Forward direction of the character. Used together with the \a slowWhenNotFacingTarget parameter.
*
* Note that all vectors are 2D vectors, not 3D vectors.
*
* \returns The clamped velocity in world units per second.
*/
public static Vector2 ClampVelocity(Vector2 velocity, float maxSpeed, float slowdownFactor, bool slowWhenNotFacingTarget, Vector2 forward)
{
// Max speed to use for this frame
var currentMaxSpeed = maxSpeed * slowdownFactor;
// Check if the agent should slow down in case it is not facing the direction it wants to move in
if (slowWhenNotFacingTarget && (forward.x != 0 || forward.y != 0))
{
float currentSpeed;
var normalizedVelocity = VectorMath.Normalize(velocity.ToPFV2(), out currentSpeed);
float dot = Vector2.Dot(normalizedVelocity.ToUnityV2(), forward);
// Lower the speed when the character's forward direction is not pointing towards the desired velocity
// 1 when velocity is in the same direction as forward
// 0.2 when they point in the opposite directions
float directionSpeedFactor = Mathf.Clamp(dot + 0.707f, 0.2f, 1.0f);
currentMaxSpeed *= directionSpeedFactor;
currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);
// Angle between the forwards direction of the character and our desired velocity
float angle = Mathf.Acos(Mathf.Clamp(dot, -1, 1));
// Clamp the angle to 20 degrees
// We cannot keep the velocity exactly in the forwards direction of the character
// because we use the rotation to determine in which direction to rotate and if
// the velocity would always be in the forwards direction of the character then
// the character would never rotate.
// Allow larger angles when near the end of the path to prevent oscillations.
angle = Mathf.Min(angle, (20f + 180f * (1 - slowdownFactor * slowdownFactor)) * Mathf.Deg2Rad);
float sin = Mathf.Sin(angle);
float cos = Mathf.Cos(angle);
// Determine if we should rotate clockwise or counter-clockwise to move towards the current velocity
sin *= Mathf.Sign(normalizedVelocity.x * forward.y - normalizedVelocity.y * forward.x);
// Rotate the #forward vector by #angle radians
// The rotation is done using an inlined rotation matrix.
// See https://en.wikipedia.org/wiki/Rotation_matrix
return(new Vector2(forward.x * cos + forward.y * sin, forward.y * cos - forward.x * sin) * currentSpeed);
}
else
{
return(Vector2.ClampMagnitude(velocity, currentMaxSpeed));
}
}
