163 lines
4.3 KiB
C++
163 lines
4.3 KiB
C++
/**** Includes ****/
|
|
#include "utils.h"
|
|
#include "interpolate.h"
|
|
|
|
using namespace util;
|
|
|
|
/**** Private definitions ****/
|
|
/**** Private constants ****/
|
|
/**** Private variables ****/
|
|
/**** Private function declarations ****/
|
|
/**** Public function definitions ****/
|
|
uint16_t util::interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis)
|
|
{
|
|
// validate axis length
|
|
if(len_axis==0) return 0; // Empty data set
|
|
if(len_axis==1) return y_values[0]; // Only one data point
|
|
|
|
uint16_t y;
|
|
|
|
uint8_t i = find_interval_end_index(x, x_axis, len_axis);
|
|
if(i==0)
|
|
{
|
|
//Less then start
|
|
y = y_values[0];
|
|
}
|
|
else if(i==len_axis)
|
|
{
|
|
//More than end
|
|
y = y_values[len_axis-1];
|
|
}
|
|
else
|
|
{
|
|
// Do interpolate
|
|
y = interpolate(x, x_axis[i-1], x_axis[i], y_values[i-1], y_values[i]);
|
|
}
|
|
|
|
return y;
|
|
}
|
|
|
|
uint16_t util::interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values)
|
|
{
|
|
// validate axis length
|
|
if((len_x_axis==0)&&(len_y_axis==0)) return 0; // Empty data set
|
|
if((len_x_axis==1)&&(len_y_axis==1)) return z_values[0]; // Only one data point
|
|
|
|
uint8_t ix = find_interval_end_index(x, x_axis, len_x_axis);
|
|
uint8_t iy = find_interval_end_index(y, y_axis, len_y_axis);
|
|
|
|
// Check corners - easy answers
|
|
if((ix==0)&&(iy==0))
|
|
{
|
|
return z_values[0]; //[0][0] [Y][X]
|
|
}
|
|
else if((ix==len_x_axis)&&(iy==0))
|
|
{
|
|
return z_values[len_x_axis-1]; //[0][end]
|
|
}
|
|
else if((ix==0)&&(iy==len_y_axis))
|
|
{
|
|
uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
|
|
return z_values[i]; //[end][0]
|
|
}
|
|
else if((ix==len_x_axis)&&(iy==len_y_axis))
|
|
{
|
|
uint16_t i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
|
|
return z_values[i]; //[end][end]
|
|
};
|
|
|
|
// Check boundaries - 1D interpolation
|
|
if(ix==0)
|
|
{
|
|
// On ix=0 line
|
|
uint16_t i = 0;
|
|
uint16_t z0 = z_values[i];
|
|
i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
|
|
uint16_t z1 = z_values[i];
|
|
return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
|
|
}
|
|
else if(ix==len_x_axis)
|
|
{
|
|
// On ix=END line
|
|
uint16_t i = len_x_axis-1;
|
|
uint16_t z0 = z_values[i];
|
|
i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
|
|
uint16_t z1 = z_values[i];
|
|
return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
|
|
}
|
|
else if(iy==0)
|
|
{
|
|
// On iy=0 line
|
|
uint16_t i = 0;
|
|
uint16_t z0 = z_values[i];
|
|
i = len_x_axis-1;
|
|
uint16_t z1 = z_values[i];
|
|
return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
|
|
}
|
|
else if(iy==len_y_axis)
|
|
{
|
|
// On iy=END line
|
|
uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
|
|
uint16_t z0 = z_values[i];
|
|
i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
|
|
uint16_t z1 = z_values[i];
|
|
return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
|
|
}
|
|
|
|
// Do interpolation
|
|
// Get axis values
|
|
uint16_t x0 = x_axis[ix-1];
|
|
uint16_t x1 = x_axis[ix];
|
|
uint16_t y0 = y_axis[iy-1];
|
|
uint16_t y1 = y_axis[iy];
|
|
|
|
// Do y0 line calculation
|
|
// Get z values at x0 and x1 points on y0 line
|
|
uint16_t i = index2d_to_index1d(ix-1, iy-1, len_x_axis);
|
|
uint16_t z0 = z_values[i];
|
|
uint16_t z1 = z_values[i+1];
|
|
// Interpolate z value on y0 line
|
|
uint16_t zy0 = interpolate(x, x0, x1, z0, z1);
|
|
|
|
// Do y1 line calculation
|
|
// Get z values at x0 and x1 points on y1 line
|
|
i = index2d_to_index1d(ix-1, iy, len_x_axis);
|
|
z0 = z_values[i];
|
|
z1 = z_values[i+1];
|
|
// Interpolate z value on y0 line
|
|
uint16_t zy1 = interpolate(x, x0, x1, z0, z1);
|
|
|
|
// Do calculation in y axis on xz line
|
|
return interpolate(y, y0, y1, zy0, zy1);
|
|
}
|
|
|
|
uint16_t util::interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
|
|
{
|
|
int32_t dy = (int32_t)y1 - (int32_t)y0;
|
|
int32_t dx = (int32_t)x1 - (int32_t)x0;
|
|
int32_t d = (int32_t)x - (int32_t)x0;
|
|
|
|
int32_t y = dy * d;
|
|
y /= dx;
|
|
y += y0;
|
|
|
|
return util::sat_cast(y);
|
|
}
|
|
|
|
uint8_t util::find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
|
|
{
|
|
for(uint8_t i=0; i<len_axis; i++)
|
|
{
|
|
if(val < axis_values[i]) return i;
|
|
continue;
|
|
}
|
|
|
|
return len_axis;
|
|
}
|
|
|
|
uint16_t util::index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
|
|
{
|
|
return ((uint16_t)len_x * iy) + ix;
|
|
}
|
|
|
|
/**** Private function definitions ****/ |