Challenge of the week!

[Gus_Issa]

ARM Cortex-M3 3D integer arithmetic 120MHz microcontroller from NXP

This look very sweet. Source code is found here http://www.serveurperso.com/temp/maths.txt

So I was wondering how fast (slow) this will be through NETMF. I converted the code to C# and I see somethign but not completely working!

Can you figure out why? Here is my C# code (runs in emulator, no need for hardware)

// Microcontroller graphic demo by Pascal Piazzalunga
// admin@ serveurperso.com http://www.serveurperso.com


using System;

using Microsoft.SPOT;
using Microsoft.SPOT.Presentation;

namespace MFConsoleApplication3
{
    public class Program
    {
        static Bitmap LCD = new Bitmap(320, 240);
        static int WINDOWX = 320;
        static int WINDOWY = 240;

        // INCREMENT = SCALE / sqrt(N) * 2
        // Optimizer-friendly values
        static int N = 1024; // Number of dots
        static int SCALE = 8192;
        static int INCREMENT = 512;

        static int SPEED = 10;

        static double PI2 = 6.283185307179586476925286766559;

        //static char buff[512];

        //SWIM_WINDOW_T win1;

        static UInt16[] sine = new UInt16[SCALE];
        static UInt16[] cosi = new UInt16[SCALE];

        static void initialize()
        {
            int i;

            for (i = 0; i < SCALE; i++)
            {
                sine[i] = (UInt16)(System.Math.Sin(PI2 * i / SCALE) * SCALE);
                cosi[i] = (UInt16)(System.Math.Cos(PI2 * i / SCALE) * SCALE);
            }
        }

        static UInt16 fastsqrt(UInt16 n)
        {
            UInt16 c = (UInt16)0x8000;
            UInt16 g = (UInt16)0x8000;
            for (; ; )
            {
                if (g * g > n)
                    g ^= c;
                c >>= 1;
                if (c == 0)
                    return g;
                g |= c;
            }
        }
        
        //void matrix(UInt16 xyz[3][N], UInt16 rgb[3][N]) 
        static int t_matrix=0;
        static void matrix(int[][] xyz, byte[][] rgb)
        {
            //static UInt32 t_matrix = 0;
            UInt16 i;
            int x = -SCALE;
            int y = -SCALE;
            UInt16 d;
            int s;

            for (i = 0; i < N; i++)
            {

                xyz[0][i] = x;
                xyz[1][i] = y;

                d = fastsqrt((UInt16)(x * x + y * y));
                s = sine[(int)((t_matrix * 30) % SCALE)] + SCALE;

                xyz[2][i] = sine[(d + s) % SCALE] *
                            sine[(t_matrix * 10) % SCALE] / SCALE / 2;
                /*
                  rgb[0][i] = (cosi[xyz[2][i] + SCALE / 2] + SCALE) *
                              (RED_COLORS - 1) / SCALE / 2;

                  rgb[1][i] = (cosi[(xyz[2][i] + SCALE / 2 + 2 * SCALE / 3) % SCALE] + SCALE) *
                              (GREEN_COLORS - 1) / SCALE / 2;

                  rgb[2][i] = (cosi[(xyz[2][i] + SCALE / 2 + SCALE / 3) % SCALE] + SCALE) *
                              (BLUE_COLORS - 1) / SCALE / 2;
                     */
                x += INCREMENT;
                if (x >= SCALE)
                {
                    x = -SCALE;
                    y += INCREMENT;
                }

            }
            t_matrix++;
        }
        
    //void rotate(Int16 xyz[3][N], unsgined byte rgb[3][N],UInt16 angleX, UInt16 angleY, UInt16 angleZ) 
        static void rotate(int[][] xyz, byte[][] rgb, int angleX, int angleY, int angleZ)
        {
            UInt16 i;
            int tmpX;
            int tmpY;
            UInt16 sinx = sine[angleX];
            UInt16 cosx = cosi[angleX];
            UInt16 siny = sine[angleY];
            UInt16 cosy = cosi[angleY];
            UInt16 sinz = sine[angleZ];
            UInt16 cosz = cosi[angleZ];

            for (i = 0; i < N; i++)
            {
                tmpX = (xyz[0][i] * cosx - xyz[2][i] * sinx) / SCALE;
                xyz[2][i] = (xyz[0][i] * sinx + xyz[2][i] * cosx) / SCALE;
                xyz[0][i] = tmpX;

                tmpY = (xyz[1][i] * cosy - xyz[2][i] * siny) / SCALE;
                xyz[2][i] = (xyz[1][i] * siny + xyz[2][i] * cosy) / SCALE;
                xyz[1][i] = tmpY;

                tmpX = (xyz[0][i] * cosz - xyz[1][i] * sinz) / SCALE;
                xyz[1][i] = (xyz[0][i] * sinz + xyz[1][i] * cosz) / SCALE;
                xyz[0][i] = tmpX;
            }
        }
 
        static int []oldProjX=new int[N];
         static int []oldProjY=new int[N];
         static int []oldDotSize=new int[N];
 
        //void draw(int16_t xyz[3][N], uint8_t rgb[3][N])
         static void draw(int[][] xyz, byte[][] rgb)
         {
             //static uint16_t oldProjX[N] = {0};
             //static uint16_t oldProjY[N] = {0};
             //static uint8_t oldDotSize[N] = {0};
             int i;
             int projX;
             int projY;
             int projZ;
             int dotSize;

             for (i = 0; i < N; i++)
             {
                 projZ = SCALE - (xyz[2][i] + SCALE) / 4;
                 projX = WINDOWX / 2 + (xyz[0][i] * projZ / SCALE) / 25;
                 projY = WINDOWY / 2 + (xyz[1][i] * projZ / SCALE) / 25;
                 dotSize = 3 - (xyz[2][i] + SCALE) * 2 / SCALE;

                 //swim_set_pen_color(&win1, 0);

                 //swim_put_circle(&win1, oldProjX[i], oldProjY[i], oldDotSize[i], 1);
                 LCD.DrawEllipse(Microsoft.SPOT.Presentation.Media.Color.Black, oldProjX[i], oldProjY[i], oldDotSize[i], oldDotSize[i]);
                 if (projX > dotSize &&
                    projY > dotSize &&
                    projX < WINDOWX - dotSize &&
                    projY < WINDOWY - dotSize)
                 {
                     /*
                  swim_set_pen_color(&win1, (rgb[0][i] << REDSHIFT) +
                                            (rgb[1][i] << GREENSHIFT) +
                                            (rgb[2][i] << BLUESHIFT));

                  swim_put_circle(&win1, projX, projY, dotSize, 1);
                     */
                     
                     LCD.DrawEllipse(Microsoft.SPOT.Presentation.Media.Color.White, projX, projY, dotSize, dotSize);
                     //LCD.Flush();
                     oldProjX[i] = projX;
                     oldProjY[i] = projY;
                     oldDotSize[i] = dotSize;
                 }
             }
         }

         public static void Main()
         {
             initialize();


             int[][] xyz = new int[3][];
             byte[][] rgb = new byte[3][];

             int angleX = 0;
             int angleY = 0;
             int angleZ = 0;

             int speedX = 0;
             int speedY = 0;
             int speedZ = 0;

             xyz[0] = new int[N];
             xyz[1] = new int[N];
             xyz[2] = new int[N];

             rgb[0] = new byte[N];
             rgb[1] = new byte[N];
             rgb[2] = new byte[N];

             while (true)
             {
                 matrix(xyz, rgb);

                 rotate(xyz, rgb, angleX, angleY, angleZ);

                 draw(xyz, rgb);

                 speedZ -= SPEED;
                 /*
                if(joyState & JOYSTICK_RIGHT)
                 speedX -= SPEED;
                else if(joyState & JOYSTICK_LEFT)
                 speedX += SPEED;
                else if(joyState & JOYSTICK_UP)
                 speedY -= SPEED;
                else if(joyState & JOYSTICK_DOWN)
                 speedY += SPEED;
                else if(ledState & KEY1)
                 speedZ -= SPEED;
                else if(ledState & KEY2)
                 speedZ += SPEED;
                else if(ledState & KEY3) {
                 speedX = 0;
                 speedY = 0;
                 speedZ = 0;
                 angleX = 0;
                 angleY = 0;
                 angleZ = 0;
                } else {
                 if(speedX > 0)
                  speedX -= SPEED;
                 else if(speedX < 0)
                  speedX += SPEED;

                 if(speedY > 0)
                  speedY -= SPEED;
                 else if(speedY < 0)
                  speedY += SPEED;

                 if(speedZ > 0)
                  speedZ -= SPEED;
                 else if(speedZ < 0)
                  speedZ += SPEED;
                }
                 */
                angleX += speedX;
                angleY += speedY;
                angleZ += speedZ;

                if(angleX >= SCALE)
                 angleX -= SCALE;
                else if(angleX < 0)
                 angleX += SCALE;

                if(angleY >= SCALE)
                 angleY -= SCALE;
                else if(angleY < 0)
                 angleY += SCALE;

                if(angleZ >= SCALE)
                 angleZ -= SCALE;
                else if(angleZ < 0)
                 angleZ += SCALE;
               

                 
                 LCD.Flush();
                 System.Threading.Thread.Sleep(10);
             }

         }
    }
}

[taylorza]

Hi Gus,

When working with fixed point arithmetic it is critical to watch the data types, using an int where there should be a unsigned int will break the system because often in 3d computations the engine will rely on the value wrapping around etc.

Here is a version with the corrected data types and I also added the colors. There are a few optimizations that could be done to this code, but I doubt it would get much faster in managed code due to the loops and array accesses I have found these to be performance killers.

// Microcontroller graphic demo by Pascal Piazzalunga
// admin@ serveurperso.com <a href="http://www.serveurperso.com" target="_blank">http://www.serveurperso.com</a>


using System;

using Microsoft.SPOT;
using Microsoft.SPOT.Presentation;

namespace MFConsoleApplication3
{
  public class Program
  {
    static Bitmap LCD = new Bitmap(320, 240);
    const Int16 WINDOWX = 320;
    const Int16 WINDOWY = 240;

    // INCREMENT = SCALE / sqrt(N) * 2
    // Optimizer-friendly values
    const Int16 N = 1024; // Number of dots
    const Int16 SCALE = 8192;
    const Int16 INCREMENT = 512;

    const Int16 SPEED = 10;

    const double PI2 = 6.283185307179586476925286766559;

    //static char buff[512];

    //SWIM_WINDOW_T win1;

    static Int16[] sine = new Int16[SCALE];
    static Int16[] cosi = new Int16[SCALE];

    static void initialize()
    {
      int i;

      for (i = 0; i < SCALE; i++)
      {
        sine[i] = (Int16)(System.Math.Sin(PI2 * i / SCALE) * SCALE);
        cosi[i] = (Int16)(System.Math.Cos(PI2 * i / SCALE) * SCALE);
      }
    }

    static UInt16 fastsqrt(UInt32 n)
    {
      UInt16 c = (UInt16)0x8000;
      UInt16 g = (UInt16)0x8000;
      for (; ; )
      {
        if (g * g > n)
          g ^= c;
        c >>= 1;
        if (c == 0)
          return g;
        g |= c;
      }
    }

    //void matrix(UInt16 xyz[3][N], UInt16 rgb[3][N]) 
    static UInt32 t_matrix = 0;
    static void matrix(Int16[][] xyz, byte[][] rgb)
    {
      //static UInt32 t_matrix = 0;
      UInt16 i;
      Int16 x = -SCALE;
      Int16 y = -SCALE;
      UInt16 d;
      UInt16 s;

      for (i = 0; i < N; i++)
      {

        xyz[0][i] = x;
        xyz[1][i] = y;

        d = fastsqrt((UInt32)(x * x + y * y));
        s = (UInt16)(sine[(t_matrix * 30) % SCALE] + SCALE);

        xyz[2][i] = (Int16)(sine[(d + s) % SCALE] *
                    sine[(t_matrix * 10) % SCALE] / SCALE / 2);

        rgb[0][i] = (byte)((cosi[xyz[2][i] + SCALE / 2] + SCALE) *
                    (255 - 1) / SCALE / 2);

        rgb[1][i] = (byte)((cosi[(xyz[2][i] + SCALE / 2 + 2 * SCALE / 3) % SCALE] + SCALE) *
                    (255 - 1) / SCALE / 2);

        rgb[2][i] = (byte)((cosi[(xyz[2][i] + SCALE / 2 + SCALE / 3) % SCALE] + SCALE) *
                    (255 - 1) / SCALE / 2);
             
        x += INCREMENT;
        if (x >= SCALE)
        {
          x = -SCALE;
          y += INCREMENT;
        }

      }
      t_matrix++;
    }

    //void rotate(Int16 xyz[3][N], unsgined byte rgb[3][N],UInt16 angleX, UInt16 angleY, UInt16 angleZ) 
    static void rotate(Int16[][] xyz, byte[][] rgb, UInt16 angleX, UInt16 angleY, UInt16 angleZ)
    {
      UInt16 i;
      Int16 tmpX;
      Int16 tmpY;
      Int16 sinx = sine[angleX];
      Int16 cosx = cosi[angleX];
      Int16 siny = sine[angleY];
      Int16 cosy = cosi[angleY];
      Int16 sinz = sine[angleZ];
      Int16 cosz = cosi[angleZ];

      for (i = 0; i < N; i++)
      {
        tmpX = (Int16)((xyz[0][i] * cosx - xyz[2][i] * sinx) / SCALE);
        xyz[2][i] = (Int16)((xyz[0][i] * sinx + xyz[2][i] * cosx) / SCALE);
        xyz[0][i] = tmpX;

        tmpY = (Int16)((xyz[1][i] * cosy - xyz[2][i] * siny) / SCALE);
        xyz[2][i] = (Int16)((xyz[1][i] * siny + xyz[2][i] * cosy) / SCALE);
        xyz[1][i] = tmpY;

        tmpX = (Int16)((xyz[0][i] * cosz - xyz[1][i] * sinz) / SCALE);
        xyz[1][i] = (Int16)((xyz[0][i] * sinz + xyz[1][i] * cosz) / SCALE);
        xyz[0][i] = tmpX;
      }
    }

    static int[] oldProjX = new int[N];
    static int[] oldProjY = new int[N];
    static int[] oldDotSize = new int[N];

    //void draw(int16_t xyz[3][N], uint8_t rgb[3][N])
    static void draw(Int16[][] xyz, byte[][] rgb)
    {
      UInt16 i;
      UInt16 projX;
      UInt16 projY;
      UInt16 projZ;
      UInt16 dotSize;

      for (i = 0; i < N; i++)
      {
        projZ = (UInt16)(SCALE - (xyz[2][i] + SCALE) / 4);
        projX = (UInt16)(WINDOWX / 2 + (xyz[0][i] * projZ / SCALE) / 25);
        projY = (UInt16)(WINDOWY / 2 + (xyz[1][i] * projZ / SCALE) / 25);
        dotSize = (UInt16)(3 - (xyz[2][i] + SCALE) * 2 / SCALE);

        //swim_set_pen_color(&win1, 0);

        //swim_put_circle(&win1, oldProjX[i], oldProjY[i], oldDotSize[i], 1);
        LCD.DrawEllipse(Microsoft.SPOT.Presentation.Media.Color.Black, oldProjX[i], oldProjY[i], oldDotSize[i], oldDotSize[i]);
        if (projX > dotSize &&
           projY > dotSize &&
           projX < WINDOWX - dotSize &&
           projY < WINDOWY - dotSize)
        {
          /*
       swim_set_pen_color(&win1, (rgb[0][i] << REDSHIFT) +
                                 (rgb[1][i] << GREENSHIFT) +
                                 (rgb[2][i] << BLUESHIFT));
 
       swim_put_circle(&win1, projX, projY, dotSize, 1);
          */
          var color = Microsoft.SPOT.Presentation.Media.ColorUtility.ColorFromRGB(rgb[0][i], rgb[1][i], rgb[2][1]);          
          LCD.DrawEllipse(color, projX, projY, dotSize, dotSize);
          //LCD.Flush();
          oldProjX[i] = projX;
          oldProjY[i] = projY;
          oldDotSize[i] = dotSize;
        }
      }
    }

    public static void Main()
    {
      initialize();


      Int16[][] xyz = new Int16[3][];
      byte[][] rgb = new byte[3][];

      Int16 angleX = 0;
      Int16 angleY = 0;
      Int16 angleZ = 0;

      Int16 speedX = 0;
      Int16 speedY = 0;
      Int16 speedZ = 0;

      xyz[0] = new Int16[N];
      xyz[1] = new Int16[N];
      xyz[2] = new Int16[N];

      rgb[0] = new byte[N];
      rgb[1] = new byte[N];
      rgb[2] = new byte[N];

      speedX -= SPEED;
      speedZ -= SPEED;

      while (true)
      {
        matrix(xyz, rgb);

        rotate(xyz, rgb, (UInt16)angleX, (UInt16)angleY, (UInt16)angleZ);

        draw(xyz, rgb);

        
        /*
       if(joyState & JOYSTICK_RIGHT)
        speedX -= SPEED;
       else if(joyState & JOYSTICK_LEFT)
        speedX += SPEED;
       else if(joyState & JOYSTICK_UP)
        speedY -= SPEED;
       else if(joyState & JOYSTICK_DOWN)
        speedY += SPEED;
       else if(ledState & KEY1)
        speedZ -= SPEED;
       else if(ledState & KEY2)
        speedZ += SPEED;
       else if(ledState & KEY3) {
        speedX = 0;
        speedY = 0;
        speedZ = 0;
        angleX = 0;
        angleY = 0;
        angleZ = 0;
       } else {
        if(speedX > 0)
         speedX -= SPEED;
        else if(speedX < 0)
         speedX += SPEED;
 
        if(speedY > 0)
         speedY -= SPEED;
        else if(speedY < 0)
         speedY += SPEED;
 
        if(speedZ > 0)
         speedZ -= SPEED;
        else if(speedZ < 0)
         speedZ += SPEED;
       }
        */
        angleX += speedX;
        angleY += speedY;
        angleZ += speedZ;

        if (angleX >= SCALE)
          angleX -= SCALE;
        else if (angleX < 0)
          angleX += SCALE;

        if (angleY >= SCALE)
          angleY -= SCALE;
        else if (angleY < 0)
          angleY += SCALE;

        if (angleZ >= SCALE)
          angleZ -= SCALE;
        else if (angleZ < 0)
          angleZ += SCALE;



        LCD.Flush();
        System.Threading.Thread.Sleep(10);
      }

    }
  }
}

[Justin]

I so knew taylorza was going to answer

[taylorza]

@ Justin - Are you saying I am predictable?

I woke up this morning, saw this, I could not wait to play with it…

[Justin]

@ taylorza - where’s the video?

[taylorza]

@Justin

@ taylorza - where’s the video?

See, not so predictable after all, you thought there would be a video, ha!

[Justin]

Talk about half measures… :whistle:

[taylorza]

Insult upon insult, lucky I have that South African thick skin…

(Gus is going to ban us soon)

[Justin]

[Gus_Issa]

This is great! Find me next time you have a hardware related question

I changed couple things and it is faster now. Still not enough. I am curious to see how would this run on G400!

[taylorza]

@Gus

This is great! Find me next time you have a hardware related question

Gus, I know better than to even attempt hardware… I always seem to let the magic smoke escape, around me LEDs pop like fire crackers on Guy Fawkes day. Fortunately you guys have made that side of things FEZ.

[Justin]

@Gus

I changed couple things and it is faster now. Still not enough. I am curious to see how would this run on G400!

and the answer is…?

[ianlee74]

Nice job, taylorza. I looked at this thing for a couple hrs last night before falling asleep on my keyboard.

[Justin]

@ ianlee74 - hahahaha, did you wake up with dribble on ya chin?

[ianlee74]

@Justin

@ ianlee74 - hahahaha, did you wake up with dribble on ya chin?

Na. I was all in the keyboard