/*

Matrix transformations.

Greg Turk, November 1996.

*/

#include <stdio.h>
#include <math.h>
#include "gtGraphics.h"
#include "matlib.h"

typedef float gtMatrix[4][4];
typedef float gtVector[3];

typedef struct StackEntry {
  gtMatrix mat;
} StackEntry;

#define STACK_MAX 10
static StackEntry stack[10];   /* the matrix stack */
static int stack_top = 0;      /* stack[stack_top] is current trans. matrix */

/* matrices for defining projections */
static gtMatrix ortho_mat;   /* orthogonal projection */
static gtMatrix persp_mat;   /* perspective projection */

/* whether we're using orthographic or perspective projection */
#define  NO_PROJECTION  0
#define  ORTHO          1
#define  PERSPECTIVE    2
static int projection = NO_PROJECTION;

/* clipping planes */
static float znear;
static float zfar;

/* which vertex we're on for line drawing */
static int num_vertices = 0;


/******************************************************************************
Print out a matrix.
******************************************************************************/

static void print_matrix(gtMatrix mat)
{
  int i,j;

  for (j = 0; j < 4; j++) {
    for (i = 0; i < 4; i++)
      printf ("%f ", mat[i][j]);
    printf ("\n");
  }
}


/******************************************************************************
Copy a matrix.

Entry:
  dest   - destination matrix
  source - source matrix to copy
******************************************************************************/

static void copy_matrix(gtMatrix dest, gtMatrix source)
{
  int i,j;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      dest[i][j] = source[i][j];
}


/******************************************************************************
Set a matrix to the identity matrix.
******************************************************************************/

static void identity_matrix(gtMatrix mat)
{
  int i,j;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      mat[i][j] = (i == j);
}


/******************************************************************************
Transpose a matrix.
******************************************************************************/

static void transpose_matrix(gtMatrix m)
{
  int i,j;
  gtMatrix m2;

  /* copy */
  copy_matrix (m2, m);

  /* transpose */
  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      m[i][j] = m2[j][i];
}


/******************************************************************************
Multiply two matrices.

Entry:
  m1,m2 - matrices to multipy

Exit:
  dest - result of m1 * m2
******************************************************************************/

static void mult_matrix(gtMatrix dest, gtMatrix m1, gtMatrix m2)
{
  int i,j,k;
  gtMatrix m;

  /* perform multiplication */

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++) {
      m[i][j] = 0;
      for (k = 0; k < 4; k++)
	m[i][j] += m1[k][j] * m2[i][k];
    }

  /* copy the answer */

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      dest[i][j] = m[i][j];
}


/******************************************************************************
Apply a matrix transformation to a vector.

Entry:
  v - vector to transform
  m - matrix to apply

Exit:
  v - transformed vector
******************************************************************************/

void apply_matrix(gtVector v, gtMatrix m)
{
  int i;
  gtVector vtemp;

  /* multiply the matrix times the vector */
 
  for (i = 0; i < 3; i++)
    vtemp[i] = v[0] * m[0][i] + v[1] * m[1][i] + v[2] * m[2][i] + m[3][i];

  /* copy the result into the destination */

  for (i = 0; i < 3; i++)
    v[i] = vtemp[i];
}


/******************************************************************************
Normalize a vector.
******************************************************************************/

void normalize_vector(gtVector v)
{
  float len,recip;

  len = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];

  if (len == 0) {
    fprintf (stderr, "normalize_vector: zero length vector\n");
    return;
  }

  recip = 1.0 / sqrt (len);
  v[0] *= recip;
  v[1] *= recip;
  v[2] *= recip;
}


/******************************************************************************
Print length of vector.  For debugging.
******************************************************************************/

void print_length(gtVector v)
{
  float len,recip;

  len = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];

  if (len == 0) {
    fprintf (stderr, "zero length vector\n");
    return;
  }

  len = sqrt(len);
  printf ("len = %f\n", len);
}


/******************************************************************************
Form the cross-product of two vectors.

Entry:
  a,b - vectors to form cross-product of

Exit:
  c - a cross b
******************************************************************************/

void cross(gtVector c, gtVector a, gtVector b)
{
  gtVector vtemp;

  /* perform cross-product */
  vtemp[0] = a[1]*b[2] - a[2]*b[1];
  vtemp[1] = a[2]*b[0] - a[0]*b[2];
  vtemp[2] = a[0]*b[1] - a[1]*b[0];

  /* copy the result */
  c[0] = vtemp[0];
  c[1] = vtemp[1];
  c[2] = vtemp[2];
}


/******************************************************************************
Initialize the matrix stack.
******************************************************************************/

void gtInitialize()
{
  identity_matrix (stack[0].mat);
}


/******************************************************************************
Push the matrix stack.
******************************************************************************/

void gtPushMatrix()
{
  /* check for stack overflow */

  if (stack_top + 1 == STACK_MAX) {
    fprintf (stderr, "Too many matrix pushes\n");
    return;
  }

  /* replicate the top matrix */
  copy_matrix (stack[stack_top+1].mat, stack[stack_top].mat);
  stack_top++;
}


/******************************************************************************
Pop the matrix stack.
******************************************************************************/

void gtPopMatrix()
{
  /* check for stack underflow */

  if (stack_top == 0) {
    fprintf (stderr, "Matrix stack underflow\n");
    return;
  }

  /* forget the top of the stack */

  stack_top--;
}


/******************************************************************************
Append a translation matrix to the current transformation matrix.

Entry:
  tx,ty,tz - translation amounts in x,y,z
******************************************************************************/

void gtTranslate(float tx, float ty, float tz)
{
  gtMatrix mat;

  identity_matrix (mat);
  mat[3][0] = tx;
  mat[3][1] = ty;
  mat[3][2] = tz;

  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, mat);
}


/******************************************************************************
Append a scaling matrix to the current transformation matrix.

Entry:
  sx,sy,sz - scaling amounts in x,y,z
******************************************************************************/

void gtScale(float sx, float sy, float sz)
{
  gtMatrix mat;

  identity_matrix (mat);
  mat[0][0] = sx;
  mat[1][1] = sy;
  mat[2][2] = sz;

  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, mat);
}


/******************************************************************************
Append a rotation matrix to the current transformation matrix.

Entry:
  angle    - number of degrees to rotate around a given axis
  ax,ay,az - the given axis
******************************************************************************/

void gtRotate(float angle, float ax, float ay, float az)
{
  gtMatrix r;
  gtMatrix r1,r2,r3;
  float theta;
  gtVector n;
  gtVector a,b,c;

  a[0] = ax;
  a[1] = ay;
  a[2] = az;
  normalize_vector(a);

  /* create vector not parallel to "a" */

  ax = fabs(a[0]);
  ay = fabs(a[1]);
  az = fabs(a[2]);

  n[0] = n[1] = n[2] = 0;

  if (ax > ay) {
    if (ay > az)
      n[2] = 1;    /* z is smallest */
    else
      n[1] = 1;    /* y is smallest */
  }
  else {
    if (ax > az)
      n[2] = 1;    /* z is smallest */
    else
      n[0] = 1;    /* x is smallest */
  }

  /* create "b" orthogonal to "a" */

  cross (b, a, n);
  normalize_vector(b);

  /* create "c" orthogonal to "a" and "b" */

  cross (c, a, b);

  /* make matrix that rotates a,b,c into x,y,z axes */

  identity_matrix (r1);
  r1[0][0] = a[0];
  r1[1][0] = a[1];
  r1[2][0] = a[2];
  r1[0][1] = b[0];
  r1[1][1] = b[1];
  r1[2][1] = b[2];
  r1[0][2] = c[0];
  r1[1][2] = c[1];
  r1[2][2] = c[2];

  /* make matrix for rotation by theta degrees around x-axis */

  theta = angle * 3.1415926535 / 180.0;
  identity_matrix (r2);
  r2[1][1] =  cos(theta);
  r2[2][2] =  cos(theta);
  r2[1][2] =  sin(theta);
  r2[2][1] = -sin(theta);

  /* make matrix that is inverse of r1 */
  copy_matrix (r3, r1);
  transpose_matrix (r3);

  /* compose these matrices for final matrix r = r3 * r2 * r1 */
  mult_matrix (r, r3, r2);
  mult_matrix (r, r, r1);

  /* append to current transformation matrix */
  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, r);
}


/******************************************************************************
Specify a viewing matrix and multiply the current transformation by this.

Entry:
  fx,fy,fz - our view position
  ax,ay,az - point to look at
  ux,uy,uz - up vector (from the origin)
******************************************************************************/

void gtLookAt(
  float fx,
  float fy,
  float fz,
  float ax,
  float ay,
  float az,
  float ux,
  float uy,
  float uz
)
{
  gtVector u,x,y,z;
  gtMatrix r,t,v;

  /* form a vector in the direction from "from" to "at" */
  z[0] = ax - fx;
  z[1] = ay - fy;
  z[2] = az - fz;
  normalize_vector(z);

  /* find vector "x" perpendicular to "z" and "up" */
  u[0] = ux;
  u[1] = uy;
  u[2] = uz;
  cross (x, z, u);
  normalize_vector(x);

  /* find vector "y" perpendicular to "x" and "z" */
  cross (y, x, z);

  /* create matrix that rotates "x" "y" and "z" into the x, y and z axes */

  identity_matrix (r);
  r[0][0] = x[0];
  r[1][0] = x[1];
  r[2][0] = x[2];
  r[0][1] = y[0];
  r[1][1] = y[1];
  r[2][1] = y[2];
  r[0][2] = -z[0];
  r[1][2] = -z[1];
  r[2][2] = -z[2];

  /* translate "from" to the origin */

  identity_matrix (t);
  t[3][0] = -fx;
  t[3][1] = -fy;
  t[3][2] = -fz;

  /* view matrix = rotation * translation */

  mult_matrix (v, r, t);

  /* apply to the current transformation matrix */

  mult_matrix (stack[stack_top].mat, stack[stack_top].mat, v);
}


/******************************************************************************
Specify an orthographic projection.

Entry:
  left,right - x coordinates of clipping box
  bottom,top - y coordinates
  near,far   - z clipping planes
******************************************************************************/

void gtOrtho(
  float left,
  float right,
  float bottom,
  float top,
  float near,
  float far
)
{
  int w,h;
  float sx,sy;

  znear = near;
  zfar = far;

  gtGetFramebufferSize (&w, &h);

  gtPushMatrix();

  /* map [left,right] to [0,w] and [bottom,top] to [0,h] */

  identity_matrix (stack[stack_top].mat);
  sx = (w - 1) / (right - left);
  sy = (h - 1) / (top - bottom);
  gtTranslate (-sx * left, -sy * bottom, 0.0);
  gtScale (sx, sy, 1.0);
  copy_matrix (ortho_mat, stack[stack_top].mat);

  gtPopMatrix();

  projection = ORTHO;
}


/******************************************************************************
Specify a perspective projection.

Entry:
  fovy   - vertical field of view in degrees
  aspect - ratio of vertical to horizontal field of view (ignored)
  near   - near clipping plane
  far    - far clipping plane
******************************************************************************/

void gtPerspective(float fovy, float aspect, float near, float far)
{
  float theta;
  float s;
  float sx,sy;
  int w,h;

  znear = near;
  zfar = far;

  gtGetFramebufferSize (&w, &h);

  theta = fovy * 3.1415926535 / 180.0;
  theta *= 0.5;
  s = tan(theta);
  sx = (w - 1) / (2 * s);
  sy = (h - 1) / (2 * s);

  gtPushMatrix();
  identity_matrix (stack[stack_top].mat);
  gtScale (sx, sy, 1.0);
  gtTranslate (s, s, 0.0);
  copy_matrix (persp_mat, stack[stack_top].mat);
  gtPopMatrix();

  projection = PERSPECTIVE;
}


/******************************************************************************
Start accepting line drawing commands.
******************************************************************************/

void gtBegin()
{
  num_vertices = 0;
}


/******************************************************************************
Finish accepting line drawing commands.  Doesn't really do anything.
******************************************************************************/

void gtEnd()
{
}


/******************************************************************************
Specify an endpoint of a line.  Draws a line between pairs of such points.

Entry:
  x,y,z - coordinates of an endpoint of a line segment
******************************************************************************/

void gtVertex3f(float x, float y, float z)
{
  int result;
  static gtVector v0;
  static gtVector v1;

  /* if we're getting the first endpoint, just save the info */

  if (num_vertices == 0) {
    v0[0] = x;
    v0[1] = y;
    v0[2] = z;
    apply_matrix (v0, stack[stack_top].mat);
    num_vertices = 1;
    return;
  }

  /* if we get here, we've got both endpoints */

  v1[0] = x;
  v1[1] = y;
  v1[2] = z;
  apply_matrix (v1, stack[stack_top].mat);
  num_vertices = 0;

  /* perform near/far clipping */

  result = near_far_clip (-znear, -zfar,
			  &v0[0], &v0[1], &v0[2],
			  &v1[0], &v1[1], &v1[2]);

  /* don't draw anything if the entire line is clipped away */
  if (result == 0)
    return;

  /* orthographic case */

  if (projection == ORTHO) {
    apply_matrix (v0, ortho_mat);
    apply_matrix (v1, ortho_mat);
  }
  else if (projection = PERSPECTIVE) {
    if (v0[2] != 0) {
      v0[0] /= fabs (v0[2]);
      v0[1] /= fabs (v0[2]);
    }
    if (v1[2] != 0) {
      v1[0] /= fabs (v1[2]);
      v1[1] /= fabs (v1[2]);
    }
    apply_matrix (v0, persp_mat);
    apply_matrix (v1, persp_mat);
  }
  else {
    fprintf (stderr, "No projection specified\n");
    return;
  }

  /* draw a line between the two endpoints */

  draw_line (v0[0], v0[1], v1[0], v1[1]);
}