1 /*
2 * Copyright (c) 2024 Embeint Inc
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #ifndef ZEPHYR_INCLUDE_ZEPHYR_MATH_INTERPOLATION_H_
8 #define ZEPHYR_INCLUDE_ZEPHYR_MATH_INTERPOLATION_H_
9
10 #include <stdint.h>
11 #include <math.h>
12
13 #ifdef __cplusplus
14 extern "C" {
15 #endif
16
17 /**
18 * @file
19 * @brief Provide linear interpolation functions
20 */
21
22 /**
23 * @brief Perform a linear interpolation across an arbitrary curve
24 *
25 * @note Result rounding occurs away from 0, e.g:
26 * 1.5 -> 2, -5.5 -> -6
27 *
28 * @param x_axis Ascending list of X co-ordinates for @a y_axis data points
29 * @param y_axis Y co-ordinates for each X data point
30 * @param len Length of the @a x_axis and @a y_axis arrays
31 * @param x X co-ordinate to lookup
32 *
33 * @retval y_axis[0] if x < x_axis[0]
34 * @retval y_axis[len - 1] if x > x_axis[len - 1]
35 * @retval int32_t Linear interpolation between the two nearest @a y_axis values.
36 */
linear_interpolate(const int32_t * x_axis,const int32_t * y_axis,uint8_t len,int32_t x)37 static inline int32_t linear_interpolate(const int32_t *x_axis, const int32_t *y_axis, uint8_t len,
38 int32_t x)
39 {
40 float rise, run, slope;
41 int32_t x_shifted;
42 uint8_t idx_low = 0;
43
44 /* Handle out of bounds values */
45 if (x <= x_axis[0]) {
46 return y_axis[0];
47 } else if (x >= x_axis[len - 1]) {
48 return y_axis[len - 1];
49 }
50
51 /* Find the lower x axis bucket */
52 while (x >= x_axis[idx_low + 1]) {
53 idx_low++;
54 }
55
56 /* Shift input to origin */
57 x_shifted = x - x_axis[idx_low];
58 if (x_shifted == 0) {
59 return y_axis[idx_low];
60 }
61
62 /* Local slope */
63 rise = y_axis[idx_low + 1] - y_axis[idx_low];
64 run = x_axis[idx_low + 1] - x_axis[idx_low];
65 slope = rise / run;
66
67 /* Apply slope, undo origin shift and round */
68 return roundf(y_axis[idx_low] + (slope * x_shifted));
69 }
70
71 #ifdef __cplusplus
72 }
73 #endif
74
75 #endif /* ZEPHYR_INCLUDE_ZEPHYR_MATH_INTERPOLATION_H_ */
76
