Templatize interpolation table to make it more geneirc.
Change-Id: I7da0698486775b3e2482fa99fa172d6e1b6028ac
diff --git a/frc971/shooter_interpolation/interpolation.h b/frc971/shooter_interpolation/interpolation.h
index 933c95e..dd0e316 100644
--- a/frc971/shooter_interpolation/interpolation.h
+++ b/frc971/shooter_interpolation/interpolation.h
@@ -1,33 +1,85 @@
#ifndef FRC971_SHOOTER_INTERPOLATION_INTERPOLATION_H_
#define FRC971_SHOOTER_INTERPOLATION_INTERPOLATION_H_
+#include <algorithm>
#include <utility>
#include <vector>
namespace frc971 {
namespace shooter_interpolation {
-// Struct for shot angle and power
-struct ShotParams {
- double angle;
- double power;
-};
+double Blend(double coefficient, double a1, double a2);
+template <typename YValue>
class InterpolationTable {
public:
+ using Point = ::std::pair<double, YValue>;
InterpolationTable() = default;
- InterpolationTable(
- const ::std::vector<::std::pair<double, ShotParams>> &table);
+ InterpolationTable(const ::std::vector<Point> &table);
// Uses the interpolation table to calculate the optimal shooter angle and
// power for a shot
- ShotParams GetShooterData(double distance) const;
+ YValue Get(double x) const;
+
+ bool GetInRange(double x, YValue* type) const;
private:
// Contains the list of angle entries in the interpolation table
- ::std::vector<::std::pair<double, ShotParams>> table_;
+ ::std::vector<Point> table_;
};
+template <typename YValue>
+InterpolationTable<YValue>::InterpolationTable(const ::std::vector<Point> &table)
+ : table_(table) {
+ ::std::sort(table_.begin(), table_.end(),
+ [](const Point &a, const Point &b) {
+ return a.first < b.first;
+ });
+ }
+
+template <typename YValue>
+YValue InterpolationTable<YValue>::Get(double x) const {
+ // Points to to the smallest item such that it->first >= dist, or end() if no
+ // such item exists.
+ auto it = ::std::lower_bound(table_.begin(), table_.end(), x,
+ [](const Point &a,
+ double dist) { return a.first < dist; });
+ if (it == table_.begin()) {
+ return it->second;
+ } else if (it == table_.end()) {
+ return table_.back().second;
+ } else {
+ auto x_a2 = it;
+ auto x_a1 = it - 1;
+ double x1 = x_a1->first;
+ double x2 = x_a2->first;
+ double coefficient = (x - x1) / (x2 - x1);
+ return YValue::BlendY(coefficient, x_a1->second, x_a2->second);
+ }
+}
+
+template <typename YValue>
+bool InterpolationTable<YValue>::GetInRange(double x, YValue* result) const {
+ // Points to to the smallest item such that it->first >= dist, or end() if no
+ // such item exists.
+ auto it = ::std::lower_bound(table_.begin(), table_.end(), x,
+ [](const Point &a,
+ double dist) { return a.first < dist; });
+ if (it == table_.begin()) {
+ return false;
+ } else if (it == table_.end()) {
+ return false;
+ } else {
+ auto x_a2 = it;
+ auto x_a1 = it - 1;
+ double x1 = x_a1->first;
+ double x2 = x_a2->first;
+ double coefficient = (x - x1) / (x2 - x1);
+ *result = YValue::BlendY(coefficient, x_a1->second, x_a2->second);
+ return true;
+ }
+}
+
} // namespace shooter_interpolation
} // namespace frc971