/* * Matrix.cpp * * Copyright (C) 1999 Stephen F. White *  * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program (see the file "COPYING" for details); if  * not, write to the Free Software Foundation, Inc., 675 Mass Ave,  * Cambridge, MA 02139, USA. */#include <stdio.h>#include <string.h>#include "stdafx.h"#include "Matrix.h"Matrix::Matrix(){}Matrix::Matrix(float mat[4][4]){   for (int i=0;i<4;i++)      for (int k=0;k<4;k++)         _mat[i][k]=mat[i][k];}MatrixMatrix::operator *(const Matrix &m) const{    Matrix	r;    for (int i = 0; i < 4; i++) {	for (int j = 0; j < 4; j++) {	    r._mat[i][j] = 0.0f;	    for (int k = 0; k < 4; k++) {		r._mat[i][j] += _mat[k][j] * m._mat[i][k];	    }	}    }    return r;}Vec3fMatrix::operator *(const Vec3f &v) const{    Vec3f	r;    float	iw;    r.x = _mat[0][0]*v.x + _mat[1][0]*v.y + _mat[2][0]*v.z + _mat[3][0];    r.y = _mat[0][1]*v.x + _mat[1][1]*v.y + _mat[2][1]*v.z + _mat[3][1];    r.z = _mat[0][2]*v.x + _mat[1][2]*v.y + _mat[2][2]*v.z + _mat[3][2];    iw = 1.0f / (_mat[0][3]*v.x + _mat[1][3]*v.y + _mat[2][3]*v.z + _mat[3][3]);        return r * iw;}voidMatrix::getValue(float mat[4][4]) const{    for (int i = 0; i < 4; i++) {	for (int j = 0; j < 4; j++) {	    mat[i][j] = _mat[i][j];	}    }}voidMatrix::swapRows(int i, int j){    float	tmp;    for (int k = 0; k < 4; k++) {	tmp = _mat[k][i];	_mat[k][i] = _mat[k][j];	_mat[k][j] = tmp;    }}MatrixMatrix::invert() const	      // Gauss-Jordan elimination with partial pivoting{    Matrix a(*this),          // As a evolves from original mat into identity           b(identity());     // b evolves from identity into inverse(a)        int  i, j, i1, k;    // Loop over cols of a from left to right, eliminating above and below diag    for (j = 0; j < 4; j++) {   	// Find largest pivot in column j among rows j..2	i1 = j;                 // Row with largest pivot candidate	for ( i = j + 1; i < 4; i++ )	    if ( fabs( a._mat[j][i] ) > fabs( a._mat[j][i1] ) )		i1 = i;	// Swap rows i1 and j in a and b to put pivot on diagonal	if (i1 != j) {	    a.swapRows(i1, j);	    b.swapRows(i1, j);	}	// Scale row j to have a unit diagonal	if (a._mat[j][j] == 0.0f) {	    fprintf(stderr, "Matrix::invert():  singular matrix, can't invert\n");	    return b;	}	float	s = 1.0f / a._mat[j][j];	for (i = 0; i < 4; i++) {	    b._mat[i][j] *= s;	    a._mat[i][j] *= s;	}	// Eliminate off-diagonal elems in col j of a, doing identical ops to b	for (i = 0; i < 4; i++) {	    if (i != j) {		float t = a._mat[j][i];		for (k = 0; k < 4; k++) {		    b._mat[k][i] -= t * b._mat[k][j];		    a._mat[k][i] -= t * a._mat[k][j];		}	    }	}    }    return b;}MatrixMatrix::identity(){    Matrix r;    r._mat[0][1] = r._mat[0][2] = r._mat[0][3] = 0.0f;    r._mat[1][0] = r._mat[1][2] = r._mat[1][3] = 0.0f;    r._mat[2][0] = r._mat[2][1] = r._mat[2][3] = 0.0f;    r._mat[3][0] = r._mat[3][1] = r._mat[3][2] = 0.0f;    r._mat[0][0] = r._mat[1][1] = r._mat[2][2] = r._mat[3][3] = 1.0f;    return r;}