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 */
013public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
014  private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
015
016  /**
017   * Inserts a key-value pair.
018   *
019   * @param key The key.
020   * @param value The value.
021   */
022  public void put(K key, Matrix<R, C> value) {
023    m_map.put(key, value);
024  }
025
026  /**
027   * Returns the value associated with a given key.
028   *
029   * <p>If there's no matching key, the value returned will be a linear interpolation between the
030   * keys before and after the provided one.
031   *
032   * @param key The key.
033   * @return The value associated with the given key.
034   */
035  public Matrix<R, C> get(K key) {
036    Matrix<R, C> val = m_map.get(key);
037    if (val == null) {
038      K ceilingKey = m_map.ceilingKey(key);
039      K floorKey = m_map.floorKey(key);
040
041      if (ceilingKey == null && floorKey == null) {
042        return null;
043      }
044      if (ceilingKey == null) {
045        return m_map.get(floorKey);
046      }
047      if (floorKey == null) {
048        return m_map.get(ceilingKey);
049      }
050      Matrix<R, C> floor = m_map.get(floorKey);
051      Matrix<R, C> ceiling = m_map.get(ceilingKey);
052
053      return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
054    } else {
055      return val;
056    }
057  }
058
059  /**
060   * Return the value interpolated between val1 and val2 by the interpolant d.
061   *
062   * @param val1 The lower part of the interpolation range.
063   * @param val2 The upper part of the interpolation range.
064   * @param d The interpolant in the range [0, 1].
065   * @return The interpolated value.
066   */
067  public Matrix<R, C> interpolate(Matrix<R, C> val1, Matrix<R, C> val2, double d) {
068    var dydx = val2.minus(val1);
069    return dydx.times(d).plus(val1);
070  }
071
072  /**
073   * Return where within interpolation range [0, 1] q is between down and up.
074   *
075   * @param up Upper part of interpolation range.
076   * @param q Query.
077   * @param down Lower part of interpolation range.
078   * @return Interpolant in range [0, 1].
079   */
080  public double inverseInterpolate(K up, K q, K down) {
081    double upperToLower = up.doubleValue() - down.doubleValue();
082    if (upperToLower <= 0) {
083      return 0.0;
084    }
085    double queryToLower = q.doubleValue() - down.doubleValue();
086    if (queryToLower <= 0) {
087      return 0.0;
088    }
089    return queryToLower / upperToLower;
090  }
091}