fix(math): update range mapper implementation (#229)

* Update range mapper implementation so that the scaling starts at the deadband edge (reduce discontinuities in standard output range)
* Update range mapper implementation to fix bugs arising from comparing to T(0) when it should have instead been comparing against center_ or output_center_
* Update math example to have better range mapper test - and one which outputs csv for easier plotting / checking
* Update range mapper docstrings to have more text and warning about input inversion

Author: finger563

components/math/example/main/math_example.cpp

@@ -115,52 +115,68 @@ extern "C" void app_main(void) {
   logger.info("=== range mapper ===");
   {
     //! [range_mapper example]
+    static constexpr float deadband = 12.0f;
+    static constexpr float min = 0.0f;
+    static constexpr float center = 127.0f;
+    static constexpr float max = 255.0f;
     // Default will have output range [-1, 1]
-    espp::RangeMapper<float> rm({.center = 127, .deadband = 12, .minimum = 0, .maximum = 255});
+    espp::RangeMapper<float> rm(
+        {.center = center, .deadband = deadband, .minimum = min, .maximum = max});
     // You can explicitly set output center/range. In this case the output will
     // be in the range [0, 1024]
-    espp::RangeMapper<float> rm2({.center = 127,
-                                  .deadband = 12,
-                                  .minimum = 0,
-                                  .maximum = 255,
+    espp::RangeMapper<float> rm2({.center = center,
+                                  .deadband = deadband,
+                                  .minimum = min,
+                                  .maximum = max,
                                   .output_center = 512,
                                   .output_range = 512});
     // You can also invert the input distribution, such that input values are
     // compared against the input min/max instead of input center. NOTE: this
     // also showcases the use of a non-centered input distribution.
-    espp::FloatRangeMapper rm3({.center = 0,
-                                .deadband = 12,
-                                .minimum = 0,
-                                .maximum = 255,
+    espp::FloatRangeMapper rm3({.center = center,
+                                .deadband = deadband,
+                                .minimum = min,
+                                .maximum = max,
                                 .invert_input = true,
-                                .output_center = 0,
-                                .output_range = 1024});
+                                .output_center = 512,
+                                .output_range = 512});
     // You can even invert the ouput distribution
     espp::FloatRangeMapper rm4({
-        .center = 127,
-        .deadband = 12,
-        .minimum = 0,
-        .maximum = 255,
+        .center = center,
+        .deadband = deadband,
+        .minimum = min,
+        .maximum = max,
         .invert_output = true,
     });
-    auto vals =
-        std::array<float, 14>{-10, 0, 10, 50, 100, 120, 127, 135, 150, 200, 240, 250, 255, 275};
+    auto vals = std::vector<float>{min - 10, min, min + 5, min + 10, min + deadband,
+                                   // should show as approx -.66 and -.33
+                                   min + (center - deadband - min) * .33f,
+                                   min + (center - deadband - min) * .66f, center - deadband,
+                                   center - 7, center, center + 7, center + deadband,
+                                   // should show as approx .33 and .66
+                                   center + deadband + (max - center - deadband) * .33f,
+                                   center + deadband + (max - center - deadband) * .66f,
+                                   max - deadband, max - 10, max - 5, max, max + 10};
     // test the mapping and unmapping
     fmt::print("Mapping [0, 255] -> [-1, 1]\n");
+    fmt::print("% value, mapped, unmapped\n");
     for (const auto &v : vals) {
-      fmt::print("{} -> {} -> {} \n", v, rm.map(v), rm.unmap(rm.map(v)));
+      fmt::print("{}, {}, {}\n", v, rm.map(v), rm.unmap(rm.map(v)));
     }
     fmt::print("Mapping [0, 255] -> [0, 1024]\n");
+    fmt::print("% value, mapped, unmapped\n");
     for (const auto &v : vals) {
-      fmt::print("{} -> {} -> {} \n", v, rm2.map(v), rm2.unmap(rm2.map(v)));
+      fmt::print("{}, {}, {}\n", v, rm2.map(v), rm2.unmap(rm2.map(v)));
     }
     fmt::print("Mapping Inverted [0, 255] -> [1024, 0]\n");
+    fmt::print("% value, mapped, unmapped\n");
     for (const auto &v : vals) {
-      fmt::print("{} -> {} -> {} \n", v, rm3.map(v), rm3.unmap(rm3.map(v)));
+      fmt::print("{}, {}, {}\n", v, rm3.map(v), rm3.unmap(rm3.map(v)));
     }
     fmt::print("Mapping [0, 255] -> Inverted [1, -1]\n");
+    fmt::print("% value, mapped, unmapped\n");
     for (const auto &v : vals) {
-      fmt::print("{} -> {} -> {} \n", v, rm4.map(v), rm4.unmap(rm4.map(v)));
+      fmt::print("{}, {}, {}\n", v, rm4.map(v), rm4.unmap(rm4.map(v)));
     }
     //! [range_mapper example]
   }

components/math/include/range_mapper.hpp

@@ -45,10 +45,14 @@ template <typename T> class RangeMapper {
     T deadband; /**< Deadband amount around (+-) the center for which output will be 0. */
     T minimum;  /**< Minimum value for the input range. */
     T maximum;  /**< Maximum value for the input range. */
-    bool invert_input{
-        false}; /**< Whether to invert the input distribution (default false). @note If true will
-                   compute the input relative to min/max instead of to center. */
-    T output_center{T(0)}; /**< The center for the output. Default 0. */
+    bool invert_input{false}; /**< Whether to invert the input distribution (default false).
+                                 @note If true will compute the input relative to min/max
+                                 instead of to center. @warning This introduces a
+                                 discontinuity at the center value and ambiguity around the
+                                 min/max values when un-mapping back to the input
+                                 distribution. For these reasons this setting is not
+                                 recommended and may be replaced in future revisions. */
+    T output_center{T(0)};    /**< The center for the output. Default 0. */
     T output_range{T(1)}; /**< The range (+/-) from the center for the output. Default 1. @note Will
                              be passed through std::abs() to ensure it is positive. */
     bool invert_output{
@@ -89,8 +93,8 @@ template <typename T> class RangeMapper {
     output_range_ = std::abs(config.output_range);
     output_min_ = output_center_ - output_range_;
     output_max_ = output_center_ + output_range_;
-    pos_range_ = (maximum_ - center_) / output_range_;
-    neg_range_ = std::abs(minimum_ - center_) / output_range_;
+    pos_range_ = (maximum_ - (center_ + deadband_)) / output_range_;
+    neg_range_ = (center_ - deadband_ - minimum_) / output_range_;
     invert_output_ = config.invert_output;
   }
 
@@ -138,41 +142,72 @@ template <typename T> class RangeMapper {
   T map(const T &v) const {
     T clamped = std::clamp(v, minimum_, maximum_);
     T calibrated{0};
+    bool positive_input = clamped >= center_;
     if (invert_input_) {
       // if we invert the input, then we are comparing against the min/max
-      calibrated = clamped >= T(0) ? maximum_ - clamped : minimum_ - clamped;
+      calibrated = positive_input ? maximum_ - clamped : minimum_ - clamped;
+      // if it's within the deadband, return the output center
+      if (std::abs(calibrated) < deadband_) {
+        return output_center_;
+      }
+      // remove the deadband from the calibrated value
+      calibrated = positive_input ? calibrated + deadband_ : calibrated - deadband_;
     } else {
       // normally we compare against center
       calibrated = clamped - center_;
+      // if it's within the deadband, return the output center
+      if (std::abs(calibrated) < deadband_) {
+        return output_center_;
+      }
+      // remove the deadband from the calibrated value
+      calibrated = positive_input ? calibrated - deadband_ : calibrated + deadband_;
     }
-    if (std::abs(calibrated) < deadband_) {
-      return output_center_;
-    }
-    T output = calibrated >= T(0) ? calibrated / pos_range_ + output_center_
-                                  : calibrated / neg_range_ + output_center_;
+    T output = positive_input ? calibrated / pos_range_ + output_center_
+                              : calibrated / neg_range_ + output_center_;
     if (invert_output_) {
       output = -output;
     }
-    return output;
+    return std::clamp(output, output_min_, output_max_);
   }
 
   /**
    * @brief Unmap a value \p v from the configured output range (centered,
    *        default [-1,1]) back into the input distribution.
    * @param T&v Value from the centered output distribution.
    * @return Value within the input distribution.
+   * @note If `invert_input` is true, then the max/min of the input range both
+   *       map to the output center, which means that unmapping a value at the
+   *       output center is ambiguous. In this case unmap() will return the
+   *       maximum input value.
    */
   T unmap(const T &v) const {
-    T calibrated =
-        v >= T(0) ? (v - output_center_) * pos_range_ : (v - output_center_) * neg_range_;
+    T clamped = std::clamp(v, output_min_, output_max_);
+    T calibrated{0};
     if (invert_output_) {
-      calibrated = -calibrated;
+      clamped = -clamped;
+    }
+    bool positive_output = clamped >= output_center_;
+    if (positive_output) {
+      calibrated = (clamped - output_center_) * pos_range_;
+    } else {
+      calibrated = (clamped - output_center_) * neg_range_;
     }
-    T clamped = calibrated + center_;
     if (invert_input_) {
-      clamped = calibrated >= T(0) ? maximum_ - calibrated : minimum_ - calibrated;
+      if (clamped == output_center_) {
+        // NOTE: we cannot know if the original input value was minimum or maximum
+        //       so we return the maximum value
+        return maximum_;
+      }
+      calibrated =
+          positive_output ? maximum_ - calibrated + deadband_ : minimum_ - calibrated + deadband_;
+    } else {
+      if (clamped == output_center_) {
+        return center_;
+      }
+      calibrated =
+          positive_output ? calibrated + center_ + deadband_ : calibrated + center_ - deadband_;
     }
-    return std::clamp(clamped, minimum_, maximum_);
+    return std::clamp(calibrated, minimum_, maximum_);
   }
 
 protected: