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