/* ----------------------------------------------------------------------------- 
 * matrix.c
 *
 *     Some 4x4 matrix operations
 * 
 * Author(s) : David Beazley (beazley@cs.uchicago.edu)
 * Copyright (C) 1995-1996
 *
 * See the file LICENSE for information on usage and redistribution.	
 * ----------------------------------------------------------------------------- */

#define MATRIX
#include "gifplot.h"
#include <math.h>

/* ------------------------------------------------------------------------
   Matrix new_Matrix()

   Create a new 4x4 matrix.
   ------------------------------------------------------------------------ */
Matrix
new_Matrix() {
  Matrix m;
  m = (Matrix) malloc(16*sizeof(double));
  return m;
}

/* ------------------------------------------------------------------------
   delete_Matrix(Matrix *m);

   Destroy a matrix
   ------------------------------------------------------------------------ */

void
delete_Matrix(Matrix m) {
  if (m)
    free((char *) m);
}

/* ------------------------------------------------------------------------
   Matrix Matrix_copy(Matrix a)

   Makes a copy of matrix a and returns it.
   ------------------------------------------------------------------------ */

Matrix Matrix_copy(Matrix a) {
  int i;
  Matrix r = 0;
  if (a) {
    r = new_Matrix();
    if (r) {
      for (i = 0; i < 16; i++)
	r[i] = a[i];
    }
  }
  return r;
}
    
/* ------------------------------------------------------------------------
   Matrix_multiply(Matrix a, Matrix b, Matrix c)

   Multiplies a*b = c
   c may be one of the source matrices
   ------------------------------------------------------------------------ */
void
Matrix_multiply(Matrix a, Matrix b, Matrix c) {
  double temp[16];
  int    i,j,k;

  for (i =0; i < 4; i++)
    for (j = 0; j < 4; j++) {
      temp[i*4+j] = 0.0;
      for (k = 0; k < 4; k++) 
	temp[i*4+j] += a[i*4+k]*b[k*4+j];
    }
  for (i = 0; i < 16; i++)
    c[i] = temp[i];
}
  
/* ------------------------------------------------------------------------
   Matrix_identity(Matrix a)

   Puts an identity matrix in matrix a
   ------------------------------------------------------------------------ */

void
Matrix_identity(Matrix a) {
  int i;
  for (i = 0; i < 16; i++) a[i] = 0;
  a[0] = 1;
  a[5] = 1;
  a[10] = 1;
  a[15] = 1;
}

/* ------------------------------------------------------------------------
   Matrix_zero(Matrix a)
   
   Puts a zero matrix in matrix a
   ------------------------------------------------------------------------ */
void
Matrix_zero(Matrix a) {
  int i;
  for (i = 0; i < 16; i++) a[i] = 0;
}

/* ------------------------------------------------------------------------
   Matrix_transpose(Matrix a, Matrix result)

   Transposes matrix a and puts it in result.
   ------------------------------------------------------------------------ */
void
Matrix_transpose(Matrix a, Matrix result) {
  double temp[16];
  int i,j;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++) 
      temp[4*i+j] = a[4*j+i];

  for (i = 0; i < 16; i++)
    result[i] = temp[i];
}


/* ------------------------------------------------------------------------
   Matrix_gauss(Matrix a, Matrix b)

   Solves ax=b for x, using Gaussian elimination. Destroys a.
   Really only used for calculating inverses of 4x4 transformation
   matrices.
   ------------------------------------------------------------------------ */

void Matrix_gauss(Matrix a, Matrix b) {
  int ipiv[4], indxr[4], indxc[4];
  int i,j,k,l,ll;
  int irow=0, icol=0;
  double big, pivinv;
  double dum;
  for (j = 0; j < 4; j++)
    ipiv[j] = 0;
  for (i = 0; i < 4; i++) {
    big = 0;
    for (j = 0; j < 4; j++) {
      if (ipiv[j] != 1) {
	for (k = 0; k < 4; k++) {
	  if (ipiv[k] == 0) {
	    if (fabs(a[4*j+k]) >= big) {
	      big = fabs(a[4*j+k]);
	      irow = j;
	      icol = k;
	    }
	  } else if (ipiv[k] > 1)
	    return;  /* Singular matrix */
	}
      }
    }
    ipiv[icol] = ipiv[icol]+1;
    if (irow != icol) {
      for (l = 0; l < 4; l++) {
	dum = a[4*irow+l];
	a[4*irow+l] = a[4*icol+l];
	a[4*icol+l] = dum;
      }
      for (l = 0; l < 4; l++) {
	dum = b[4*irow+l];
	b[4*irow+l] = b[4*icol+l];
	b[4*icol+l] = dum;
      }
    }
    indxr[i] = irow;
    indxc[i] = icol;
    if (a[4*icol+icol] == 0) return;
    pivinv = 1.0/a[4*icol+icol];
    a[4*icol+icol] = 1.0;
    for (l = 0; l < 4; l++)
      a[4*icol+l] = a[4*icol+l]*pivinv;
    for (l = 0; l < 4; l++)
      b[4*icol+l] = b[4*icol+l]*pivinv;
    for (ll = 0; ll < 4; ll++) {
      if (ll != icol) {
	dum = a[4*ll+icol];
	a[4*ll+icol] = 0;
	for (l = 0; l < 4; l++)
	  a[4*ll+l] = a[4*ll+l] - a[4*icol+l]*dum;
	for (l = 0; l < 4; l++)
	  b[4*ll+l] = b[4*ll+l] - b[4*icol+l]*dum;
      }
    }
  }
  for (l = 3; l >= 0; l--) {
    if (indxr[l] != indxc[l]) {
      for (k = 0; k < 4; k++) {
	dum = a[4*k+indxr[l]];
	a[4*k+indxr[l]] = a[4*k+indxc[l]];
	a[4*k+indxc[l]] = dum;
      }
    }
  }
}

/* ------------------------------------------------------------------------
   Matrix_invert(Matrix a, Matrix inva)

   Inverts Matrix a and places the result in inva.
   Relies on the Gaussian Elimination code above. (See Numerical recipes).
   ------------------------------------------------------------------------ */
void
Matrix_invert(Matrix a, Matrix inva) {

  double  temp[16];
  int     i;

  for (i = 0; i < 16; i++)
    temp[i] = a[i];
  Matrix_identity(inva);
  Matrix_gauss(temp,inva);
}
    
/* ------------------------------------------------------------------------
   Matrix_transform(Matrix a, GL_Vector *r, GL_Vector *t)

   Transform a vector.    a*r ----> t
   ------------------------------------------------------------------------ */

void   Matrix_transform(Matrix a, GL_Vector *r, GL_Vector *t) {

  double  rx, ry, rz, rw;

  rx = r->x;
  ry = r->y;
  rz = r->z;
  rw = r->w;
  t->x = a[0]*rx + a[1]*ry + a[2]*rz + a[3]*rw;
  t->y = a[4]*rx + a[5]*ry + a[6]*rz + a[7]*rw;
  t->z = a[8]*rx + a[9]*ry + a[10]*rz + a[11]*rw;
  t->w = a[12]*rx + a[13]*ry + a[14]*rz + a[15]*rw;
}

/* ------------------------------------------------------------------------
   Matrix_transform4(Matrix a, double x, double y, double z, double w, GL_Vector *t)

   Transform a vector from a point specified as 4 doubles
   ------------------------------------------------------------------------ */

void   Matrix_transform4(Matrix a, double rx, double ry, double rz, double rw,
			 GL_Vector *t) {

  t->x = a[0]*rx + a[1]*ry + a[2]*rz + a[3]*rw;
  t->y = a[4]*rx + a[5]*ry + a[6]*rz + a[7]*rw;
  t->z = a[8]*rx + a[9]*ry + a[10]*rz + a[11]*rw;
  t->w = a[12]*rx + a[13]*ry + a[14]*rz + a[15]*rw;
}

/* ---------------------------------------------------------------------
   Matrix_translate(Matrix a, double tx, double ty, double tz)

   Put a translation matrix in Matrix a
   ---------------------------------------------------------------------- */

void Matrix_translate(Matrix a, double tx, double ty, double tz) {
  Matrix_identity(a);
  a[3] = tx;
  a[7] = ty;
  a[11] = tz;
  a[15] = 1;
}

/* -----------------------------------------------------------------------
   Matrix_rotatex(Matrix a, double deg)

   Produce an x-rotation matrix for given angle in degrees.
   ----------------------------------------------------------------------- */
void
Matrix_rotatex(Matrix a, double deg) {
  double r;

  r = 3.1415926*deg/180.0;
  Matrix_zero(a);
  a[0] = 1.0;
  a[5] = cos(r);
  a[6] = -sin(r);
  a[9] = sin(r);
  a[10] = cos(r);
  a[15] = 1.0;
}

/* -----------------------------------------------------------------------
   Matrix_rotatey(Matrix a, double deg)

   Produce an y-rotation matrix for given angle in degrees.
   ----------------------------------------------------------------------- */
void
Matrix_rotatey(Matrix a, double deg) {
  double r;

  r = 3.1415926*deg/180.0;
  Matrix_zero(a);
  a[0] = cos(r);
  a[2] = sin(r);
  a[5] = 1.0;
  a[8] = -sin(r);
  a[10] = cos(r);
  a[15] = 1;
  
}
/* -----------------------------------------------------------------------
   Matrix_RotateZ(Matrix a, double deg)

   Produce an z-rotation matrix for given angle in degrees.
   ----------------------------------------------------------------------- */
void
Matrix_rotatez(Matrix a, double deg) {
  double r;

  r = 3.1415926*deg/180.0;
  Matrix_zero(a);
  a[0] = cos(r);
  a[1] = -sin(r);
  a[4] = sin(r);
  a[5] = cos(r);
  a[10] = 1.0;
  a[15] = 1.0;
}


/* A debugging routine */

void Matrix_set(Matrix a, int i, int j, double val) {
  a[4*j+i] = val;
}

void Matrix_print(Matrix a) {
  int i,j;
  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      fprintf(stdout,"%10f ",a[4*i+j]);
    }
    fprintf(stdout,"\n");
  }
  fprintf(stdout,"\n");
}