Snabb atan2 med integers

[Rick81]

Jag har ett algoritm som behöver göra många atan2 beräkningar på kort tid.

Jag använder GCC till en 32 bits ARM9 processor och mattebiblioteket har endast double atan2(double X, double Y) vilket följaktligen tar lång tid eftersom processorn måste jobba med doubles.

Jag behöver en snabb atan2 som använder integers istället.

Processorn har varken flyttal eller hårdvarudivision, så helst skulle man undvika detta. Därmot har den mycket RAM/flash så ett lockuptable med någon form av approximering mellan elementen skulle kunna fungera.

Helst skulle jag behöva en upplösning på grader med 5 decimaler men då multiplicerat med en ex 100000 för att slippa float och double. Dvs när atan2 returnerar 9123540 så betyder det 91.23450 grader.

Så det jag är ute efter är

int angle = atan(int X, int Y) //Returnerar grader * 100000

int angleDegrees = angle/100000; //Omvandla till hela grader

En optimerad assembler funktion vore givetvis bra, men annars C-kod för integer funkar också.

Tips på detta?

[bearing]

Jag har skrivit en sån funktion för 8-bit variabler. Den använder CORDIC-metoden. Du kan säkert hitta exempelkod på nätet för 32-bit variabler, annars kan jag visa min kod ikväll när jag kommit hem.

[Rick81]

Det vore intressant att se den koden. För tydlighetens skull så är inparametrarna x och y 16 bitars int.

[bearing]

x och y är koordinater i en cirkel med centrum i koordinaten (128, 128). Returvärdet angle är en vinkel om 192 steg på ett varv, så siffran 96 betyder 180°, 48 betyder 90°, osv.

Jag har gått ifrån standard CORDIC en aning, för att kunna använda endast unsigned. I standard CORDIC hoppar vinkeln mellan positiv och negativ tills den blir noll. I min funktion går den mot noll endast från positivt håll. Jag tror att detta går fortare, eller iaf lika fort, och utan att tappa noggrannhet.

Observera att jag har "unrollat" loopen för att försöka spara enstaka klockcykler, men det behövs nog inte på en snabbare processor.

Funktionen tar 50-60 klockcykler på en ATmega, har jag för mig.

Kod: Markera allt

static uint8_t arctan(uint8_t x, uint8_t y)
{
  uint8_t angle = 0;
  uint8_t tempx;
  uint8_t tempy;
  const uint8_t atan_table[] = {24, 14, 7, 4, 2, 1};

  if (x < 128)      //Quadrant 2 or 3
  {
    angle+=96;      //Add 180°
    x = ~x;         //and invert x
    y = ~y;         //and y;
  }

  if (y < 128)      //Quadrant 4 
  {
    angle-=48;      //Remove 90°
    if (angle == 208)
    {
      angle = 192-48;
    }
    tempx = x;
    x= ~y;
    y = tempx;
  }

  x-=128;
  y-=128;

  if (y != 0)
  { 
    //Step 0
    tempx = x;
    if (y >= tempx)
    {
      angle+=atan_table[0];
      x+=y;
      y-=tempx;
    }
  
    //Step 1
    tempx = x>>1;
    if (y >= tempx)
    {
      angle+=atan_table[1];
      tempy=y>>1;
      x+=tempy;
      y-=tempx;
    }

    //Step 2
    tempx = x>>2;
    if (y >= tempx)
    {
      angle+=atan_table[2];
      tempy=y>>2;
      x+=tempy;
      y-=tempx;
    }

    //Step 3
    tempx = x>>3;
    if (y >= tempx)
    {
      angle+=atan_table[3];
      tempy=y>>3;
      x+=tempy;
      y-=tempx;
    }
  
    //Step 4
    tempx = x>>4;
    if (y >= tempx)
    {
      angle+=atan_table[4];
      tempy=y>>4;
      x+=tempy;
      y-=tempx;
    }

    //Step 5
    tempx = x>>5;
    if (y >= tempx)
    {
      angle+=atan_table[5];
      tempy=y>>5;
      x+=tempy;
      y-=tempx;
    }
  }

  return angle;
}