矩阵的逆

// Function name   : matrixInvert
// Description     : resolve the invert of the matrix, 
//					 the size of matrix is N*N
// Return type     : int 
// Argument        : int N
// Argument        : double input[]
// Argument        : double output[]
int matrixInvert( int N, double input[], double output[] )
{
	// Receives an array of dimension NxN as input.  This is passed as a one-
	// dimensional array of N-squared size.  It produces the inverse of the
	// input matrix, returned as output, also of size N-squared.  The Gauss-
	// Jordan Elimination method is used.  (Adapted from a BASIC routine in
	// "Basic Scientific Subroutines Vol. 1", courtesy of Scott Edwards.)

	// Array elements 0...N-1 are for the first row, N...2N-1 are for the
	// second row, etc.

	// We need to have a temporary array of size N x 2N.  We'll refer to the
	// "left" and "right" halves of this array.

	int row, col;
	int tempSize = 2 * N * N;
	double* temp = (double*) new double[ tempSize ];
	double ftemp;

	if ( temp == 0 ) {

		fprintf(stderr, "matrixInvert(): ERROR - memory allocation failed.\n");
		return false;
	}

	// First create a double-width matrix with the input array on the left
	// and the identity matrix on the right.

	for ( row = 0; row < N; row++ )
	{
		for ( col = 0; col < N; col++ )
		{
			// Our index into the temp array is X2 because it's twice as wide
			// as the input matrix.

			temp[ 2*row*N + col ] = input[ row*N+col ];	// left = input matrix
			temp[ 2*row*N + col + N ] = 0.0f;			// right = 0
		}
		temp[ 2*row*N + row + N ] = 1.0f;		// 1 on the diagonal of RHS
	}

	// Now perform row-oriented operations to convert the left hand side
	// of temp to the identity matrix.  The inverse of input will then be
	// on the right.

	int max;
	int k = 0;
	for ( k = 0; k < N; k++ )
	{
		if ( k+1 < N )	// if not on the last row
		{              
			max = k;
			for ( row = k+1; row < N; row++ ) // find the maximum element
			{  
				if ( fabs( temp[row*2*N + k] ) > fabs( temp[max*2*N + k] ) )
				{
					max = row;
				}
			}

			if ( max != k )	// swap all the elements in the two rows
			{        
				for ( col=k; col<2*N; col++ )
				{
					ftemp = temp[k*2*N + col];
					temp[k*2*N + col] = temp[max*2*N + col];
					temp[max*2*N + col] = ftemp;
				}
			}
		}

		ftemp = temp[ k*2*N + k ];
		if ( ftemp == 0.0f ) // matrix cannot be inverted
		{
			delete[] temp;
			return false;
		}

		for ( col = k; col < 2*N; col++ )
		{
			temp[ k*2*N + col ] /= ftemp;
		}

		for ( row = 0; row < N; row++ )
		{
			if ( row != k )
			{
				ftemp = temp[ row*2*N + k ];
				for ( col = k; col < 2*N; col++ ) 
				{
					temp[ row*2*N + col ] -= ftemp * temp[ k*2*N + col ];
				}
			}
		}
	}

	// Retrieve inverse from the right side of temp

	for ( row = 0; row < N; row++ )
	{
		for ( col = 0; col < N; col++ )
		{
			output[row*N + col] = temp[row*2*N + col + N ];
		}
	}

	delete [] temp;       // free memory
	return true;
}