Diff: shape_drawer.cpp

Revision:

5:495d64d8934d

Parent:

4:75498bd2e742

Child:

6:baece3338bfe

diff -r 75498bd2e742 -r 495d64d8934d shape_drawer.cpp
--- a/shape_drawer.cpp	Fri May 01 03:26:50 2015 +0000
+++ b/shape_drawer.cpp	Fri May 01 05:25:19 2015 +0000
@@ -1,3 +1,280 @@
 #include "constants.h"
+#include "mbed.h"
+#include <math.h>
+
+static char work_buffer[SLICES][WIDTH];
+
+static void erase(){
+    for(int i = 0; i < SLICES; i++){
+        for(int j = 0; j < WIDTH; j++){
+            work_buffer[i][j] = 0x00;    
+        }    
+    }    
+}
+
+void draw_circle(float r, float h){    
+    for(int i = 0; i < SLICES; i++){
+        work_buffer[i][(int)rint(r)] |= (0x01 << (int)rint(h));    
+    }   
+}
+
+void draw_wavecircle(float r, float freq, float disp){
+    for(int i = 0; i < SLICES; i++){
+        float h = 3.5*sin(2.0 * M_PI * (i + disp) * freq / SLICES) + 3.5;
+        work_buffer[i][(int)rint(r)] |= (0x01 << (int)rint(h));
+    }      
+}
+
+#define VERT 1
+#define HORZ 0
+
+static void draw_zvert_line(float x, float y, float z1, float z2);
+static void draw_orig_line(float x1, float x2, float z1, float z2, int ori);
+static void draw_perp_line(float a1, float a2, float b, float z1, float z2, int ori);
+static void draw_arb_line(float x1, float x2, float y1, float y2, float z1, float z2);
+
+static void norm_angle(float &p){
+    p = p < 0 ? p + 2 * M_PI : p;  
+    p = p > 2 * M_PI ? p - 2 * M_PI : p;     
+}
+
+static float deg2rad(float p){
+    return 2 * M_PI * p / SLICES;    
+}
+
+static float rad2deg(float p){
+    return p * SLICES / (2 * M_PI);    
+}
+
+static void step_i(float a, float b, int &i){
+    float diff = abs(a - b);
+    if(diff > SLICES / 2)
+        i = a > b ? i + 1: i - 1;
+    else
+        i = a > b ? i - 1: i + 1; 
+        
+    i = (i == SLICES) ? 0 : i;
+    i = (i == -1) ? SLICES - 1 : i;
+}
+
+void draw_line(float x1, float y1, float z1, float x2, float y2, float z2){
+    
+    //Perfectly vertical depth lines
+    if(x1 == x2 && y1 == y2){
+        draw_zvert_line(x1, y1, z1, z2);    
+    }
+    
+    //Perfectly horizontal lines through origin
+    else if(y1 == 0 && y2 == 0){
+        draw_orig_line(x1, x2, z1, z2, HORZ);        
+    }
+    
+    //Perfectly vertical lines through origin
+    else if(x1 == 0 && x2 == 0){
+        draw_orig_line(y1, y2, z1, z2, VERT);        
+    }
+      
+    //Perfectly horizontal lines
+    else if(y1 == y2){
+        draw_perp_line(x1, x2, y1, z1, z2, HORZ);   
+    }
+    
+    //Perfectly vertical lines
+    // r = b / r cos p
+    else if(x1 == x2){
+        draw_perp_line(y1, y2, x1, z1, z2, VERT);
+    }
+    
+    //Arbitrary lines
+    else {
+        draw_arb_line(x1, x2, y1, y2, z1, z2);
+    }     
+}
 
-static char 
\ No newline at end of file
+static void draw_arb_line(float x1, float x2, float y1, float y2, float z1, float z2){
+    // y = mx + b, 
+    // r = b / (sin p - m cos p)
+    // 
+    // if m is close to infinity, better to use 
+    // x = ny + c
+    // r = c / (cos p - n sin p)
+    
+    float m_1 = 1.0*(y2 - y1)/(x2 - x1); 
+    float m_2 = 1.0*(x2 - x1)/(y2 - y1);
+    float d = sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1));
+    float m_h = (z2 - z1)/d;
+    float m, p1, p2, b, h;
+    int ori;
+        
+    //x = m_2 y + b is steeper than y = m_1 x + b
+    if(abs(m_2) > abs(m_1)){
+        m = m_1;
+        b = y1 - m * x1;  
+        ori = HORZ; 
+    } else {
+        m = m_2;    
+        b = x1 - m * y1;
+        ori = VERT;
+    }
+    
+    p1 = atan2(y1, x1);
+    p2 = atan2(y2, x2);
+        
+    norm_angle(p1);
+    norm_angle(p2);
+        
+    h = z1;
+        
+    int i1 = rad2deg(p1);
+    int i2 = rad2deg(p2);
+        
+    while(true){
+        float p = deg2rad(i1);
+        float r = (ori == HORZ) ? b / (sin(p) - m * cos(p)) : b / (cos(p) - m * sin(p));
+            
+        float dx = r * cos(p);
+        float dy = r * sin(p);
+            
+        float dd = sqrt((x1 - dx)*(x1 - dx) + (y1 - dy)*(y1 - dy));
+        h = z1 + m_h * dd;
+            
+        if((int)rint(r) < WIDTH)
+            work_buffer[i1][(int)rint(r)] |= (0x01 << (int)rint(h));
+            
+        if(i1 == i2)
+            break;
+                
+        step_i(i1, i2, i1);
+    }    
+}
+
+static void draw_perp_line(float a1, float a2, float b, float z1, float z2, int ori){
+    float p1 = (ori == HORZ) ? atan2(b, a1) : atan2(a1, b);
+    float p2 = (ori == HORZ) ? atan2(b, a2) : atan2(a2, b); 
+    
+    norm_angle(p1);
+    norm_angle(p2);
+    
+    int i1 = rad2deg(p1);
+    int i2 = rad2deg(p2);
+    
+    float m = (z2 - z1)/(a2 - a1);
+    
+    while(true){    
+        float p = deg2rad(i1);
+        float r = (ori == HORZ) ? b / sin(p) : b / cos(p);
+        float d = (ori == HORZ) ? b / tan(p) : b * tan(p);
+        float h = z1 + m * (d - a1);
+        
+        if((int)rint(r) < WIDTH)
+            work_buffer[i1][(int)rint(r)] |= (0x01 << (int)rint(h));
+        
+        if(i1 == i2)
+            break;
+            
+        step_i(i1, i2, i1);
+    }    
+}
+
+static void draw_orig_line(float r1, float r2, float z1, float z2, int ori){
+    int p1, p2;
+    float m;
+    
+    if(ori == HORZ){
+        p1 = 0;
+        p2 = SLICES / 2 - 1;
+        m = (1.0*z2 - 1.0*z1)/(1.0*r2 - 1.0*r1);
+    } else {
+        p1 = SLICES / 4 - 1;
+        p2 = 3 * SLICES / 4 - 1;
+        m = (1.0*z2 - 1.0*z1)/(1.0*r2 - 1.0*r1);    
+    }
+    
+    int i = r1;
+    while(true){
+        float h = z1 + m * (i - r1);
+        
+        if(i >= 0){
+            work_buffer[p1][i] |= (0x01 << (int)rint(h));    
+        } else if(i <= 0){
+            work_buffer[p2][-i] |= (0x01 << (int)rint(h));  
+        }
+        
+        if(i == r2)
+            break;    
+            
+        printf("i: %d, h: %d\n", i, (int)rint(h));
+            
+        i = r1 > r2 ? i - 1 : i + 1;
+    }     
+}
+
+static void draw_zvert_line(float x, float y, float z1, float z2){
+    float r = sqrt(1.0*x*x + 1.0*y*y);
+    float p = atan2(y, x);
+    
+    //set p to 0 - 2p range, and convert to slice index
+    norm_angle(p);
+    p = rad2deg(p);
+    
+    int i = z1;
+    while(true){
+        work_buffer[(int)rint(p)][(int)rint(r)] |= (0x01 << i);
+        
+        if(i == z2)
+            break;
+        
+        step_i(z1, z2, i);
+    }     
+}
+
+/*
+    Moves shapes in work_buffer over to display_buffer in a format that accounts for
+    displaced blades on display
+*/
+static void init_displacements();
+static int displacements[HEIGHT];
+
+void move_buffer(char (*buffer)[SLICES][WIDTH]){
+    static bool initialized = false;
+    
+    if(!initialized){
+        init_displacements();    
+    }
+    
+    for(int i = 0; i < SLICES; i++){
+        for(int j = 0; j < WIDTH; j++){
+            char bit = 0x00;
+            
+            for(int h = 0; h < HEIGHT; h++){
+                bit |= (work_buffer[(i + displacements[h]) % SLICES][j] & (0x01 << h)); 
+            }
+            
+            (*buffer)[i][j] = bit;
+        }    
+    } 
+    
+    erase();
+}
+
+/*
+    Initializes buffer used to adjust for vertical misalignment of display blades
+*/
+static void init_displacements(){
+    
+    for(int i = 0; i < HEIGHT; i++){
+        switch(i){
+            case 0: displacements[i] = 0;               break;
+            case 1: displacements[i] = 4 * SLICES / 8;  break; 
+            case 2: displacements[i] = 7 * SLICES / 8 - 5; break;
+            case 3: displacements[i] = 3 * SLICES / 8 - 8;  break;
+            case 4: displacements[i] = 6 * SLICES / 8 - 6;  break;
+            case 5: displacements[i] = 2 * SLICES / 8 - 7;  break;
+            case 6: displacements[i] = 5 * SLICES / 8 - 7;  break;
+            case 7: displacements[i] = 1 * SLICES / 8 - 8;  break;
+        }    
+    }     
+}
+
+
+