draw.c

#include <stdio.h>
#include <stdlib.h>

#include "ml6.h"
#include "display.h"
#include "draw.h"
#include "matrix.h"
#include "math.h"
#include "gmath.h"
#include "symtab.h"

/*======== void draw_scanline() ==========
  Inputs: int x0, double z0, int x1, double z1, int y
          screen s, zbuffer zb, color c
  
  Line algorithm for flat shading - single color
  ====================*/
void draw_scanline(int x0, double z0, int x1, double z1, int y, screen s, zbuffer zb, color c) {
  int tx, tz;
  if (x0 > x1) {
    tx = x0;
    tz = z0;
    x0 = x1;
    z0 = z1;
    x1 = tx;
    z1 = tz;
  }

  double delta_z;
  delta_z = (x1 - x0) != 0 ? (z1 - z0) / (x1 - x0 + 1) : 0;
  int x;
  double z = z0;

  for(x=x0; x <= x1; x++) {
    plot(s, zb, c, x, y, z);
    z+= delta_z;
  }
}

/*======== void draw_scanline_gouraud() ==========
  Inputs: Scanline endpoints with colors
  
  Gouraud shading - interpolates colors across scanline
  ====================*/
void draw_scanline_gouraud(int x0, double z0, color c0, int x1, double z1, color c1, 
                            int y, screen s, zbuffer zb) {
  int tx;
  double tz;
  color tc;
  
  if (x0 > x1) {
    tx = x0; x0 = x1; x1 = tx;
    tz = z0; z0 = z1; z1 = tz;
    tc = c0; c0 = c1; c1 = tc;
  }

  double delta_z = (x1 - x0) != 0 ? (z1 - z0) / (x1 - x0 + 1) : 0;
  double delta_r = (x1 - x0) != 0 ? ((double)c1.red - c0.red) / (x1 - x0 + 1) : 0;
  double delta_g = (x1 - x0) != 0 ? ((double)c1.green - c0.green) / (x1 - x0 + 1) : 0;
  double delta_b = (x1 - x0) != 0 ? ((double)c1.blue - c0.blue) / (x1 - x0 + 1) : 0;
  
  int x;
  double z = z0;
  double r = c0.red;
  double g = c0.green;
  double b = c0.blue;
  color c;

  for(x = x0; x <= x1; x++) {
    c.red = (unsigned char)(r < 0 ? 0 : (r > 255 ? 255 : r));
    c.green = (unsigned char)(g < 0 ? 0 : (g > 255 ? 255 : g));
    c.blue = (unsigned char)(b < 0 ? 0 : (b > 255 ? 255 : b));
    
    plot(s, zb, c, x, y, z);
    
    z += delta_z;
    r += delta_r;
    g += delta_g;
    b += delta_b;
  }
}

/*======== void draw_scanline_phong() ==========
  Inputs: Scanline endpoints with normals and lighting info
  
  Phong shading - interpolates normals and calculates lighting per pixel
  ====================*/
void draw_scanline_phong(int x0, double z0, double n0[3], int x1, double z1, double n1[3],
                          int y, screen s, zbuffer zb, double *view, struct light **lights, 
                          int num_lights, color ambient, struct constants *reflect) {
  int tx;
  double tz;
  double tn[3];
  
  if (x0 > x1) {
    tx = x0; x0 = x1; x1 = tx;
    tz = z0; z0 = z1; z1 = tz;
    tn[0] = n0[0]; tn[1] = n0[1]; tn[2] = n0[2];
    n0[0] = n1[0]; n0[1] = n1[1]; n0[2] = n1[2];
    n1[0] = tn[0]; n1[1] = tn[1]; n1[2] = tn[2];
  }

  double delta_z = (x1 - x0) != 0 ? (z1 - z0) / (x1 - x0 + 1) : 0;
  double delta_nx = (x1 - x0) != 0 ? (n1[0] - n0[0]) / (x1 - x0 + 1) : 0;
  double delta_ny = (x1 - x0) != 0 ? (n1[1] - n0[1]) / (x1 - x0 + 1) : 0;
  double delta_nz = (x1 - x0) != 0 ? (n1[2] - n0[2]) / (x1 - x0 + 1) : 0;
  
  int x;
  double z = z0;
  double normal[3] = {n0[0], n0[1], n0[2]};

  for(x = x0; x <= x1; x++) {
    double norm_copy[3] = {normal[0], normal[1], normal[2]};
    normalize(norm_copy);
    
    color c = get_lighting(norm_copy, view, ambient, lights, num_lights, reflect);
    
    plot(s, zb, c, x, y, z);
    
    z += delta_z;
    normal[0] += delta_nx;
    normal[1] += delta_ny;
    normal[2] += delta_nz;
  }
}


/*======== void scanline_convert() ==========
  Flat shading version - original implementation
  ====================*/
void scanline_convert( struct matrix *points, int i, screen s, zbuffer zb, color il ) {

  int top, mid, bot, y;
  int distance0, distance1, distance2;
  double x0, x1, y0, y1, y2, dx0, dx1, z0, z1, dz0, dz1;
  int flip = 0;

  z0 = z1 = dz0 = dz1 = 0;

  y0 = points->m[1][i];
  y1 = points->m[1][i+1];
  y2 = points->m[1][i+2];

  //find bot, mid, top
  if ( y0 <= y1 && y0 <= y2) {
    bot = i;
    if (y1 <= y2) {
      mid = i+1;
      top = i+2;
    }
    else {
      mid = i+2;
      top = i+1;
    }
  }
  else if (y1 <= y0 && y1 <= y2) {
    bot = i+1;
    if (y0 <= y2) {
      mid = i;
      top = i+2;
    }
    else {
      mid = i+2;
      top = i;
    }
  }
  else {
    bot = i+2;
    if (y0 <= y1) {
      mid = i;
      top = i+1;
    }
    else {
      mid = i+1;
      top = i;
    }
  }

  x0 = points->m[0][bot];
  x1 = points->m[0][bot];
  z0 = points->m[2][bot];
  z1 = points->m[2][bot];
  y = (int)(points->m[1][bot]);

  distance0 = (int)(points->m[1][top]) - y + 1;
  distance1 = (int)(points->m[1][mid]) - y + 1;
  distance2 = (int)(points->m[1][top]) - (int)(points->m[1][mid]) + 1;

  dx0 = distance0 > 0 ? (points->m[0][top]-points->m[0][bot])/distance0 : 0;
  dx1 = distance1 > 0 ? (points->m[0][mid]-points->m[0][bot])/distance1 : 0;
  dz0 = distance0 > 0 ? (points->m[2][top]-points->m[2][bot])/distance0 : 0;
  dz1 = distance1 > 0 ? (points->m[2][mid]-points->m[2][bot])/distance1 : 0;

  while ( y <= (int)points->m[1][top] ) {
    if ( !flip && y >= (int)(points->m[1][mid]) ) {
      flip = 1;
      dx1 = distance2 > 0 ? (points->m[0][top]-points->m[0][mid])/distance2 : 0;
      dz1 = distance2 > 0 ? (points->m[2][top]-points->m[2][mid])/distance2 : 0;
      x1 = points->m[0][mid];
      z1 = points->m[2][mid];
    }
    draw_scanline(x0, z0, x1, z1, y, s, zb, il);

    x0+= dx0;
    x1+= dx1;
    z0+= dz0;
    z1+= dz1;
    y++;
  }
}

/*======== void scanline_convert_gouraud() ==========
  Gouraud shading - interpolates vertex colors
  ====================*/
void scanline_convert_gouraud(struct matrix *points, int i, screen s, zbuffer zb,
                               color c0, color c1, color c2) {
  int top, mid, bot, y;
  int distance0, distance1, distance2;
  double x0, x1, y0, y1, y2, dx0, dx1, z0, z1, dz0, dz1;
  int flip = 0;
  
  // Use DOUBLES for color interpolation to maintain precision!
  double r0, g0, b0, r1, g1, b1;
  double dr0, dg0, db0, dr1, dg1, db1;

  y0 = points->m[1][i];
  y1 = points->m[1][i+1];
  y2 = points->m[1][i+2];

  // Find bot, mid, top and assign corresponding colors
  color c_bot, c_mid, c_top;
  if ( y0 <= y1 && y0 <= y2) {
    bot = i; c_bot = c0;
    if (y1 <= y2) {
      mid = i+1; c_mid = c1;
      top = i+2; c_top = c2;
    }
    else {
      mid = i+2; c_mid = c2;
      top = i+1; c_top = c1;
    }
  }
  else if (y1 <= y0 && y1 <= y2) {
    bot = i+1; c_bot = c1;
    if (y0 <= y2) {
      mid = i; c_mid = c0;
      top = i+2; c_top = c2;
    }
    else {
      mid = i+2; c_mid = c2;
      top = i; c_top = c0;
    }
  }
  else {
    bot = i+2; c_bot = c2;
    if (y0 <= y1) {
      mid = i; c_mid = c0;
      top = i+1; c_top = c1;
    }
    else {
      mid = i+1; c_mid = c1;
      top = i; c_top = c0;
    }
  }

  x0 = points->m[0][bot];
  x1 = points->m[0][bot];
  z0 = points->m[2][bot];
  z1 = points->m[2][bot];
  y = (int)(points->m[1][bot]);
  
  // Initialize edge colors as DOUBLES for precision
  r0 = c_bot.red;
  g0 = c_bot.green;
  b0 = c_bot.blue;
  
  r1 = c_bot.red;
  g1 = c_bot.green;
  b1 = c_bot.blue;

  distance0 = (int)(points->m[1][top]) - y + 1;
  distance1 = (int)(points->m[1][mid]) - y + 1;
  distance2 = (int)(points->m[1][top]) - (int)(points->m[1][mid]) + 1;

  dx0 = distance0 > 0 ? (points->m[0][top]-points->m[0][bot])/distance0 : 0;
  dx1 = distance1 > 0 ? (points->m[0][mid]-points->m[0][bot])/distance1 : 0;
  dz0 = distance0 > 0 ? (points->m[2][top]-points->m[2][bot])/distance0 : 0;
  dz1 = distance1 > 0 ? (points->m[2][mid]-points->m[2][bot])/distance1 : 0;
  
  // Color deltas
  dr0 = distance0 > 0 ? ((double)c_top.red - c_bot.red) / distance0 : 0;
  dg0 = distance0 > 0 ? ((double)c_top.green - c_bot.green) / distance0 : 0;
  db0 = distance0 > 0 ? ((double)c_top.blue - c_bot.blue) / distance0 : 0;
  
  dr1 = distance1 > 0 ? ((double)c_mid.red - c_bot.red) / distance1 : 0;
  dg1 = distance1 > 0 ? ((double)c_mid.green - c_bot.green) / distance1 : 0;
  db1 = distance1 > 0 ? ((double)c_mid.blue - c_bot.blue) / distance1 : 0;

  while ( y <= (int)points->m[1][top] ) {
    if ( !flip && y >= (int)(points->m[1][mid]) ) {
      flip = 1;
      dx1 = distance2 > 0 ? (points->m[0][top]-points->m[0][mid])/distance2 : 0;
      dz1 = distance2 > 0 ? (points->m[2][top]-points->m[2][mid])/distance2 : 0;
      x1 = points->m[0][mid];
      z1 = points->m[2][mid];
      
      // Reset edge 1 colors to mid vertex
      r1 = c_mid.red;
      g1 = c_mid.green;
      b1 = c_mid.blue;
      
      dr1 = distance2 > 0 ? ((double)c_top.red - c_mid.red) / distance2 : 0;
      dg1 = distance2 > 0 ? ((double)c_top.green - c_mid.green) / distance2 : 0;
      db1 = distance2 > 0 ? ((double)c_top.blue - c_mid.blue) / distance2 : 0;
    }
    
    // Convert doubles to color for scanline drawing
    color col0 = {
      (unsigned char)(r0 < 0 ? 0 : (r0 > 255 ? 255 : r0)),
      (unsigned char)(g0 < 0 ? 0 : (g0 > 255 ? 255 : g0)),
      (unsigned char)(b0 < 0 ? 0 : (b0 > 255 ? 255 : b0))
    };
    
    color col1 = {
      (unsigned char)(r1 < 0 ? 0 : (r1 > 255 ? 255 : r1)),
      (unsigned char)(g1 < 0 ? 0 : (g1 > 255 ? 255 : g1)),
      (unsigned char)(b1 < 0 ? 0 : (b1 > 255 ? 255 : b1))
    };
    
    draw_scanline_gouraud(x0, z0, col0, x1, z1, col1, y, s, zb);

    x0 += dx0;
    x1 += dx1;
    z0 += dz0;
    z1 += dz1;
    
    // Add to DOUBLE values to maintain precision
    r0 += dr0;
    g0 += dg0;
    b0 += db0;
    
    r1 += dr1;
    g1 += dg1;
    b1 += db1;
    
    y++;
  }
}

/*======== void scanline_convert_phong() ==========
  Phong shading - interpolates vertex normals
  ====================*/
void scanline_convert_phong(struct matrix *points, int i, screen s, zbuffer zb,
                             double n0[3], double n1[3], double n2[3],
                             double *view, struct light **lights, int num_lights, color ambient,
                             struct constants *reflect) {
  int top, mid, bot, y;
  int distance0, distance1, distance2;
  double x0, x1, y0, y1, y2, dx0, dx1, z0, z1, dz0, dz1;
  int flip = 0;
  
  double norm0[3], norm1[3];
  double dn0[3], dn1[3];

  y0 = points->m[1][i];
  y1 = points->m[1][i+1];
  y2 = points->m[1][i+2];

  double *n_bot, *n_mid, *n_top;
  if ( y0 <= y1 && y0 <= y2) {
    bot = i; n_bot = n0;
    if (y1 <= y2) {
      mid = i+1; n_mid = n1;
      top = i+2; n_top = n2;
    }
    else {
      mid = i+2; n_mid = n2;
      top = i+1; n_top = n1;
    }
  }
  else if (y1 <= y0 && y1 <= y2) {
    bot = i+1; n_bot = n1;
    if (y0 <= y2) {
      mid = i; n_mid = n0;
      top = i+2; n_top = n2;
    }
    else {
      mid = i+2; n_mid = n2;
      top = i; n_top = n0;
    }
  }
  else {
    bot = i+2; n_bot = n2;
    if (y0 <= y1) {
      mid = i; n_mid = n0;
      top = i+1; n_top = n1;
    }
    else {
      mid = i+1; n_mid = n1;
      top = i; n_top = n0;
    }
  }

  x0 = points->m[0][bot];
  x1 = points->m[0][bot];
  z0 = points->m[2][bot];
  z1 = points->m[2][bot];
  y = (int)(points->m[1][bot]);
  
  norm0[0] = n_bot[0]; norm0[1] = n_bot[1]; norm0[2] = n_bot[2];
  norm1[0] = n_bot[0]; norm1[1] = n_bot[1]; norm1[2] = n_bot[2];

  distance0 = (int)(points->m[1][top]) - y + 1;
  distance1 = (int)(points->m[1][mid]) - y + 1;
  distance2 = (int)(points->m[1][top]) - (int)(points->m[1][mid]) + 1;

  dx0 = distance0 > 0 ? (points->m[0][top]-points->m[0][bot])/distance0 : 0;
  dx1 = distance1 > 0 ? (points->m[0][mid]-points->m[0][bot])/distance1 : 0;
  dz0 = distance0 > 0 ? (points->m[2][top]-points->m[2][bot])/distance0 : 0;
  dz1 = distance1 > 0 ? (points->m[2][mid]-points->m[2][bot])/distance1 : 0;
  
  dn0[0] = distance0 > 0 ? (n_top[0] - n_bot[0]) / distance0 : 0;
  dn0[1] = distance0 > 0 ? (n_top[1] - n_bot[1]) / distance0 : 0;
  dn0[2] = distance0 > 0 ? (n_top[2] - n_bot[2]) / distance0 : 0;
  
  dn1[0] = distance1 > 0 ? (n_mid[0] - n_bot[0]) / distance1 : 0;
  dn1[1] = distance1 > 0 ? (n_mid[1] - n_bot[1]) / distance1 : 0;
  dn1[2] = distance1 > 0 ? (n_mid[2] - n_bot[2]) / distance1 : 0;

  while ( y <= (int)points->m[1][top] ) {
    if ( !flip && y >= (int)(points->m[1][mid]) ) {
      flip = 1;
      dx1 = distance2 > 0 ? (points->m[0][top]-points->m[0][mid])/distance2 : 0;
      dz1 = distance2 > 0 ? (points->m[2][top]-points->m[2][mid])/distance2 : 0;
      x1 = points->m[0][mid];
      z1 = points->m[2][mid];
      norm1[0] = n_mid[0]; norm1[1] = n_mid[1]; norm1[2] = n_mid[2];
      
      dn1[0] = distance2 > 0 ? (n_top[0] - n_mid[0]) / distance2 : 0;
      dn1[1] = distance2 > 0 ? (n_top[1] - n_mid[1]) / distance2 : 0;
      dn1[2] = distance2 > 0 ? (n_top[2] - n_mid[2]) / distance2 : 0;
    }
    
    draw_scanline_phong(x0, z0, norm0, x1, z1, norm1, y, s, zb, 
                        view, lights, num_lights, ambient, reflect);

    x0 += dx0;
    x1 += dx1;
    z0 += dz0;
    z1 += dz1;
    
    norm0[0] += dn0[0];
    norm0[1] += dn0[1];
    norm0[2] += dn0[2];
    
    norm1[0] += dn1[0];
    norm1[1] += dn1[1];
    norm1[2] += dn1[2];
    
    y++;
  }
}


/*======== void draw_polygons() ==========
  Updated to support multiple shading modes
  ====================*/
void draw_polygons( struct matrix *polygons, screen s, zbuffer zb,
                    double *view, struct light **lights, int num_lights, color ambient,
                    struct constants *reflect, int shading_mode) {
  if ( polygons->lastcol < 3 ) {
    printf("Need at least 3 points to draw a polygon!\n");
    return;
  }

  int point;
  double *normal;
  
  if (shading_mode == SHADING_GOURAUD || shading_mode == SHADING_PHONG) {
    double (*vertex_normals)[3] = malloc(polygons->lastcol * sizeof(*vertex_normals));
    calculate_vertex_normals(polygons, vertex_normals);
    
    for (point = 0; point < polygons->lastcol - 2; point += 3) {
      normal = calculate_normal(polygons, point);
      
      if (normal[2] > 0) {
        if (shading_mode == SHADING_GOURAUD) {
          color c0 = get_lighting(vertex_normals[point], view, ambient, lights, num_lights, reflect);
          color c1 = get_lighting(vertex_normals[point+1], view, ambient, lights, num_lights, reflect);
          color c2 = get_lighting(vertex_normals[point+2], view, ambient, lights, num_lights, reflect);
          
          scanline_convert_gouraud(polygons, point, s, zb, c0, c1, c2);
        }
        else if (shading_mode == SHADING_PHONG) {
          scanline_convert_phong(polygons, point, s, zb,
                                 vertex_normals[point],
                                 vertex_normals[point+1],
                                 vertex_normals[point+2],
                                 view, lights, num_lights, ambient, reflect);
        }
      }
      free(normal);
    }
    
    free(vertex_normals);
  }
  else if (shading_mode == SHADING_WIREFRAME) {
    for (point = 0; point < polygons->lastcol - 2; point += 3) {
      normal = calculate_normal(polygons, point);
      
      color WIREFRAME_COLOR = {WIREFRAME_RED, WIREFRAME_GREEN, WIREFRAME_BLUE};
      
      if (normal[2] > 0) {
        draw_line(polygons->m[0][point], polygons->m[1][point], polygons->m[2][point],
                polygons->m[0][point+1], polygons->m[1][point+1], polygons->m[2][point+1],
                s, zb, WIREFRAME_COLOR);
      
        draw_line(polygons->m[0][point+1], polygons->m[1][point+1], polygons->m[2][point+1],
                polygons->m[0][point+2], polygons->m[1][point+2], polygons->m[2][point+2],
                s, zb, WIREFRAME_COLOR);
                
        draw_line(polygons->m[0][point+2], polygons->m[1][point+2], polygons->m[2][point+2],
                polygons->m[0][point], polygons->m[1][point], polygons->m[2][point],
                s, zb, WIREFRAME_COLOR);
      }
      free(normal);
    }
  }
  else {
    for (point = 0; point < polygons->lastcol - 2; point += 3) {
      normal = calculate_normal(polygons, point);
      
      if (normal[2] > 0) {
        color i = get_lighting(normal, view, ambient, lights, num_lights, reflect);
        scanline_convert(polygons, point, s, zb, i);
      }
      free(normal);
    }
  }
}


/*======== void add_polygon() ==========
  Inputs:   struct matrix *polygons
            double x0, y0, z0, x1, y1, z1, x2, y2, z2
  Returns:
  Adds the vertices to the polygon matrix
  ====================*/
void add_polygon( struct matrix *polygons,
                  double x0, double y0, double z0,
                  double x1, double y1, double z1,
                  double x2, double y2, double z2 ) {
  add_point(polygons, x0, y0, z0);
  add_point(polygons, x1, y1, z1);
  add_point(polygons, x2, y2, z2);
}

/*======== void add_box() ==========
  Adds a rectangular prism to the polygon matrix
  ====================*/
void add_box( struct matrix *polygons,
              double x, double y, double z,
              double width, double height, double depth ) {
  double x0, y0, z0, x1, y1, z1;
  x0 = x;
  x1 = x+width;
  y0 = y;
  y1 = y-height;
  z0 = z;
  z1 = z-depth;

  //front
  add_polygon(polygons, x, y, z, x1, y1, z, x1, y, z);
  add_polygon(polygons, x, y, z, x, y1, z, x1, y1, z);
  //back
  add_polygon(polygons, x1, y, z1, x, y1, z1, x, y, z1);
  add_polygon(polygons, x1, y, z1, x1, y1, z1, x, y1, z1);

  //right side
  add_polygon(polygons, x1, y, z, x1, y1, z1, x1, y, z1);
  add_polygon(polygons, x1, y, z, x1, y1, z, x1, y1, z1);
  //left side
  add_polygon(polygons, x, y, z1, x, y1, z, x, y, z);
  add_polygon(polygons, x, y, z1, x, y1, z1, x, y1, z);

  //top
  add_polygon(polygons, x, y, z1, x1, y, z, x1, y, z1);
  add_polygon(polygons, x, y, z1, x, y, z, x1, y, z);
  //bottom
  add_polygon(polygons, x, y1, z, x1, y1, z1, x1, y1, z);
  add_polygon(polygons, x, y1, z, x, y1, z1, x1, y1, z1);
}

/*======== void add_sphere() ==========
  Adds a sphere to the polygon matrix
  ====================*/
void add_sphere( struct matrix * edges,
                 double cx, double cy, double cz,
                 double r, int step ) {

  struct matrix *points = generate_sphere(cx, cy, cz, r, step);
  int p0, p1, p2, p3, lat, longt;
  int latStop, longStop, latStart, longStart;
  latStart = 0;
  latStop = step;
  longStart = 1;
  longStop = step;

  for ( lat = latStart; lat < latStop; lat++ ) {
    for ( longt = longStart; longt < longStop; longt++ ) {
      p0 = lat * (step+1) + longt;
      p1 = p0 + 1;
      p2 = (p1 + step) % (step * (step+1));
      p3 = (p0 + step) % (step * (step+1));

      add_polygon( edges, points->m[0][p0],
                   points->m[1][p0],
                   points->m[2][p0],
                   points->m[0][p1],
                   points->m[1][p1],
                   points->m[2][p1],
                   points->m[0][p2],
                   points->m[1][p2],
                   points->m[2][p2]);
      add_polygon( edges, points->m[0][p0],
                   points->m[1][p0],
                   points->m[2][p0],
                   points->m[0][p2],
                   points->m[1][p2],
                   points->m[2][p2],
                   points->m[0][p3],
                   points->m[1][p3],
                   points->m[2][p3]);
    }
  }
  free_matrix(points);
}

/*======== void generate_sphere() ==========
  Generates sphere points
  ====================*/
struct matrix * generate_sphere(double cx, double cy, double cz,
                                double r, int step ) {

  struct matrix *points = new_matrix(4, step * step);
  int circle, rotation, rot_start, rot_stop, circ_start, circ_stop;
  double x, y, z, rot, circ;

  rot_start = 0;
  rot_stop = step;
  circ_start = 0;
  circ_stop = step;

  for (rotation = rot_start; rotation < rot_stop; rotation++) {
    rot = (double)rotation / step;

    for(circle = circ_start; circle <= circ_stop; circle++){
      circ = (double)circle / step;

      x = r * cos(M_PI * circ) + cx;
      y = r * sin(M_PI * circ) *
        cos(2*M_PI * rot) + cy;
      z = r * sin(M_PI * circ) *
        sin(2*M_PI * rot) + cz;

      add_point(points, x, y, z);
    }
  }

  return points;
}

/*======== void add_torus() ==========
  Adds a torus to the polygon matrix
  ====================*/
void add_torus( struct matrix * edges,
                double cx, double cy, double cz,
                double r1, double r2, int step ) {

  struct matrix *points = generate_torus(cx, cy, cz, r1, r2, step);
  int p0, p1, p2, p3, lat, longt;
  int latStop, longStop, latStart, longStart;
  latStart = 0;
  latStop = step;
  longStart = 0;
  longStop = step;

  for ( lat = latStart; lat < latStop; lat++ ) {
    for ( longt = longStart; longt < longStop; longt++ ) {
      p0 = lat * step + longt;
      if (longt == step - 1)
        p1 = p0 - longt;
      else
        p1 = p0 + 1;
      p2 = (p1 + step) % (step * step);
      p3 = (p0 + step) % (step * step);

      add_polygon( edges, points->m[0][p0],
                   points->m[1][p0],
                   points->m[2][p0],
                   points->m[0][p3],
                   points->m[1][p3],
                   points->m[2][p3],
                   points->m[0][p2],
                   points->m[1][p2],
                   points->m[2][p2]);
      add_polygon( edges, points->m[0][p0],
                   points->m[1][p0],
                   points->m[2][p0],
                   points->m[0][p2],
                   points->m[1][p2],
                   points->m[2][p2],
                   points->m[0][p1],
                   points->m[1][p1],
                   points->m[2][p1]);
    }
  }
  free_matrix(points);
}

/*======== void generate_torus() ==========
  Generates torus points
  ====================*/
struct matrix * generate_torus( double cx, double cy, double cz,
                                double r1, double r2, int step ) {

  struct matrix *points = new_matrix(4, step * step);
  int circle, rotation, rot_start, rot_stop, circ_start, circ_stop;
  double x, y, z, rot, circ;

  rot_start = 0;
  rot_stop = step;
  circ_start = 0;
  circ_stop = step;

  for (rotation = rot_start; rotation < rot_stop; rotation++) {
    rot = (double)rotation / step;

    for(circle = circ_start; circle < circ_stop; circle++){
      circ = (double)circle / step;

      x = cos(2*M_PI * rot) *
        (r1 * cos(2*M_PI * circ) + r2) + cx;
      y = r1 * sin(2*M_PI * circ) + cy;
      z = -1*sin(2*M_PI * rot) *
        (r1 * cos(2*M_PI * circ) + r2) + cz;

      add_point(points, x, y, z);
    }
  }
  return points;
}

/*======== void add_circle() ==========
  Adds a circle to the edge matrix
  ====================*/
void add_circle( struct matrix *edges,
                 double cx, double cy, double cz,
                 double r, int step ) {
  double x0, y0, x1, y1, t;
  int i;

  x0 = r + cx;
  y0 = cy;
  for (i=1; i<=step; i++) {
    t = (double)i/step;
    x1 = r * cos(2 * M_PI * t) + cx;
    y1 = r * sin(2 * M_PI * t) + cy;

    add_edge(edges, x0, y0, cz, x1, y1, cz);
    x0 = x1;
    y0 = y1;
  }
}

/*======== void add_curve() ==========
  Adds a curve to the edge matrix
  ====================*/
void add_curve( struct matrix *edges,
                double x0, double y0,
                double x1, double y1,
                double x2, double y2,
                double x3, double y3,
                int step, int type ) {
  double t, x, y;
  int i;
  struct matrix *xcoefs;
  struct matrix *ycoefs;

  xcoefs = generate_curve_coefs(x0, x1, x2, x3, type);
  ycoefs = generate_curve_coefs(y0, y1, y2, y3, type);

  for (i=1; i<=step; i++) {
    t = (double)i/step;

    x = xcoefs->m[0][0] *t*t*t + xcoefs->m[1][0] *t*t+
      xcoefs->m[2][0] *t + xcoefs->m[3][0];
    y = ycoefs->m[0][0] *t*t*t + ycoefs->m[1][0] *t*t+
      ycoefs->m[2][0] *t + ycoefs->m[3][0];

    add_edge(edges, x0, y0, 0, x, y, 0);
    x0 = x;
    y0 = y;
  }

  free_matrix(xcoefs);
  free_matrix(ycoefs);
}

/*======== void add_point() ==========
  Adds a point to the matrix
  ====================*/
void add_point( struct matrix * points, double x, double y, double z) {

  if ( points->lastcol == points->cols )
    grow_matrix( points, points->lastcol + 100 );

  points->m[0][ points->lastcol ] = x;
  points->m[1][ points->lastcol ] = y;
  points->m[2][ points->lastcol ] = z;
  points->m[3][ points->lastcol ] = 1;
  points->lastcol++;
}

/*======== void add_edge() ==========
  Adds an edge to the matrix
  ====================*/
void add_edge( struct matrix * points,
	       double x0, double y0, double z0,
	       double x1, double y1, double z1) {
  add_point( points, x0, y0, z0 );
  add_point( points, x1, y1, z1 );
}

/*======== void draw_lines() ==========
  Draws all lines in the matrix
  ====================*/
void draw_lines( struct matrix * points, screen s, zbuffer zb, color c) {

 if ( points->lastcol < 2 ) {
   printf("Need at least 2 points to draw a line!\n");
   return;
 }

 int point;
 for (point=0; point < points->lastcol-1; point+=2)
   draw_line( points->m[0][point],
              points->m[1][point],
              points->m[2][point],
              points->m[0][point+1],
              points->m[1][point+1],
              points->m[2][point+1],
              s, zb, c);
}

/*======== void draw_line() ==========
  Bresenham's line algorithm
  ====================*/
void draw_line(int x0, int y0, double z0,
               int x1, int y1, double z1,
               screen s, zbuffer zb, color c) {

  int x, y, d, A, B;
  int dy_east, dy_northeast, dx_east, dx_northeast, d_east, d_northeast;
  int loop_start, loop_end;
  double distance;
  double z, dz;

  int xt, yt;
  if (x0 > x1) {
    xt = x0;
    yt = y0;
    x0 = x1;
    y0 = y1;
    z0 = z1;
    x1 = xt;
    y1 = yt;
  }

  x = x0;
  y = y0;
  A = 2 * (y1 - y0);
  B = -2 * (x1 - x0);
  int wide = 0;
  int tall = 0;
  
  if ( abs(x1 - x0) >= abs(y1 - y0) ) {
    wide = 1;
    loop_start = x;
    loop_end = x1;
    dx_east = dx_northeast = 1;
    dy_east = 0;
    d_east = A;
    distance = x1 - x + 1;
    if ( A > 0 ) {
      d = A + B/2;
      dy_northeast = 1;
      d_northeast = A + B;
    }
    else {
      d = A - B/2;
      dy_northeast = -1;
      d_northeast = A - B;
    }
  }
  else {
    tall = 1;
    dx_east = 0;
    dx_northeast = 1;
    distance = abs(y1 - y) + 1;
    if ( A > 0 ) {
      d = A/2 + B;
      dy_east = dy_northeast = 1;
      d_northeast = A + B;
      d_east = B;
      loop_start = y;
      loop_end = y1;
    }
    else {
      d = A/2 - B;
      dy_east = dy_northeast = -1;
      d_northeast = A - B;
      d_east = -1 * B;
      loop_start = y1;
      loop_end = y;
    }
  }

  z = z0;
  dz = (z1 - z0) / distance;
  while ( loop_start < loop_end ) {

    plot( s, zb, c, x, y, z);
    if ( (wide && ((A > 0 && d > 0) ||
                   (A < 0 && d < 0)))
         ||
         (tall && ((A > 0 && d < 0 ) ||
                   (A < 0 && d > 0) ))) {
      y+= dy_northeast;
      d+= d_northeast;
      x+= dx_northeast;
    }
    else {
      x+= dx_east;
      y+= dy_east;
      d+= d_east;
    }
    z+= dz;
    loop_start++;
  }
  plot( s, zb, c, x1, y1, z );
}