Diff: WaveDemo.cpp

Revision:

0:d1e4929f6956

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WaveDemo.cpp	Fri Oct 03 13:37:30 2014 +0000
@@ -0,0 +1,295 @@
+/******************************************************************************
+ * Includes
+ *****************************************************************************/
+
+#include "mbed.h"
+
+#include "LcdController.h"
+#include "EaLcdBoard.h"
+#include "WaveDemo.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
+ *****************************************************************************/
+
+
+/******************************************************************************
+ * Local functions
+ *****************************************************************************/
+
+uint16_t WaveDemo::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 WaveDemo::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 WaveDemo::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 WaveDemo::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
+ *****************************************************************************/
+WaveDemo::WaveDemo(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 WaveDemo::run(EaLcdBoardGPIO& lcdBoard, uint32_t loops, uint32_t delayMs)
+{
+  printf("WaveDemo, %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;
+   
+  }
+//wait_ms(delayMs);
+   wait_ms(1000);
+
+}
+