File content as of revision 12:e99cc1e9d928:

#define N_PTS 500
#define N_LNS 2000
#define N_HEIGHTS 36
#define N_SIDE 6

#include "gfx.h"
#include "mbed.h"
#include "main.h"

int n_pts = 0;
int n_lns = 0;
int n_frame = 0;
int n_cubes = 0;

char gfx_stat[40];

point_t* pts;
line_t* lns;
cube_t* cubes;
point_t temp_point;

float heights[N_HEIGHTS];

float c_i[3] = {-.1, -.1, -.1};
float c1i[3] = {.1, -.1, -.1};
float c2i[3] = {-.1, .1, -.1};
float c3i[3] = {-.1, -.1, .1};
float c_j[3] = {.1, .1, .1};
float c1j[3] = {.1, .1, -.1};
float c2j[3] = {-.1, .1, .1};
float c3j[3] = {.1, -.1, .1};
float x_rotmat[3][3] = {{1, 0, 0}, {0, cos(.02), -sin(.02)}, {0, sin(.02), cos(.02)}};
float y_rotmat[3][3] = {{cos(.03), 0, sin(.03)}, {0, 1, 0}, { -sin(.03), 0, cos(.03)}};
float z_rotmat[3][3] = {{cos(.02), -sin(.02), 0}, {sin(.02), cos(.02), 0}, {0, 0, 1}};
float temp[3] = {0, 0, 0};
float temp2[3] = {0, 0, 0};

float x_off = 0.f;
float y_off = 0.f;
float pitch = 0.f;
float yaw = 0.f;

unsigned int m_z=12434,m_w=33254;
void draw_lines();
 
unsigned int rnd() {
    m_z = 36969 * (m_z & 65535) + (m_z >>16);
    m_w = 18000 * (m_w & 65535) + (m_w >>16);
    return ((m_z <<16) + m_w);
}



void mat_vec_mult(float a[][3], float *b, float *c, uint8_t inv) {
  for (int i = 0; i < 3; i++) {
    if(inv)
        c[i] =  (a[0][i] * b[0] + a[1][i] * b[1] + a[2][i] * b[2]);
    else
        c[i] =  (a[i][0] * b[0] + a[i][1] * b[1] + a[i][2] * b[2]);
  }
}

void point_mult(float a[][3], point_t* b, float* c, uint8_t inv)
{
      for (int i = 0; i < 3; i++) {
    if(inv)
    {
        c[i] =  (a[0][i] * b->x + a[1][i] * b->y + a[2][i] * b->z);
    }
    else
    {
        c[i] =  (a[i][0] * b->x + a[i][1] * b->y + a[i][2] * b->z);
    }
  }
}



void add_cube()
{
    
}

char* get_gfx_stat()
{
    n_frame++;
    //sprintf(gfx_stat,"%d/%d, %d/%d, %d",n_lns,N_LNS,n_pts,N_PTS,n_frame%1000);
    sprintf(gfx_stat,"%.3f/%.3f",pitch,yaw);
    return gfx_stat;
}

point_t* new_point()
{
    if(n_pts >= N_PTS - 1) return NULL;
    return &pts[n_pts++];
}

line_t* new_linet()
{
    if(n_lns >= N_LNS - 1) return NULL;
    return &lns[n_lns++];
}

void rotate_cube(uint8_t x, uint8_t y, uint8_t z,uint8_t inv);

unsigned int PRNG()
{
    // our initial starting seed is 5323
    static unsigned int nSeed = 5323;

    // Take the current seed and generate a new value from it
    // Due to our use of large constants and overflow, it would be
    // very hard for someone to predict what the next number is
    // going to be from the previous one.
    nSeed = (8253729 * nSeed + 2396403); 

    // Take the seed and return a value between 0 and 32767
    return nSeed  % 32767;
}

void update_x_rot();
void new_frame(float pitch_rate, float yaw_rate)
{
    pitch += pitch_rate * .05f;
    yaw += yaw_rate * .05f;
    //printf("yaw: %.3f \r\n",yaw);
//    if(c == 'w') y_off += .01f;
//    if(c == 's') y_off -= .01f;
//    if(c == 'a') x_off += .01f;
//    if(c == 'd') x_off -= .01f;
//    if(c == 'q') pitch += .01f;
//    if(c == 'e') pitch -= .01f;
    
    update_x_rot();
    draw_lines();

}

void rotate_cube(uint8_t x, uint8_t y, uint8_t z, uint8_t inv)
{
      if (x) {
    //multiply the cube point by the rotation matrix and store the array in temp
    mat_vec_mult(x_rotmat, c_i, temp,inv);
    //copy temp vector into point vector, updating it.
    memcpy(c_i, temp, sizeof(c_i));
    mat_vec_mult(x_rotmat, c1i, temp,inv);
    memcpy(c1i, temp, sizeof(c1i));
    mat_vec_mult(x_rotmat, c2i, temp,inv);
    memcpy(c2i, temp, sizeof(c2i));
    mat_vec_mult(x_rotmat, c3i, temp,inv);
    memcpy(c3i, temp, sizeof(c3i));
    mat_vec_mult(x_rotmat, c_j, temp,inv);
    memcpy(c_j, temp, sizeof(c_j));
    mat_vec_mult(x_rotmat, c1j, temp,inv);
    memcpy(c1j, temp, sizeof(c1j));
    mat_vec_mult(x_rotmat, c2j, temp,inv);
    memcpy(c2j, temp, sizeof(c2j));
    mat_vec_mult(x_rotmat, c3j, temp,inv);
    memcpy(c3j, temp, sizeof(c3j));
  }
  if (y) {

    mat_vec_mult(y_rotmat, c_i, temp,inv);
    memcpy(c_i, temp, sizeof(c_i));
    mat_vec_mult(y_rotmat, c1i, temp,inv);
    memcpy(c1i, temp, sizeof(c1i));
    mat_vec_mult(y_rotmat, c2i, temp,inv);
    memcpy(c2i, temp, sizeof(c2i));
    mat_vec_mult(y_rotmat, c3i, temp,inv);
    memcpy(c3i, temp, sizeof(c3i));
    mat_vec_mult(y_rotmat, c_j, temp,inv);
    memcpy(c_j, temp, sizeof(c_j));
    mat_vec_mult(y_rotmat, c1j, temp,inv);
    memcpy(c1j, temp, sizeof(c1j));
    mat_vec_mult(y_rotmat, c2j, temp,inv);
    memcpy(c2j, temp, sizeof(c2j));
    mat_vec_mult(y_rotmat, c3j, temp,inv);
    memcpy(c3j, temp, sizeof(c3j));
  }
  //
  if (z) {
    mat_vec_mult(z_rotmat, c_i, temp,inv);
    memcpy(c_i, temp, sizeof(c_i));
    mat_vec_mult(z_rotmat, c1i, temp,inv);
    memcpy(c1i, temp, sizeof(c1i));
    mat_vec_mult(z_rotmat, c2i, temp,inv);
    memcpy(c2i, temp, sizeof(c2i));
    mat_vec_mult(z_rotmat, c3i, temp,inv);
    memcpy(c3i, temp, sizeof(c3i));
    mat_vec_mult(z_rotmat, c_j, temp,inv);
    memcpy(c_j, temp, sizeof(c_j));
    mat_vec_mult(z_rotmat, c1j, temp,inv);
    memcpy(c1j, temp, sizeof(c1j));
    mat_vec_mult(z_rotmat, c2j, temp,inv);
    memcpy(c2j, temp, sizeof(c2j));
    mat_vec_mult(z_rotmat, c3j, temp,inv);
    memcpy(c3j, temp, sizeof(c3j));
  }
}

void init_gfx()
{
    new_line();
    draw_vincent_string("GFX START!");
    pts = (point_t*)malloc(N_PTS * sizeof(point_t));
    lns = (line_t*)malloc(N_LNS * sizeof(line_t)); 
    cubes = (cube_t*)malloc(10 * sizeof(cube_t));
    
    for(int i = 0; i < N_HEIGHTS; i++)
    {
        uint32_t a = rnd();
        heights[i] = .2f * (float)a / (float)INT_MAX;
        new_point();
        pts[i].z = heights[i];
        pts[i].y = 0.001f;
        pts[i].x = 0.001f;
        //sprintf("%f\n",heights[i]);
    }
    
    
    for(int xi = 0; xi < N_SIDE; xi++)
    {
        for(int yi = 0; yi < N_SIDE; yi++)
        {
            // base
            int pti = xi + N_SIDE*yi;
            pts[pti].x = -.5f + .7f*( (float) (xi) )/( (float) (N_SIDE) ) + .15f;
            pts[pti].y = -.5f + .7f*( (float) (yi) )/( (float) (N_SIDE) ) + .15f;
            // go x+?
            if(  (xi < (N_SIDE - 1)))
            {
                int ptn = (xi+1) + N_SIDE*yi;
                line_t* lin = new_linet();
                lin->a = pts + pti;
                lin->b = pts + ptn;    
            }
            // go x-?
            if( (xi > 0))
            {
                int ptn = (xi-1) + N_SIDE*yi;
                line_t* lin = new_linet();
                lin->a = pts + pti;
                lin->b = pts + ptn;    
            }      
            
            // go y+
            if(  (yi < (N_SIDE - 1)))
            {
                int ptn = (xi) + N_SIDE*(1+yi);
                line_t* lin = new_linet();
                lin->a = pts + pti;
                lin->b = pts + ptn;    
            }
            
            // go y-?
            if( (yi > 0))
            {
                int ptn = (xi) + N_SIDE*(yi - 1);
                line_t* lin = new_linet();
                lin->a = pts + pti;
                lin->b = pts + ptn;    
            }    
            
                     
        }
    }
}

void bring_in_range()
{
    while(x_off > .5f) x_off = x_off - .25f;
    while(x_off < -.5f) x_off = x_off + .25f;
    while(y_off > .5f) y_off = y_off - .25f;
    while(y_off < -.5f) y_off = y_off + .25f;

}

void update_x_rot()
{
 x_rotmat[1][1] = cos(pitch);
 x_rotmat[1][2] = -sin(pitch);
 x_rotmat[2][1] = sin(pitch);
 x_rotmat[2][2] = cos(pitch);
 
 y_rotmat[0][0] = cos(yaw);
 y_rotmat[0][2] = sin(yaw);
 y_rotmat[2][0] -sin(yaw);
 y_rotmat[2][2] = cos(yaw);
 //float y_rotmat[3][3] = {{cos(.03), 0, sin(.03)}, {0, 1, 0}, { -sin(.03), 0, cos(.03)}};
 
 
 //float x_rotmat[3][3] = {{1, 0, 0}, {0, cos(.02), -sin(.02)}, {0, sin(.02), cos(.02)}};
}
 
void draw_lines()
{
//    for(int i = 0; i < n_pts; i++)
//    {
//        point_mult(x_rotmat,pts + i,temp,0);
//        pts[i].x = temp[0];
//        pts[i].y = temp[1];
//        pts[i].z = temp[2];   
//        point_mult(y_rotmat,pts + i,temp,0);
//        pts[i].x = temp[0];
//        pts[i].y = temp[1];
//        pts[i].z = temp[2];   
//    }
    
    for(int i = 0; i < n_lns; i++)
    {
        line_t* l = lns + i;
        point_t* a = l->a;
        point_t* b = l->b;
        //point_mult(x_rotmat,a,temp,0);
        //point_mult(x_rotmat,b,temp2,0);
        bring_in_range();
        
        point_mult(x_rotmat,a,temp,0);
        temp_point.x = temp[0];
        temp_point.y = temp[1];
        temp_point.z = temp[2];
        point_mult(y_rotmat,&temp_point,temp2,0);
        float x1 = temp2[0] + x_off;
        float y1 = temp2[1] + y_off;
        
        point_mult(x_rotmat,b,temp,0);
        temp_point.x = temp[0];
        temp_point.y = temp[1];
        temp_point.z = temp[2];
        point_mult(y_rotmat,&temp_point,temp2,0);
        float x2 = temp2[0] + x_off;
        float y2 = temp2[1] + y_off;
        
        
//        float x1 = temp[0] + x_off;
//        float x2 = temp2[0] + x_off;
//        float y1 = temp[1] + y_off;
//        float y2 = temp2[1] + y_off;

        draw_gfx_line(x1,y1,x2,y2);
        //printf("i: %d, a: %d, b: %d\n",i,l->a,l->b);
        //printf("%.3f, %.3f, %.3f, %.3f\n",l->a->x,l->a->y,l->b->x,l->b->y);
        //draw_gfx_line(l->a->x,l->a->y,l->b->x,l->b->y);
        
    }   
}