[wpimath] Add TimeInterpolatableBuffer (#2695)

These classes are useful for storing previous robot positions to use in conjunction with the upcoming pose estimators.

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: cttew <cttewari@gmail.com>

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

@@ -84,4 +84,17 @@ public final class MathUtil {
   public static double angleModulus(double angleRadians) {
     return inputModulus(angleRadians, -Math.PI, Math.PI);
   }
+
+  /**
+   * Perform linear 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 is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @SuppressWarnings("ParameterName")
+  public static double interpolate(double startValue, double endValue, double t) {
+    return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -8,12 +8,13 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /** Represents a 2d pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose2d {
+public class Pose2d implements Interpolatable<Pose2d> {
   private final Translation2d m_translation;
   private final Rotation2d m_rotation;
 
@@ -242,4 +243,18 @@ public class Pose2d {
   public int hashCode() {
     return Objects.hash(m_translation, m_rotation);
   }
+
+  @Override
+  @SuppressWarnings("ParameterName")
+  public Pose2d interpolate(Pose2d endValue, double t) {
+    if (t < 0) {
+      return this;
+    } else if (t >= 1) {
+      return endValue;
+    } else {
+      var twist = this.log(endValue);
+      var scaledTwist = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
+      return this.exp(scaledTwist);
+    }
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -8,12 +8,14 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Rotation2d {
+public class Rotation2d implements Interpolatable<Rotation2d> {
   private final double m_value;
   private final double m_cos;
   private final double m_sin;
@@ -198,4 +200,10 @@ public class Rotation2d {
   public int hashCode() {
     return Objects.hash(m_value);
   }
+
+  @Override
+  @SuppressWarnings("ParameterName")
+  public Rotation2d interpolate(Rotation2d endValue, double t) {
+    return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t));
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -8,6 +8,8 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /**
@@ -20,7 +22,7 @@ import java.util.Objects;
 @SuppressWarnings({"ParameterName", "MemberName"})
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Translation2d {
+public class Translation2d implements Interpolatable<Translation2d> {
   private final double m_x;
   private final double m_y;
 
@@ -196,4 +198,11 @@ public class Translation2d {
   public int hashCode() {
     return Objects.hash(m_x, m_y);
   }
+
+  @Override
+  public Translation2d interpolate(Translation2d endValue, double t) {
+    return new Translation2d(
+        MathUtil.interpolate(this.getX(), endValue.getX(), t),
+        MathUtil.interpolate(this.getY(), endValue.getY(), t));
+  }
 }

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

@@ -0,0 +1,25 @@
+// 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;
+
+/**
+ * An object should extend interpolatable if you wish to interpolate between a lower and upper
+ * bound, such as a robot position on the field between timesteps. This behavior can be linear or
+ * nonlinear.
+ *
+ * @param <T> The class that is interpolatable.
+ */
+public interface Interpolatable<T> {
+  /**
+   * Return the interpolated value. This object is assumed to be the starting position, or lower
+   * bound.
+   *
+   * @param endValue 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.
+   */
+  @SuppressWarnings("ParameterName")
+  T interpolate(T endValue, double t);
+}

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

@@ -0,0 +1,149 @@
+// 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;
+import java.util.NavigableMap;
+import java.util.TreeMap;
+
+/**
+ * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application
+ * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot
+ * pose at the time when vision or other global measurement were recorded is necessary, or for
+ * recording the past angles of mechanisms as measured by encoders.
+ *
+ * @param <T> The type stored in this buffer.
+ */
+public class TimeInterpolatableBuffer<T> {
+  private final double m_historySize;
+  private final InterpolateFunction<T> m_interpolatingFunc;
+  private final NavigableMap<Double, T> m_buffer = new TreeMap<>();
+
+  private TimeInterpolatableBuffer(
+      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+    this.m_historySize = historySizeSeconds;
+    this.m_interpolatingFunc = interpolateFunction;
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer.
+   *
+   * @param interpolateFunction The function used to interpolate between values.
+   * @param historySizeSeconds The history size of the buffer.
+   * @param <T> The type of data to store in the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static <T> TimeInterpolatableBuffer<T> createBuffer(
+      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+    return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
+   *
+   * @param historySizeSeconds The history size of the buffer.
+   * @param <T> The type of {@link Interpolatable} to store in the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer(
+      double historySizeSeconds) {
+    return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySizeSeconds);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer to store Double values.
+   *
+   * @param historySizeSeconds The history size of the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySizeSeconds) {
+    return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds);
+  }
+
+  /**
+   * Add a sample to the buffer.
+   *
+   * @param timeSeconds The timestamp of the sample.
+   * @param sample The sample object.
+   */
+  public void addSample(double timeSeconds, T sample) {
+    cleanUp(timeSeconds);
+    m_buffer.put(timeSeconds, sample);
+  }
+
+  /**
+   * Removes samples older than our current history size.
+   *
+   * @param time The current timestamp.
+   */
+  private void cleanUp(double time) {
+    while (!m_buffer.isEmpty()) {
+      var entry = m_buffer.firstEntry();
+      if (time - entry.getKey() >= m_historySize) {
+        m_buffer.remove(entry.getKey());
+      } else {
+        return;
+      }
+    }
+  }
+
+  /** Clear all old samples. */
+  public void clear() {
+    m_buffer.clear();
+  }
+
+  /**
+   * Sample the buffer at the given time. If the buffer is empty, this will return null.
+   *
+   * @param timeSeconds The time at which to sample.
+   * @return The interpolated value at that timestamp. Might be null.
+   */
+  @SuppressWarnings("UnnecessaryParentheses")
+  public T getSample(double timeSeconds) {
+    if (m_buffer.isEmpty()) {
+      return null;
+    }
+
+    // Special case for when the requested time is the same as a sample
+    var nowEntry = m_buffer.get(timeSeconds);
+    if (nowEntry != null) {
+      return nowEntry;
+    }
+
+    var topBound = m_buffer.ceilingEntry(timeSeconds);
+    var bottomBound = m_buffer.floorEntry(timeSeconds);
+
+    // Return null if neither sample exists, and the opposite bound if the other is null
+    if (topBound == null && bottomBound == null) {
+      return null;
+    } else if (topBound == null) {
+      return bottomBound.getValue();
+    } else if (bottomBound == null) {
+      return topBound.getValue();
+    } else {
+      // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
+      // between the current time and bottom bound) and (the difference between top and bottom
+      // bounds).
+      return m_interpolatingFunc.interpolate(
+          bottomBound.getValue(),
+          topBound.getValue(),
+          ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+    }
+  }
+
+  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.
+     */
+    @SuppressWarnings("ParameterName")
+    T interpolate(T start, T end, double t);
+  }
+}

wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp

@@ -0,0 +1,26 @@
+// 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.
+
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+
+namespace frc {
+
+// Template specialization to ensure that Pose2d uses pose exponential
+template <>
+TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
+    units::second_t historySize)
+    : m_historySize(historySize),
+      m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) {
+        if (t < 0) {
+          return start;
+        } else if (t >= 1) {
+          return end;
+        } else {
+          Twist2d twist = start.Log(end);
+          Twist2d scaledTwist = twist * t;
+          return start.Exp(scaledTwist);
+        }
+      }) {}
+
+}  // namespace frc

wpimath/src/main/native/cpp/trajectory/Trajectory.cpp

@@ -6,6 +6,7 @@
 
 #include <algorithm>
 
+#include <wpi/MathExtras.h>
 #include <wpi/json.h>
 
 #include "units/math.h"
@@ -25,7 +26,7 @@ bool Trajectory::State::operator!=(const Trajectory::State& other) const {
 Trajectory::State Trajectory::State::Interpolate(State endValue,
                                                  double i) const {
   // Find the new [t] value.
-  const auto newT = Lerp(t, endValue.t, i);
+  const auto newT = wpi::Lerp(t, endValue.t, i);
 
   // Find the delta time between the current state and the interpolated state.
   const auto deltaT = newT - t;
@@ -58,8 +59,8 @@ Trajectory::State Trajectory::State::Interpolate(State endValue,
       newS / endValue.pose.Translation().Distance(pose.Translation());
 
   return {newT, newV, acceleration,
-          Lerp(pose, endValue.pose, interpolationFrac),
-          Lerp(curvature, endValue.curvature, interpolationFrac)};
+          wpi::Lerp(pose, endValue.pose, interpolationFrac),
+          wpi::Lerp(curvature, endValue.curvature, interpolationFrac)};
 }
 
 Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {

wpimath/src/main/native/include/frc/geometry/Twist2d.h

@@ -53,5 +53,15 @@ struct WPILIB_DLLEXPORT Twist2d {
    * @return Whether the two objects are not equal.
    */
   bool operator!=(const Twist2d& other) const { return !operator==(other); }
+
+  /**
+   * Scale this by a given factor.
+   *
+   * @param factor The factor by which to scale.
+   * @return The scaled Twist2d.
+   */
+  Twist2d operator*(double factor) {
+    return Twist2d{dx * factor, dy * factor, dtheta * factor};
+  }
 };
 }  // namespace frc

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -0,0 +1,128 @@
+// 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.
+
+#pragma once
+
+#include <array>
+#include <functional>
+#include <map>
+#include <utility>
+#include <vector>
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * The TimeInterpolatableBuffer provides an easy way to estimate past
+ * measurements. One application might be in conjunction with the
+ * DifferentialDrivePoseEstimator, where knowledge of the robot pose at the time
+ * when vision or other global measurement were recorded is necessary, or for
+ * recording the past angles of mechanisms as measured by encoders.
+ *
+ * When sampling this buffer, a user-provided function or wpi::Lerp can be
+ * used. For Pose2ds, we use Twists.
+ *
+ * @tparam T The type stored in this buffer.
+ */
+template <typename T>
+class TimeInterpolatableBuffer {
+ public:
+  /**
+   * Create a new TimeInterpolatableBuffer.
+   *
+   * @param historySize  The history size of the buffer.
+   * @param func The function used to interpolate between values.
+   */
+  TimeInterpolatableBuffer(units::second_t historySize,
+                           std::function<T(const T&, const T&, double)> func)
+      : m_historySize(historySize), m_interpolatingFunc(func) {}
+
+  /**
+   * Create a new TimeInterpolatableBuffer. By default, the interpolation
+   * function is wpi::Lerp except for Pose2d, which uses the pose exponential.
+   *
+   * @param historySize  The history size of the buffer.
+   */
+  explicit TimeInterpolatableBuffer(units::second_t historySize)
+      : m_historySize(historySize),
+        m_interpolatingFunc([](const T& start, const T& end, double t) {
+          return wpi::Lerp(start, end, t);
+        }) {}
+
+  /**
+   * Add a sample to the buffer.
+   *
+   * @param time   The timestamp of the sample.
+   * @param sample The sample object.
+   */
+  void AddSample(units::second_t time, T sample) {
+    // Add the new state into the vector.
+    if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
+      m_pastSnapshots.emplace_back(time, sample);
+    } else {
+      m_pastSnapshots.insert(
+          std::upper_bound(
+              m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+              [](auto t, const auto& pair) { return t < pair.first; }),
+          std::pair(time, sample));
+    }
+    while (time - m_pastSnapshots[0].first > m_historySize) {
+      m_pastSnapshots.erase(m_pastSnapshots.begin());
+    }
+  }
+
+  /** Clear all old samples. */
+  void Clear() { m_pastSnapshots.clear(); }
+
+  /**
+   * Sample the buffer at the given time. If there are no elements in the
+   * buffer, calling this function results in undefined behavior.
+   *
+   * @param time The time at which to sample the buffer.
+   */
+  T Sample(units::second_t time) {
+    // We will perform a binary search to find the index of the element in the
+    // vector that has a timestamp that is equal to or greater than the vision
+    // measurement timestamp.
+
+    if (time <= m_pastSnapshots.front().first) {
+      return m_pastSnapshots.front().second;
+    }
+    if (time > m_pastSnapshots.back().first) {
+      return m_pastSnapshots.back().second;
+    }
+    if (m_pastSnapshots.size() < 2) {
+      return m_pastSnapshots[0].second;
+    }
+
+    // Get the iterator which has a key no less than the requested key.
+    auto upper_bound = std::lower_bound(
+        m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+        [](const auto& pair, auto t) { return t > pair.first; });
+
+    auto lower_bound = upper_bound - 1;
+
+    double t = ((time - lower_bound->first) /
+                (upper_bound->first - lower_bound->first));
+
+    return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
+  }
+
+ private:
+  units::second_t m_historySize;
+  std::vector<std::pair<units::second_t, T>> m_pastSnapshots;
+  std::function<T(const T&, const T&, double)> m_interpolatingFunc;
+};
+
+// Template specialization to ensure that Pose2d uses pose exponential
+template <>
+WPILIB_DLLEXPORT TimeInterpolatableBuffer<Pose2d>::TimeInterpolatableBuffer(
+    units::second_t historySize);
+
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/Trajectory.h

@@ -157,20 +157,6 @@ class WPILIB_DLLEXPORT Trajectory {
  private:
   std::vector<State> m_states;
   units::second_t m_totalTime = 0_s;
-
-  /**
-   * Linearly interpolates between two values.
-   *
-   * @param startValue The start value.
-   * @param endValue The end value.
-   * @param t The fraction for interpolation.
-   *
-   * @return The interpolated value.
-   */
-  template <typename T>
-  static T Lerp(const T& startValue, const T& endValue, const double t) {
-    return startValue + (endValue - startValue) * t;
-  }
 };
 
 WPILIB_DLLEXPORT

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

@@ -0,0 +1,43 @@
+// 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+public class TimeInterpolatableBufferTest {
+  @Test
+  public void testInterpolation() {
+    TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+    buffer.addSample(0, new Rotation2d());
+    assertEquals(0, buffer.getSample(0).getRadians(), 0.001);
+    buffer.addSample(1, new Rotation2d(1));
+    assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001);
+    assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001);
+    buffer.addSample(3, new Rotation2d(2));
+    assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001);
+
+    buffer.addSample(10.5, new Rotation2d(2));
+    assertEquals(new Rotation2d(1), buffer.getSample(0));
+  }
+
+  @Test
+  public void testPose2d() {
+    TimeInterpolatableBuffer<Pose2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
+
+    // We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
+    buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
+    buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
+    Pose2d sample = buffer.getSample(0.5);
+
+    assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
+    assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);
+    assertEquals(45, sample.getRotation().getDegrees(), 0.01);
+  }
+}

wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp

@@ -0,0 +1,38 @@
+// 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.
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+TEST(TimeInterpolatableBufferTest, Interpolation) {
+  frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
+
+  buffer.AddSample(0_s, frc::Rotation2d(0_rad));
+  EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad));
+  buffer.AddSample(1_s, frc::Rotation2d(1_rad));
+  EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad));
+  EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad));
+  buffer.AddSample(3_s, frc::Rotation2d(2_rad));
+  EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad));
+
+  buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
+  EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
+}
+
+TEST(TimeInterpolatableBufferTest, Pose2d) {
+  frc::TimeInterpolatableBuffer<frc::Pose2d> buffer{10_s};
+  // We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
+  buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
+  buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
+  frc::Pose2d sample = buffer.Sample(0.5_s);
+  EXPECT_TRUE(std::abs(sample.X().to<double>() - (1 - 1 / std::sqrt(2))) <
+              0.01);
+  EXPECT_TRUE(std::abs(sample.Y().to<double>() - (1 / std::sqrt(2))) < 0.01);
+  EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to<double>() - 45) < 0.01);
+}

wpiutil/src/main/native/include/wpi/MathExtras.h

@@ -845,6 +845,20 @@ constexpr int sgn(T val) {
   return (T(0) < val) - (val < T(0));
 }
 
+/**
+ * Linearly interpolates between two values.
+ *
+ * @param startValue The start value.
+ * @param endValue The end value.
+ * @param t The fraction for interpolation.
+ *
+ * @return The interpolated value.
+ */
+template <typename T>
+constexpr T Lerp(const T& startValue, const T& endValue, double t) {
+  return startValue + (endValue - startValue) * t;
+}
+
 } // namespace wpi
 
 #endif