diff options
Diffstat (limited to 'common/accel-lookup.hpp')
| -rw-r--r-- | common/accel-lookup.hpp | 306 |
1 files changed, 306 insertions, 0 deletions
diff --git a/common/accel-lookup.hpp b/common/accel-lookup.hpp new file mode 100644 index 0000000..99f39e9 --- /dev/null +++ b/common/accel-lookup.hpp @@ -0,0 +1,306 @@ +#pragma once + +#include "rawaccel-base.hpp" +#include "utility.hpp" + +#include <math.h> + +namespace rawaccel { + + struct linear_range { + double start; + double stop; + int num; + + template <typename Func> + void for_each(Func fn) const + { + double interval = (stop - start) / (num - 1); + for (int i = 0; i < num; i++) { + fn(i * interval + start); + } + } + + int size() const + { + return num; + } + }; + + + // represents the range [2^start, 2^stop], with num - 1 + // elements linearly spaced between each exponential step + struct fp_rep_range { + int start; + int stop; + int num; + + template <typename Func> + void for_each(Func fn) const + { + for (int e = 0; e < stop - start; e++) { + double exp_scale = scalbn(1, e + start) / num; + + for (int i = 0; i < num; i++) { + fn((i + num) * exp_scale); + } + } + + fn(scalbn(1, stop)); + } + + int size() const + { + return (stop - start) * num + 1; + } + }; + + template <typename Lookup> + struct lut_base { + enum { capacity = SPACED_LUT_CAPACITY }; + using value_t = float; + + template <typename Func> + void fill(Func fn) + { + auto* self = static_cast<Lookup*>(this); + + self->range.for_each([&, fn, i = 0](double x) mutable { + self->data[i++] = static_cast<value_t>(fn(x)); + }); + } + + }; + + struct linear_lut : lut_base<linear_lut> { + linear_range range; + bool transfer = false; + value_t data[capacity] = {}; + + double operator()(double x) const + { + if (x > range.start) { + double range_dist = range.stop - range.start; + double idx_f = (x - range.start) * (range.num - 1) / range_dist; + + unsigned idx = min(static_cast<int>(idx_f), range.size() - 2); + + if (idx < capacity - 1) { + double y = lerp(data[idx], data[idx + 1], idx_f - idx); + if (transfer) y /= x; + return y; + } + } + + double y = data[0]; + if (transfer) y /= range.start; + return y; + } + + linear_lut(const spaced_lut_args& args) : + range({ + args.start, + args.stop, + args.num_elements + }), + transfer(args.transfer) {} + + linear_lut(const accel_args& args) : + linear_lut(args.spaced_args) {} + }; + + struct binlog_lut : lut_base<binlog_lut> { + fp_rep_range range; + double x_start; + bool transfer = false; + value_t data[capacity] = {}; + + double operator()(double x) const + { + int e = min(ilogb(x), range.stop - 1); + + if (e >= range.start) { + int idx_int_log_part = e - range.start; + double idx_frac_lin_part = scalbn(x, -e) - 1; + double idx_f = range.num * (idx_int_log_part + idx_frac_lin_part); + + unsigned idx = min(static_cast<int>(idx_f), range.size() - 2); + + if (idx < capacity - 1) { + double y = lerp(data[idx], data[idx + 1], idx_f - idx); + if (transfer) y /= x; + return y; + } + } + + double y = data[0]; + if (transfer) y /= x_start; + return y; + } + + binlog_lut(const spaced_lut_args& args) : + range({ + static_cast<int>(args.start), + static_cast<int>(args.stop), + args.num_elements + }), + x_start(scalbn(1, range.start)), + transfer(args.transfer) {} + + binlog_lut(const accel_args& args) : + binlog_lut(args.spaced_args) {} + }; + + struct si_pair { + float slope = 0; + float intercept = 0; + }; + + struct arbitrary_lut_point { + float applicable_speed = 0; + si_pair slope_intercept = {}; + }; + + struct arbitrary_lut { + enum { capacity = ARB_LUT_CAPACITY }; + + fp_rep_range range; + arbitrary_lut_point data[capacity] = {}; + int log_lookup[capacity] = {}; + double first_point_speed; + double last_point_speed; + int last_arbitrary_index; + int last_log_lookup_index; + double last_log_lookup_speed; + double first_log_lookup_speed; + bool velocity_points; + + double operator()(double speed) const + { + int index = 0; + int last_arb_index = last_arbitrary_index; + int last_log_index = last_log_lookup_index; + + if (speed <= 0) return 1; + + if (unsigned(last_arb_index) < capacity && + unsigned(last_log_index) < capacity && + speed > first_point_speed) + { + if (speed > last_point_speed) + { + index = last_arb_index; + } + else if (speed > last_log_lookup_speed) + { + int last_log = log_lookup[last_log_index]; + if (unsigned(last_log) >= capacity) return 1; + index = search_from(last_log, last_arb_index, speed); + } + else if (speed < first_log_lookup_speed) + { + index = search_from(0, last_arb_index, speed); + } + else + { + int log_index = get_log_index(speed); + if (unsigned(log_index) >= capacity) return 1; + int arbitrary_index = log_lookup[log_index]; + if (arbitrary_index < 0) return 1; + index = search_from(arbitrary_index, last_arb_index, speed); + } + + } + + return apply(index, speed); + } + + int inline get_log_index(double speed) const + { + double speed_log = log(speed) - range.start; + int index = (int)floor(speed_log * range.num); + return index; + } + + int inline search_from(int index, int last, double speed) const + { + do + { + index++; + } + while (index <= last && data[index].applicable_speed < speed); + + return index - 1; + } + + double inline apply(int index, double speed) const + { + auto [slope, intercept] = data[index].slope_intercept; + + if (velocity_points) + { + return slope + intercept / speed; + } + else + { + return slope * speed + intercept; + } + } + + void fill(const vec2<float>* points, int length) + { + first_point_speed = points[0].x; + last_arbitrary_index = length - 1; + // -2 because the last index in the arbitrary array is used for slope-intercept only + last_point_speed = points[length-2].x; + + int start = static_cast<int>(floor(log(first_point_speed))); + first_log_lookup_speed = exp(start*1.0); + int end = static_cast<int>(floor(log(last_point_speed))); + last_log_lookup_speed = exp(end*1.0); + int num = end > start ? static_cast<int>(capacity / (end - start)) : 1; + range = fp_rep_range{ start, end, num }; + last_log_lookup_index = end > start ? num * (end - start) - 1 : 0; + + vec2<float> current = {0, velocity_points ? 0.0f : 1.0f }; + vec2<float> next; + int log_index = 0; + double log_inner_iterator = range.start; + double log_inner_slice = 1.0 / (range.num * 1.0); + double log_value = exp(log_inner_iterator); + + for (int i = 0; i < length; i++) + { + next = points[i]; + double slope = (next.y - current.y) / (next.x - current.x); + double intercept = next.y - slope * next.x; + si_pair current_si = { + static_cast<float>(slope), + static_cast<float>(intercept) + }; + arbitrary_lut_point current_lut_point = { + static_cast<float>(current.x), + current_si + }; + + this->data[i] = current_lut_point; + + while (log_value < next.x && log_inner_iterator < end) + { + this->log_lookup[log_index] = i; + log_index++; + log_inner_iterator += log_inner_slice; + log_value = exp(log_inner_iterator); + } + + current = next; + } + } + + arbitrary_lut(const accel_args& args) + { + velocity_points = args.arb_args.velocity; + fill(args.arb_args.data, args.arb_args.length); + } + }; +} |