2023-Robot
Robot code for 2023 FRC season by Argos, FRC team #1756
Loading...
Searching...
No Matches
interpolation.h
Go to the documentation of this file.
1
4
5#pragma once
6
7#include <algorithm>
8#include <array>
9
10namespace argos_lib {
11
17 template <class T, class V>
21
22 constexpr InterpMapPoint(T in, V out) : inVal(in), outVal(out) {}
23
24 constexpr bool operator<(const InterpMapPoint<T, V>& other) { return inVal < other.inVal; }
25 constexpr bool operator==(const InterpMapPoint<T, V>& other) { return inVal == other.inVal; }
26 };
27
28 template <class T, class V>
29 constexpr bool operator<(const InterpMapPoint<T, V>& a, const T& b) {
30 return a.inVal < b;
31 }
32
33 template <class T, class V>
34 constexpr bool operator<(const T& a, const InterpMapPoint<T, V>& b) {
35 return a < b.inVal;
36 }
37
45 template <class T, int size, class V = T>
47 public:
48 InterpolationMap() = delete;
54 constexpr InterpolationMap(std::array<InterpMapPoint<T, V>, size> initArray) : m_mapArray(initArray) {
55 // assert(("Map must contain at least one value.", !initArray.empty()));
56 // assert(("Map values must be sorted.", std::is_sorted(initArray.cbegin(), initArray.cend())));
57 }
58
65 constexpr V Map(const T inVal) const {
66 if (inVal >= m_mapArray.back().inVal) {
67 return m_mapArray.back().outVal;
68 } else if (inVal <= m_mapArray.front().inVal) {
69 return m_mapArray.front().outVal;
70 } else {
71 auto afterPoint{std::lower_bound(m_mapArray.cbegin(), m_mapArray.cend(), inVal)};
72 auto beforePoint{std::prev(afterPoint)};
73 const auto lerpPct = (inVal - beforePoint->inVal) / (afterPoint->inVal - beforePoint->inVal);
74 return beforePoint->outVal + lerpPct * (afterPoint->outVal - beforePoint->outVal);
75 }
76 }
77
83 constexpr V operator()(const T inVal) const { return Map(inVal); }
84
85 private:
86 std::array<InterpMapPoint<T, V>, size> m_mapArray;
87 };
88
89} // namespace argos_lib
Performs linear interpolation of a value based on a set of input->output mapping points.
Definition interpolation.h:46
constexpr V Map(const T inVal) const
Generate interpolated output based on input.
Definition interpolation.h:65
constexpr V operator()(const T inVal) const
Generate interpolated output based on input.
Definition interpolation.h:83
std::array< InterpMapPoint< T, V >, size > m_mapArray
Definition interpolation.h:86
constexpr InterpolationMap(std::array< InterpMapPoint< T, V >, size > initArray)
Constructs new interpolation map.
Definition interpolation.h:54
Definition swap_controllers_command.h:12
constexpr bool operator<(const InterpMapPoint< T, V > &a, const T &b)
Definition interpolation.h:29
Point that helps generate an interpolation map.
Definition interpolation.h:18
constexpr InterpMapPoint(T in, V out)
Definition interpolation.h:22
constexpr bool operator<(const InterpMapPoint< T, V > &other)
Definition interpolation.h:24
T inVal
Definition interpolation.h:19
V outVal
Definition interpolation.h:20
constexpr bool operator==(const InterpMapPoint< T, V > &other)
Definition interpolation.h:25