001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math;
006
007import java.util.TreeMap;
008
009/**
010 * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
011 * from points that are defined. This uses linear interpolation.
012 *
013 * @param <K> Key type.
014 * @param <R> Number of matrix rows.
015 * @param <C> Number of matrix columns.
016 */
017public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
018  private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
019
020  /** Default constructor. */
021  public InterpolatingMatrixTreeMap() {}
022
023  /**
024   * Inserts a key-value pair.
025   *
026   * @param key The key.
027   * @param value The value.
028   */
029  public void put(K key, Matrix<R, C> value) {
030    m_map.put(key, value);
031  }
032
033  /**
034   * Returns the value associated with a given key.
035   *
036   * <p>If there's no matching key, the value returned will be a linear interpolation between the
037   * keys before and after the provided one.
038   *
039   * @param key The key.
040   * @return The value associated with the given key.
041   */
042  public Matrix<R, C> get(K key) {
043    Matrix<R, C> val = m_map.get(key);
044    if (val == null) {
045      K ceilingKey = m_map.ceilingKey(key);
046      K floorKey = m_map.floorKey(key);
047
048      if (ceilingKey == null && floorKey == null) {
049        return null;
050      }
051      if (ceilingKey == null) {
052        return m_map.get(floorKey);
053      }
054      if (floorKey == null) {
055        return m_map.get(ceilingKey);
056      }
057      Matrix<R, C> floor = m_map.get(floorKey);
058      Matrix<R, C> ceiling = m_map.get(ceilingKey);
059
060      return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
061    } else {
062      return val;
063    }
064  }
065
066  /**
067   * Return the value interpolated between val1 and val2 by the interpolant d.
068   *
069   * @param val1 The lower part of the interpolation range.
070   * @param val2 The upper part of the interpolation range.
071   * @param d The interpolant in the range [0, 1].
072   * @return The interpolated value.
073   */
074  public Matrix<R, C> interpolate(Matrix<R, C> val1, Matrix<R, C> val2, double d) {
075    var dydx = val2.minus(val1);
076    return dydx.times(d).plus(val1);
077  }
078
079  /**
080   * Return where within interpolation range [0, 1] q is between down and up.
081   *
082   * @param up Upper part of interpolation range.
083   * @param q Query.
084   * @param down Lower part of interpolation range.
085   * @return Interpolant in range [0, 1].
086   */
087  public double inverseInterpolate(K up, K q, K down) {
088    double upperToLower = up.doubleValue() - down.doubleValue();
089    if (upperToLower <= 0) {
090      return 0.0;
091    }
092    double queryToLower = q.doubleValue() - down.doubleValue();
093    if (queryToLower <= 0) {
094      return 0.0;
095    }
096    return queryToLower / upperToLower;
097  }
098}