| James Kuszmaul | 6035c32 | 2024-01-31 22:27:53 -0800 | [diff] [blame] | 1 | #ifndef FRC971_MATH_INTERPOLATE_H_ |
| 2 | #define FRC971_MATH_INTERPOLATE_H_ |
| 3 | namespace frc971::math { |
| 4 | |
| 5 | // Takes a and b linear interpolates between the two based on the scalar t. |
| 6 | // If t == 0, returns a; if t == 1.0, returns b. |
| 7 | // The semantics of this should be identical to std::lerp(). |
| 8 | template <typename T, typename Scalar> |
| 9 | T lerp(const T &a, const T &b, Scalar t) { |
| 10 | return (static_cast<Scalar>(1.0) - t) * a + t * b; |
| 11 | } |
| 12 | |
| 13 | // For two points (x1, y1) and (x2, y2) uses a linear interpolation |
| 14 | // to identify the value of y at x. |
| 15 | // Will linearly extrapolate if x is outside of [x1, x2]. |
| 16 | template <typename T, typename Scalar> |
| 17 | T Interpolate(Scalar x1, Scalar x2, const T &y1, const T &y2, Scalar x) { |
| 18 | return lerp(y1, y2, (x - x1) / (x2 - x1)); |
| 19 | } |
| 20 | } // namespace frc971::math |
| 21 | #endif // FRC971_MATH_INTERPOLATE_H_ |