[wpiutil,wpimath] Add generic InterpolatingTreeMap (#5372)

wpimath/src/main/java/edu/wpi/first/math/MathUtil.java

@@ -146,6 +146,26 @@ public final class MathUtil {
     return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
   }
 
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  public static double inverseInterpolate(double startValue, double endValue, double q) {
+    double totalRange = endValue - startValue;
+    if (totalRange <= 0) {
+      return 0.0;
+    }
+    double queryToStart = q - startValue;
+    if (queryToStart <= 0) {
+      return 0.0;
+    }
+    return queryToStart / totalRange;
+  }
+
   /**
    * Checks if the given value matches an expected value within a certain tolerance.
    *

wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ */
+public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+  InterpolatingDoubleTreeMap() {
+    super(InverseInterpolator.forDouble(), Interpolator.forDouble());
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java

@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import java.util.Comparator;
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * <p>{@code K} must implement {@link Comparable}, or a {@link Comparator} on {@code K} can be
+ * provided.
+ *
+ * @param <K> The type of keys held in this map.
+ * @param <V> The type of values held in this map.
+ */
+public class InterpolatingTreeMap<K, V> {
+  private final TreeMap<K, V> m_map;
+
+  private final InverseInterpolator<K> m_inverseInterpolator;
+  private final Interpolator<V> m_interpolator;
+
+  /**
+   * Constructs an InterpolatingTreeMap.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator, Interpolator<V> interpolator) {
+    m_map = new TreeMap<>();
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+  }
+
+  /**
+   * Constructs an InterpolatingTreeMap using {@code comparator}.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   * @param comparator Comparator to use on keys.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator,
+      Interpolator<V> interpolator,
+      Comparator<K> comparator) {
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+    m_map = new TreeMap<>(comparator);
+  }
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, V value) {
+    m_map.put(key, value);
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * <p>If there's no matching key, the value returned will be an interpolation between the keys
+   * before and after the provided one, using the {@link Interpolator} and {@link
+   * InverseInterpolator} provided.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public V get(K key) {
+    V val = m_map.get(key);
+    if (val == null) {
+      K ceilingKey = m_map.ceilingKey(key);
+      K floorKey = m_map.floorKey(key);
+
+      if (ceilingKey == null && floorKey == null) {
+        return null;
+      }
+      if (ceilingKey == null) {
+        return m_map.get(floorKey);
+      }
+      if (floorKey == null) {
+        return m_map.get(ceilingKey);
+      }
+      V floor = m_map.get(floorKey);
+      V ceiling = m_map.get(ceilingKey);
+
+      return m_interpolator.interpolate(
+          floor, ceiling, m_inverseInterpolator.inverseInterpolate(floorKey, ceilingKey, key));
+    } else {
+      return val;
+    }
+  }
+
+  /** Clears the contents. */
+  public void clear() {
+    m_map.clear();
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An interpolation function that returns a value interpolated between an upper and lower bound.
+ * This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link Interpolator} will operate on.
+ */
+@FunctionalInterface
+public interface Interpolator<T> {
+  /**
+   * Perform interpolation between two values.
+   *
+   * @param startValue The value to start at.
+   * @param endValue The value to end at.
+   * @param t How far between the two values to interpolate. This should be bounded to [0, 1].
+   * @return The interpolated value.
+   */
+  T interpolate(T startValue, T endValue, double t);
+
+  static Interpolator<Double> forDouble() {
+    return MathUtil::interpolate;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An inverse interpolation function which determines where within an interpolation range an object
+ * lies. This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link InverseInterpolator} will operate on.
+ */
+@FunctionalInterface
+public interface InverseInterpolator<T> {
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  double inverseInterpolate(T startValue, T endValue, T q);
+
+  static InverseInterpolator<Double> forDouble() {
+    return MathUtil::inverseInterpolate;
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java

@@ -19,11 +19,10 @@ import java.util.TreeMap;
  */
 public final class TimeInterpolatableBuffer<T> {
   private final double m_historySize;
-  private final InterpolateFunction<T> m_interpolatingFunc;
+  private final Interpolator<T> m_interpolatingFunc;
   private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
 
-  private TimeInterpolatableBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+  private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySizeSeconds) {
     this.m_historySize = historySizeSeconds;
     this.m_interpolatingFunc = interpolateFunction;
   }
@@ -37,7 +36,7 @@ public final class TimeInterpolatableBuffer<T> {
    * @return The new TimeInterpolatableBuffer.
    */
   public static <T> TimeInterpolatableBuffer<T> createBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+      Interpolator<T> interpolateFunction, double historySizeSeconds) {
     return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
   }
 
@@ -143,17 +142,4 @@ public final class TimeInterpolatableBuffer<T> {
   public NavigableMap<Double, T> getInternalBuffer() {
     return m_pastSnapshots;
   }
-
-  public interface InterpolateFunction<T> {
-    /**
-     * Return the interpolated value. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param start The lower bound, or start.
-     * @param end The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    T interpolate(T start, T end, double t);
-  }
 }