1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
|
#pragma once
#include "rawaccel-base.hpp"
#include "utility.hpp"
#include <math.h>
namespace rawaccel {
// 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;
}
};
struct si_pair {
float slope = 0;
float intercept = 0;
};
struct arbitrary_lut_point {
float applicable_speed = 0;
si_pair slope_intercept = {};
};
struct lookup {
enum { capacity = LUT_POINTS_CAPACITY };
fp_rep_range range;
bool velocity_points;
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;
double operator()(double speed, const accel_args&) 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 float* raw_data, int raw_length)
{
auto* points = reinterpret_cast<const vec2<float>*>(raw_data);
int length = raw_length / 2;
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;
}
}
lookup(const accel_args& args)
{
velocity_points = args.gain;
fill(args.data, args.length);
}
};
}
|