/* -----------------------------------------------------------------------------
* 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");
}
