File content as of revision 1:1c2ea8267953:

/******************************************************************************
 * Includes
 *****************************************************************************/

#include "mbed.h"

#include "LcdController.h"
#include "EaLcdBoard.h"
#include "SphereDemo.h"

#include <math.h>

/******************************************************************************
 * Typedefs and defines
 *****************************************************************************/

#define PI2 6.283185307179586476925286766559

/* Red color mask, 565 mode */
#define REDMASK       0xF800
/* Red shift value, 565 mode */
#define REDSHIFT      11
/* Green color mask, 565 mode */
#define GREENMASK     0x07E0
/* Green shift value, 565 mode */
#define GREENSHIFT    5
/* Blue color mask, 565 mode */
#define BLUEMASK      0x001F
/* Blue shift value, 565 mode */
#define BLUESHIFT     0

/* Number of colors in 565 mode */
#define NUM_COLORS    65536
/* Number of red colors in 565 mode */
#define RED_COLORS    0x20
/* Number of green colors in 565 mode */
#define GREEN_COLORS  0x40
/* Number of blue colors in 565 mode */
#define BLUE_COLORS   0x20

/******************************************************************************
 * Local variables
 *****************************************************************************/


/******************************************************************************
 * External variables
 *****************************************************************************/
extern EaLcdBoard lcdBoard;
extern bool abortTest;

/******************************************************************************
 * Local functions
 *****************************************************************************/

uint16_t SphereDemo::fastsqrt(uint32_t n) const {
 uint16_t c = 0x8000;
 uint16_t g = 0x8000;
 for(;;) {
  if(g*g > n)
   g ^= c;
  c >>= 1;
  if(c == 0)
   return g;
  g |= c;
 }
}

void SphereDemo::matrix(int16_t xyz[3][N], uint8_t rgb[3][N]) {
 static uint32_t t = 0;
 uint16_t i;
 int16_t x = -SCALE;
 int16_t y = -SCALE;
 uint16_t d;
 uint16_t s;

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

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

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

  xyz[2][i] = sine[(d + s) % SCALE] *
              sine[(t * 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++;
}

void SphereDemo::rotate(int16_t xyz[3][N], uint8_t rgb[3][N],
                   uint16_t angleX, uint16_t angleY, uint16_t angleZ) {
 uint16_t i;
 int16_t tmpX;
 int16_t tmpY;
 int16_t sinx = sine[angleX];
 int16_t cosx = cosi[angleX];
 int16_t siny = sine[angleY];
 int16_t cosy = cosi[angleY];
 int16_t sinz = sine[angleZ];
 int16_t 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;
 }
}

void SphereDemo::draw(int16_t xyz[3][N], uint8_t rgb[3][N]) {
// static uint16_t oldProjX[N] = {0};
// static uint16_t oldProjY[N] = {0};
// static uint8_t oldDotSize[N] = {0};
 uint16_t i;
 uint16_t projX;
 uint16_t projY;
 uint16_t projZ;
 uint16_t dotSize;
 static uint8_t cnt=0;

  if (cnt == 0)
  {
    cnt = 1;
    pFrmBuf = pFrmBuf1;
  }
  else if (cnt == 1)
  {
    cnt = 2;
    pFrmBuf = pFrmBuf2;
  }
  else
  {
    cnt = 0;
    pFrmBuf = pFrmBuf3;
  }
  
  graphics.setFrameBuffer(pFrmBuf);

  memset((void*)(pFrmBuf), 0x00, windowX * windowY * 2);

 for(i = 0; i < N; i++) {
  projZ   = SCALE - (xyz[2][i] + SCALE) / 4; //4;
  projX   = windowX / 2 + (xyz[0][i] * projZ / SCALE) / 40; //EA 25;
  projY   = windowY / 2 + (xyz[1][i] * projZ / SCALE) / 40; //EA 25;
  dotSize = 3 - (xyz[2][i] + SCALE) * 2 / SCALE;
//EA  put_circle(oldProjX[i], oldProjY[i], 0, dotSize, 1);

  if((projX > dotSize) &&
     (projY > dotSize) &&
     (projX < (windowX - dotSize)) &&
     (projY < (windowY - dotSize))) {

   graphics.put_circle(projX, projY, (rgb[0][i] << REDSHIFT) + (rgb[1][i] << GREENSHIFT) + (rgb[2][i] << BLUESHIFT), dotSize, 1);

//   oldProjX[i] = projX;
//   oldProjY[i] = projY;
//   oldDotSize[i] = dotSize;
  }
 }
}



/******************************************************************************
 * Public functions
 *****************************************************************************/
SphereDemo::SphereDemo(uint8_t *pFrameBuf, uint16_t dispWidth, uint16_t dispHeight) 
    : graphics((uint16_t *)pFrameBuf, dispWidth, dispHeight)
{
    this->windowX = dispWidth;
    this->windowY = dispHeight;
    this->pFrmBuf  = (uint16_t *)pFrameBuf;
    this->pFrmBuf1 = (uint16_t *)pFrameBuf;
    this->pFrmBuf2 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*2);
    this->pFrmBuf3 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*4);

    for(uint16_t i = 0; i < SCALE; i++) {
        sine[i] = (int)(sin(PI2 * i / SCALE) * SCALE);
        cosi[i] = (int)(cos(PI2 * i / SCALE) * SCALE);
    }
}

void SphereDemo::run(uint32_t loops, uint32_t delayMs)
{
  printf("SphereDemo, %d loops, %dms delay\n", loops, delayMs);
  
    int16_t angleX = 0;
    int16_t angleY = 1000;
    int16_t angleZ = 0;
    
    int16_t speedX = 0;
    int16_t speedY = 0;
    int16_t speedZ = 0;
    
    int16_t xyz[3][N];
    uint8_t rgb[3][N];
    
    loops = 2*820;
  for(int32_t n=0;n<loops;n++) {

   matrix(xyz, rgb);

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

   draw(xyz, rgb);
  //update framebuffer
  lcdBoard.setFrameBuffer((uint32_t)this->pFrmBuf);

#if 0
   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
#endif
    {
    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 += 0; //speedX;
   angleY += 0; //speedY;
   angleZ += 2; //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;
   
    if (abortTest) {
      return;
    }   
  }
//wait_ms(delayMs);
   wait_ms(1000);

}