void LDLDecomposition<T>::getPseudoInverse(MatrixT& Ainv) const
{
Ainv.resize(LDL.n,LDL.n);
VectorT temp(LDL.n,Zero),y,x;
for(int i=0;i<LDL.n;i++) {
temp(i)=One;
LBackSub(temp,y);
for(int j=0;j<y.n;j++) {
if(!FuzzyZero(LDL(j,j),zeroTolerance))
y(j) = y(j)/LDL(j,j);
else
y(j) = 0.0;
}
LTBackSub(y,x);
//fill in a column
for(int j=0;j<LDL.n;j++)
Ainv(j,i)=x(j);
temp(i)=Zero;
}
T tol = Ainv.maxAbsElement()*Epsilon;
for(int i=0;i<LDL.n;i++)
for(int j=0;j<i;j++) {
if(!FuzzyEquals(Ainv(i,j),Ainv(j,i),tol))
LOG4CXX_INFO(KrisLibrary::logger(),Ainv);
Assert(FuzzyEquals(Ainv(i,j),Ainv(j,i),tol));
Ainv(i,j)=Ainv(j,i) = 0.5*(Ainv(i,j)+Ainv(j,i));
}
}
void LDLDecomposition<T>::backSub(const VectorT& b, VectorT& x) const
{
//LDLt*x=b
//DLt*x=L^-1*b=y
//Lt*x=D^-1*y=y'
//x=(Lt^-1)y
VectorT y;
LBackSub(b,y);
DBackSub(y,y);
LTBackSub(y,x);
}
bool LDLDecomposition<T>::backSub(const VectorT& b, VectorT& x) const
{
//LDLt*x=b
//DLt*x=L^-1*b=y
//Lt*x=D^-1*y=y'
//x=(Lt^-1)y
VectorT y;
LBackSub(b,y);
bool res=DBackSub(y,y);
LTBackSub(y,x);
return res;
}
void LDLDecomposition<T>::getInverse(MatrixT& Ainv) const
{
Ainv.resize(LDL.n,LDL.n);
VectorT temp(LDL.n,Zero),y,x;
for(int i=0;i<LDL.n;i++) {
temp(i)=One;
LBackSub(temp,y);
DBackSub(y,y);
LTBackSub(y,x);
//fill in a column
for(int j=0;j<LDL.n;j++)
Ainv(j,i)=x(j);
temp(i)=Zero;
}
}
bool LDLDecomposition<T>::getInverse(MatrixT& Ainv) const
{
Ainv.resize(LDL.n,LDL.n);
bool res=true;
VectorT temp(LDL.n,Zero),y,x;
for(int i=0;i<LDL.n;i++) {
temp(i)=One;
LBackSub(temp,y);
if(!DBackSub(y,y)) res=false;
LTBackSub(y,x);
//fill in a column
for(int j=0;j<LDL.n;j++)
Ainv(j,i)=x(j);
temp(i)=Zero;
}
return true;
}