Line data    Source code

       1           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             :  */
      37           1 : 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_ */