diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 4004ab4d2ad..f6230f114ad 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/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,21 @@ public static double lerp(double a, double b, double t) { return a + (b - a) * t; } + /** + * Perform linear interpolation between two values with units. + * + * @param The unit of the Measure. + * @param The type of the Measure. + * @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 > M lerp(M startValue, M endValue, double t) { + return (M) startValue.plus(endValue.minus(startValue).times(Math.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 +63,29 @@ public static double inverseLerp(double a, double b, double q) { } } + /** + * Return where within interpolation range [0, 1] q is between startValue and endValue. + * + * @param The unit of the Measure. + * @param The type of the Measure. + * @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 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 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. diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java index 37192cf9ec5..37ae5e348d2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java +++ b/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 { static Interpolator forDouble() { return MathUtil::lerp; } + + /** + * Returns interpolator for a Measure. + * + * @param The unit of the Measure. + * @param The type of the Measure. + * @return Interpolator for a Measure. + */ + static > Interpolator forMeasure() { + return MathUtil::lerp; + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java index 92f33e7de96..6f10e8471a6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java +++ b/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 { static InverseInterpolator forDouble() { return MathUtil::inverseLerp; } + + /** + * Returns inverse interpolator for a Measure. + * + * @param The unit of the Measure. + * @param The type of the Measure. + * @return Inverse interpolator for a Measure. + */ + static > InverseInterpolator forMeasure() { + return MathUtil::inverseLerp; + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index d4262db4e16..88895c962e8 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/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,28 @@ 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 testApplyDeadbandUnityScale() { // < 0 diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java index b04b40ab3c0..a00cac0e5db 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java +++ b/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 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))); + } }