Merge pull request #14 from JacobPalecki/GainCap

Gain cap

2 files changed, 100 insertions, 7 deletions

diff --git a/common/accel-base.hpp b/common/accel-base.hpp
index 91510a4..60b7362 100644
--- a/common/accel-base.hpp
+++ b/common/accel-base.hpp
@@ -24,6 +24,7 @@ namespace rawaccel {
         double exponent = 2;
         double midpoint = 0;
         double power_scale = 1;
+        double gain_cap = 0;
         vec2d weight = { 1, 1 };
     };
 
@@ -68,6 +69,7 @@ namespace rawaccel {
         /// <param name="args">Arguments to verified.</param>
         void verify(const accel_args& args) const {
             if (args.accel < 0) bad_arg("accel can not be negative, use a negative weight to compensate");
+            if (args.gain_cap > 0 && weight.x != weight.y) bad_arg("weight x and y values must be equal with a gain cap");
         }
 
         accel_base() = default;
diff --git a/common/rawaccel.hpp b/common/rawaccel.hpp
index 7aa1eb2..8dc4825 100644
--- a/common/rawaccel.hpp
+++ b/common/rawaccel.hpp
@@ -78,6 +78,83 @@ namespace rawaccel {
     /// <summary> Tagged union to hold all accel implementations and allow "polymorphism" via a visitor call. </summary>
     using accel_impl_t = tagged_union<accel_linear, accel_classic, accel_natural, accel_logarithmic, accel_sigmoid, accel_power, accel_noaccel>;
 
+    /// <summary> Struct to hold information about applying a gain cap. </summary>
+    struct velocity_gain_cap {
+
+        // <summary> The minimum speed past which gain cap is applied. </summary>
+        double threshold = 0;
+
+        // <summary> The gain at the point of cap </summary>
+        double slope = 0;
+
+        // <summary> The intercept for the line with above slope to give continuous velocity function </summary>
+        double intercept = 0;
+
+        // <summary> Whether or not velocity gain cap is enabled </summary>
+        bool cap_gain_enabled = false;
+
+        /// <summary>
+        /// Initializes a velocity gain cap for a certain speed threshold
+        /// by estimating the slope at the threshold and creating a line
+        /// with that slope for output velocity calculations.
+        /// </summary>
+        /// <param name="speed"> The speed at which velocity gain cap will kick in </param>
+        /// <param name="offset"> The offset applied in accel calculations </param>
+        /// <param name="accel"> The accel implementation used in the containing accel_fn </param>
+        velocity_gain_cap(double speed, double offset, accel_impl_t accel)
+        {
+            if (speed <= 0) return;
+
+            // Estimate gain at cap point by taking line between two input vs output velocity points.
+            // First input velocity point is at cap; for second pick a velocity a tiny bit larger.
+            double speed_second = 1.001 * speed;
+            double speed_diff = speed_second - speed;
+
+            // Return if by glitch or strange values the difference in points is 0.
+            if (speed_diff == 0) return;
+
+            cap_gain_enabled = true;
+
+            // Find the corresponding output velocities for the two points.
+            // Subtract offset for acceleration, like in accel_fn()
+			double out_first = accel.visit([=](auto&& impl) {
+                double accel_val = impl.accelerate(speed-offset);
+                return impl.scale(accel_val); 
+            }).x * speed;
+			double out_second = accel.visit([=](auto&& impl) {
+                double accel_val = impl.accelerate(speed_second-offset);
+                return impl.scale(accel_val); 
+            }).x * speed_second;
+
+            // Calculate slope and intercept from two points.
+            slope = (out_second - out_first) / speed_diff;
+            intercept = out_first - slope * speed;
+
+            threshold = speed;
+        }
+
+        /// <summary>
+        /// Applies velocity gain cap to speed.
+        /// Returns scale value by which to multiply input to place on gain cap line.
+        /// </summary>
+        /// <param name="speed"> Speed to be capped </param>
+        /// <returns> Scale multiplier for input </returns>
+        inline double operator()(double speed) const {
+			return  slope + intercept / speed;
+        }
+
+        /// <summary>
+        /// Whether gain cap should be applied to given speed.
+        /// </summary>
+        /// <param name="speed"> Speed to check against threshold. </param>
+        /// <returns> Whether gain cap should be applied. </returns>
+        inline bool should_apply(double speed) const {
+            return cap_gain_enabled && speed > threshold;
+        }
+
+        velocity_gain_cap() = default;
+    };
+
     struct accel_fn_args {
         accel_args acc_args;
         int accel_mode = accel_impl_t::id<accel_noaccel>;
@@ -105,17 +182,20 @@ namespace rawaccel {
         /// <summary> The object which sets a min and max for the acceleration scale. </summary>
         vec2<accel_scale_clamp> clamp;
 
+        velocity_gain_cap gain_cap = velocity_gain_cap();
+
         accel_function(const accel_fn_args& args) {
             if (args.time_min <= 0) bad_arg("min time must be positive");
             if (args.acc_args.offset < 0) bad_arg("offset must not be negative");
 
             accel.tag = args.accel_mode;
-            accel.visit([&](auto& impl){ impl = { args.acc_args }; });
+            accel.visit([&](auto& impl) { impl = { args.acc_args }; });
 
             time_min = args.time_min;
             speed_offset = args.acc_args.offset;
             clamp.x = accel_scale_clamp(args.cap.x);
             clamp.y = accel_scale_clamp(args.cap.y);
+			gain_cap = velocity_gain_cap(args.acc_args.gain_cap, speed_offset, accel);
         }
 
         /// <summary>
@@ -127,12 +207,23 @@ namespace rawaccel {
         inline vec2d operator()(const vec2d& input, milliseconds time) const {
             double mag = sqrtsd(input.x * input.x + input.y * input.y);
             double time_clamped = clampsd(time, time_min, 100);
-            double speed = maxsd(mag / time_clamped - speed_offset, 0);
-
-            vec2d scale = accel.visit([=](auto&& impl) {
-                double accel_val = impl.accelerate(speed);
-                return impl.scale(accel_val); 
-            });
+            double raw_speed = mag / time_clamped;
+                
+            vec2d scale;
+            
+            // gain_cap needs raw speed for velocity line calculation
+            if (gain_cap.should_apply(raw_speed))
+            {
+                double gain_cap_scale = gain_cap(raw_speed);
+                scale = { gain_cap_scale, gain_cap_scale };
+            }
+            else
+            {
+                scale = accel.visit([=](auto&& impl) {
+                    double accel_val = impl.accelerate(maxsd(mag / time_clamped - speed_offset, 0));
+                    return impl.scale(accel_val);
+                    });
+            }
 
             return {
                 input.x * clamp.x(scale.x),