math Functions: exponential function with constraints

src/lib/mathlib/math/Functions.hpp

@@ -152,4 +152,55 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low,
 	}
 }
 
+/*
+ * Exponential function of the form Y_out = a*b^X + c
+ *
+ * Y_max    |   *
+ *          |    *
+ *          |      *
+ *          |        *
+ *          |           *
+ * Y_middle |               *
+ *          |                    *
+ * Y_min    |                          *    *
+ *          | __________________________________
+ *              0           1               2
+ *
+ *
+ * @param X in the range [0,2]
+ * @param Y_min minimum output at X = 2
+ * @param Y_mid middle output at X = 1
+ * @param Y_max maximum output at X = 0
+ */
+template<typename T>
+const T expontialFromLimits(const T &X, const T &Y_min, const T &Y_mid, const T &Y_max)
+{
+	T SIGMA_NORM = (T)0.001;
+	// If Y_mid is exactly in the middle, then just apply linear approach.
+	bool use_linear_approach = false;
+
+	if (((Y_max + Y_min) * (T)0.5) - Y_mid < SIGMA_NORM) {
+		use_linear_approach = true;
+	}
+
+	T Y_out;
+
+	if (use_linear_approach) {
+		// Y_out =  m*x+q
+		float slope = -(Y_max - Y_min) / (T)2.0;
+		Y_out = slope * X + Y_max;
+
+	} else {
+		// Y_out = a*b^X + c with constraints Y_max = 0, Y_middle = 1, Y_min = 2
+		T a = -((Y_mid - Y_max) * (Y_mid - Y_max))
+		      / ((T)2.0 * Y_mid - Y_max - Y_min);
+		T c = Y_max - a;
+		T b = (Y_mid - c) / a;
+		Y_out = a * powf(b, X) + c;
+	}
+
+	// Y_out needs to be in between max and min
+	return constrain(Y_out, Y_min, Y_max);
+
+}
 } /* namespace math */