90 lines
2.6 KiB
C++
90 lines
2.6 KiB
C++
#ifndef INTERPOLATOR_H
|
|
#define INTERPOLATOR_H
|
|
|
|
#include "../Assertions.h"
|
|
#include "../Exception.h"
|
|
#include "../data/Timestamp.h"
|
|
|
|
#include <vector>
|
|
#include <algorithm>
|
|
|
|
/**
|
|
* interpolate between two adjacent values based on their key
|
|
*/
|
|
template <typename Key, typename Value> class Interpolator {
|
|
|
|
public:
|
|
|
|
/** value at key */
|
|
struct InterpolatorEntry {
|
|
Key key;
|
|
Value value;
|
|
InterpolatorEntry(const Key key, const Value& value) : key(key), value(value) {;}
|
|
};
|
|
|
|
protected:
|
|
|
|
/** all added entries, SORTED by their key */
|
|
std::vector<InterpolatorEntry> entries;
|
|
|
|
public:
|
|
|
|
/** add a new value with its key. entries must be added in sorted order! */
|
|
void add(const Key key, const Value& value) {
|
|
Assert::isTrue (entries.empty() || entries.back().key < key, "entries must be ordered!");
|
|
entries.push_back(InterpolatorEntry(key, value));
|
|
}
|
|
|
|
/** get the smallest added key */
|
|
Key getMinKey() const {return entries.front().key;}
|
|
|
|
/** get the largest added key */
|
|
Key getMaxKey() const {return entries.back().key;}
|
|
|
|
/** get the interpolated value for the given key */
|
|
Value get(const Key key) const {
|
|
|
|
const int idx2 = getIdxAfter(key);
|
|
if (idx2 == 0) {return entries.front().value;} // key < everything within the list
|
|
if (idx2 == -1) {return entries.back().value;} // key > everything within the list
|
|
|
|
// interpolate
|
|
const int idx1 = (idx2 > 0) ? (idx2 - 1) : (idx2);
|
|
const float percent = getPercent(key, entries[idx1].key, entries[idx2].key); //(key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key);
|
|
const Value res = entries[idx1].value + (entries[idx2].value - entries[idx1].value) * percent;
|
|
return res;
|
|
|
|
}
|
|
|
|
protected:
|
|
|
|
/** special interpolation for the timestamp class */
|
|
static inline float getPercent(const Timestamp key, const Timestamp t1, const Timestamp t2) {
|
|
return (key - t1).ms() / (float) (t2 - t1).ms();
|
|
}
|
|
|
|
/** interpolation for generic datatypes [int, float, double, ..] */
|
|
template <typename T> static inline float getPercent(const T key, const T t1, const T t2) {
|
|
return (key - t1) / (float) (t2 - t1);
|
|
}
|
|
|
|
/** get the nearest index for the given key */
|
|
int getIdxAfter(const Key key) const {
|
|
|
|
// compare key against one entry
|
|
static auto comp = [] (const Key& key, const InterpolatorEntry& ie1) {return key < ie1.key;};
|
|
|
|
// find the next entry > than key
|
|
const auto it = std::upper_bound(entries.begin(), entries.end(), key, comp);
|
|
|
|
// none found? (key > everything within the list)
|
|
return (it == entries.end()) ? (-1) : (it-entries.begin());
|
|
//if (it == entries.end()) {throw Exception("key not found: " + std::to_string(key));}
|
|
//return it - entries.begin();
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // INTERPOLATOR_H
|