简单的二维数组查表插值算法实现

查找表

//角度和长度表
const Rod_Angle_LookTable Angle_LookTable[LOOKTABLE_SIZE] = 
{
	{-5,115.00}, {0, 115.48}, {5, 116.32}, {10,117.52}, {15,119.04}, 
	{20,120.86}, {25,122.96}, {30,125.31}, {35,127.88}, {40,130.63},
	{45,133.54}, {50,136.56}, {55,139.68}, {60,142.87}, {65,146.08},
	{70,149.31}, {75,152.52}, {80,155.70}, {85,158.82}, {90,161.85}, {95,164.79}
}; 

求数据变化比例

static float_t Calc_Slope(float_t x0,float_t x1,float_t y0,float_t y1)
{
	//x表示角度，y表示长度
	return (x1 - x0)/(y1 - y0); //斜率
}

查找插值

//根据长度查角度
static float_t LookTable_ForAngle(float_t rod_len) 
{
	if(rod_len > Angle_LookTable[LOOKTABLE_SIZE - 1].len) //大于MAX
	{
		//根据max 和 max-1往外推 = DATA[MAX - 1] + (DATA[MAX - 1] - DATA[MAX - 2]) * K
		float_t len1 = Angle_LookTable[LOOKTABLE_SIZE - 2].len;
		float_t len2 = Angle_LookTable[LOOKTABLE_SIZE - 1].len;
		float_t angle1 = Angle_LookTable[LOOKTABLE_SIZE - 2].angle;
		float_t angle2 = Angle_LookTable[LOOKTABLE_SIZE - 1].angle;
		float_t interpolated_angle = Angle_LookTable[LOOKTABLE_SIZE - 1].angle + 
									(angle2 - angle1) * Calc_Slope(angle1,angle2,len1,len2);
		return interpolated_angle;
	}
	else if (rod_len < Angle_LookTable[0].len)
	{
		//根据DATA[0] 和 DATA[1]往外推  = DATA[0] - (DATA[1] - DATA[0]) * K
		float_t len1 = Angle_LookTable[0].len;
		float_t len2 = Angle_LookTable[1].len;
		float_t angle1 = Angle_LookTable[0].angle;
		float_t angle2 = Angle_LookTable[1].angle;
		float_t interpolated_angle = Angle_LookTable[0].angle - 
									(angle2 - angle1) * Calc_Slope(angle1,angle2,len1,len2);
		return interpolated_angle;
	}
	else
	{
		for(int i=0;i<LOOKTABLE_SIZE - 1;i++)
		{
			if(rod_len >= Angle_LookTable[i].len && rod_len <= Angle_LookTable[i+1].len)
			{
				float_t len1 = Angle_LookTable[i].len;
				float_t len2 = Angle_LookTable[i+1].len;
				float_t angle1 = Angle_LookTable[i].angle;
				float_t angle2 = Angle_LookTable[i+1].angle;
				
				float_t interpolated_angle = angle1 + (angle2 - angle1) * Calc_Slope(angle1,angle2,len1,len2);
				return interpolated_angle;
			}
		}
	}	
	return 0.0f;
}