diff --git a/Source/Engine/Core/Math/Vector2.cs b/Source/Engine/Core/Math/Vector2.cs index 2af365638..568c51764 100644 --- a/Source/Engine/Core/Math/Vector2.cs +++ b/Source/Engine/Core/Math/Vector2.cs @@ -58,6 +58,7 @@ using Mathr = FlaxEngine.Mathf; */ using System; using System.Globalization; +using System.ComponentModel; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -953,6 +954,91 @@ namespace FlaxEngine return result; } + /// + /// Performs a gradual change of a vector towards a specified target over time + /// + /// Current vector. + /// Target vector. + /// Used to store the current velocity. + /// Determines the approximate time it should take to reach the target vector. + /// Defines the upper limit on the speed of the Smooth Damp. + public static Vector2 SmoothDamp(Vector2 current, Vector2 target, ref Vector2 currentVelocity, float smoothTime, float maxSpeed) + { + return SmoothDamp(current, target, ref currentVelocity, smoothTime, maxSpeed, Time.DeltaTime); + } + + /// + /// Performs a gradual change of a vector towards a specified target over time + /// + /// Current vector. + /// Target vector. + /// Used to store the current velocity. + /// Determines the approximate time it should take to reach the target vector. + public static Vector2 SmoothDamp(Vector2 current, Vector2 target, ref Vector2 currentVelocity, float smoothTime) + { + return SmoothDamp(current, target, ref currentVelocity, smoothTime, float.PositiveInfinity, Time.DeltaTime); + } + + /// + /// Performs a gradual change of a vector towards a specified target over time + /// + /// Current vector. + /// Target vector. + /// Used to store the current velocity. + /// Determines the approximate time it should take to reach the target vector. + /// Defines the upper limit on the speed of the Smooth Damp. + /// Delta Time, represents the time elapsed since last frame. + public static Vector2 SmoothDamp(Vector2 current, Vector2 target, ref Vector2 currentVelocity, float smoothTime, [DefaultValue("float.PositiveInfinity")] float maxSpeed, [DefaultValue("Time.DeltaTime")] float deltaTime) + { + smoothTime = Mathf.Max(0.0001f, smoothTime); + Real a = 2f / smoothTime; + Real b = a * deltaTime; + Real e = 1f / (1f + b + 0.48f * b * b + 0.235f * b * b * b); + + Real change_x = current.X - target.X; + Real change_y = current.Y - target.Y; + Vector2 originalTo = target; + + Real maxChangeSpeed = maxSpeed * smoothTime; + Real changeSq = maxChangeSpeed * maxChangeSpeed; + Real sqrDist = change_x * change_x + change_y * change_y; + if (sqrDist > changeSq) + { + var dist = (Real)Math.Sqrt(sqrDist); + change_x = change_x / dist * maxChangeSpeed; + change_y = change_y / dist * maxChangeSpeed; + } + + target.X = current.X - change_x; + target.Y = current.Y - change_y; + + Real temp_x = (currentVelocity.X + a * change_x) * deltaTime; + Real temp_y = (currentVelocity.Y + a * change_y) * deltaTime; + + currentVelocity.X = (currentVelocity.X - a * temp_x) * e; + currentVelocity.Y = (currentVelocity.Y - a * temp_y) * e; + + Real output_x = target.X + (change_x + temp_x) * e; + Real output_y = target.Y + (change_y + temp_y) * e; + + Real x1 = originalTo.X - current.X; + Real y1 = originalTo.Y - current.Y; + + Real x2 = output_x - originalTo.X; + Real y2 = output_y - originalTo.Y; + + if (x1 * x2 + y1 * y2 > 0) + { + output_x = originalTo.X; + output_y = originalTo.Y; + + currentVelocity.X = (output_x - originalTo.X) / deltaTime; + currentVelocity.Y = (output_y - originalTo.Y) / deltaTime; + } + + return new Vector2(output_x, output_y); + } + /// /// Performs a cubic interpolation between two vectors. /// diff --git a/Source/Engine/Core/Math/Vector3.cs b/Source/Engine/Core/Math/Vector3.cs index 4442d3334..59d1f63db 100644 --- a/Source/Engine/Core/Math/Vector3.cs +++ b/Source/Engine/Core/Math/Vector3.cs @@ -58,6 +58,7 @@ using Mathr = FlaxEngine.Mathf; */ using System; using System.Globalization; +using System.ComponentModel; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -1042,6 +1043,103 @@ namespace FlaxEngine return result; } + /// + /// Performs a gradual change of a vector towards a specified target over time + /// + /// Current vector. + /// Target vector. + /// Used to store the current velocity. + /// Determines the approximate time it should take to reach the target vector. + /// Defines the upper limit on the speed of the Smooth Damp. + public static Vector3 SmoothDamp(Vector3 current, Vector3 target, ref Vector3 currentVelocity, float smoothTime, float maxSpeed) + { + return SmoothDamp(current, target, ref currentVelocity, smoothTime, maxSpeed, Time.DeltaTime); + } + + /// + /// Performs a gradual change of a vector towards a specified target over time + /// + /// Current vector. + /// Target vector. + /// Used to store the current velocity. + /// Determines the approximate time it should take to reach the target vector. + public static Vector3 SmoothDamp(Vector3 current, Vector3 target, ref Vector3 currentVelocity, float smoothTime) + { + return SmoothDamp(current, target, ref currentVelocity, smoothTime, float.PositiveInfinity, Time.DeltaTime); + } + + /// + /// Performs a gradual change of a vector towards a specified target over time + /// + /// Current vector. + /// Target vector. + /// Used to store the current velocity. + /// Determines the approximate time it should take to reach the target vector. + /// Defines the upper limit on the speed of the Smooth Damp. + /// Delta Time, represents the time elapsed since last frame. + public static Vector3 SmoothDamp(Vector3 current, Vector3 target, ref Vector3 currentVelocity, float smoothTime, [DefaultValue("float.PositiveInfinity")] float maxSpeed, [DefaultValue("Time.DeltaTime")] float deltaTime) + { + smoothTime = Mathf.Max(0.0001f, smoothTime); + Real a = 2f / smoothTime; + Real b = a * deltaTime; + Real e = 1f / (1f + b + 0.48f * b * b + 0.235f * b * b * b); + + Real change_x = current.X - target.X; + Real change_y = current.Y - target.Y; + Real change_z = current.Z - target.Z; + + Vector3 originalTo = target; + + Real maxChangeSpeed = maxSpeed * smoothTime; + Real changeSq = maxChangeSpeed * maxChangeSpeed; + Real sqrLen = change_x * change_x + change_y * change_y + change_z * change_z; + if (sqrLen > changeSq) + { + var len = (Real)Math.Sqrt(sqrLen); + change_x = change_x / len * maxChangeSpeed; + change_y = change_y / len * maxChangeSpeed; + change_z = change_z / len * maxChangeSpeed; + } + + target.X = current.X - change_x; + target.Y = current.Y - change_y; + target.Z = current.Z - change_z; + + Real temp_x = (currentVelocity.X + a * change_x) * deltaTime; + Real temp_y = (currentVelocity.Y + a * change_y) * deltaTime; + Real temp_z = (currentVelocity.Z + a * change_z) * deltaTime; + + currentVelocity.X = (currentVelocity.X - a * temp_x) * e; + currentVelocity.Y = (currentVelocity.Y - a * temp_y) * e; + currentVelocity.Z = (currentVelocity.Z - a * temp_z) * e; + + Real output_x = target.X + (change_x + temp_x) * e; + Real output_y = target.Y + (change_y + temp_y) * e; + Real output_z = target.Z + (change_z + temp_z) * e; + + Real x1 = originalTo.X - current.X; + Real y1 = originalTo.Y - current.Y; + Real z1 = originalTo.Z - current.Z; + + Real x2 = output_x - originalTo.X; + Real y2 = output_y - originalTo.Y; + Real z2 = output_z - originalTo.Z; + + if (x1 * x2 + y1 * y2 + z1 * z2 > 0) + { + output_x = originalTo.X; + output_y = originalTo.Y; + output_z = originalTo.Z; + + currentVelocity.X = (output_x - originalTo.X) / deltaTime; + currentVelocity.Y = (output_y - originalTo.Y) / deltaTime; + currentVelocity.Z = (output_z - originalTo.Z) / deltaTime; + } + + return new Vector3(output_x, output_y, output_z); + } + + /// /// Performs a cubic interpolation between two vectors. ///