matrix.cpp

FreeWRLduneInputDevice和FreeWRL一起可以让用户用带有6DoF的输入设备检索3D VRML/X3D数据。它基于FreeWRL的"/tmp/inpdev"扩展传感器输入接口和w

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

Matrix
Matrix::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;
}

Vec3f
Matrix::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;
}

void
Matrix::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];
	}
    }
}

void
Matrix::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;
    }
}

Matrix
Matrix::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;
}

Matrix
Matrix::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;
}