Vector3D.ClampMagnitude C# (CSharp) Method

    public static Vector3D ClampMagnitude(Vector3D vector, double maxLength)
    {
        if (vector.sqrMagnitude > maxLength * maxLength)
        {
            return vector.normalized * maxLength;
        }
        return vector;
    }

    public static Vector3D SmoothDamp(Vector3D current, Vector3D target, ref Vector3D currentVelocity, double smoothTime, double maxSpeed, double deltaTime)
    {
        smoothTime = Mathd.Max(0.0001d, smoothTime);
        double   num1      = 2d / smoothTime;
        double   num2      = num1 * deltaTime;
        double   num3      = (1.0d / (1.0d + num2 + 0.479999989271164d * num2 * num2 + 0.234999999403954d * num2 * num2 * num2));
        Vector3D vector    = current - target;
        Vector3D vector3_1 = target;
        double   maxLength = maxSpeed * smoothTime;
        Vector3D vector3_2 = Vector3D.ClampMagnitude(vector, maxLength);

        target = current - vector3_2;
        Vector3D vector3_3 = (currentVelocity + num1 * vector3_2) * deltaTime;

        currentVelocity = (currentVelocity - num1 * vector3_3) * num3;
        Vector3D vector3_4 = target + (vector3_2 + vector3_3) * num3;

        if (Vector3D.Dot(vector3_1 - current, vector3_4 - vector3_1) > 0.0)
        {
            vector3_4       = vector3_1;
            currentVelocity = (vector3_4 - vector3_1) / deltaTime;
        }
        return(vector3_4);
    }