// General, 4x4 matrix inversion
void MatrixInvert(matrix4_t& matrix)
{
int i,j;
int index[4];
float column[4];
matrix4_t inverse;
if (!LU_Decompose (matrix, index))
{
// singular matrix
assert(0);
return;
}
for (j=0; j < 4; ++j)
{
for (i=0; i < 4; ++i)
{
column[i] = 0;
}
column[j] = 1;
LU_Backsub (matrix, index, column);
for (i=0; i < 4; ++i)
{
inverse[i][j] = column[i];
}
}
memcpy (matrix.Base(), inverse.Base(), sizeof(matrix4_t));
}
LUDiv<T>::LUDiv(const GenMatrix<T>& A, bool inplace) :
pimpl(new LUDiv_Impl(A,inplace))
{
TMVAssert(A.isSquare());
if (pimpl->istrans) {
if (pimpl->inplace) TMVAssert(A.transpose() == pimpl->LUx);
else pimpl->LUx = A.transpose();
} else {
if (inplace) TMVAssert(A == pimpl->LUx);
else pimpl->LUx = A;
}
LU_Decompose(pimpl->LUx,pimpl->P);
}
void lin_alg::LU_Solve(double **A, double *b, double *x, int size, bool &error)
{
// Solve the system of linear equations A.x=b using the LU Decomposition of A
// R. Sheehan 28 - 2 - 2013
double **L = matrix(size,size);
double **U = matrix(size,size);
// Compute the lower and upper factors of the LU Decomposition
LU_Decompose(A,L,U,size,error);
if(!error){
double sum;
// Solve the set of equations L.y = b by forward substitution
double *y = vector(size);
y[1] = b[1];
for(int i=2; i<=size; i++){
sum = 0.0;
for(int j=1; j<=i-1; j++){
sum = sum + L[i][j]*y[j];
}
y[i] = b[i] - sum;
}
//print_vector(y,size);
// Solve the set of equations U.x = y by back-substitution
if( fabs(U[size][size])<1.0e-15 ){
// this condition could be prevented by pivoting
// see "Numerical Recipes in C" by Press et al for implmentation of LU Decomposition with partial pivoting
cout<<"Solution cannot be found\n";
}
else{
x[size] = y[size] / U[size][size];
for(int i=size-1; i>=1; i--){
sum = 0.0;
for(int j=i+1; j<=size; j++){
sum = sum+U[i][j]*x[j];
}
x[i] = (y[i]-sum) / U[i][i];
}
}
//print_vector(x,size);
delete[] y;
}
else{
cout<<"Solution cannot be computed because LU decomposition was not found\n";
}
delete[] L;
delete[] U;
}
