SCRIPT Move java files

wpimath/src/main/java/org/wpilib/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.
+ */
+@SuppressWarnings("PMD.ImplicitFunctionalInterface")
+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.
+   */
+  T interpolate(T endValue, double t);
+}

wpimath/src/main/java/org/wpilib/math/interpolation/InterpolatingDoubleTreeMap.java

@@ -0,0 +1,44 @@
+// 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.Map;
+
+/**
+ * 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>Example of use:
+ *
+ * <pre><code>
+ * InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+ * table.put(0.0, 0.0);
+ * table.put(1.0, 10.0);
+ * table.put(2.0, 30.0);
+ * // ...
+ * double result = table.get(1.5); // Returns 20.0
+ * </code></pre>
+ */
+public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+  /** Default constructor. */
+  public InterpolatingDoubleTreeMap() {
+    super(InverseInterpolator.forDouble(), Interpolator.forDouble());
+  }
+
+  /**
+   * Creates an {@link InterpolatingDoubleTreeMap} from the given entries.
+   *
+   * @param entries The entries to add to the map.
+   * @return The map filled with the {@code entries}.
+   */
+  @SafeVarargs
+  public static InterpolatingDoubleTreeMap ofEntries(Map.Entry<Double, Double>... entries) {
+    InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap();
+    for (var entry : entries) {
+      map.put(entry.getKey(), entry.getValue());
+    }
+    return map;
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/InterpolatingMatrixTreeMap.java

@@ -0,0 +1,98 @@
+// 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;
+
+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.
+ *
+ * @param <K> Key type.
+ * @param <R> Number of matrix rows.
+ * @param <C> Number of matrix columns.
+ */
+public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
+  private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
+
+  /** Default constructor. */
+  public InterpolatingMatrixTreeMap() {}
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, Matrix<R, C> 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 a linear interpolation between the
+   * keys before and after the provided one.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public Matrix<R, C> get(K key) {
+    Matrix<R, C> 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);
+      }
+      Matrix<R, C> floor = m_map.get(floorKey);
+      Matrix<R, C> ceiling = m_map.get(ceilingKey);
+
+      return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
+    } else {
+      return val;
+    }
+  }
+
+  /**
+   * Return the value interpolated between val1 and val2 by the interpolant d.
+   *
+   * @param val1 The lower part of the interpolation range.
+   * @param val2 The upper part of the interpolation range.
+   * @param d The interpolant in the range [0, 1].
+   * @return The interpolated value.
+   */
+  public Matrix<R, C> interpolate(Matrix<R, C> val1, Matrix<R, C> val2, double d) {
+    var dydx = val2.minus(val1);
+    return dydx.times(d).plus(val1);
+  }
+
+  /**
+   * Return where within interpolation range [0, 1] q is between down and up.
+   *
+   * @param up Upper part of interpolation range.
+   * @param q Query.
+   * @param down Lower part of interpolation range.
+   * @return Interpolant in range [0, 1].
+   */
+  public double inverseInterpolate(K up, K q, K down) {
+    double upperToLower = up.doubleValue() - down.doubleValue();
+    if (upperToLower <= 0) {
+      return 0.0;
+    }
+    double queryToLower = q.doubleValue() - down.doubleValue();
+    if (queryToLower <= 0) {
+      return 0.0;
+    }
+    return queryToLower / upperToLower;
+  }
+}

wpimath/src/main/java/org/wpilib/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/org/wpilib/math/interpolation/Interpolator.java

@@ -0,0 +1,35 @@
+// 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);
+
+  /**
+   * Returns interpolator for Double.
+   *
+   * @return Interpolator for Double.
+   */
+  static Interpolator<Double> forDouble() {
+    return MathUtil::lerp;
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/InverseInterpolator.java

@@ -0,0 +1,35 @@
+// 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);
+
+  /**
+   * Returns inverse interpolator for Double.
+   *
+   * @return Inverse interpolator for Double.
+   */
+  static InverseInterpolator<Double> forDouble() {
+    return MathUtil::inverseLerp;
+  }
+}

wpimath/src/main/java/org/wpilib/math/interpolation/TimeInterpolatableBuffer.java

@@ -0,0 +1,151 @@
+// 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.Optional;
+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 final class TimeInterpolatableBuffer<T> {
+  private final double m_historySize;
+  private final Interpolator<T> m_interpolatingFunc;
+  private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
+
+  /**
+   * Constructs a TimeInterpolatableBuffer.
+   *
+   * @param interpolateFunction Interpolation function.
+   * @param historySize The history size of the buffer in seconds.
+   */
+  private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySize) {
+    this.m_historySize = historySize;
+    this.m_interpolatingFunc = interpolateFunction;
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer.
+   *
+   * @param interpolateFunction The function used to interpolate between values.
+   * @param historySize The history size of the buffer in seconds.
+   * @param <T> The type of data to store in the buffer.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static <T> TimeInterpolatableBuffer<T> createBuffer(
+      Interpolator<T> interpolateFunction, double historySize) {
+    return new TimeInterpolatableBuffer<>(interpolateFunction, historySize);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
+   *
+   * @param historySize The history size of the buffer in seconds.
+   * @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 historySize) {
+    return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySize);
+  }
+
+  /**
+   * Create a new TimeInterpolatableBuffer to store Double values.
+   *
+   * @param historySize The history size of the buffer in seconds.
+   * @return The new TimeInterpolatableBuffer.
+   */
+  public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySize) {
+    return new TimeInterpolatableBuffer<>(MathUtil::lerp, historySize);
+  }
+
+  /**
+   * Add a sample to the buffer.
+   *
+   * @param time The timestamp of the sample in seconds.
+   * @param sample The sample object.
+   */
+  public void addSample(double time, T sample) {
+    cleanUp(time);
+    m_pastSnapshots.put(time, sample);
+  }
+
+  /**
+   * Removes samples older than our current history size.
+   *
+   * @param time The current timestamp in seconds.
+   */
+  private void cleanUp(double time) {
+    while (!m_pastSnapshots.isEmpty()) {
+      var entry = m_pastSnapshots.firstEntry();
+      if (time - entry.getKey() >= m_historySize) {
+        m_pastSnapshots.remove(entry.getKey());
+      } else {
+        return;
+      }
+    }
+  }
+
+  /** Clear all old samples. */
+  public void clear() {
+    m_pastSnapshots.clear();
+  }
+
+  /**
+   * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
+   *
+   * @param time The time at which to sample in seconds.
+   * @return The interpolated value at that timestamp or an empty Optional.
+   */
+  public Optional<T> getSample(double time) {
+    if (m_pastSnapshots.isEmpty()) {
+      return Optional.empty();
+    }
+
+    // Special case for when the requested time is the same as a sample
+    var nowEntry = m_pastSnapshots.get(time);
+    if (nowEntry != null) {
+      return Optional.of(nowEntry);
+    }
+
+    var topBound = m_pastSnapshots.ceilingEntry(time);
+    var bottomBound = m_pastSnapshots.floorEntry(time);
+
+    // Return null if neither sample exists, and the opposite bound if the other is null
+    if (topBound == null && bottomBound == null) {
+      return Optional.empty();
+    } else if (topBound == null) {
+      return Optional.of(bottomBound.getValue());
+    } else if (bottomBound == null) {
+      return Optional.of(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 Optional.of(
+          m_interpolatingFunc.interpolate(
+              bottomBound.getValue(),
+              topBound.getValue(),
+              (time - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+    }
+  }
+
+  /**
+   * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
+   * stored within this buffer.
+   *
+   * @return The internal sample buffer.
+   */
+  public NavigableMap<Double, T> getInternalBuffer() {
+    return m_pastSnapshots;
+  }
+}