matrixnew.cpp

在人脸检测的基础之上,对嘴部的运动表情进行分析,进行语音模拟.

// Matrix.cpp: implementation of the Matrix class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "SeqProcess.h"
#include "MatrixNew.h"
#include "FnMath.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

// Create a math functions object
static FnMath fMath;

/////////////////////////////////////////////////////////////////////////////////

Matrix::Matrix()
{
	matrix = NULL;
	row = 0;
	column = 0;
}

Matrix::Matrix( int r, int c)
{
	matrix = NULL;
	row = 0;
	column = 0;
	setSize( r, c); // allocate memory
	*this = 0.0; // set all entries to 0 if non-empty
}

Matrix::Matrix( const Matrix& m)
{
	matrix = NULL;
	row = 0;
	column = 0;
	*this = m;
}

Matrix::Matrix( double f)
{
	// 1x1 matrix
	matrix = (double**) new long[1];
	matrix[0] = (double*) new double[1];
	matrix[0][0] = f;
	row = column = 1;
}

Matrix::~Matrix()
{
	empty();
}

void Matrix::setSize( int r, int c)
{
	// Row and column sizes must be both nonnegative
	// or both zero
	ASSERT( r > 0 && c > 0 || r == 0 && c == 0);
	
	if( r == 0) // or c == 0
	{
		empty();
	}
	else if( r != row || c != column)
	{
		// old and new sizes do not match
		empty(); // delete the old memory if any
		
		row = r;
		column = c;
		matrix = (double**) new double*[row];
		for( int i = 0; i < row; i++)
		{
			matrix[i] = (double*) new double[column];
			// initialize all elements to zero
			for( int j = 0; j < column; j++)
			{
				matrix[i][j] = 0.0;
			}
		}
	}
}

void Matrix::empty( void)
{
	// Delete all matrix data
	if( matrix)
	{
		for( int i = 0; i < row; i++)
		{
			delete[] matrix[i];
		}
		delete[] matrix;
	}

	matrix = NULL;
	row = 0;
	column = 0;
}

Matrix& Matrix::operator=( const Matrix& m) // Setting equal to a matrix
{
	// Cannot copy a matrix onto itself
	if( this == &m) return *this;  

	setSize( m.row, m.column);
	
	if( matrix)
	{
		for( int i = 0; i < row; i++)
		{
			for( int j = 0; j < column; j++)
			{
				matrix[i][j] = m.matrix[i][j];
			}
		}
	}
	return *this;
}

Matrix& Matrix::operator=( double a)
{
	if(matrix) 
	{
		for( int i = 0; i < row; i++)
		{
			for( int j = 0; j < column; j++)
			{
				matrix[i][j] = a;
			}
		}
	}
	return *this;
}

double& Matrix::operator()( int i, int j)
{
	// Make sure the indeces are in range
	if( ! ( i >= 0 && i < row && j >= 0 && j < column))
	{
		double* d = new double;
		return *d;
	}

	ASSERT( i >= 0 && i < row && j >= 0 && j < column);
	
	return matrix[i][j];
}

double& Matrix::operator[]( int i)
{
	// Make sure the index is in range
	ASSERT( i >= 0 && i < row*column);
	
    return matrix[i/column][i%column];
}

Matrix& Matrix::operator*=( const Matrix& m)
{
	// Matrices must be compatible for multiplication
	ASSERT( column == m.row);
	
	Matrix t(row, m.column);
	
	for( int i = 0; i < row; i++)
	{
		for( int j = 0; j < m.column; j++)
		{
			for( int k = 0; k < column; k++)
			{
				t.matrix[i][j] += matrix[i][k] * m.matrix[k][j];
			}
		}
	}
	
	*this = t;
	
	return *this;
}

Matrix& Matrix::operator+=( const Matrix& m)
{
	// Matrices must be compatible for addition
	ASSERT( column == m.column && row == m.row);
	
    for( int i = 0; i < row; i++)
    {
		for( int j = 0; j < column; j++)
		{
			matrix[i][j] += m.matrix[i][j];
		}
    }
	
    return *this;
}

Matrix& Matrix::operator-=( const Matrix& m)
{
	// Matrices must be compatible for subtraction
	ASSERT( column == m.column && row == m.row);
	
    for( int i = 0; i < row; i++)
    {
		for( int j = 0; j < column; j++)
		{
			matrix[i][j] -= m.matrix[i][j];
		}
    }
	
    return *this;
}

Matrix Matrix::operator|( Matrix& m2)  // Concatenate two matrices
{
	if( ! m2.matrix)
	{
		// m2 is empty
		return *this;
	}
	else if( ! matrix)
	{
		// this matrix is empty
		return m2;
	}
	else
	{
		int newRow = fMath.Max( row, m2.row);
		int newCol = column + m2.column;
		int i, j;
		Matrix mc( newRow, newCol); // all entries are set to 0
		
		for( i = 0; i < row; i++)
		{
			for( j = 0; j < column; j++)
			{
				mc.matrix[i][j] = matrix[i][j];
			}
		}
		
		int offset = column;
		for( i = 0; i < m2.row; i++)
		{
			for( j = 0; j < m2.column; j++)
			{
				mc.matrix[i][j+offset] = m2.matrix[i][j];
			}
		}
		
		return mc;
	}
}

Matrix Matrix::operator|( Vector& v)  // Concatenate a matrix and a vector
{
	Matrix m2 = v.matrixCol();  // Make a column matrix from vector v
	return (*this) | m2;
}

// set to identity matrix
void Matrix::setIdentity()
{
	ASSERT( row == column);

    for( int i = 0; i < row; i++)
    {
		for( int j = 0; j < column; j++)
		{
			if( i == j)
			{
				matrix[i][j] = 1.0;
			}
			else
			{
				matrix[i][j] = 0.0;
			}
		}
    }
}

// left multiply with a diagonal matrix
// Vector contains the diagonal entries
// vec * mat
Matrix& Matrix::leftMultDiag( Vector& vec)
{
	ASSERT( vec.getN() == row);

    for( int i = 0; i < row; i++)
    {
		for( int j = 0; j < column; j++)
		{
			matrix[i][j] *= vec(i); // NOTE i
		}
    }
	
    return *this;
}

// right multiply with a diagonal matrix
// Vector contains the diagonal entries
// mat * vec
Matrix& Matrix::rightMultDiag( Vector& vec)
{
	ASSERT( vec.getN() == column);

    for( int i = 0; i < row; i++)
    {
		for( int j = 0; j < column; j++)
		{
			matrix[i][j] *= vec(j); // NOTE j
		}
    }
	
    return *this;
}

Vector Matrix::vectorFromRow( int r)  // Extract a row into a vector
{
	Vector v(column);
	for( int i = 0; i < column; i++)
	{
		v[i] = matrix[r][i];
	}
	return v;
}

Vector Matrix::vectorFromCol( int c)  // Extract a column into a vector
{
	Vector v(row);
	for( int i = 0; i < row; i++)
	{
		v[i] = matrix[i][c];
	}
	return v;
}

int Matrix::getRow()
{
	return row;
}

int Matrix::getColumn()
{
	return column;
}



//======================================================================
Matrix& Matrix::adjoint()  // For 3x3 matrices only!
{
	ASSERT( row == 3 && column == 3);
	
	Matrix t(3,3);
	
    for( int i = 0; i < 3; i++)
    {
		for( int j = 0; j < 3; j++)
		{
			t.matrix[j][i] = matrix[(i+1)%3][(j+1)%3] * matrix[(i+2)%3][(j+2)%3]
				- matrix[(i+1)%3][(j+2)%3] * matrix[(i+2)%3][(j+1)%3];
		}
    }
	
    *this = t;
	
	return *this;  
}

Matrix& Matrix::ludcmp( int* indx, double* d)
{
    // matrix must not be empty
	ASSERT( matrix);
	
	// matrix must be square
	ASSERT( column == row);
	
    int     i,imax,j,k,n;
    double   big,dum,sum,temp;
    
    n=row;
    Vector vv(n);
    *d=1.0;
	// find the largest entry in the matrix
    for( i=0;i<n;i++)
    {
		big=0.0;
		for( j=0;j<n;j++)
		{
			if ((temp=fabs(matrix[i][j])) > big) big=temp;
		}
		// make sure no row is all zeros
		ASSERT( fabs(big) > TINY);
		vv[i]=1.0/big;
    }
    for( j=0;j<n;j++)
    {
		for( i=0;i<j;i++)
		{
			sum=matrix[i][j];
			for( k=0;k<i;k++)
			{
				sum -= matrix[i][k]*matrix[k][j];
			}
			matrix[i][j]=sum;
		}
		big=0.0;
		for( i=j;i<n;i++) 
		{  
			sum=matrix[i][j];
			for( k=0;k<j;k++)
			{
				sum -= matrix[i][k]*matrix[k][j];
			}
			matrix[i][j]=sum;
			if( ( dum=vv[i]*fabs(sum)) >= big) 
			{
				big=dum;
				imax=i;
			}
		}
		if( j != imax)
		{
			for( k=0;k<n;k++)
			{
				dum=matrix[imax][k];
				matrix[imax][k]=matrix[j][k];
				matrix[j][k]=dum;
			}
			*d = -(*d);
			vv[imax]=vv[j];
		}
		indx[j]=imax;
		// diagonal entries must be non-zero
		ASSERT( fabs(matrix[j][j]) > TINY);
		if( j != n)
		{
			dum=1.0/(matrix[j][j]);
			for( i=j+1;i<n;i++) matrix[i][j] *= dum;
		}     
    }  // for j
    return  *this;
} // procedure

Matrix& Matrix::lubksb(int* indx, Vector& b)
{
    // matrix must not be empty
	ASSERT( matrix);
	
	// matrix must be square
	ASSERT( column == row);
	
    int    n,i,ii=-1,ip,j;
    double  sum;
    n=row;
    for( i=0;i<n;i++)
    {
		ip=indx[i];
		sum=b[ip];
		b[ip]=b[i];
		if (ii>=0)
		{
			for( j=ii;j<=i-1;j++) 
			{
				sum -= matrix[i][j]*b[j];
			}
		}
		else if(sum) 
		{
			ii=i;
		}
		b[i]=sum;
    }
    for( i=n-1;i>=0;i--)
    {
		sum=b[i];
		for( j=i+1;j<n;j++) 
		{
			sum -= matrix[i][j]*b[j];
		}
		// diagonal entries must be non-zero
		ASSERT( fabs(matrix[i][i]) > TINY);
		b[i]=sum/matrix[i][i];
	}
    return *this;
}

Matrix& Matrix::inverse()
{
	// matrix must not be empty
	ASSERT( matrix);
	
	// matrix must be square
	ASSERT( column == row);
	
	int   *index,n,i,j;
	double d;