[wpimath] Add interpolators for measures

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

@@ -6,6 +6,8 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
 
 /** Math utility functions. */
 public final class MathUtil {
@@ -26,6 +28,20 @@ public static double lerp(double a, double b, double t) {
     return a + (b - a) * t;
   }
 
+  /**
+   * Perform linear interpolation between two values with units.
+   *
+   * @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 is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @SuppressWarnings("unchecked")
+  public static <U extends Unit, M extends Measure<U>> M lerp(
+      M startValue, M endValue, double t) {
+    return (M) startValue.plus(endValue.minus(startValue).times(MathUtil.clamp(t, 0, 1)));
+  }
+
   /**
    * Returns the interpolant t that reflects where q is with respect to the range (a, b). In other
    * words, returns t such that q = a + (b - a)t. If a = b, then returns 0.
@@ -46,6 +62,51 @@ public static double inverseLerp(double a, double b, double q) {
     }
   }
 
+  /**
+   * 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 <U extends Unit, M extends Measure<U>> double inverseLerp(
+      M startValue, M endValue, M q) {
+    double totalRange = endValue.minus(startValue).in(startValue.unit());
+    if (totalRange <= 0) {
+      return 0.0;
+    }
+    double queryToStart = q.minus(startValue).in(startValue.unit());
+    if (queryToStart <= 0) {
+      return 0.0;
+    }
+    return queryToStart / totalRange;
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low The lower boundary to which to clamp value.
+   * @param high The higher boundary to which to clamp value.
+   * @return The clamped value.
+   */
+  public static int clamp(int value, int low, int high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low The lower boundary to which to clamp value.
+   * @param high The higher boundary to which to clamp value.
+   * @return The clamped value.
+   */
+  public static double clamp(double value, double low, double high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
   /**
    * Returns 0.0 if the given value is within the specified range around zero. The remaining range
    * between the deadband and the maximum magnitude is scaled from 0.0 to the maximum magnitude.

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

@@ -5,6 +5,8 @@
 package edu.wpi.first.math.interpolation;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
 
 /**
  * An interpolation function that returns a value interpolated between an upper and lower bound.
@@ -32,4 +34,15 @@ public interface Interpolator<T> {
   static Interpolator<Double> forDouble() {
     return MathUtil::lerp;
   }
+
+  /**
+   * Returns interpolator for a Measure.
+   *
+   * @param <U> The unit of the Measure.
+   * @param <M> The type of the Measure.
+   * @return Interpolator for a Measure.
+   */
+  static <U extends Unit, M extends Measure<U>> Interpolator<M> forMeasure() {
+    return MathUtil::lerp;
+  }
 }

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

@@ -5,6 +5,8 @@
 package edu.wpi.first.math.interpolation;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Unit;
 
 /**
  * An inverse interpolation function which determines where within an interpolation range an object
@@ -32,4 +34,15 @@ public interface InverseInterpolator<T> {
   static InverseInterpolator<Double> forDouble() {
     return MathUtil::inverseLerp;
   }
+
+  /**
+   * Returns inverse interpolator for a Measure.
+   *
+   * @param <U> The unit of the Measure.
+   * @param <M> The type of the Measure.
+   * @return Inverse interpolator for a Measure.
+   */
+  static <U extends Unit, M extends Measure<U>> InverseInterpolator<M> forMeasure() {
+    return MathUtil::inverseLerp;
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math;
 
+import static edu.wpi.first.units.Units.Centimeters;
+import static edu.wpi.first.units.Units.Feet;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -49,6 +53,43 @@ void testLerp() {
     assertEquals(9.0, MathUtil.lerp(1.0, 5.0, 2.0));
   }
 
+  @Test
+  void testLerpWithSameUnits() {
+    assertEquals(Meters.of(50), MathUtil.lerp(Meters.of(0), Meters.of(100), 0.5));
+    assertEquals(Meters.of(-50), MathUtil.lerp(Meters.of(0), Meters.of(-100), 0.5));
+    assertEquals(Meters.of(0), MathUtil.lerp(Meters.of(-50), Meters.of(50), 0.5));
+    assertEquals(Meters.of(-25), MathUtil.lerp(Meters.of(-50), Meters.of(50), 0.25));
+    assertEquals(Meters.of(25), MathUtil.lerp(Meters.of(-50), Meters.of(50), 0.75));
+
+    assertEquals(Meters.of(0), MathUtil.lerp(Meters.of(0), Meters.of(-100), -0.5));
+  }
+
+  @Test
+  void testLerpWithDifferentUnits() {
+    assertEquals(Inches.of(6), MathUtil.lerp(Meters.of(0), Feet.of(1), 0.5));
+    assertEquals(Inches.of(-6), MathUtil.lerp(Meters.of(0), Feet.of(-1), 0.5));
+    assertEquals(Inches.of(0), MathUtil.lerp(Centimeters.of(-500), Meters.of(5), 0.5));
+    assertEquals(Inches.of(-6), MathUtil.lerp(Feet.of(-1), Inches.of(12), 0.25));
+    assertEquals(Inches.of(6), MathUtil.lerp(Feet.of(-1), Inches.of(12), 0.75));
+
+    assertEquals(Inches.of(0), MathUtil.lerp(Inches.of(0), Feet.of(-1), -0.5));
+  }
+
+  @Test
+  void testClamp() {
+    // int
+    assertEquals(5, MathUtil.clamp(10, 1, 5));
+
+    // double
+    assertEquals(5.5, MathUtil.clamp(10.5, 1.5, 5.5));
+
+    // negative int
+    assertEquals(-5, MathUtil.clamp(-10, -5, -1));
+
+    // negative double
+    assertEquals(-5.5, MathUtil.clamp(-10.5, -5.5, -1.5));
+  }
+
   @Test
   void testApplyDeadbandUnityScale() {
     // < 0

wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java

@@ -6,6 +6,8 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Distance;
 import org.junit.jupiter.api.Test;
 
 class InterpolatingTreeMapTest {
@@ -45,4 +47,44 @@ void testInterpolation() {
 
     assertEquals(375.0, table.get(150.0));
   }
+
+  @Test
+  void testMeasureInterpolation() {
+    InterpolatingTreeMap<Distance, Distance> table =
+        new InterpolatingTreeMap<>(InverseInterpolator.forMeasure(), Interpolator.forMeasure());
+
+    table.put(Units.Inches.of(Units.Meters.of(125.0).in(Units.Inches)), Units.Meters.of(450.0));
+    table.put(Units.Meters.of(200.0), Units.Feet.of(Units.Meters.of(510.0).in(Units.Feet)));
+    table.put(
+        Units.Centimeters.of(Units.Meters.of(268.0).in(Units.Centimeters)), Units.Meters.of(525.0));
+    table.put(
+        Units.Inches.of(Units.Meters.of(312.0).in(Units.Inches)),
+        Units.Feet.of(Units.Meters.of(550.0).in(Units.Feet)));
+    table.put(Units.Meters.of(326.0), Units.Meters.of(650.0));
+
+    // Key below minimum gives the smallest value
+    assertEquals(Units.Meters.of(450.0), table.get(Units.Meters.of(100.0)));
+
+    // Minimum key gives exact value
+    assertEquals(Units.Meters.of(450.0), table.get(Units.Meters.of(125.0)));
+
+    // Key gives interpolated value
+    assertEquals(Units.Meters.of(480.0), table.get(Units.Meters.of(162.5)));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(Units.Meters.of(510.0), table.get(Units.Meters.of(200.0)));
+
+    // Maximum key gives exact value
+    assertEquals(Units.Meters.of(650.0), table.get(Units.Meters.of(326.0)));
+
+    // Key above maximum gives largest value
+    assertEquals(Units.Meters.of(650.0), table.get(Units.Meters.of(400.0)));
+
+    table.clear();
+
+    table.put(Units.Meters.of(100.0), Units.Meters.of(250.0));
+    table.put(Units.Meters.of(200.0), Units.Meters.of(500.0));
+
+    assertEquals(Units.Meters.of(375.0), table.get(Units.Meters.of(150.0)));
+  }
 }