Add an InterpolatingMeasureTreeMap

[ApolloFops]

Is your feature request related to a problem? Please describe.

Whenever trying to use the InterpolatingDoubleTreeMap to store measurements, you can't really take advantage of the unit-safety of the Units library, and if you need to convert between units, you can often end up with stuff like this.

map.put(Feet.of(5.0).in(Meters),  RPM.of(10.0).in(RadiansPerSecond));
map.put(Feet.of(10.0).in(Meters), RPM.of(20.0).in(RadiansPerSecond));
map.put(Feet.of(20.0).in(Meters), RPM.of(50.0).in(RadiansPerSecond));

And it isn't great to get values from either, especially if you want it back in Units.

RadiansPerSecond.of(map.get(10.0)).in(RotationsPerSecond);

Describe the solution you'd like

Some sort of map that instead takes in Measures would be much better in this sort of a situation. You could define it something like this.

map.put(Feet.of(5.0),  RPM.of(10.0));
map.put(Feet.of(10.0), RPM.of(20.0));
map.put(Feet.of(20.0), RPM.of(50.0));

And then when you need a value, you don't need to think about what units it's in.

map.get(Meters.of(3.0)).in(RotationsPerSecond);

Describe alternatives you've considered

Mainly just the stuff above with just converting when you add/fetch the keys.

Additional context

I'd be willing to implement this myself (in fact, I already have something working, which you can find here, with a sample of it being used here - this is pretty heavily based off of some of @Team254's code). The main thing I would like to ask for some help with though is figuring out some sort of way to remove the requirement to pass in both the Measure type and the Unit type, because that makes it a little annoying/unintuitive to create these. Not sure if that's really possible though.

[ApolloFops]

(having #8178 in place would make this very simple to implement)

[rzblue]

InterpolatingTreeMap should be used with custom interpolation/inverse interpolation functions (ie, the functions added in #8178); there's no need for a unit-specific type.

[ApolloFops]

InterpolatingTreeMap should be used with custom interpolation/inverse interpolation functions (ie, the functions added in #8178); there's no need for a unit-specific type.

This does seem like a common enough use case though that it might warrant a custom type just to make it a bit simpler/more intuitive for people to use (like how InterpolatingDoubleTreeMap is just a super simple wrapper around the InterpolatingTreeMap because that's a very common use case). But that solution's pretty clean too, just might warrant an example in the docs I guess.

[SamCarlberg]

I've been using these interpolators this season. We could potentially pull these into wpimath, but dedicated measure-specific subclasses seem unnecessary.

  @SuppressWarnings("unchecked")
  public static <U extends Unit, M extends Measure<U>> Interpolator<M> unitInterpolator() {
    return (startValue, endValue, t) -> (M) endValue.minus(startValue).times(MathUtil.clamp(t, 0, 1)).plus(startValue);
  }

  public static <U extends Unit, M extends Measure<U>> InverseInterpolator<M> inverseUnitInterpolator() {
    return (startValue, endValue, q) -> {
      var totalRange = endValue.minus(startValue);
      if (totalRange.baseUnitMagnitude() <= 0) {
        return 0;
      }

      var queryToStart = q.minus(startValue);
      if (queryToStart.baseUnitMagnitude() <= 0) {
        return 0;
      }

      return ((Dimensionless) queryToStart.div(totalRange)).in(Value);
    };
  }

  // Example usage
  InterpolatingTreeMap<Distance, AngularVelocity> speeds = new InterpolatingTreeMap<>(reverseUnitInterpolator(), unitInterpolator());
  speeds.put(Feet.of(5), RPM.of(10));
  speeds.put(Feet.of(10), RPM.of(20));
  speeds.put(Feet.of(20), RPM.of(50));

  AngularVelocity speed = speeds.get(Feet.of(7.5));
  speed.in(RPM); // => 15

[auscompgeek]

@SamCarlberg those interpolators look very similar to #8178