BiLinear Interpolation

I put together some code for Bilinear interpolation.
The formula gave me a bit of a hard time, but eventually I got it to work. First I tried the formula from Wiki (link below) but it didn’t work for me. Then I tried to use the map() function, it was close but i want perfect. My issue with the map() function was that it seems to round down a little and i used it like this

int r1;
int r2;
int p;
r1=map(x, x1, x2, q11, q12)
r2=map(x, x1, x2, q21, q22)
p=map(y, y1, y2, r1, r2)

The idea is to sample 4 cell and and give the Quadrant names relative to their X Y values
q12 = Quadrant (1,2)
then you interpolate between them horizontally to get vitural points
r1 and r2
then you interpolate vertically along the y axis

Eventually I just broke the Wiki formula down into three bits like i did with the map function and it proved more accurate. Floating point values help also to prevent truncation (i believe this was my issue)

This is what the map function page says the math behind the map function is

long map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

So here is my current issue that I am having a hard time overcoming. I want to compress my code a but and as always i would like to optimize it as much as possible. Custom functions are a nice neat way to make your code more readable to colleagues. I work with a team of 3 other people. Whats going on is how do i get the array into my custom function from the main loop. I tried a few times but just cant sort it out. I also google it and came to this web page which describes 3 methods. But they dont seem to work for me. Probably because i do not fully understand the description.

http://www.cplusplus.com/forum/articles/20881/

Here is my Code

  /*Biliner Interpolation
   *
   *Code testing to search a table, find 4 cells that should be interpolated 
   *and actually interpolate them.  Intended use is for a Electric boost control
   *system to control the Waste Gate Duty of a solenoid.  
   * 
   * No Circuit
   * 
   * Created 2/16/2017
   * by George Katslas
   * 
   */
  
  
  const byte a = 8;                                                      //The Size of the X axis
  const byte b = 8;                                                      //The size of the Y axis
  
  int xax [a] = {0, 500, 1000, 2000, 3000, 4000, 5000, 7000};           //X axis Data for testing
  int yax [b] = {0, 10, 15, 20, 30, 35, 40, 45};                        //Y axis Data for testing
  int zdat [a] [b] = {                                                  //Z Data for testing
        {70, 71, 72, 73, 74, 75, 76, 77},
        {60, 61, 62, 63, 64, 65, 66, 67},
        {50, 51, 52, 53, 54, 55, 56, 57},
        {40, 41, 42, 43, 44, 45, 46, 47},
        {30, 31, 32, 33, 34, 35, 36, 37},
        {20, 21, 22, 23, 24, 25, 26, 27},
        {10, 11, 12, 13, 14, 15, 16, 17},
        {00, 01, 02, 03, 04, 05, 06, 07}
      };
      
  byte xmin = 0;                                                         //container for the X min
  byte xmax = 0;                                                         //container for the X max
  byte ymin = 0;                                                         //container for the Y min
  byte ymax = 0;                                                         //container for the Y Max

void setup() {
 Serial.begin(250000);                                                  //Open Serial Com 
}

void loop() {

  for (byte i = 0; i < a; i++) {                                         //Prints the X axis
    Serial.print(F("\t"));                                              //Tab
    Serial.print(xax[i]);                                               
  }

  for (byte i = 0; i < b; i++) {                                         //for the length of the y axis
    Serial.println();                                                   //print return
    Serial.print(yax[i]);                                               //print the Y Value
    Serial.print(F("\t"));                                               //Tab
    for(byte j = 0; j < a; j++){                                         //For the legnth of the X axis
      Serial.print(zdat[i] [j]);                                          //Print the data
      Serial.print(F("\t"));                                              //Tab
    }      
  }
  
  Serial.println();                                                     //Print a return
  Serial.println(F("enter X val"));                                     //Ask for X val
  int input = getInput();                                               //Call custom function to read serial
  int xin = input;                                                      //Store the input as X input
  Serial.println(F("enter Y val"));                                     //Ask for Y val
  input = getInput();                                                   //Call custom function to read serial input
  int yin = input;                                                      //Store the input as Y input
  
//axis crawler  I want to make this a custom function as its called twice
  byte axisCell = 0;                                                    //variable to store the Cell Number 0 indexed!!
  while(axisCell < a) {                                                 //wile the cell number is less than size of axis
    if(xin > xax[axisCell]) axisCell++;                                 //if X input is less than the value store in current cell then increment the cell number
    else {                                                              //else  
      axisCell--;
      if(axisCell < 0){                                                 //if the cell number less than 0 
        xmin=0;                                                         //Xmin set 0
        xmax=1;                                                         //Xmax set 1
      }
      else {                                                            //else
        xmin=axisCell;                                                  //Xmin is Cell number 
        xmax=(axisCell+1);                                                //Xmax is Cell number + 1  
      }
      axisCell = 0;                                                     //Set the cell number to its default value
      break;                                                            //break loop min max found
    }
  }
//axis crawler for the Y axis
  while(axisCell < b) {
    if(yin > yax[axisCell]) axisCell++;
    else {
      axisCell--;
      if(axisCell < 0){
        ymin=0;
        ymax=1;
      }
      else {
        ymin=axisCell;
        ymax=(axisCell+1);
      }
      axisCell=0;
      break;
    }
   }
   
  int q11 = zdat [ymin] [xmin];                                         //store the data for each corresoinding quadrant
  int q12 = zdat [ymin] [xmax];
  int q21 = zdat [ymax] [xmin];
  int q22 = zdat [ymax] [xmax];
  Serial.print(F("Bilinear Interpolation "));
  Serial.println(bilinIntrp(xin, xax[xmin], xax[xmax], yin, yax[ymin], yax[ymax], q11, q12, q21, q22));     //ohh baby
}

/* This Doesnt work  :(
int findMax(int val, int axis[]) {
  int axisCell = 0;
  while(axisCell < a) {
    if(val < axis[axisCell]) axisCell++;
    else return axisCell;
  }
  
  
}
*/

int bilinIntrp(float x, float x1, float x2, float y, float y1, float y2, float q11, float q12, float q21, float q22) {

  float r1 = 0;                                             
  float r2 = 0;
  float p = 0;
  r1 = ((x2-x)/(x2-x1)*q11)+((x-x1)/(x2-x1)*q12);    //linear interpolation between the lower two quadrants along the X axis
  r2 = ((x2-x)/(x2-x1)*q21)+((x-x1)/(x2-x1)*q22);    //linear interpolation between the upper two quadrants along the X axis                   
  p = ((y2-y)/(y2-y1)*r1)+((y-y1)/(y2-y1)*r2);       //linear interpolation between the two virtual points r1 and r2 along the Y axis
  return p;                                          //return the Magic number P value of our point

}

int getInput(){
  int result;
  while(Serial.available()==0);                                               //Wait For Serial Data
  while(Serial.available() > 0) {                                             //Data is available
    result = Serial.parseInt();                                               //Parse The Input
  } 
  return result;                                                              //Return The Result
}

Whats going on is how do i get the array into my custom function from the main loop.

Which array and which custom function? You've not shown any of that above...

J-M-L:
Which array and which custom function? You’ve not shown any of that above…

this function

int findMax(int val, int axis[]) {
  int axisCell = 0;
  while(axisCell < a) {
    if(val < axis[axisCell]) axisCell++;
    else return axisCell;
  }
  
  
}

either array
one at a time

  int xax [a] = {0, 500, 1000, 2000, 3000, 4000, 5000, 7000};           //X axis Data for testing
  int yax [b] = {0, 10, 15, 20, 30, 35, 40, 45};                        //Y axis Data for testing

What is a in your function? Is that the size of the array? Should be a parameter
Your function does not return anything if val is lower than all the elements

im aware of the < comparison, its in error should be >

J-M-L:
What is a in your function? Is that the size of the array? Should be a parameter
Your function does not return anything if val is lower than all the elements

Sorry i misread your question, "a" is he size of the array.

If you don't want things rounded down/off then STOP using integer math!. Look up integer math it is not what you learned at school! Even more so when done with any computer!

Mark

I’d do something like this

Quickly hacked from your wikipedia reference so that you can see the maths based on their notation

Would need some optimization and not double checked at all… will likely compile, not sure what it does :slight_smile:

// Those arrays needs to be sorted in ascending order
// Need at least 2 points
double X [] = {0, 500, 1000, 2000, 3000, 4000, 5000, 7000};           //X axis Data for testing
double Y [] = {0, 10, 15, 20, 30, 35, 40, 45};                        //Y axis Data for testing

const int Xcount = sizeof(X) / sizeof(X[0]);
const int Ycount = sizeof(Y) / sizeof(Y[0]);

// points in 3D space (X[x], Y[y], Z[x][y])
double Z[Xcount][Ycount] = {                                        //Z Data for testing
  {70, 71, 72, 73, 74, 75, 76, 77},
  {60, 61, 62, 63, 64, 65, 66, 67},
  {50, 51, 52, 53, 54, 55, 56, 57},
  {40, 41, 42, 43, 44, 45, 46, 47},
  {30, 31, 32, 33, 34, 35, 36, 37},
  {20, 21, 22, 23, 24, 25, 26, 27},
  {10, 11, 12, 13, 14, 15, 16, 17},
  {00, 01, 02, 03, 04, 05, 06, 07}
};


double bilinearXY(int x, int y)
{
  int xIndex, yIndex;

  if ((x < X[0]) || (x > X[Xcount - 1])) {
    Serial.println(F("x not in range"));
    return -1; // arbitrary...
  }

  if ((y < Y[0]) || (y > Y[Ycount - 1])) {
    Serial.println(F("y not in range"));
    return -1; // arbitrary...
  }

  for (int i = Xcount - 2; i >= 1 ; --i)
    if (x >= X[i]) {
      xIndex = i;
      break;
    }

  for (int i = Ycount - 2; i >= 1 ; --i)
    if (y >= Y[i]) {
      yIndex = i;
      break;
    }

  Serial.print(F("X:")); Serial.print(x); Serial.print(F(" in [")); Serial.print(X[xIndex]); Serial.print(F(",")); Serial.print(X[xIndex + 1]);
  Serial.print(F("] and Y:")); Serial.print(y); Serial.print(F(" in [")); Serial.print(Y[yIndex]); Serial.print(F(",")); Serial.print(Y[yIndex + 1]); Serial.println(F("]"));

  // https://en.wikipedia.org/wiki/Bilinear_interpolation
  // Q11 = (x1, y1), Q12 = (x1, y2), Q21 = (x2, y1), and Q22 = (x2, y2)
  double x1, y1, x2, y2;
  double fQ11, fQ12, fQ21, fQ22;
  double fxy1, fxy2, fxy;

  x1 = X[xIndex];
  x2 = X[xIndex + 1];
  y1 = Y[yIndex];
  y2 = Y[yIndex + 1];

  fQ11 = Z[xIndex][yIndex];
  fQ12 = Z[xIndex][yIndex + 1];
  fQ21 = Z[xIndex + 1][yIndex];
  fQ22 = Z[xIndex + 1][yIndex + 1];

  fxy1 = ((x2 - x) / (x2 - x1)) * fQ11 + ((x - x1) / (x2 - x1)) * fQ21;
  fxy2 = ((x2 - x) / (x2 - x1)) * fQ12 + ((x - x1) / (x2 - x1)) * fQ22;

  fxy = ((y2 - y) / (y2 - y1)) * fxy1 + ((y - y1) / (y2 - y1)) * fxy2;

  return fxy;

}

void setup() {
  Serial.begin(115200);
  Serial.println(bilinearXY(120,33));
}

void loop() {}

J-M-L:
I’d do something like this

Quickly hacked from your wikipedia reference so that you can see the maths based on their notation

Would need some optimization and not double checked at all… will likely compile, not sure what it does :slight_smile:

// Those arrays needs to be sorted in ascending order

// Need at least 2 points
double X = {0, 500, 1000, 2000, 3000, 4000, 5000, 7000};          //X axis Data for testing
double Y = {0, 10, 15, 20, 30, 35, 40, 45};                        //Y axis Data for testing

const int Xcount = sizeof(X) / sizeof(X[0]);
const int Ycount = sizeof(Y) / sizeof(Y[0]);

// points in 3D space (X, Y[y], Z[y])
double Z[Xcount][Ycount] = {                                        //Z Data for testing
  {70, 71, 72, 73, 74, 75, 76, 77},
  {60, 61, 62, 63, 64, 65, 66, 67},
  {50, 51, 52, 53, 54, 55, 56, 57},
  {40, 41, 42, 43, 44, 45, 46, 47},
  {30, 31, 32, 33, 34, 35, 36, 37},
  {20, 21, 22, 23, 24, 25, 26, 27},
  {10, 11, 12, 13, 14, 15, 16, 17},
  {00, 01, 02, 03, 04, 05, 06, 07}
};

double bilinearXY(int x, int y)
{
  int xIndex, yIndex;

if ((x < X[0]) || (x > X[Xcount - 1])) {
    Serial.println(F(“x not in range”));
    return -1; // arbitrary…
  }

if ((y < Y[0]) || (y > Y[Ycount - 1])) {
    Serial.println(F(“y not in range”));
    return -1; // arbitrary…
  }

for (int i = Xcount - 2; i >= 1 ; --i)
    if (x >= X[i]) {
      xIndex = i;
      break;
    }

for (int i = Ycount - 2; i >= 1 ; --i)
    if (y >= Y[i]) {
      yIndex = i;
      break;
    }

Serial.print(F(“X:”)); Serial.print(x); Serial.print(F(" in [")); Serial.print(X[xIndex]); Serial.print(F(",")); Serial.print(X[xIndex + 1]);
  Serial.print(F("] and Y:")); Serial.print(y); Serial.print(F(" in [")); Serial.print(Y[yIndex]); Serial.print(F(",")); Serial.print(Y[yIndex + 1]); Serial.println(F("]"));

// Bilinear interpolation - Wikipedia
  // Q11 = (x1, y1), Q12 = (x1, y2), Q21 = (x2, y1), and Q22 = (x2, y2)
  double x1, y1, x2, y2;
  double fQ11, fQ12, fQ21, fQ22;
  double fxy1, fxy2, fxy;

x1 = X[xIndex];
  x2 = X[xIndex + 1];
  y1 = Y[yIndex];
  y2 = Y[yIndex + 1];

fQ11 = Z[xIndex][yIndex];
  fQ12 = Z[xIndex][yIndex + 1];
  fQ21 = Z[xIndex + 1][yIndex];
  fQ22 = Z[xIndex + 1][yIndex + 1];

fxy1 = ((x2 - x) / (x2 - x1)) * fQ11 + ((x - x1) / (x2 - x1)) * fQ21;
  fxy2 = ((x2 - x) / (x2 - x1)) * fQ12 + ((x - x1) / (x2 - x1)) * fQ22;

fxy = ((y2 - y) / (y2 - y1)) * fxy1 + ((y - y1) / (y2 - y1)) * fxy2;

return fxy;

}

void setup() {
  Serial.begin(115200);
  Serial.println(bilinearXY(120,33));
}

void loop() {}

Thank You,

It does compile, returned a NAN result. But I’ll sort through it, but your code looks a lot more organized than mine. I apreciate the help.

On what architecture are you running it? I'll try later this morning to compile and run it

I already noted a bug

for (int i = Xcount - 2; [color=red]i >= 1[/color] ; --i) should not be >=1 but >=0
Same for Ycount otherwise will be a pb if the data you try to find is within the first interval, xIndex or yIndex won't be initialized properly.

Alternatively You can keep >=1 if you initialize those index to 0 though a time the start of the function

  int xIndex=0, yIndex=0;

So I tried the code (with the 0 initialization) on a UNO and I see in the console

X:120 in [0.00,500.00] and Y:33 in [30.00,35.00]
72.20

does not seem too crazy