File content as of revision 4:7a9f0515d0a0:

/*
 *  gfx3d.cpp -- 3D stuff for MBED (just playing!)
 *  Copyright (C) 2015 Fred Barnes, University of Kent  <frmb@kent.ac.uk>
 */

/** gfx3d library
 *
 *  This is a library for primitive 3D graphics.  No classes, just C functions.
 */
 

#include "mbed.h"
#include "C12832.h"
#include "gfx3d.h"

#define DISPLAY_WIDTH   (128)
#define DISPLAY_HEIGHT  (32)

#define ZB_YSHIFT       (7)

/* grotty: single global Z buffer that matches the LCD size */
static int16_t zbuffer[DISPLAY_HEIGHT * DISPLAY_WIDTH];

#define ZBUFFER(X,Y) zbuffer[(Y << ZB_YSHIFT) | X]

/**
 *  clears the Z buffer (sets all to maximum)
 */
void gfx3d_clear_zb (void)
{
    int i;
    int lim = (DISPLAY_HEIGHT * DISPLAY_WIDTH) >> 1;
    uint32_t *halfbuf = (uint32_t *)zbuffer;
    
    for (i=0; i<lim; i++) {
        halfbuf[i] = 0x7fff7fff;
    }
}



/*
 *  inlined 16-bit minimum, returns argument number
 */
static inline int int16_min3 (int16_t v0, int16_t v1, int16_t v2)
{
    if (v0 < v1) {
        if (v0 < v2) {
            return 0;
        }
        return 2;
    }
    if (v0 < v2) {
        return 1;
    }
    if (v1 < v2) {
        return 1;
    }
    return 2;
}

/*
 *  inlined 16-bit maximum, returns argument number
 */
static inline int int16_max3 (int16_t v0, int16_t v1, int16_t v2)
{
    if (v0 > v1) {
        if (v0 > v2) {
            return 0;
        }
        return 2;
    }
    if (v0 > v2) {
        return 1;
    }
    if (v1 > v2) {
        return 1;
    }
    return 2;
}


/** rotates a base set of points into a new set (demoscene style)
 *
 *  @param src Source points.
 *  @param dst Destination points.
 *  @param npnts Number of points.
 *  @param a Angle to rotate by (0-255).
 */
void gfx3d_rotate_demo (const g3d_p3_t *src, g3d_p3_t *dst, const int npnts, const angle_t a)
{
    float sinval = gfx3d_sin (a);
    float cosval = gfx3d_cos (a);
    int i;
    
    for (i=0; i<npnts; i++) {
        float x1 = (src[i].x * cosval) + (src[i].y * sinval);
        float y1 = (src[i].y * cosval) - (src[i].x * sinval);
        float z1 = (src[i].z * cosval) - (x1 * sinval);
        float t;
        
        dst[i].x = (x1 * cosval) + (src[i].z * sinval);
        t = (y1 * cosval) + (z1 * sinval);
        dst[i].z = (z1 * cosval) - (y1 * sinval);
        dst[i].y = t;
    }
    
}


/*
 *  translates a set of 3D points.  'src' and 'dst' can be the same
 */
void gfx3d_translate (const g3d_p3_t *src, g3d_p3_t *dst, const int npnts, const g3d_p3_t tx)
{
    int i;
    
    if (tx.x != 0.0f) {
        for (i=0; i<npnts; i++) {
            dst[i].x = src[i].x + tx.x;
        }
    } else if (src != dst) {
        for (i=0; i<npnts; i++) {
            dst[i].x = src[i].x;
        }
    }
    if (tx.y != 0.0f) {
        for (i=0; i<npnts; i++) {
            dst[i].y = src[i].y + tx.y;
        }
    } else if (src != dst) {
        for (i=0; i<npnts; i++) {
            dst[i].y = src[i].y;
        }
    }
    if (tx.z != 0.0f) {
        for (i=0; i<npnts; i++) {
            dst[i].z = src[i].z + tx.z;
        }
    } else if (src != dst) {
        for (i=0; i<npnts; i++) {
            dst[i].z = src[i].z;
        }
    }
}


/**
 * projects a set of 3D points into a 2D space (pretty crude)
 */
void gfx3d_project (const g3d_p3_t *src, g3d_2p3_t *dst, const int npnts)
{
    int i;
    
    for (i=0; i<npnts; i++) {
        float ez = src[i].z;
        
        dst[i].z = (int16_t)((ez + G3D_ZBADD) * G3D_ZBSCALE);
        ez += G3D_Z_DEPTH;
        dst[i].x = (int16_t)((src[i].x / ez) * G3D_X_SCALE) + G3D_X2_SHIFT;
        dst[i].y = (int16_t)((src[i].y / ez) * G3D_Y_SCALE) + G3D_Y2_SHIFT;
    }
}


/**
 * takes a set of 8 projected points and creates a set of <=12 triangular polygons representing the surface of a cube
 */
void gfx3d_cubify_points (const g3d_2p3_t *src, g3d_poly_t *dst, int *npoly, const int backfaces)
{
    static const int cubemap3[12][3] = {{1,0,4}, {1,5,4}, {2,6,5}, {2,1,5}, {3,2,6}, {3,7,6}, {3,7,4}, {3,0,4}, {3,2,1}, {3,0,1}, {7,4,5}, {7,6,5}};
    static const int cubemap4[6][4] = {{1,0,4,5}, {2,1,5,6}, {3,2,6,7}, {0,3,7,4}, {3,0,1,2}, {6,5,4,7}};
    
    int i, pidx;
    int norms[6];
    
    *npoly = 12;                /* assume all to start with */
    
    /* compute normals */
    for (i=0; i<6; i++) {
        const int *face = cubemap4[i];
        
        int norm = ((src[face[3]].y - src[face[0]].y) * (src[face[1]].x - src[face[0]].x)) -
                    ((src[face[1]].y - src[face[0]].y) * (src[face[3]].x - src[face[0]].x));
        if (!backfaces && (norm >= 0)) {
            /* not showing this one */
            *npoly = *npoly - 2;
        }
        norms[i] = norm;
    }
    
    /* wind polygons */
    pidx = 0;
    for (i=0; (i<12) && (pidx < *npoly); i++) {
        if (!backfaces && (norms[i>>1] >= 0)) {
            /* not showing this one */
        } else {
            int p;
            
            dst[pidx].norm = norms[i>>1];
            for (p=0; p<3; p++) {
                dst[pidx].pts[p].x = src[cubemap3[i][p]].x;
                dst[pidx].pts[p].y = src[cubemap3[i][p]].y;
                dst[pidx].pts[p].z = src[cubemap3[i][p]].z;
            }
            pidx++;
        }
    }
}


#if 0
/**
 *  fixes scan-line starts/ends in a g3d_polyscan_t structure based on edge
 */
static void gfx3d_polyfix (g3d_polyscan_t *dst, int16_t x1, int16_t y1, int32_t z1, int16_t x2, int16_t y2, int32_t z2, const float co_a, const float co_b, const float co_c, const float co_d)
{
    if (y2 < y1) {
        /* swap around: make sure we go in the same direction (down) */
        int16_t t16;
        int32_t t32;
        
        t16 = x1;
        x1 = x2;
        x2 = t16;
        
        t16 = y1;
        y1 = y2;
        y2 = t16;
        
        t32 = z1;
        z1 = z2;
        z2 = t32;
    }
    
    /* scan limit */
    if (y1 < dst->scan_s) {
        dst->scan_s = y1;
    }
    if (y2 > dst->scan_e) {
        dst->scan_e = y2;
    }
    if (y1 == y2) {
        /* flat polygon */
        dst->scans[y1][0] = x1;
        dst->scans[y1][1] = x2;
        dst->zscan[y1][0] = z1;
        dst->zscan[y1][1] = z2;
    } else {
        /* vaguely complex polygon */
        int x, y, z, step, z_step;
        
        x = (int)x1 << 16;
        step = ((int)(x2 - x1) << 16) / (int)(y2 - y1);
        x += step;
        
        z = z1;
        z_step = (z2 - z1) / ((y2 - y1) + 1);
        y1++;
        
        for (y=y1; y<=y2; y++) {
            if ((y >= 0) && (y < 32)) {
                int shx = x >> 16;
                
                if (x < 0) {
                    shx = shx - 65536;
                }
                if (dst->scans[y][0] == 0xff) {
                    /* start of scan */
                    uint8_t x8 = (uint8_t)(shx & 0xff);
                    
                    if (x8 == 0xff) {
                        x8 = 128;
                    }
                    dst->scans[y][0] = x8;
                    if (co_c != 0.0f) {
                        dst->zscan[y][0] = (int32_t)(((-(co_a * (float)shx) - (co_b * (float)y)) - co_d) / co_c);
                    } else {
                        dst->zscan[y][0] = z;
                    }
                } else {
                    /* end of scan */
                    uint8_t x8 = (uint8_t)(shx & 0xff);
                    
                    if (x8 == 0xff) {
                        x8 = 128;
                    }
                    dst->scans[y][1] = x8;
                    if (co_c != 0.0f) {
                        dst->zscan[y][1] = (int32_t)(((-(co_a * (float)shx) - (co_b * (float)y)) - co_d) / co_c);
                    } else {
                        dst->zscan[y][1] = z;
                    }
                }
            }
            x += step;
            z += z_step;
        }
    }
}


/**
 *  takes a polygon structure and generates a set of scanline data from it
 */
void gfx3d_polyscan (const g3d_poly_t *src, g3d_polyscan_t *dst)
{
    int i;
    float co_a, co_b, co_c, co_d;
    float x1 = (float)src->x[0];
    float y1 = (float)src->y[0];
    float z1 = (float)src->z[0];
    float x2 = (float)src->x[1];
    float y2 = (float)src->y[1];
    float z2 = (float)src->z[1];
    float x3 = (float)src->x[2];
    float y3 = (float)src->y[2];
    float z3 = (float)src->z[2];
    
    for (i=0; i<32; i++) {
        dst->scans[i][0] = 0xff;
        dst->scans[i][1] = 0xff;
        dst->zscan[i][0] = 0;
        dst->zscan[i][1] = 0;
    }
    dst->scan_s = 32;
    dst->scan_e = 0;
    dst->norm = src->norm;
    
    /* coefficients for plane equations, Herne & Baker p308 */
    co_a = ((y1 * (z2 - z3)) + (y2 * (z3 - z1))) + (y3 * (z1 - z2));
    co_b = ((z1 * (x2 - x3)) + (z2 * (x3 - x1))) + (z3 * (x1 - x2));
    co_c = ((x1 * (y2 - y3)) + (x2 * (y3 - y1))) + (x3 * (y1 - y2));
    co_d = ((-x1 * ((y2 * z3) - (y3 * z2))) - (x2 * ((y3 * z1) - (y1 * z3)))) - (x3 * ((y1 * z2) - (y2 * z1)));
    
    gfx3d_polyfix (dst, src->x[0], src->y[0], src->z[0], src->x[1], src->y[1], src->z[1], co_a, co_b, co_c, co_d);
    gfx3d_polyfix (dst, src->x[1], src->y[1], src->z[1], src->x[2], src->y[2], src->z[2], co_a, co_b, co_c, co_d);
    gfx3d_polyfix (dst, src->x[2], src->y[2], src->z[2], src->x[0], src->y[0], src->z[0], co_a, co_b, co_c, co_d);
    
    if (dst->scan_s < 0) {
        dst->scan_s = 0;
    } else if (dst->scan_s >= 32) {
        dst->scan_s = 31;
    }
    if (dst->scan_e < 0) {
        dst->scan_e = 0;
    } else if (dst->scan_e >= 32) {
        dst->scan_e = 31;
    }
    
}
#endif

static inline void gfx3d_swap_points (g3d_2p3_t *p1, g3d_2p3_t *p2)
{
    uint32_t *v1 = (uint32_t *)p1;
    uint32_t *v2 = (uint32_t *)p2;
    uint32_t tmp;
    
    tmp = *v1;
    *v1 = *v2;
    *v2 = tmp;
    
    v1++, v2++;
    tmp = *v1;
    *v1 = *v2;
    *v2 = tmp;
}

void gfx3d_sort_poly (g3d_poly_t *p)
{
    /* arranges points in the polygon into left->right order -- crude */
    if (p->pts[1].x < p->pts[0].x) {
        /* point in 1 left of 0, swap */
        gfx3d_swap_points (&(p->pts[0]), &(p->pts[1]));
    }
    if (p->pts[2].x < p->pts[0].x) {
        /* point in 2 left of 0, swap */
        gfx3d_swap_points (&(p->pts[0]), &(p->pts[2]));
    }
    if (p->pts[2].x < p->pts[1].x) {
        /* point in 2 left of 1, swap */
        gfx3d_swap_points (&(p->pts[1]), &(p->pts[2]));
    }
}


static inline void gfx3d_edgebuf_z_fixin (const g3d_2p3_t *p0, const g3d_2p3_t *p1, uint8_t *yedge, uint16_t *zedge, uint16_t xoff)
{
    int32_t y = (p0->y << 16) | 0x8000;     /* half-way... */
    int32_t ydelta;
    int32_t z = (p0->z << 16) | 0x8000;     /* half-way... */
    int32_t zdelta;
    int x;

    if (p0->x == p1->x) {
        /* same X position -- should have been dealt with */
        return;
    }
    
    ydelta = ((int32_t)(p1->y - p0->y) << 16) / (int32_t)(p1->x - p0->x);
    zdelta = ((int32_t)(p1->z - p0->z) << 16) / (int32_t)(p1->x - p0->x);
    
    for (x = p0->x; x <= p1->x; x++) {
        int16_t rc_y;
        int16_t rc_z;
        
        if (x < 0) {
            y += ydelta;
            z += zdelta;
            continue;
        }
        if (x >= DISPLAY_WIDTH) {
            /* can't have any more */
            return;
        }
        
        rc_y = (y >> 16);
        if (rc_y < 0) {
            y += ydelta;
            z += zdelta;
            continue;
        }
        if (rc_y >= DISPLAY_HEIGHT) {
            y += ydelta;
            z += zdelta;
            continue;
        }
        yedge[x - xoff] = rc_y;
        y += ydelta;
        
        rc_z = (z >> 16);
        zedge[x - xoff] = rc_z;
        z += zdelta;
    }
}


/**
 *  takes a polygon and fills an edge-buffer with it.  Assumes triangular and sorted poly.
 */
void gfx3d_edgebuf_z (const g3d_poly_t *src, g3d_edgebuf_t *dst)
{
    if (src->pts[0].x < 0) {
        /* left-hand point off-screen */
        dst->xoff = 0;
    } else {
        dst->xoff = src->pts[0].x;
    }
    dst->s_end = (src->pts[2].x - src->pts[0].x) + 1;
    
    if (src->pts[0].x == src->pts[2].x) {
        /* vertical line only */
        if (src->pts[0].y <= src->pts[1].y) {
            /* p0 is above p1 */
            if (src->pts[1].y < 0) {
                /* off top */
                dst->s_end = 0;
            } else if (src->pts[0].y >= DISPLAY_HEIGHT) {
                /* off bottom */
                dst->s_end = 0;
            } else {
                if (src->pts[0].y < 0) {
                    dst->start[0] = 0;
                } else {
                    dst->start[0] = src->pts[0].y;
                }
                if (src->pts[1].y >= DISPLAY_HEIGHT) {
                    dst->end[0] = DISPLAY_HEIGHT - 1;
                } else {
                    dst->end[0] = src->pts[1].y;
                }
                dst->zstart[0] = src->pts[0].z;
                dst->zend[0] = src->pts[1].z;
            }
        } else {
            if (src->pts[0].y < 0) {
                /* off top */
                dst->s_end = 0;
            } else if (src->pts[1].y >= DISPLAY_HEIGHT) {
                /* off bottom */
                dst->s_end = 0;
            } else {
                if (src->pts[1].y < 0) {
                    dst->start[0] = 0;
                } else {
                    dst->start[0] = src->pts[1].y;
                }
                if (src->pts[0].y >= DISPLAY_HEIGHT) {
                    dst->end[0] = DISPLAY_HEIGHT - 1;
                } else {
                    dst->end[0] = src->pts[0].y;
                }
                dst->zstart[0] = src->pts[1].z;
                dst->zend[0] = src->pts[0].z;
            }
        }
        return;
    }
    
    /* figure out whether the middle point belongs above or below the longest edge */
    if (((src->pts[2].y - src->pts[0].y) * (src->pts[1].x - src->pts[0].x)) < ((src->pts[1].y - src->pts[0].y) * (src->pts[2].x - src->pts[0].x))) {
        /* middle point is on the top-portion */
        gfx3d_edgebuf_z_fixin (&(src->pts[0]), &(src->pts[1]), dst->start, dst->zstart, dst->xoff);
        gfx3d_edgebuf_z_fixin (&(src->pts[1]), &(src->pts[2]), dst->start, dst->zstart, dst->xoff);
        gfx3d_edgebuf_z_fixin (&(src->pts[0]), &(src->pts[2]), dst->end, dst->zend, dst->xoff);
    } else {
        /* middle point is on the bottom-portion */
        gfx3d_edgebuf_z_fixin (&(src->pts[0]), &(src->pts[2]), dst->start, dst->zstart, dst->xoff);
        gfx3d_edgebuf_z_fixin (&(src->pts[0]), &(src->pts[1]), dst->end, dst->zend, dst->xoff);
        gfx3d_edgebuf_z_fixin (&(src->pts[1]), &(src->pts[2]), dst->end, dst->zend, dst->xoff);
    }
}


/**
 *  takes an edge-buffer and draws its wireframe on the given LCD (start and end).
 */
void gfx3d_wireedge (const g3d_edgebuf_t *edge, C12832 &lcd)
{
    int x;
    
    if (edge->s_end == 1) {
        /* special case: vertical line */
        int xp = edge->xoff;
        int y0 = edge->start[0];
        int y1 = edge->end[0];
        int ys = (y1 >= y0) ? 1 : -1;
        int y;
        
        int16_t z0 = edge->zstart[0];
        int16_t z1 = edge->zend[0];
        int32_t zval = (z0 << 16) | 0x8000;    /* half-way.. */
        
        if ((y0 == y1) || ((y0 + ys) == y1)) {
            /* one or two point, since we're narrow, just y0 */
            if (z0 <= ZBUFFER (xp, y0)) {
                ZBUFFER (xp, y0) = z0;
                lcd.pixel (xp, y0, 1);
            }
        } else {
            /* long vertical poly, set all points except at y1 */
            int32_t zdelta = ((int32_t)(z1 - z0) << 16) / (int32_t)(y1 - y0);
            int16_t rc_z = z0;

            for (y = y0; y != y1; y += ys) {
                zval += zdelta;
                rc_z = (zval >> 16);
                if (rc_z <= ZBUFFER (xp, y)) {
                    ZBUFFER (xp, y) = rc_z;
                    lcd.pixel (xp, y, 1);
                }
            }
        }
        return;
    }
    
    for (x=0; x<edge->s_end; x++) {
        int xp = x + edge->xoff;
        int y0 = edge->start[x];
        int y1 = edge->end[x];
        int ys = (y1 >= y0) ? 1 : -1;
        int y;
        
        int16_t z0 = edge->zstart[x];
        int16_t z1 = edge->zend[x];
        
        int32_t zval = (z0 << 16) | 0x8000;     /* half-way.. */
        
        if (y0 == y1) {
            /* point, but we don't plot assuming it belongs to the adjoining polygon */
        } else if ((y0 + ys) == y1) {
            /* two-point poly, just plot y0 */
            if (z0 <= ZBUFFER (xp, y0)) {
                ZBUFFER (xp, y0) = z0;
                lcd.pixel (xp, y0, 1);
            }
        } else {
            /* more than two points, shrink y1 to avoid plotting the last one here */
            int32_t zdelta = ((int32_t)(z1 - z0) << 16) / (int32_t)(y1 - y0);
            int16_t rc_z = z0;

            y1 -= ys;

            /* start at y0 */
            y = y0;
            if (rc_z <= ZBUFFER (xp, y)) {
                ZBUFFER (xp, y) = rc_z;
                lcd.pixel (xp, y, 1);
            }

            for (y += ys; y != y1; y += ys) {
                zval += zdelta;
                rc_z = (zval >> 16);
                if (rc_z <= ZBUFFER (xp, y)) {
                    ZBUFFER (xp, y) = rc_z;
                    lcd.pixel (xp, y, 0);
                }
            }
            /* last pixel at y1,z1 */
            rc_z = z1;
            if (rc_z <= ZBUFFER (xp, y1)) {
                ZBUFFER (xp, y1) = rc_z;
                lcd.pixel (xp, y1, 1);
            }
        }
        
        /* end of x-loop */
    }
}


/**
 *  takes a polygon and draws its wireframe on the given LCD.
 */
void gfx3d_wirepoly (const g3d_poly_t *src, C12832 &lcd)
{
    #if G3D_MAX_POLY_POINTS == 3
    lcd.line (src->pts[0].x, src->pts[0].y, src->pts[1].x, src->pts[1].y, 1);
    lcd.line (src->pts[1].x, src->pts[1].y, src->pts[2].x, src->pts[2].y, 1);
    lcd.line (src->pts[2].x, src->pts[2].y, src->pts[0].x, src->pts[0].y, 1);
    #endif
}


/**
 *  takes a polygon and draws its wireframe on the given LCD, taking Z buffering into consideration
 */
void gfx3d_wirepoly_z (const g3d_poly_t *src, C12832 &lcd)
{
    #if G3D_MAX_POLY_POINTS == 3
    #endif
}



/**
 *  takes a set of 8 projected points and draws a wireframe cube on the given LCD.
 */
void gfx3d_wirecube (const g3d_2p3_t *src, C12832 &lcd)
{
    lcd.line (src[0].x, src[0].y, src[1].x, src[1].y, 1);
    lcd.line (src[1].x, src[1].y, src[2].x, src[2].y, 1);
    lcd.line (src[2].x, src[2].y, src[3].x, src[3].y, 1);
    lcd.line (src[3].x, src[3].y, src[0].x, src[0].y, 1);
    
    lcd.line (src[4].x, src[4].y, src[5].x, src[5].y, 1);
    lcd.line (src[5].x, src[5].y, src[6].x, src[6].y, 1);
    lcd.line (src[6].x, src[6].y, src[7].x, src[7].y, 1);
    lcd.line (src[7].x, src[7].y, src[4].x, src[4].y, 1);

    lcd.line (src[0].x, src[0].y, src[4].x, src[4].y, 1);
    lcd.line (src[1].x, src[1].y, src[5].x, src[5].y, 1);
    lcd.line (src[2].x, src[2].y, src[6].x, src[6].y, 1);
    lcd.line (src[3].x, src[3].y, src[7].x, src[7].y, 1);
}