SphereDemo.cpp

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