#include "TDecompLU.h"
#include "TMath.h"
ClassImp(TDecompLU)
TDecompLU::TDecompLU()
{
fSign = 0.0;
fNIndex = 0;
fIndex = 0;
fImplicitPivot = 0;
}
TDecompLU::TDecompLU(Int_t nrows)
{
fSign = 1.0;
fNIndex = nrows;
fIndex = new Int_t[fNIndex];
memset(fIndex,0,fNIndex*sizeof(Int_t));
fImplicitPivot = 0;
fLU.ResizeTo(nrows,nrows);
}
TDecompLU::TDecompLU(Int_t row_lwb,Int_t row_upb)
{
const Int_t nrows = row_upb-row_lwb+1;
fSign = 1.0;
fNIndex = nrows;
fIndex = new Int_t[fNIndex];
memset(fIndex,0,fNIndex*sizeof(Int_t));
fRowLwb = row_lwb;
fColLwb = row_lwb;
fImplicitPivot = 0;
fLU.ResizeTo(row_lwb,row_lwb+nrows-1,row_lwb,row_lwb+nrows-1);
}
TDecompLU::TDecompLU(const TMatrixD &a,Double_t tol,Int_t implicit)
{
R__ASSERT(a.IsValid());
if (a.GetNrows() != a.GetNcols() || a.GetRowLwb() != a.GetColLwb()) {
Error("TDecompLU(const TMatrixD &","matrix should be square");
return;
}
SetBit(kMatrixSet);
fCondition = a.Norm1();
fImplicitPivot = implicit;
fTol = a.GetTol();
if (tol > 0)
fTol = tol;
fSign = 1.0;
fNIndex = a.GetNcols();
fIndex = new Int_t[fNIndex];
memset(fIndex,0,fNIndex*sizeof(Int_t));
fRowLwb = a.GetRowLwb();
fColLwb = a.GetColLwb();
fLU.ResizeTo(a);
fLU = a;
}
TDecompLU::TDecompLU(const TDecompLU &another) : TDecompBase(another)
{
fNIndex = 0;
fIndex = 0;
*this = another;
}
Bool_t TDecompLU::Decompose()
{
if (TestBit(kDecomposed)) return kTRUE;
if ( !TestBit(kMatrixSet) ) {
Error("Decompose()","Matrix has not been set");
return kFALSE;
}
Int_t nrZeros = 0;
Bool_t ok;
if (fImplicitPivot)
ok = DecomposeLUCrout(fLU,fIndex,fSign,fTol,nrZeros);
else
ok = DecomposeLUGauss(fLU,fIndex,fSign,fTol,nrZeros);
if (!ok) SetBit(kSingular);
else SetBit(kDecomposed);
return ok;
}
const TMatrixD TDecompLU::GetMatrix()
{
if (TestBit(kSingular)) {
Error("GetMatrix()","Matrix is singular");
return TMatrixD();
}
if ( !TestBit(kDecomposed) ) {
if (!Decompose()) {
Error("GetMatrix()","Decomposition failed");
return TMatrixD();
}
}
TMatrixD mL = fLU;
TMatrixD mU = fLU;
Double_t * const pU = mU.GetMatrixArray();
Double_t * const pL = mL.GetMatrixArray();
const Int_t n = fLU.GetNcols();
for (Int_t irow = 0; irow < n; irow++) {
const Int_t off_row = irow*n;
for (Int_t icol = 0; icol < n; icol++) {
if (icol > irow) pL[off_row+icol] = 0.;
else if (icol < irow) pU[off_row+icol] = 0.;
else pL[off_row+icol] = 1.;
}
}
TMatrixD a = mL*mU;
Double_t * const pA = a.GetMatrixArray();
for (Int_t i = n-1; i >= 0; i--) {
const Int_t j = fIndex[i];
if (j != i) {
const Int_t off_j = j*n;
const Int_t off_i = i*n;
for (Int_t k = 0; k < n; k++) {
const Double_t tmp = pA[off_j+k];
pA[off_j+k] = pA[off_i+k];
pA[off_i+k] = tmp;
}
}
}
return a;
}
void TDecompLU::SetMatrix(const TMatrixD &a)
{
R__ASSERT(a.IsValid());
ResetStatus();
if (a.GetNrows() != a.GetNcols() || a.GetRowLwb() != a.GetColLwb()) {
Error("TDecompLU(const TMatrixD &","matrix should be square");
return;
}
SetBit(kMatrixSet);
fCondition = a.Norm1();
fSign = 1.0;
if (fNIndex != a.GetNcols()) {
fNIndex = a.GetNcols();
delete [] fIndex;
fIndex = new Int_t[fNIndex];
memset(fIndex,0,fNIndex*sizeof(Int_t));
}
fRowLwb = a.GetRowLwb();
fColLwb = a.GetColLwb();
fLU.ResizeTo(a);
fLU = a;
}
Bool_t TDecompLU::Solve(TVectorD &b)
{
R__ASSERT(b.IsValid());
if (TestBit(kSingular)) {
Error("Solve()","Matrix is singular");
return kFALSE;
}
if ( !TestBit(kDecomposed) ) {
if (!Decompose()) {
Error("Solve()","Decomposition failed");
return kFALSE;
}
}
if (fLU.GetNrows() != b.GetNrows() || fLU.GetRowLwb() != b.GetLwb()) {
Error("Solve(TVectorD &","vector and matrix incompatible");
return kFALSE;
}
const Int_t n = fLU.GetNrows();
const Double_t *pLU = fLU.GetMatrixArray();
Double_t *pb = b.GetMatrixArray();
Int_t i;
for (i = 0; i < n ; i++) {
const Int_t off_i = i*n;
if (TMath::Abs(pLU[off_i+i]) < fTol) {
Error("Solve(TVectorD &b)","LU[%d,%d]=%.4e < %.4e",i,i,pLU[off_i+i],fTol);
return kFALSE;
}
}
Int_t nonzero = -1;
for (i = 0; i < n; i++) {
const Int_t off_i = i*n;
const Int_t iperm = fIndex[i];
Double_t r = pb[iperm];
pb[iperm] = pb[i];
if (nonzero >= 0)
for (Int_t j = nonzero; j < i; j++)
r -= pLU[off_i+j]*pb[j];
else if (r != 0.0)
nonzero = i;
pb[i] = r;
}
for (i = n-1; i >= 0; i--) {
const Int_t off_i = i*n;
Double_t r = pb[i];
for (Int_t j = i+1; j < n; j++)
r -= pLU[off_i+j]*pb[j];
pb[i] = r/pLU[off_i+i];
}
return kTRUE;
}
Bool_t TDecompLU::Solve(TMatrixDColumn &cb)
{
TMatrixDBase *b = const_cast<TMatrixDBase *>(cb.GetMatrix());
R__ASSERT(b->IsValid());
if (TestBit(kSingular)) {
Error("Solve()","Matrix is singular");
return kFALSE;
}
if ( !TestBit(kDecomposed) ) {
if (!Decompose()) {
Error("Solve()","Decomposition failed");
return kFALSE;
}
}
if (fLU.GetNrows() != b->GetNrows() || fLU.GetRowLwb() != b->GetRowLwb()) {
Error("Solve(TMatrixDColumn &","vector and matrix incompatible");
return kFALSE;
}
const Int_t n = fLU.GetNrows();
const Double_t *pLU = fLU.GetMatrixArray();
Int_t i;
for (i = 0; i < n ; i++) {
const Int_t off_i = i*n;
if (TMath::Abs(pLU[off_i+i]) < fTol) {
Error("Solve(TMatrixDColumn &cb)","LU[%d,%d]=%.4e < %.4e",i,i,pLU[off_i+i],fTol);
return kFALSE;
}
}
Double_t *pcb = cb.GetPtr();
const Int_t inc = cb.GetInc();
Int_t nonzero = -1;
for (i = 0; i < n; i++) {
const Int_t off_i = i*n;
const Int_t off_i2 = i*inc;
const Int_t iperm = fIndex[i];
const Int_t off_iperm = iperm*inc;
Double_t r = pcb[off_iperm];
pcb[off_iperm] = pcb[off_i2];
if (nonzero >=0)
for (Int_t j = nonzero; j <= i-1; j++)
r -= pLU[off_i+j]*pcb[j*inc];
else if (r != 0.0)
nonzero = i;
pcb[off_i2] = r;
}
for (i = n-1; i >= 0; i--) {
const Int_t off_i = i*n;
const Int_t off_i2 = i*inc;
Double_t r = pcb[off_i2];
for (Int_t j = i+1; j < n; j++)
r -= pLU[off_i+j]*pcb[j*inc];
pcb[off_i2] = r/pLU[off_i+i];
}
return kTRUE;
}
Bool_t TDecompLU::TransSolve(TVectorD &b)
{
R__ASSERT(b.IsValid());
if (TestBit(kSingular)) {
Error("TransSolve()","Matrix is singular");
return kFALSE;
}
if ( !TestBit(kDecomposed) ) {
if (!Decompose()) {
Error("TransSolve()","Decomposition failed");
return kFALSE;
}
}
if (fLU.GetNrows() != b.GetNrows() || fLU.GetRowLwb() != b.GetLwb()) {
Error("TransSolve(TVectorD &","vector and matrix incompatible");
return kFALSE;
}
const Int_t n = fLU.GetNrows();
const Double_t *pLU = fLU.GetMatrixArray();
Double_t *pb = b.GetMatrixArray();
Int_t i;
for (i = 0; i < n ; i++) {
const Int_t off_i = i*n;
if (TMath::Abs(pLU[off_i+i]) < fTol) {
Error("TransSolve(TVectorD &b)","LU[%d,%d]=%.4e < %.4e",i,i,pLU[off_i+i],fTol);
return kFALSE;
}
}
for (i = 0; i < n; i++) {
const Int_t off_i = i*n;
Double_t r = pb[i];
for (Int_t j = 0; j < i ; j++) {
const Int_t off_j = j*n;
r -= pLU[off_j+i]*pb[j];
}
pb[i] = r/pLU[off_i+i];
}
Int_t nonzero = -1;
for (i = n-1 ; i >= 0; i--) {
Double_t r = pb[i];
if (nonzero >= 0) {
for (Int_t j = i+1; j <= nonzero; j++) {
const Int_t off_j = j*n;
r -= pLU[off_j+i]*pb[j];
}
} else if (r != 0.0)
nonzero = i;
const Int_t iperm = fIndex[i];
pb[i] = pb[iperm];
pb[iperm] = r;
}
return kTRUE;
}
Bool_t TDecompLU::TransSolve(TMatrixDColumn &cb)
{
TMatrixDBase *b = const_cast<TMatrixDBase *>(cb.GetMatrix());
R__ASSERT(b->IsValid());
if (TestBit(kSingular)) {
Error("TransSolve()","Matrix is singular");
return kFALSE;
}
if ( !TestBit(kDecomposed) ) {
if (!Decompose()) {
Error("TransSolve()","Decomposition failed");
return kFALSE;
}
}
if (fLU.GetNrows() != b->GetNrows() || fLU.GetRowLwb() != b->GetRowLwb()) {
Error("TransSolve(TMatrixDColumn &","vector and matrix incompatible");
return kFALSE;
}
const Int_t n = fLU.GetNrows();
const Int_t lwb = fLU.GetRowLwb();
const Double_t *pLU = fLU.GetMatrixArray();
Int_t i;
for (i = 0; i < n ; i++) {
const Int_t off_i = i*n;
if (TMath::Abs(pLU[off_i+i]) < fTol) {
Error("TransSolve(TMatrixDColumn &cb)","LU[%d,%d]=%.4e < %.4e",i,i,pLU[off_i+i],fTol);
return kFALSE;
}
}
for (i = 0; i < n; i++) {
const Int_t off_i = i*n;
Double_t r = cb(i+lwb);
for (Int_t j = 0; j < i ; j++) {
const Int_t off_j = j*n;
r -= pLU[off_j+i]*cb(j+lwb);
}
cb(i+lwb) = r/pLU[off_i+i];
}
Int_t nonzero = -1;
for (i = n-1 ; i >= 0; i--) {
Double_t r = cb(i+lwb);
if (nonzero >= 0) {
for (Int_t j = i+1; j <= nonzero; j++) {
const Int_t off_j = j*n;
r -= pLU[off_j+i]*cb(j+lwb);
}
} else if (r != 0.0)
nonzero = i;
const Int_t iperm = fIndex[i];
cb(i+lwb) = cb(iperm+lwb);
cb(iperm+lwb) = r;
}
return kTRUE;
}
void TDecompLU::Det(Double_t &d1,Double_t &d2)
{
if ( !TestBit(kDetermined) ) {
if ( !TestBit(kDecomposed) )
Decompose();
TDecompBase::Det(d1,d2);
fDet1 *= fSign;
SetBit(kDetermined);
}
d1 = fDet1;
d2 = fDet2;
}
Bool_t TDecompLU::Invert(TMatrixD &inv)
{
if (inv.GetNrows() != GetNrows() || inv.GetNcols() != GetNcols() ||
inv.GetRowLwb() != GetRowLwb() || inv.GetColLwb() != GetColLwb()) {
Error("Invert(TMatrixD &","Input matrix has wrong shape");
return kFALSE;
}
inv.UnitMatrix();
const Bool_t status = MultiSolve(inv);
return status;
}
TMatrixD TDecompLU::Invert(Bool_t &status)
{
const Int_t rowLwb = GetRowLwb();
const Int_t rowUpb = rowLwb+GetNrows()-1;
TMatrixD inv(rowLwb,rowUpb,rowLwb,rowUpb);
inv.UnitMatrix();
status = MultiSolve(inv);
return inv;
}
void TDecompLU::Print(Option_t *opt) const
{
TDecompBase::Print(opt);
printf("fImplicitPivot = %d\n",fImplicitPivot);
printf("fSign = %f\n",fSign);
printf("fIndex:\n");
for (Int_t i = 0; i < fNIndex; i++)
printf("[%d] = %d\n",i,fIndex[i]);
fLU.Print("fLU");
}
TDecompLU &TDecompLU::operator=(const TDecompLU &source)
{
if (this != &source) {
TDecompBase::operator=(source);
fLU.ResizeTo(source.fLU);
fLU = source.fLU;
fSign = source.fSign;
fImplicitPivot = source.fImplicitPivot;
if (fNIndex != source.fNIndex) {
if (fIndex)
delete [] fIndex;
fNIndex = source.fNIndex;
fIndex = new Int_t[fNIndex];
}
memcpy(fIndex,source.fIndex,fNIndex*sizeof(Int_t));
}
return *this;
}
Bool_t TDecompLU::DecomposeLUCrout(TMatrixD &lu,Int_t *index,Double_t &sign,
Double_t tol,Int_t &nrZeros)
{
const Int_t n = lu.GetNcols();
Double_t *pLU = lu.GetMatrixArray();
Double_t work[kWorkMax];
Bool_t isAllocated = kFALSE;
Double_t *scale = work;
if (n > kWorkMax) {
isAllocated = kTRUE;
scale = new Double_t[n];
}
sign = 1.0;
nrZeros = 0;
for (Int_t i = 0; i < n ; i++) {
const Int_t off_i = i*n;
Double_t max = 0.0;
for (Int_t j = 0; j < n; j++) {
const Double_t tmp = TMath::Abs(pLU[off_i+j]);
if (tmp > max)
max = tmp;
}
scale[i] = (max == 0.0 ? 0.0 : 1.0/max);
}
for (Int_t j = 0; j < n; j++) {
const Int_t off_j = j*n;
for (Int_t i = 0; i < j; i++) {
const Int_t off_i = i*n;
Double_t r = pLU[off_i+j];
for (Int_t k = 0; k < i; k++) {
const Int_t off_k = k*n;
r -= pLU[off_i+k]*pLU[off_k+j];
}
pLU[off_i+j] = r;
}
Double_t max = 0.0;
Int_t imax = 0;
for (Int_t i = j; i < n; i++) {
const Int_t off_i = i*n;
Double_t r = pLU[off_i+j];
for (Int_t k = 0; k < j; k++) {
const Int_t off_k = k*n;
r -= pLU[off_i+k]*pLU[off_k+j];
}
pLU[off_i+j] = r;
const Double_t tmp = scale[i]*TMath::Abs(r);
if (tmp >= max) {
max = tmp;
imax = i;
}
}
if (j != imax) {
const Int_t off_imax = imax*n;
for (Int_t k = 0; k < n; k++ ) {
const Double_t tmp = pLU[off_imax+k];
pLU[off_imax+k] = pLU[off_j+k];
pLU[off_j+k] = tmp;
}
sign = -sign;
scale[imax] = scale[j];
}
index[j] = imax;
if (pLU[off_j+j] != 0.0) {
if (TMath::Abs(pLU[off_j+j]) < tol)
nrZeros++;
if (j != n-1) {
const Double_t tmp = 1.0/pLU[off_j+j];
for (Int_t i = j+1; i < n; i++) {
const Int_t off_i = i*n;
pLU[off_i+j] *= tmp;
}
}
} else {
::Error("TDecompLU::DecomposeLUCrout","matrix is singular");
return kFALSE;
}
}
if (isAllocated)
delete [] scale;
return kTRUE;
}
Bool_t TDecompLU::DecomposeLUGauss(TMatrixD &lu,Int_t *index,Double_t &sign,
Double_t tol,Int_t &nrZeros)
{
const Int_t n = lu.GetNcols();
Double_t *pLU = lu.GetMatrixArray();
sign = 1.0;
nrZeros = 0;
index[n-1] = n-1;
for (Int_t j = 0; j < n-1; j++) {
const Int_t off_j = j*n;
Double_t max = TMath::Abs(pLU[off_j+j]);
Int_t i_pivot = j;
for (Int_t i = j+1; i < n; i++) {
const Int_t off_i = i*n;
const Double_t mLUij = TMath::Abs(pLU[off_i+j]);
if (mLUij > max) {
max = mLUij;
i_pivot = i;
}
}
if (i_pivot != j) {
const Int_t off_ipov = i_pivot*n;
for (Int_t k = 0; k < n; k++ ) {
const Double_t tmp = pLU[off_ipov+k];
pLU[off_ipov+k] = pLU[off_j+k];
pLU[off_j+k] = tmp;
}
sign = -sign;
}
index[j] = i_pivot;
const Double_t mLUjj = pLU[off_j+j];
if (mLUjj != 0.0) {
if (TMath::Abs(mLUjj) < tol)
nrZeros++;
for (Int_t i = j+1; i < n; i++) {
const Int_t off_i = i*n;
const Double_t mLUij = pLU[off_i+j]/mLUjj;
pLU[off_i+j] = mLUij;
for (Int_t k = j+1; k < n; k++) {
const Double_t mLUik = pLU[off_i+k];
const Double_t mLUjk = pLU[off_j+k];
pLU[off_i+k] = mLUik-mLUij*mLUjk;
}
}
} else {
::Error("TDecompLU::DecomposeLUGauss","matrix is singular");
return kFALSE;
}
}
return kTRUE;
}
Bool_t TDecompLU::InvertLU(TMatrixD &lu,Double_t tol,Double_t *det)
{
if (det)
*det = 0.0;
if (lu.GetNrows() != lu.GetNcols() || lu.GetRowLwb() != lu.GetColLwb()) {
::Error("TDecompLU::InvertLU","matrix should be square");
return kFALSE;
}
const Int_t n = lu.GetNcols();
Double_t *pLU = lu.GetMatrixArray();
Int_t worki[kWorkMax];
Bool_t isAllocatedI = kFALSE;
Int_t *index = worki;
if (n > kWorkMax) {
isAllocatedI = kTRUE;
index = new Int_t[n];
}
Double_t sign = 1.0;
Int_t nrZeros = 0;
if (!DecomposeLUCrout(lu,index,sign,tol,nrZeros) || nrZeros > 0) {
if (isAllocatedI)
delete [] index;
::Error("TDecompLU::InvertLU","matrix is singular, %d diag elements < tolerance of %.4e",nrZeros,tol);
return kFALSE;
}
if (det) {
Double_t d1;
Double_t d2;
const TVectorD diagv = TMatrixDDiag_const(lu);
DiagProd(diagv,tol,d1,d2);
d1 *= sign;
*det = d1*TMath::Power(2.0,d2);
}
Int_t j;
for (j = 0; j < n; j++) {
const Int_t off_j = j*n;
pLU[off_j+j] = 1./pLU[off_j+j];
const Double_t mLU_jj = -pLU[off_j+j];
Double_t *pX = pLU+j;
Int_t k;
for (k = 0; k <= j-1; k++) {
const Int_t off_k = k*n;
if ( pX[off_k] != 0.0 ) {
const Double_t tmp = pX[off_k];
for (Int_t i = 0; i <= k-1; i++) {
const Int_t off_i = i*n;
pX[off_i] += tmp*pLU[off_i+k];
}
pX[off_k] *= pLU[off_k+k];
}
}
for (k = 0; k <= j-1; k++) {
const Int_t off_k = k*n;
pX[off_k] *= mLU_jj;
}
}
Double_t workd[kWorkMax];
Bool_t isAllocatedD = kFALSE;
Double_t *pWorkd = workd;
if (n > kWorkMax) {
isAllocatedD = kTRUE;
pWorkd = new Double_t[n];
}
for (j = n-1; j >= 0; j--) {
for (Int_t i = j+1; i < n; i++) {
const Int_t off_i = i*n;
pWorkd[i] = pLU[off_i+j];
pLU[off_i+j] = 0.0;
}
if (j < n-1) {
const Double_t *mp = pLU+j+1;
Double_t *tp = pLU+j;
for (Int_t irow = 0; irow < n; irow++) {
Double_t sum = 0.;
const Double_t *sp = pWorkd+j+1;
for (Int_t icol = 0; icol < n-1-j ; icol++)
sum += *mp++ * *sp++;
*tp = -sum + *tp;
mp += j+1;
tp += n;
}
}
}
if (isAllocatedD)
delete [] pWorkd;
for (j = n-1; j >= 0; j--) {
const Int_t jperm = index[j];
if (jperm != j) {
for (Int_t i = 0; i < n; i++) {
const Int_t off_i = i*n;
const Double_t tmp = pLU[off_i+jperm];
pLU[off_i+jperm] = pLU[off_i+j];
pLU[off_i+j] = tmp;
}
}
}
if (isAllocatedI)
delete [] index;
return kTRUE;
}