1 /*
2 * Copyright (c) 2019 Kevin Townsend (KTOWN)
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <math.h>
8 #include <errno.h>
9 #include <zephyr/kernel.h>
10 #include <zsl/zsl.h>
11 #include <zsl/interp.h>
12
13 int
zsl_interp_lerp(zsl_real_t v0,zsl_real_t v1,zsl_real_t t,zsl_real_t * v)14 zsl_interp_lerp(zsl_real_t v0, zsl_real_t v1, zsl_real_t t, zsl_real_t *v)
15 {
16 int rc;
17
18 /* Ensure t = 0.0..1.0 */
19 if ((t < 0.0f) || (t > 1.0f)) {
20 rc = -EINVAL;
21 *v = NAN;
22 goto err;
23 }
24
25 *v = (1.0f - t) * v0 + t * v1;
26
27 return 0;
28 err:
29 return rc;
30 }
31
32 int
zsl_interp_find_x(struct zsl_interp_xy xy[],size_t n,zsl_real_t x,int * idx)33 zsl_interp_find_x(struct zsl_interp_xy xy[], size_t n, zsl_real_t x, int *idx)
34 {
35 int rc;
36 unsigned int idx_upper; /* Upper limit */
37 unsigned int idx_mid; /* Midpoint */
38 unsigned int idx_lower; /* Lower limit */
39 int order; /* Ascending (1) or descending (0) */
40
41 /* Init lower and upper limits. */
42 idx_lower = 0;
43 idx_upper = n;
44
45 /* Make sure we have an appropriately large dataset. */
46 if (n < 2) {
47 *idx = -1;
48 rc = -EINVAL;
49 goto err;
50 }
51
52 /* Determine order (1 = ascending, 0 = descending). */
53 order = (xy[n - 1].x >= xy[0].x);
54
55 /* xy[0] and xy[n-1] bounds checks. */
56 if ((x > xy[n - 1].x && order) || (x < xy[n - 1].x && !order)) {
57 /* Out of bounds on the high end. */
58 *idx = n;
59 rc = -EINVAL;
60 goto err;
61 } else if ((x < xy[0].x && order) || (x > xy[0].x && !order)) {
62 /* Out of bounds on the low end. */
63 *idx = -1;
64 rc = -EINVAL;
65 goto err;
66 }
67 ;
68
69 /* Repetitive mid-point computation until a match is made. */
70 while (idx_upper - idx_lower > 1) {
71 idx_mid = (idx_upper + idx_lower) >> 1;
72 if ((x >= xy[idx_mid].x && order) ||
73 (x <= xy[idx_mid].x && !order)) {
74 /* Set lower limit to current mid-point. */
75 idx_lower = idx_mid;
76 } else {
77 /* Set upper limit to current mid-point. */
78 idx_upper = idx_mid;
79 }
80 }
81
82 /* Set the output index value. */
83 if (x == xy[0].x) {
84 /* Return absolute lower limit. */
85 *idx = 0;
86 } else if (x == xy[n - 1].x) {
87 /* Return absolute upper limit. */
88 *idx = n - 2;
89 } else {
90 /* Return a value in between. */
91 *idx = idx_lower;
92 }
93
94 return 0;
95 err:
96 return rc;
97 }
98
99 int
zsl_interp_nn(struct zsl_interp_xy * xy1,struct zsl_interp_xy * xy3,zsl_real_t x2,zsl_real_t * y2)100 zsl_interp_nn(struct zsl_interp_xy *xy1, struct zsl_interp_xy *xy3,
101 zsl_real_t x2, zsl_real_t *y2)
102 {
103 int rc;
104 zsl_real_t delta;
105
106 /* Make sure there is a delta x between xy1 and xy3. */
107 delta = xy3->x - xy1->x;
108 if (delta < 1E-6F && -delta < 1E-6F) {
109 rc = -EINVAL;
110 *y2 = NAN;
111 goto err;
112 }
113
114 /* Ensure that x1 <= x2 <= x3. */
115 if ((x2 < xy1->x) || (x2 > xy3->x)) {
116 rc = -EINVAL;
117 goto err;
118 }
119
120 /* Determine which value is closest, rounding up on 0.5. */
121 *y2 = x2 >= (xy1->x * 1.5f) ? xy3->y : xy1->y;
122
123 return 0;
124 err:
125 return rc;
126 }
127
128 int
zsl_interp_nn_arr(struct zsl_interp_xy xy[],size_t n,zsl_real_t x,zsl_real_t * y)129 zsl_interp_nn_arr(struct zsl_interp_xy xy[], size_t n,
130 zsl_real_t x, zsl_real_t *y)
131 {
132 int rc;
133 int idx;
134
135 /* Find the starting position in xy[] for x. */
136 rc = zsl_interp_find_x(xy, n, x, &idx);
137 if (rc) {
138 *y = NAN;
139 goto err;
140 }
141
142 /* Perform nearest neighbour interp. between xy[idx] and xy[idx+1]. */
143 rc = zsl_interp_nn(&xy[idx], &xy[idx + 1], x, y);
144 if (rc) {
145 *y = NAN;
146 goto err;
147 }
148
149 return 0;
150 err:
151 return rc;
152 }
153
154 int
zsl_interp_lin_y(struct zsl_interp_xy * xy1,struct zsl_interp_xy * xy3,zsl_real_t x2,zsl_real_t * y2)155 zsl_interp_lin_y(struct zsl_interp_xy *xy1, struct zsl_interp_xy *xy3,
156 zsl_real_t x2, zsl_real_t *y2)
157 {
158 int rc;
159 zsl_real_t delta;
160
161 /* Make sure there is a delta on x between xy1 and xy3. */
162 delta = xy3->x - xy1->x;
163 if (delta < 1E-6F && -delta < 1E-6F) {
164 rc = -EINVAL;
165 *y2 = NAN;
166 goto err;
167 }
168
169 /* Ensure that x2 >= x1 && x2 <= x3. */
170 if ((x2 < xy1->x) || (x2 > xy3->x)) {
171 rc = -EINVAL;
172 *y2 = NAN;
173 goto err;
174 }
175
176 /*
177 * (x2 - x1)(y3 - y1)
178 * y2 = ------------------- + y1
179 * (x3 - x1)
180 */
181
182 *y2 = ((x2 - xy1->x) * (xy3->y - xy1->y)) / (xy3->x - xy1->x) + xy1->y;
183
184 return 0;
185 err:
186 return rc;
187 }
188
189 int
zsl_interp_lin_y_arr(struct zsl_interp_xy xy[],size_t n,zsl_real_t x,zsl_real_t * y)190 zsl_interp_lin_y_arr(struct zsl_interp_xy xy[], size_t n,
191 zsl_real_t x, zsl_real_t *y)
192 {
193 int rc;
194 int idx;
195
196 /* Find the starting position in xy[] for x. */
197 rc = zsl_interp_find_x(xy, n, x, &idx);
198 if (rc) {
199 *y = NAN;
200 goto err;
201 }
202
203 /* Perform linear interpolation of x between xy[idx] and xy[idx+1]. */
204 rc = zsl_interp_lin_y(&xy[idx], &xy[idx + 1], x, y);
205 if (rc) {
206 *y = NAN;
207 goto err;
208 }
209
210 return 0;
211 err:
212 return rc;
213 }
214
215 int
zsl_interp_lin_x(struct zsl_interp_xy * xy1,struct zsl_interp_xy * xy3,zsl_real_t y2,zsl_real_t * x2)216 zsl_interp_lin_x(struct zsl_interp_xy *xy1, struct zsl_interp_xy *xy3,
217 zsl_real_t y2, zsl_real_t *x2)
218 {
219 int rc;
220 zsl_real_t min, max, delta;
221
222 /* Make sure there is a delta on x between xy1 and xy3. */
223 delta = xy3->x - xy1->x;
224 if (delta < 1E-6F && -delta < 1E-6F) {
225 rc = -EINVAL;
226 *x2 = NAN;
227 goto err;
228 }
229
230 /* Ensure that y2 is within the range of min and max. */
231 min = xy1->y < xy3->y ? xy1->y : xy3->y;
232 max = xy1->y < xy3->y ? xy3->y : xy1->y;
233 if ((y2 < min) || (y2 > max)) {
234 rc = -EINVAL;
235 *x2 = NAN;
236 goto err;
237 }
238
239 /*
240 * (y3 - y2) * x1 + (y2 - y1) * x3
241 * x2 = -------------------------------
242 * y3 - y1
243 */
244
245 *x2 = ((xy3->y - y2) * xy1->x + (y2 - xy1->y) *
246 xy3->x) / (xy3->y - xy1->y);
247
248 return 0;
249 err:
250 return rc;
251 }
252
253 int
zsl_interp_cubic_calc(struct zsl_interp_xyc xyc[],size_t n,zsl_real_t yp1,zsl_real_t ypn)254 zsl_interp_cubic_calc(struct zsl_interp_xyc xyc[], size_t n, zsl_real_t yp1,
255 zsl_real_t ypn)
256 {
257 int rc;
258 int i;
259 int k;
260 zsl_real_t sigma;
261 zsl_real_t p;
262 zsl_real_t qn;
263 zsl_real_t un;
264 zsl_real_t *u;
265
266 /* Make sure we have at least three values. */
267 if (n < 3) {
268 rc = -EINVAL;
269 goto err;
270 }
271
272 u = (zsl_real_t *)k_malloc((n - 1) * sizeof(zsl_real_t));
273 if (u == NULL) {
274 rc = -ENOMEM;
275 goto err;
276 }
277
278 /* TODO: Remove 'f' restrictions? */
279 if (yp1 > 0.99e30f) {
280 xyc[0].y2 = u[0] = 0.0f;
281 } else {
282 xyc[0].y2 = -0.5f;
283 u[0] = (3.0f / (xyc[1].x - xyc[0].x)) *
284 ((xyc[1].y - xyc[0].y) / (xyc[1].x - xyc[0].x) - yp1);
285 }
286
287 for (i = 1; i < n - 1; i++) {
288 /* Break out common values. */
289 zsl_real_t x_i_im1 = xyc[i].x - xyc[i - 1].x;
290 zsl_real_t x_ip1_im1 = xyc[i + 1].x - xyc[i - 1].x;
291 sigma = x_i_im1 / x_ip1_im1;
292 p = sigma * xyc[i - 1].y2 + 2.0f;
293 xyc[i].y2 = (sigma - 1.0f) / p;
294 u[i] = (xyc[i + 1].y - xyc[i].y) / (xyc[i + 1].x - xyc[i].x) -
295 (xyc[i].y - xyc[i - 1].y) / (x_i_im1);
296 u[i] = (6.0f * u[i] / (x_ip1_im1) - sigma * u[i - 1]) / p;
297 }
298
299 if (ypn > 0.99e30f) {
300 qn = un = 0.0f;
301 } else {
302 qn = 0.5f;
303 un = (3.0f / (xyc[n - 1].x - xyc[n - 2].x)) *
304 (ypn - (xyc[n - 1].y -
305 xyc[n - 2].y) / (xyc[n - 1].x - xyc[n - 2].x));
306 }
307
308 xyc[n - 1].y2 = (un - qn * u[n - 2]) / (qn * xyc[n - 2].y2 + 1.0f);
309
310 for (k = n - 2; k >= 0; k--) {
311 xyc[k].y2 = xyc[k].y2 * xyc[k + 1].y2 + u[k];
312 }
313
314 k_free(u);
315
316 return 0;
317 err:
318 return rc;
319 }
320
321 int
zsl_interp_cubic_arr(struct zsl_interp_xyc xyc[],size_t n,zsl_real_t x,zsl_real_t * y)322 zsl_interp_cubic_arr(struct zsl_interp_xyc xyc[], size_t n,
323 zsl_real_t x, zsl_real_t *y)
324 {
325 int rc;
326 int k; /* Array index value for mid point. */
327 int klo; /* Array index value for low point. */
328 int khi; /* Array index value for high point. */
329 static int pklo; /* Per. low pnt for repeat bisection search. */
330 static int pkhi; /* Per. high pnt for repeat bisection search. */
331 zsl_real_t h; /* xyc[j+1].x - xyc[j].x */
332 zsl_real_t a; /* (xyc[j+1].x - x) / h */
333 zsl_real_t b; /* (x - xyc[j].x) / h */
334
335 pklo = 0;
336 pkhi = 1;
337
338 /* Make sure we have at least three values. */
339 if (n < 3) {
340 rc = -EINVAL;
341 goto err;
342 }
343
344 /* First check if pklo and pkhi from the last run are still a valid
345 * match. The static values are maintained across calls to this
346 * function, allowing us to avoid unnecessarily performing a full array
347 * search. */
348 if (xyc[pklo].x <= x && xyc[pkhi].x > x) {
349 klo = pklo;
350 khi = pkhi;
351 } else {
352 /* Search the full array for x using bisection. */
353 klo = 0;
354 khi = n - 1;
355 while (khi - klo > 1) {
356 /* Set the midpoint based on the current high/low
357 * points.. */
358 k = (khi + klo) >> 1;
359 /* Determine whether we need to search in upper or
360 * lower half. */
361 if (xyc[k].x > x) {
362 /* If the current midpoint is greater than
363 * search value 'x' set the high marker to the
364 * current midpoint, which will cause search to
365 * continue in the lower half. */
366 khi = k;
367 } else {
368 /* Otherwise set the low marker to the current
369 * midpoint, which will cause search to
370 * continue in the upper half. */
371 klo = k;
372 }
373 /* Search until results are reduced to neighbouring xyc
374 * values. */
375 }
376 /* Persist pklo and pkhi for future calls to this function. */
377 pklo = klo;
378 pkhi = khi;
379 }
380
381 h = xyc[khi].x - xyc[klo].x;
382 if (h == 0) {
383 /* No diff = invalid x input! */
384 rc = -EINVAL;
385 goto err;
386 }
387
388 /* Calculate coefficients for hi-x (a) and x-lo (b). */
389 a = (xyc[khi].x - x) / h;
390 b = (x - xyc[klo].x) / h;
391
392 /* Interpolate for y based on a, b using prev. calculated y2 vals. */
393 *y = a * xyc[klo].y + b * xyc[khi].y +
394 ((a * a * a - a) * xyc[klo].y2 + (b * b * b - b) *
395 xyc[khi].y2) * (h * h) / 6.0f;
396
397 return 0;
398 err:
399 return rc;
400 }
401
