File content as of revision 3:15f457b3bdbd:

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

#include "mbed.h"

#include "LcdController.h"
#include "EaLcdBoard.h"
#include "CubeDemo.h"

#include "Image.h"

//#include "wchar.h"
#include <math.h>


extern const unsigned char cube_image1[];
extern int cube_image1_sz;
extern const unsigned char cube_image2[];
extern int cube_image2_sz;


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

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


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

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




/* (c) 2008 by Denis Markovic
   I give hereby permission to Anders Rosvall of Embedded Artists to use this function in his ARM7 board
   demo program, as long as I'm not hold responsible for bugs in the code and I take not responsibility
   for any damage used by that code :-) */

#define TRIANG_PROJ_SFT 16

static unsigned short DivTab[] =
{0xffff, 0x7fff, 0x5554, 0x3fff, 0x3332, 0x2aa9, 0x2491,
  0x1fff, 0x1c70, 0x1998, 0x1744, 0x1554, 0x13b0, 0x1248, 0x1110,
  0x0fff, 0x0f0e, 0x0e37, 0x0d78, 0x0ccb, 0x0c2f, 0x0ba1, 0x0b20,
  0x0aa9, 0x0a3c, 0x09d7, 0x097a, 0x0923, 0x08d2, 0x0887, 0x0841,
  0x07ff, 0x07c0, 0x0786, 0x074f, 0x071b, 0x06ea, 0x06bb, 0x068f,
  0x0665, 0x063d, 0x0617, 0x05f3, 0x05d0, 0x05af, 0x058f, 0x0571,
  0x0554, 0x0538, 0x051d, 0x0504, 0x04eb, 0x04d3, 0x04bc, 0x04a6,
  0x0491, 0x047c, 0x0468, 0x0455, 0x0443, 0x0431, 0x0420, 0x040f,
  0x03ff, 0x03ef, 0x03df, 0x03d1, 0x03c2, 0x03b4, 0x03a7, 0x039a,
  0x038d, 0x0380, 0x0374, 0x0368, 0x035d, 0x0352, 0x0347, 0x033c,
  0x0332, 0x0328, 0x031e, 0x0314, 0x030b, 0x0302, 0x02f9, 0x02f0,
  0x02e7, 0x02df, 0x02d7, 0x02cf, 0x02c7, 0x02bf, 0x02b8, 0x02b0,
  0x02a9, 0x02a2, 0x029b, 0x0294, 0x028e, 0x0287, 0x0281, 0x027b,
  0x0275, 0x026f, 0x0269, 0x0263, 0x025d, 0x0258, 0x0252, 0x024d,
  0x0248, 0x0242, 0x023d, 0x0238, 0x0233, 0x022f, 0x022a, 0x0225,
  0x0221, 0x021c, 0x0218, 0x0213, 0x020f, 0x020b, 0x0207, 0x0203,
  0x01ff, 0x01fb, 0x01f7, 0x01f3, 0x01ef, 0x01eb, 0x01e8, 0x01e4,
  0x01e0, 0x01dd, 0x01d9, 0x01d6, 0x01d3, 0x01cf, 0x01cc, 0x01c9,
  0x01c6, 0x01c2, 0x01bf, 0x01bc, 0x01b9, 0x01b6, 0x01b3, 0x01b1,
  0x01ae, 0x01ab, 0x01a8, 0x01a5, 0x01a3, 0x01a0, 0x019d, 0x019b,
  0x0198, 0x0196, 0x0193, 0x0191, 0x018e, 0x018c, 0x0189, 0x0187,
  0x0185, 0x0182, 0x0180, 0x017e, 0x017c, 0x0179, 0x0177, 0x0175,
  0x0173, 0x0171, 0x016f, 0x016d, 0x016b, 0x0169, 0x0167, 0x0165,
  0x0163, 0x0161, 0x015f, 0x015d, 0x015b, 0x0159, 0x0157, 0x0156,
  0x0154, 0x0152, 0x0150, 0x014f, 0x014d, 0x014b, 0x0149, 0x0148,
  0x0146, 0x0145, 0x0143, 0x0141, 0x0140, 0x013e, 0x013d, 0x013b,
  0x013a, 0x0138, 0x0137, 0x0135, 0x0134, 0x0132, 0x0131, 0x012f,
  0x012e, 0x012d, 0x012b, 0x012a, 0x0128, 0x0127, 0x0126, 0x0124,
  0x0123, 0x0122, 0x0120, 0x011f, 0x011e, 0x011d, 0x011b, 0x011a,
  0x0119, 0x0118, 0x0117, 0x0115, 0x0114, 0x0113, 0x0112, 0x0111,
  0x0110, 0x010e, 0x010d, 0x010c, 0x010b, 0x010a, 0x0109, 0x0108,
  0x0107, 0x0106, 0x0105, 0x0104, 0x0103, 0x0102, 0x0101, 0x0100,
  0x00ff, 0x00fe, 0x00fd, 0x00fc, 0x00fb, 0x00fa, 0x00f9, 0x00f8,
  0x00f7, 0x00f6, 0x00f5, 0x00f4, 0x00f3, 0x00f2, 0x00f1, 0x00f0,
  0x00ef, 0x00ef, 0x00ee, 0x00ed, 0x00ec, 0x00eb, 0x00ea, 0x00e9,
  0x00e9, 0x00e8, 0x00e7, 0x00e6, 0x00e5, 0x00e4, 0x00e4, 0x00e3,
  0x00e2, 0x00e1, 0x00e0, 0x00e0, 0x00df, 0x00de, 0x00dd, 0x00dd,
  0x00dc, 0x00db, 0x00da, 0x00da, 0x00d9, 0x00d8, 0x00d8, 0x00d7,
  0x00d6, 0x00d5, 0x00d5, 0x00d4, 0x00d3, 0x00d3, 0x00d2, 0x00d1,
  0x00d1, 0x00d0, 0x00cf, 0x00cf, 0x00ce, 0x00cd, 0x00cd, 0x00cc,
  0x00cb, 0x00cb, 0x00ca, 0x00c9, 0x00c9, 0x00c8, 0x00c8, 0x00c7,
  0x00c6, 0x00c6, 0x00c5, 0x00c4, 0x00c4, 0x00c3, 0x00c3, 0x00c2,
  0x00c2, 0x00c1, 0x00c0, 0x00c0, 0x00bf, 0x00bf, 0x00be, 0x00be,
  0x00bd, 0x00bc, 0x00bc, 0x00bb, 0x00bb, 0x00ba, 0x00ba, 0x00b9,
  0x00b9, 0x00b8, 0x00b8, 0x00b7, 0x00b7, 0x00b6, 0x00b6, 0x00b5,
  0x00b5, 0x00b4, 0x00b4, 0x00b3, 0x00b3, 0x00b2, 0x00b2, 0x00b1,
  0x00b1, 0x00b0, 0x00b0, 0x00af, 0x00af, 0x00ae, 0x00ae, 0x00ad,
  0x00ad, 0x00ac, 0x00ac, 0x00ab, 0x00ab, 0x00ab, 0x00aa, 0x00aa,
  0x00a9, 0x00a9, 0x00a8, 0x00a8, 0x00a7, 0x00a7, 0x00a7, 0x00a6,
  0x00a6, 0x00a5, 0x00a5, 0x00a4, 0x00a4, 0x00a4, 0x00a3, 0x00a3,
  0x00a2, 0x00a2, 0x00a2, 0x00a1, 0x00a1, 0x00a0, 0x00a0, 0x00a0,
  0x009f, 0x009f, 0x009e, 0x009e, 0x009e, 0x009d, 0x009d, 0x009c,
  0x009c, 0x009c, 0x009b, 0x009b, 0x009b, 0x009a, 0x009a, 0x0099,
  0x0099, 0x0099, 0x0098, 0x0098, 0x0098, 0x0097, 0x0097, 0x0097,
  0x0096, 0x0096, 0x0096, 0x0095, 0x0095, 0x0094, 0x0094, 0x0094,
  0x0093, 0x0093, 0x0093, 0x0092, 0x0092, 0x0092, 0x0091, 0x0091,
  0x0091, 0x0090, 0x0090, 0x0090, 0x008f, 0x008f, 0x008f, 0x008f,
  0x008e, 0x008e, 0x008e, 0x008d, 0x008d, 0x008d, 0x008c, 0x008c,
  0x008c, 0x008b, 0x008b, 0x008b, 0x008b, 0x008a, 0x008a, 0x008a,
  0x0089, 0x0089, 0x0089, 0x0088, 0x0088, 0x0088, 0x0088, 0x0087,
  0x0087, 0x0087, 0x0086, 0x0086, 0x0086, 0x0086, 0x0085, 0x0085,
  0x0085, 0x0085, 0x0084, 0x0084, 0x0084, 0x0083, 0x0083, 0x0083,
  0x0083, 0x0082, 0x0082, 0x0082, 0x0082, 0x0081, 0x0081, 0x0081,
  0x0081, 0x0080, 0x0080, 0x0080, 0x0080, 0x007f, 0x007f, 0x007f,
};

/* (c) 2008 by Denis Markovic
   I give hereby permission to Anders Rosvall of Embedded Artists to use this function in his ARM7 board
   demo program, as long as I'm not hold responsible for bugs in the code and I take not responsibility
   for any damage used by that code :-) */
void CubeDemo::CpPixel16Fast(int xSrc, int ySrc, int x, int y, Surface_t *SrcRP_p, Surface_t *DstRP_p)
{
  unsigned short *src_p, *dst_p;

  src_p = (unsigned short *) SrcRP_p->pixels;
  dst_p = (unsigned short *) DstRP_p->pixels;

  dst_p[y * DstRP_p->w + x] = src_p[ySrc * SrcRP_p->w + xSrc];

  return;
}

/* (c) 2008 by Denis Markovic
   I give hereby permission to Anders Rosvall of Embedded Artists to use this function in his ARM7 board
   demo program, as long as I'm not hold responsible for bugs in the code and I take not responsibility
   for any damage used by that code :-)

   Input: SrcRP_p: pointer to type holding width and height and ptr to source pixel data
          DstRP_p: pointer to type holding width and height and ptr to destination pixel data (i.e. gfx memory to draw to)
          SrcCoords_p: pointer to array holding 3 source coordinates into triangle source accessed through SrcRP_p
          DstCoords_p: pointer to array holding 3 destination triangle coordinates in draw buffer which is accessed through DstRP_p

   Description: take triangle from source rectangular picture (raw data, 16 bit) and project it into a triangle
                in the destination picture (also 16 bit)
 */
void CubeDemo::TriangleProjectFast(Surface_t *SrcRP_p, Surface_t *DstRP_p, Coord2D_t *SrcCoords_p,  Coord2D_t *DstCoords_p)
{
  int TopCoord = 0, MiddleCoord = 1, BottomCoord = 2;
  int /*SrcStartX, SrcStartY,*/ x, y, Direction, i, j;
  long dxDstBig, dxDstSmall, dxSrcBig, dxSrcSmall, dySrcBig, dySrcSmall, DeltaY;
  int DivMultVal;

  /* 1. step: sort dst triangle points */
  if(DstCoords_p[1].y <  DstCoords_p[TopCoord].y)
  {
    TopCoord = 1;
    MiddleCoord = 0;
  }

  if(DstCoords_p[2].y <  DstCoords_p[TopCoord].y)
  {
    TopCoord = 2;
    BottomCoord = 1;
    MiddleCoord = 0;
  }

  if(DstCoords_p[BottomCoord].y < DstCoords_p[MiddleCoord].y)
  {
    int tmp;
    tmp = MiddleCoord;
    MiddleCoord = BottomCoord;
    BottomCoord = tmp;
  }

  /* so now we have the 3 sorted dst triangle points in TopCoord, MiddleCoord and BottomCoord
     (can be done much more efficient with arithmetics instead of if or use pos/neg direction
     vectors later?) */

  /* 2. step: find start and end points for this line in src and dst triangle for each
     line; start pt always on a line that originates from middle point; later replace
     all div by mult with 1/DeltaY, i.e. only one division, maybe even save 1/... in
     Tab as there are not so many possible DeltaY */
  DeltaY = (long) DstCoords_p[BottomCoord].y - DstCoords_p[TopCoord].y + 1;

  DivMultVal = ((int) DivTab[DeltaY-1] + 1);

  dxDstBig   = ((long) DstCoords_p[BottomCoord].x - DstCoords_p[TopCoord].x + 1) * DivMultVal;
  dySrcBig   = ((long) SrcCoords_p[BottomCoord].y - SrcCoords_p[TopCoord].y + 1) * DivMultVal;
  dxSrcBig   = ((long) SrcCoords_p[BottomCoord].x - SrcCoords_p[TopCoord].x + 1) * DivMultVal;

  DeltaY = (long) DstCoords_p[MiddleCoord].y - DstCoords_p[TopCoord].y + 1;

  DivMultVal = ((int) DivTab[DeltaY-1] + 1);

  dxDstSmall = ((long) DstCoords_p[MiddleCoord].x - DstCoords_p[TopCoord].x + 1) * DivMultVal;
  dySrcSmall = ((long) SrcCoords_p[MiddleCoord].y - SrcCoords_p[TopCoord].y + 1) * DivMultVal;
  dxSrcSmall = ((long) SrcCoords_p[MiddleCoord].x - SrcCoords_p[TopCoord].x + 1) * DivMultVal;

  Direction = 1;
  if(dxDstSmall > dxDstBig)
    Direction = -1;

  for(y = DstCoords_p[TopCoord].y, i=0; y <= DstCoords_p[MiddleCoord].y; y++, i++)
  { /* for each row/line */
    long MoveSrcX, MoveSrcY, P1x, P2x, P1y, P2y, SrcDeltaSteps;

    //SrcStartX = SrcCoords_p[TopCoord].x;

    P1x = SrcCoords_p[TopCoord].x + ((i * dxSrcSmall)>>TRIANG_PROJ_SFT);
    P1y = SrcCoords_p[TopCoord].y + ((i * dySrcSmall)>>TRIANG_PROJ_SFT);
    P2x = SrcCoords_p[TopCoord].x + ((i * dxSrcBig)>>TRIANG_PROJ_SFT);
    P2y = SrcCoords_p[TopCoord].y + ((i * dySrcBig)>>TRIANG_PROJ_SFT);

    {
      int xx;

      x  = DstCoords_p[TopCoord].x + ((i * dxDstSmall)>>TRIANG_PROJ_SFT);
      xx = DstCoords_p[TopCoord].x + ((i * dxDstBig)  >>TRIANG_PROJ_SFT);

      SrcDeltaSteps = xx - x;
      if(SrcDeltaSteps < 0)
    SrcDeltaSteps = -SrcDeltaSteps;

      SrcDeltaSteps++;

      DivMultVal = ((int) DivTab[SrcDeltaSteps-1] + 1);

      MoveSrcX = (P2x - P1x) * DivMultVal;
      MoveSrcY = (P2y - P1y) * DivMultVal;

      x-= Direction;

      P1x <<= TRIANG_PROJ_SFT;
      P1y <<= TRIANG_PROJ_SFT;

      do
      {
    x+= Direction;

    CpPixel16Fast(P1x >>TRIANG_PROJ_SFT, P1y >>TRIANG_PROJ_SFT, x, y, SrcRP_p, DstRP_p);

    P1x += MoveSrcX;
    P1y += MoveSrcY;

      } while(x!=xx);
    }
  }

  /* second part of triangle from middle to bottom */
  DeltaY = (long) DstCoords_p[BottomCoord].y - DstCoords_p[MiddleCoord].y+1;

  DivMultVal = ((int) DivTab[DeltaY-1] + 1);

  dxDstSmall = ((long) DstCoords_p[BottomCoord].x - DstCoords_p[MiddleCoord].x + 1) * DivMultVal;
  dySrcSmall = ((long) SrcCoords_p[BottomCoord].y - SrcCoords_p[MiddleCoord].y + 1) * DivMultVal;
  dxSrcSmall = ((long) SrcCoords_p[BottomCoord].x - SrcCoords_p[MiddleCoord].x + 1) * DivMultVal;

  y--;
  i--;

  for(j=0; y <= DstCoords_p[BottomCoord].y; y++, i++, j++)
  { /* for each row/line */
    long MoveSrcX, MoveSrcY, P1x, P2x, P1y, P2y, SrcDeltaSteps;

    //SrcStartX = SrcCoords_p[MiddleCoord].x;

    P1x = SrcCoords_p[MiddleCoord].x + ((j * dxSrcSmall)>>TRIANG_PROJ_SFT);
    P1y = SrcCoords_p[MiddleCoord].y + ((j * dySrcSmall)>>TRIANG_PROJ_SFT);
    P2x = SrcCoords_p[TopCoord].x + ((i * dxSrcBig)>>TRIANG_PROJ_SFT);
    P2y = SrcCoords_p[TopCoord].y + ((i * dySrcBig)>>TRIANG_PROJ_SFT);

    {
      int xx;

      x  = DstCoords_p[MiddleCoord].x + ((j * dxDstSmall)>>TRIANG_PROJ_SFT);
      xx = DstCoords_p[TopCoord].x    + ((i * dxDstBig)  >>TRIANG_PROJ_SFT);

      SrcDeltaSteps = xx - x;

      /* todo, fixme: direction should not have to be set here but if we do not
         do it here, sometimes calc fails, investigate (problem case: dst coords
         250/299, 321/302, 472/308 and src coord 0/0, bike w-1, 0, bike w-1/bike h-1 */
      if(SrcDeltaSteps < 0)
      {
    SrcDeltaSteps = -SrcDeltaSteps;
    Direction = -1;
      }
      else
    Direction = 1;

      SrcDeltaSteps++;

      DivMultVal = ((int) DivTab[SrcDeltaSteps-1] + 1);

      MoveSrcX = (P2x - P1x) * DivMultVal;
      MoveSrcY = (P2y - P1y) * DivMultVal;

      x-= Direction;

      P1x <<= TRIANG_PROJ_SFT;
      P1y <<= TRIANG_PROJ_SFT;

      do
      {
    x+= Direction;

    CpPixel16Fast(P1x >>TRIANG_PROJ_SFT, P1y >>TRIANG_PROJ_SFT, x, y, SrcRP_p, DstRP_p);

    P1x += MoveSrcX;
    P1y += MoveSrcY;

      } while(x!=xx);
    }
  }

  return;
}


short CubeDemo::_sin(short y) const {
    static short s1 = 0x6487;
    static short s3 = 0x2951;
    static short s5 = 0x4f6;
    long z, prod, sum;

    z = ((long)y * y) >> 12;
    prod = (z * s5) >> 16;
    sum = s3 - prod;
    prod = (z * sum) >> 16;
    sum = s1 - prod;

    // for better accuracy, round here
    return (short)((y * sum) >> 13);
 }

short CubeDemo::_cos(short y) const {
    static short c0 = 0x7fff;
    static short c2 = 0x4eea;
    static short c4 = 0x0fc4;
    long z, prod, sum;
    z = ((long)y * y) >> 12;
    prod = (z * c4) >> 16;
    sum = c2 - prod;

    // for better accuracy, round here
    prod = (z * sum) >> 15;
    return (short)(c0 - prod);
}

short CubeDemo::isine(short x) const {
    unsigned short n = (((unsigned short)x + 0x2000) >> 14) & 0x3;
    x -= n * 0x4000;
    switch(n){
        case 0:
            return _sin(x);
        case 1:
            return _cos(x);
        case 2:
            return - _sin(x);
        case 3:
            return  - _cos(x);
    }
    return 0;
 }


short CubeDemo::icosine(short x) const {
    return isine(x + 0x4000);
 }

int32_t CubeDemo::sgn(int32_t val) const
{
  if (val > 0) return 1;
  else if (val < 0) return -1;
  else return 0;
}

void CubeDemo::createCubeModel(uint32_t radius)
{
  uint32_t i,j;
  uint32_t theta, dTheta;
  static uint8_t cubeConnect[24] = {
    0, 1, 2, 3,
    1, 5, 6, 2,
    5, 4, 7, 6,
    4, 0, 3, 7,
    4, 5, 1, 0,
    3, 2, 6, 7};

  theta  = 0x2000; //PI/4
  dTheta = 0x4000; //PI/2

  for(i=0; i<8; i++)
  {
    cubeModel[i].x = radius * sgn(icosine(theta));
    cubeModel[i].y = radius * sgn(isine(theta));
    cubeModel[i].z = radius - (2 * radius) * (i / 4);
    theta += dTheta;
  }

  j=0;
  for(i=0; i<6; i++)
  {
    cubePoly[j].p1 = cubeConnect[i*4 + 3];
    cubePoly[j].p2 = cubeConnect[i*4 + 1];
    cubePoly[j].p3 = cubeConnect[i*4 + 0];
    j++;
    cubePoly[j].p1 = cubeConnect[i*4 + 3];
    cubePoly[j].p2 = cubeConnect[i*4 + 2];
    cubePoly[j].p3 = cubeConnect[i*4 + 1];
    j++;
  }
}

void CubeDemo::rotateAndProject(uint16_t rotX, uint16_t rotY, uint16_t rotZ)
{
  uint32_t i;
  long crx,srx,cry,sry,crz,srz;
  long x,y,z,tx,ty,tz;
  long distance;

  crx = icosine(rotX);
  srx = isine(rotX);
  cry = icosine(rotY);
  sry = isine(rotY);
  crz = icosine(rotZ);
  srz = isine(rotZ);
  
  for (i=0;i<8;i++)
  {
        //--> rotate around y-axis              
//      var tempX = (this.x * Math.cos(rY)) - (this.z * Math.sin(rY));
//      var tempZ = (this.x * Math.sin(rY)) + (this.z * Math.cos(rY));

        //--> rotate around x-axis      
//      this.dz  =  (tempZ * Math.cos(rX)) - (this.y * Math.sin(rX));
//      var tempY =  (tempZ * Math.sin(rX)) + (this.y * Math.cos(rX));

        //--> rotate around z-axis
//      this.dx =  (tempX * Math.cos(rZ)) + (tempY * Math.sin(rZ));
//      this.dy =  (tempY * Math.cos(rZ)) - (tempX * Math.sin(rZ));
        

    x = cubeModel[i].x;
    y = cubeModel[i].y;
    z = cubeModel[i].z;
    
        //--> rotate around y-axis
    tx = (z*sry + x*cry) / 0x7fff;
    tz = (z*cry - x*sry) / 0x7fff;

        //--> rotate around x-axis
    cubeModel[i].zr = (y*srx + tz*crx) / 0x7fff;
    ty              = (y*crx - tz*srx) / 0x7fff;

        //--> rotate around z-axis
    cubeModel[i].xr = (tx*crz - ty*srz) / 0x7fff;
    cubeModel[i].yr = (tx*srz + ty*crz) / 0x7fff;

    cubeModel[i].xr -= camX;
    cubeModel[i].yr -= camY;
    cubeModel[i].zr -= camZ;

    distance = LENS - cubeModel[i].zr;
    if (distance > 0)
    {
      cubeModel[i].scrx = X_OFFSET + (LENS * cubeModel[i].xr) / distance;
      cubeModel[i].scry = Y_OFFSET + (LENS * cubeModel[i].yr) / distance;
    }
  }
}

void CubeDemo::drawCubeZ(Surface_t *pSourcePicture, uint8_t shades)
{
  uint32_t i;
  int32_t  x1,x2,x3,y1,y2,y3,/*u1,u2,u3,v1,v2,v3,*/znormal;
  
  activeFrame.w = this->windowX;
  activeFrame.h = this->windowY;
  activeFrame.pixels = (uint16_t*)this->pFrmBuf;

  for(i=0; i<12; i++)
  {
    x1 = cubeModel[cubePoly[i].p1].scrx;  //Get triangles from "projected"
    x2 = cubeModel[cubePoly[i].p2].scrx;  //X and Y coords since Znormal
    x3 = cubeModel[cubePoly[i].p3].scrx;  //Does not require a Z coord
    y1 = cubeModel[cubePoly[i].p1].scry;  //V1= Point1 connected to V2 then
    y2 = cubeModel[cubePoly[i].p2].scry;  //V2 to V3 and so on...
    y3 = cubeModel[cubePoly[i].p3].scry;

    znormal = (x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3);
    if (znormal > 0)
    {
      //no texture?
      if (pSourcePicture == NULL)
      {
        if (shades == 0)
        {
          if ((cubePoly[i].p1 == 0) && (cubePoly[i].p2 == 1))
            graphics.put_line( cubeModel[cubePoly[i].p1].scrx, cubeModel[cubePoly[i].p1].scry, cubeModel[cubePoly[i].p2].scrx, cubeModel[cubePoly[i].p2].scry, RED);
          else
            graphics.put_line( cubeModel[cubePoly[i].p1].scrx, cubeModel[cubePoly[i].p1].scry, cubeModel[cubePoly[i].p2].scrx, cubeModel[cubePoly[i].p2].scry, SMALL_CIRCLE_FRONT_COLOR);
          graphics.put_line( cubeModel[cubePoly[i].p2].scrx, cubeModel[cubePoly[i].p2].scry, cubeModel[cubePoly[i].p3].scrx, cubeModel[cubePoly[i].p3].scry, SMALL_CIRCLE_FRONT_COLOR);
          graphics.put_line( cubeModel[cubePoly[i].p3].scrx, cubeModel[cubePoly[i].p3].scry, cubeModel[cubePoly[i].p1].scrx, cubeModel[cubePoly[i].p1].scry, SMALL_CIRCLE_FRONT_COLOR);
        }
        else
        {
          Coord2D_t SrcCoords[3], DstCoords[3];
          Surface_t a;
          static uint16_t pix;

          a.w = this->windowX;
          a.h = this->windowY;
          a.pixels = &pix;
          if ((i&1) == 0)
          {
            if ((znormal / 400) > 31)
              pix = 0x1f << 11;
            else
              pix = (znormal / 400 & 0x1f) << 11;
          }
          SrcCoords[0].x = 0;
          SrcCoords[0].y = 0;
          SrcCoords[1].x = 0;
          SrcCoords[1].y = 0;
          SrcCoords[2].x = 0;
          SrcCoords[2].y = 0;
          DstCoords[0].x = cubeModel[cubePoly[i].p1].scrx;
          DstCoords[0].y = cubeModel[cubePoly[i].p1].scry;
          DstCoords[1].x = cubeModel[cubePoly[i].p2].scrx;
          DstCoords[1].y = cubeModel[cubePoly[i].p2].scry;
          DstCoords[2].x = cubeModel[cubePoly[i].p3].scrx;
          DstCoords[2].y = cubeModel[cubePoly[i].p3].scry;
          TriangleProjectFast(&a, &activeFrame, SrcCoords, DstCoords);
        }
      }
      
      //Render with texture
      else
      {
        Coord2D_t SrcCoords[3], DstCoords[3];
#if 1
        /*
         *       Image in the file
         *  0,0_______ _______ _______
         *    |       |       |      *|                   
         *    |       |       |       |                   Image as it is used
         *    |_______|_______|_______|                 _______ _______ _______ _______
         *    |       |       |       |        \       |*      |       |       |       |
         *    |       |       |       |         \      |       | top   |       |       |
         *    |_______|_______|_______|    ======      |_______|_______|_______|_______|
         *    |       |       |       |         /      |       |       |       |       |
         *    |       |       |       |        /       | side1 | side2 | side3 | side4 |
         *    |_______|_______|_______|                |_______|_______|_______|_______|
         *    |       |       |       |                |       |       |       |       |
         *    |       |       |       |                |       | bottom|       |       |
         *    |_______|_______|_______|                |_______|_______|_______|_______|
         *                           150,200
         *  All coordinates below are in the left image above and with coordinates 0,0 in the
         *  upper left corner and 150,200 in the lower right corner.
         *
         */
#define XYZ  {49,0},{0,0},{0,50},{50,50}
        static Coord2D_t a[24] = {{49,50}, {0,50},  {0,99},  {49,99},            //bottom  6
                                  {99,50}, {50,50}, {50,99}, {99,99},            //side2   2
                                  {149,50},{100,50},{100,99},{149,99},           //top     5
                                  {50,150},{99,150},{99,199},{50,199},           //side4   4
                                  {99,49}, {99,0},  {50,0},  {50,49},            //side1   1
                                  {99,100},{99,149},{50,149},{50,100}};          //side3   3
#elif 0
        static Coord2D_t a[24] = {{49,50}, {0,50},  {0,99},  {49,99},            //bottom  6
                                  {99,50}, {50,50}, {50,99}, {99,99},            //side2   2
                                  {149,50},{100,50},{100,99},{149,99},           //top     5
                                  {99,150},{50,150},{50,199},{99,199},           //side4   4
                                  {99,49}, {99,0},  {50,0},  {50,49},            //side1   1
                                  {99,149},{99,100},{50,100},{50,149}};          //side3   3
#else                                  
        static Coord2D_t a[24] = {{49,50}, {0,50},  {0,99},  {49,99},
                                  {99,50}, {50,50}, {50,99}, {99,99},
                                  {149,50},{100,50},{100,99},{149,99},
                                  {99,199},{50,199},{50,150},{99,150},
                                  {50,0},  {99,0},  {99,49}, {50,49},  //top
                                  {50,100},{99,100},{99,149},{50,149}};//bottom
#endif                                  

        if (i&1)
        {
          SrcCoords[0].x = a[(i/2)*4+2].x;
          SrcCoords[0].y = a[(i/2)*4+2].y;
          SrcCoords[1].x = a[(i/2)*4+3].x;
          SrcCoords[1].y = a[(i/2)*4+3].y;
          SrcCoords[2].x = a[(i/2)*4+0].x;
          SrcCoords[2].y = a[(i/2)*4+0].y;
          DstCoords[0].x = cubeModel[cubePoly[i].p1].scrx;
          DstCoords[0].y = cubeModel[cubePoly[i].p1].scry;
          DstCoords[1].x = cubeModel[cubePoly[i].p2].scrx;
          DstCoords[1].y = cubeModel[cubePoly[i].p2].scry;
          DstCoords[2].x = cubeModel[cubePoly[i].p3].scrx;
          DstCoords[2].y = cubeModel[cubePoly[i].p3].scry;
          TriangleProjectFast(pSourcePicture, &activeFrame, SrcCoords, DstCoords);
        }
        else
        {
          SrcCoords[0].x = a[(i/2)*4+0].x; //0
          SrcCoords[0].y = a[(i/2)*4+0].y; //0
          SrcCoords[1].x = a[(i/2)*4+1].x; //31;
          SrcCoords[1].y = a[(i/2)*4+1].y; //0;
          SrcCoords[2].x = a[(i/2)*4+2].x; //31;
          SrcCoords[2].y = a[(i/2)*4+2].y; //31;
          DstCoords[0].x = cubeModel[cubePoly[i].p2].scrx;
          DstCoords[0].y = cubeModel[cubePoly[i].p2].scry;
          DstCoords[1].x = cubeModel[cubePoly[i].p3].scrx;
          DstCoords[1].y = cubeModel[cubePoly[i].p3].scry;
          DstCoords[2].x = cubeModel[cubePoly[i].p1].scrx;
          DstCoords[2].y = cubeModel[cubePoly[i].p1].scry;
          TriangleProjectFast(pSourcePicture, &activeFrame, SrcCoords, DstCoords);
        }
      }
    }
  }
}

void CubeDemo::render(uint32_t idx)
{
 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);

  // rendering here
  memset((void*)(pFrmBuf), BACKGROUND_COLOR, this->windowX * this->windowY * 2);

  rotateAndProject(rx, ry, 0);

  switch(mode)
  {
  case 0: //draw just points
    graphics.put_circle( cubeModel[0].scrx, cubeModel[0].scry, RED,                      2, 1);
    graphics.put_circle( cubeModel[1].scrx, cubeModel[1].scry, SMALL_CIRCLE_FRONT_COLOR, 2, 1);
    graphics.put_circle( cubeModel[2].scrx, cubeModel[2].scry, GREEN,                    2, 1);
    graphics.put_circle( cubeModel[3].scrx, cubeModel[3].scry, SMALL_CIRCLE_FRONT_COLOR, 2, 1);
    graphics.put_circle( cubeModel[4].scrx, cubeModel[4].scry, SMALL_CIRCLE_FRONT_COLOR, 2, 1);
    graphics.put_circle( cubeModel[5].scrx, cubeModel[5].scry, SMALL_CIRCLE_FRONT_COLOR, 2, 1);
    graphics.put_circle( cubeModel[6].scrx, cubeModel[6].scry, SMALL_CIRCLE_FRONT_COLOR, 2, 1);
    graphics.put_circle( cubeModel[7].scrx, cubeModel[7].scry, SMALL_CIRCLE_FRONT_COLOR, 2, 1);
    break;
  case 1: //draw lines points
    graphics.put_line( cubeModel[0].scrx, cubeModel[0].scry, cubeModel[1].scrx, cubeModel[1].scry, RED);
    graphics.put_line( cubeModel[1].scrx, cubeModel[1].scry, cubeModel[2].scrx, cubeModel[2].scry, SMALL_CIRCLE_FRONT_COLOR);
    graphics.put_line( cubeModel[2].scrx, cubeModel[2].scry, cubeModel[3].scrx, cubeModel[3].scry, GREEN);
    graphics.put_line( cubeModel[3].scrx, cubeModel[3].scry, cubeModel[0].scrx, cubeModel[0].scry, SMALL_CIRCLE_FRONT_COLOR);

    graphics.put_line( cubeModel[5].scrx, cubeModel[5].scry, cubeModel[4].scrx, cubeModel[4].scry, SMALL_CIRCLE_FRONT_COLOR);
    graphics.put_line( cubeModel[4].scrx, cubeModel[4].scry, cubeModel[7].scrx, cubeModel[7].scry, SMALL_CIRCLE_FRONT_COLOR);
    graphics.put_line( cubeModel[7].scrx, cubeModel[7].scry, cubeModel[6].scrx, cubeModel[6].scry, SMALL_CIRCLE_FRONT_COLOR);
    graphics.put_line( cubeModel[6].scrx, cubeModel[6].scry, cubeModel[5].scrx, cubeModel[5].scry, SMALL_CIRCLE_FRONT_COLOR);

    graphics.put_line( cubeModel[0].scrx, cubeModel[0].scry, cubeModel[4].scrx, cubeModel[4].scry, SMALL_CIRCLE_FRONT_COLOR);
    graphics.put_line( cubeModel[1].scrx, cubeModel[1].scry, cubeModel[5].scrx, cubeModel[5].scry, SMALL_CIRCLE_FRONT_COLOR);
    graphics.put_line( cubeModel[2].scrx, cubeModel[2].scry, cubeModel[6].scrx, cubeModel[6].scry, SMALL_CIRCLE_FRONT_COLOR);
    graphics.put_line( cubeModel[3].scrx, cubeModel[3].scry, cubeModel[7].scrx, cubeModel[7].scry, SMALL_CIRCLE_FRONT_COLOR);
    break;
  case 2: //draw cube with texture
    if (sourcePicture1.pixels == NULL)
    {
      drawCubeZ( NULL, false);
    }
    else
    {
      drawCubeZ( &sourcePicture1, false);
    }
    break;
  case 3: //draw cube with texture
    drawCubeZ( NULL, true);
    break;
  case 4: //draw cube with texture
    if (sourcePicture2.pixels == NULL)
    {
      drawCubeZ( NULL, false);
    }
    else
    {
      drawCubeZ( &sourcePicture2, false);
    }
    break;
  case 5: //draw sorted lines
    drawCubeZ( NULL, false);
    break;
  default:
    mode = 0;
    break;
  }
  ry += ry_;
  rx += rx_;
}

/***********************************************************************
 * Private functions
 **********************************************************************/

void CubeDemo::initialize()
{
    mode = 3;
    rx   = 0x9000;
    ry   = 0;
    ry_  = 350;
    rx_  = -25; //-3
    camX = 0;
    camY = 0;
    camZ = 128;

    createCubeModel(70);

    Image::ImageData_t d;
    if (Image::decode(cube_image1, cube_image1_sz, &d) == 0) {
        sourcePicture1.w = d.width;
        sourcePicture1.h = d.height;
        sourcePicture1.pixels = d.pixels;
    }
    if (Image::decode(cube_image2, cube_image2_sz, &d) == 0) {
        sourcePicture2.w = d.width;
        sourcePicture2.h = d.height;
        sourcePicture2.pixels = d.pixels;
    }
}


/******************************************************************************
 * Public functions
 *****************************************************************************/
CubeDemo::CubeDemo(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);

    sourcePicture1.w = 0;
    sourcePicture1.h = 0;
    sourcePicture1.pixels = NULL;
    sourcePicture2.w = 0;
    sourcePicture2.h = 0;
    sourcePicture2.pixels = NULL;
        
    initialize();
}

CubeDemo::~CubeDemo()
{
  if (sourcePicture1.pixels != NULL) {
    free(sourcePicture1.pixels);
  }
  if (sourcePicture2.pixels != NULL) {
    free(sourcePicture2.pixels);
  }
}
void CubeDemo::run(uint32_t loops, uint32_t delayMs) {

  printf("CubeDemo, %d loops, %dms delay\n", loops, delayMs);
  
    for(int32_t n=0;n<loops;n++) {
    
        mode = ((n/128) % 5);
        //mode = 4;
    
        //render globe
        render(n);
    
        //update framebuffer
        lcdBoard.setFrameBuffer((uint32_t)this->pFrmBuf);

        if (abortTest) {
            break;
        }      
      
        if (mode == 3)
            wait_ms(delayMs-7);
        else
            wait_ms(delayMs);
    }
}