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              :  * @defgroup math_interpolation Math Interpolation Functions
      24              :  * @ingroup utilities
      25              :  * @brief Linear interpolation utilities for mathematical operations
      26              :  * @{
      27              :  */
      28              : 
      29              : /**
      30              :  * @brief Perform a linear interpolation across an arbitrary curve
      31              :  *
      32              :  * @note Result rounding occurs away from 0, e.g:
      33              :  *       1.5 -> 2, -5.5 -> -6
      34              :  *
      35              :  * @param x_axis Ascending list of X co-ordinates for @a y_axis data points
      36              :  * @param y_axis Y co-ordinates for each X data point
      37              :  * @param len Length of the @a x_axis and @a y_axis arrays
      38              :  * @param x X co-ordinate to lookup
      39              :  *
      40              :  * @retval y_axis[0] if x < x_axis[0]
      41              :  * @retval y_axis[len - 1] if x > x_axis[len - 1]
      42              :  * @retval int32_t Linear interpolation between the two nearest @a y_axis values.
      43              :  */
      44            1 : static inline int32_t linear_interpolate(const int32_t *x_axis, const int32_t *y_axis, uint8_t len,
      45              :                                          int32_t x)
      46              : {
      47              :         float rise, run, slope;
      48              :         int32_t x_shifted;
      49              :         uint8_t idx_low = 0;
      50              : 
      51              :         /* Handle out of bounds values */
      52              :         if (x <= x_axis[0]) {
      53              :                 return y_axis[0];
      54              :         } else if (x >= x_axis[len - 1]) {
      55              :                 return y_axis[len - 1];
      56              :         }
      57              : 
      58              :         /* Find the lower x axis bucket */
      59              :         while (x >= x_axis[idx_low + 1]) {
      60              :                 idx_low++;
      61              :         }
      62              : 
      63              :         /* Shift input to origin */
      64              :         x_shifted = x - x_axis[idx_low];
      65              :         if (x_shifted == 0) {
      66              :                 return y_axis[idx_low];
      67              :         }
      68              : 
      69              :         /* Local slope */
      70              :         rise = y_axis[idx_low + 1] - y_axis[idx_low];
      71              :         run = x_axis[idx_low + 1] - x_axis[idx_low];
      72              :         slope = rise / run;
      73              : 
      74              :         /* Apply slope, undo origin shift and round */
      75              :         return roundf(y_axis[idx_low] + (slope * x_shifted));
      76              : }
      77              : 
      78              : /**
      79              :  * @}
      80              :  */
      81              : 
      82              : #ifdef __cplusplus
      83              : }
      84              : #endif
      85              : 
      86              : #endif /* ZEPHYR_INCLUDE_ZEPHYR_MATH_INTERPOLATION_H_ */