WaveDemo.cpp

00001 /******************************************************************************
00002  * Includes
00003  *****************************************************************************/
00004 
00005 #include "mbed.h"
00006 
00007 #include "LcdController.h"
00008 #include "EaLcdBoard.h"
00009 #include "WaveDemo.h"
00010 
00011 #include <math.h>
00012 
00013 /******************************************************************************
00014  * Typedefs and defines
00015  *****************************************************************************/
00016 
00017 #define PI2 6.283185307179586476925286766559
00018 
00019 /* Red color mask, 565 mode */
00020 #define REDMASK       0xF800
00021 /* Red shift value, 565 mode */
00022 #define REDSHIFT      11
00023 /* Green color mask, 565 mode */
00024 #define GREENMASK     0x07E0
00025 /* Green shift value, 565 mode */
00026 #define GREENSHIFT    5
00027 /* Blue color mask, 565 mode */
00028 #define BLUEMASK      0x001F
00029 /* Blue shift value, 565 mode */
00030 #define BLUESHIFT     0
00031 
00032 /* Number of colors in 565 mode */
00033 #define NUM_COLORS    65536
00034 /* Number of red colors in 565 mode */
00035 #define RED_COLORS    0x20
00036 /* Number of green colors in 565 mode */
00037 #define GREEN_COLORS  0x40
00038 /* Number of blue colors in 565 mode */
00039 #define BLUE_COLORS   0x20
00040 
00041 /******************************************************************************
00042  * Local variables
00043  *****************************************************************************/
00044 
00045 
00046 /******************************************************************************
00047  * External variables
00048  *****************************************************************************/
00049 
00050 
00051 /******************************************************************************
00052  * Local functions
00053  *****************************************************************************/
00054 
00055 uint16_t WaveDemo::fastsqrt(uint32_t n) const {
00056  uint16_t c = 0x8000;
00057  uint16_t g = 0x8000;
00058  for(;;) {
00059   if(g*g > n)
00060    g ^= c;
00061   c >>= 1;
00062   if(c == 0)
00063    return g;
00064   g |= c;
00065  }
00066 }
00067 
00068 void WaveDemo::matrix(int16_t xyz[3][N], uint8_t rgb[3][N]) {
00069  static uint32_t t = 0;
00070  uint16_t i;
00071  int16_t x = -SCALE;
00072  int16_t y = -SCALE;
00073  uint16_t d;
00074  uint16_t s;
00075 
00076  for(i = 0; i < N; i++) {
00077 
00078   xyz[0][i] = x;
00079   xyz[1][i] = y;
00080 
00081   d = fastsqrt(x * x + y * y);
00082   s = sine[(t * 30) % SCALE] + SCALE;
00083 
00084   xyz[2][i] = sine[(d + s) % SCALE] *
00085               sine[(t * 10) % SCALE] / SCALE / 2;
00086 
00087   rgb[0][i] = (cosi[xyz[2][i] + SCALE / 2] + SCALE) *
00088               (RED_COLORS - 1) / SCALE / 2;
00089 
00090   rgb[1][i] = (cosi[(xyz[2][i] + SCALE / 2 + 2 * SCALE / 3) % SCALE] + SCALE) *
00091               (GREEN_COLORS - 1) / SCALE / 2;
00092 
00093   rgb[2][i] = (cosi[(xyz[2][i] + SCALE / 2 + SCALE / 3) % SCALE] + SCALE) *
00094               (BLUE_COLORS - 1) / SCALE / 2;
00095 
00096   x += INCREMENT;
00097   if(x >= SCALE) {
00098    x = -SCALE;
00099    y += INCREMENT;
00100   }
00101 
00102  }
00103  t++;
00104 }
00105 
00106 void WaveDemo::rotate(int16_t xyz[3][N], uint8_t rgb[3][N],
00107                    uint16_t angleX, uint16_t angleY, uint16_t angleZ) {
00108  uint16_t i;
00109  int16_t tmpX;
00110  int16_t tmpY;
00111  int16_t sinx = sine[angleX];
00112  int16_t cosx = cosi[angleX];
00113  int16_t siny = sine[angleY];
00114  int16_t cosy = cosi[angleY];
00115  int16_t sinz = sine[angleZ];
00116  int16_t cosz = cosi[angleZ];
00117 
00118  for(i = 0; i < N; i++) {
00119   tmpX      = (xyz[0][i] * cosx - xyz[2][i] * sinx) / SCALE;
00120   xyz[2][i] = (xyz[0][i] * sinx + xyz[2][i] * cosx) / SCALE;
00121   xyz[0][i] = tmpX;
00122 
00123   tmpY      = (xyz[1][i] * cosy - xyz[2][i] * siny) / SCALE;
00124   xyz[2][i] = (xyz[1][i] * siny + xyz[2][i] * cosy) / SCALE;
00125   xyz[1][i] = tmpY;
00126 
00127   tmpX      = (xyz[0][i] * cosz - xyz[1][i] * sinz) / SCALE;
00128   xyz[1][i] = (xyz[0][i] * sinz + xyz[1][i] * cosz) / SCALE;
00129   xyz[0][i] = tmpX;
00130  }
00131 }
00132 
00133 void WaveDemo::draw(int16_t xyz[3][N], uint8_t rgb[3][N]) {
00134 // static uint16_t oldProjX[N] = {0};
00135 // static uint16_t oldProjY[N] = {0};
00136 // static uint8_t oldDotSize[N] = {0};
00137  uint16_t i;
00138  uint16_t projX;
00139  uint16_t projY;
00140  uint16_t projZ;
00141  uint16_t dotSize;
00142  static uint8_t cnt=0;
00143 
00144   if (cnt == 0)
00145   {
00146     cnt = 1;
00147     pFrmBuf = pFrmBuf1;
00148   }
00149   else if (cnt == 1)
00150   {
00151     cnt = 2;
00152     pFrmBuf = pFrmBuf2;
00153   }
00154   else
00155   {
00156     cnt = 0;
00157     pFrmBuf = pFrmBuf3;
00158   }
00159   
00160   graphics.setFrameBuffer(pFrmBuf);
00161 
00162   memset((void*)(pFrmBuf), 0x00, windowX * windowY * 2);
00163 
00164  for(i = 0; i < N; i++) {
00165   projZ   = SCALE - (xyz[2][i] + SCALE) / 4; //4;
00166   projX   = windowX / 2 + (xyz[0][i] * projZ / SCALE) / 40; //EA 25;
00167   projY   = windowY / 2 + (xyz[1][i] * projZ / SCALE) / 40; //EA 25;
00168   dotSize = 3 - (xyz[2][i] + SCALE) * 2 / SCALE;
00169 //EA  put_circle(oldProjX[i], oldProjY[i], 0, dotSize, 1);
00170 
00171   if((projX > dotSize) &&
00172      (projY > dotSize) &&
00173      (projX < (windowX - dotSize)) &&
00174      (projY < (windowY - dotSize))) {
00175 
00176    graphics.put_circle(projX, projY, (rgb[0][i] << REDSHIFT) + (rgb[1][i] << GREENSHIFT) + (rgb[2][i] << BLUESHIFT), dotSize, 1);
00177 
00178 //   oldProjX[i] = projX;
00179 //   oldProjY[i] = projY;
00180 //   oldDotSize[i] = dotSize;
00181   }
00182  }
00183 }
00184 
00185 
00186 
00187 /******************************************************************************
00188  * Public functions
00189  *****************************************************************************/
00190 WaveDemo::WaveDemo(uint8_t *pFrameBuf, uint16_t dispWidth, uint16_t dispHeight) 
00191     : graphics((uint16_t *)pFrameBuf, dispWidth, dispHeight)
00192 {
00193     this->windowX = dispWidth;
00194     this->windowY = dispHeight;
00195     this->pFrmBuf  = (uint16_t *)pFrameBuf;
00196     this->pFrmBuf1 = (uint16_t *)pFrameBuf;
00197     this->pFrmBuf2 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*2);
00198     this->pFrmBuf3 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*4);
00199 
00200     for(uint16_t i = 0; i < SCALE; i++) {
00201         sine[i] = (int)(sin(PI2 * i / SCALE) * SCALE);
00202         cosi[i] = (int)(cos(PI2 * i / SCALE) * SCALE);
00203     }
00204 }
00205 
00206 void WaveDemo::run(EaLcdBoardGPIO& lcdBoard, uint32_t loops, uint32_t delayMs)
00207 {
00208   printf("WaveDemo, %d loops, %dms delay\n", loops, delayMs);
00209   
00210     int16_t angleX = 0;
00211     int16_t angleY = 1000;
00212     int16_t angleZ = 0;
00213     
00214     int16_t speedX = 0;
00215     int16_t speedY = 0;
00216     int16_t speedZ = 0;
00217     
00218     int16_t xyz[3][N];
00219     uint8_t rgb[3][N];
00220     
00221     loops = 2*820;
00222   for(int32_t n=0;n<loops;n++) {
00223 
00224    matrix(xyz, rgb);
00225 
00226    rotate(xyz, rgb, angleX, angleY, angleZ);
00227 
00228    draw(xyz, rgb);
00229   //update framebuffer
00230   lcdBoard.setFrameBuffer((uint32_t)this->pFrmBuf);
00231 
00232 #if 0
00233    if(joyState & JOYSTICK_RIGHT)
00234     speedX -= SPEED;
00235    else if(joyState & JOYSTICK_LEFT)
00236     speedX += SPEED;
00237    else if(joyState & JOYSTICK_UP)
00238     speedY -= SPEED;
00239    else if(joyState & JOYSTICK_DOWN)
00240     speedY += SPEED;
00241    else if(ledState & KEY1)
00242     speedZ -= SPEED;
00243    else if(ledState & KEY2)
00244     speedZ += SPEED;
00245    else if(ledState & KEY3) {
00246     speedX = 0;
00247     speedY = 0;
00248     speedZ = 0;
00249     angleX = 0;
00250     angleY = 0;
00251     angleZ = 0;
00252    } else
00253 #endif
00254     {
00255     if(speedX > 0)
00256      speedX -= SPEED;
00257     else if(speedX < 0)
00258      speedX += SPEED;
00259 
00260     if(speedY > 0)
00261      speedY -= SPEED;
00262     else if(speedY < 0)
00263      speedY += SPEED;
00264 
00265     if(speedZ > 0)
00266      speedZ -= SPEED;
00267     else if(speedZ < 0)
00268      speedZ += SPEED;
00269    }
00270 
00271    angleX += 0; //speedX;
00272    angleY += 0; //speedY;
00273    angleZ += 2; //speedZ;
00274 
00275    if(angleX >= SCALE)
00276     angleX -= SCALE;
00277    else if(angleX < 0)
00278     angleX += SCALE;
00279 
00280    if(angleY >= SCALE)
00281     angleY -= SCALE;
00282    else if(angleY < 0)
00283     angleY += SCALE;
00284 
00285    if(angleZ >= SCALE)
00286     angleZ -= SCALE;
00287    else if(angleZ < 0)
00288     angleZ += SCALE;
00289    
00290   }
00291 //wait_ms(delayMs);
00292    wait_ms(1000);
00293 
00294 }
00295