inline bool LBackSubstitute(const MatrixTemplate<T>& a, const MatrixTemplate<T>& b, MatrixTemplate<T>& x)
{
if(x.isEmpty()) x.resize(a.n,b.n);
else Assert(x.m == a.n && x.n == b.n);
for(int i=0;i<x.n;i++) {
VectorTemplate<T> xi,bi;
x.getColRef(i,xi);
b.getColRef(i,bi);
if(!LBackSubstitute(a,bi,xi)) return false;
}
return true;
}
inline void Lt1BackSubstitute(const MatrixTemplate<T>& a, const MatrixTemplate<T>& b, MatrixTemplate<T>& x)
{
if(x.isEmpty())
x.resize(a.n,b.n);
else Assert(x.m == a.n && x.n == b.n);
for(int i=0;i<x.n;i++) {
VectorTemplate<T> xi,bi;
x.getColRef(i,xi);
b.getColRef(i,bi);
Lt1BackSubstitute(a,bi,xi);
}
}
bool LtBackSubstitute(const MatrixTemplate<T>& a, const VectorTemplate<T>& b, VectorTemplate<T>& x)
{
Assert(a.isSquare());
Assert(a.n == b.n);
if(x.empty()) x.resize(a.n);
else Assert(a.n == x.n);
int n=a.n;
T aii,sum;
for(int i=n-1; i>=0; i--) {
aii=a(i,i);
sum=b[i];
for(int j=i+1; j<n; j++)
sum-=a(j,i)*x[j];
if(aii == 0) {
if(!FuzzyZero(sum,(T)kBackSubZeroTolerance)) {
//LOG4CXX_ERROR(KrisLibrary::logger(),"LtBackSubstitute: dividing by zero: "<<sum<<"/"<<aii);
return false;
}
x[i]=0;
}
else
x[i]=sum/aii;
}
return true;
}
void ReduceRowEchelon(MatrixTemplate<T>& A,MatrixTemplate<T>& B)
{
if(!B.isEmpty())
Assert(B.m == A.m);
int i,j,icur,jcur;
for(icur=A.m-1;icur>=0;icur--) {
//find pivot for row icur
jcur = -1;
for(j=0;j<A.n;j++) if(A(icur,j) != 0) { jcur=j; break; }
if(jcur == -1) continue;
//normalize row by dividing by A(icur,jcur)
T scale = One/A(icur,jcur);
A(icur,jcur) = 1;
for(j=jcur+1;j<A.n;j++) A(icur,j) *= scale;
for(j=0;j<B.n;j++) B(icur,j) *= scale;
//zero out columns above (icur,jcur)
for(i=0;i<icur;i++) {
if(A(i,jcur) == 0) continue;
scale = A(i,jcur);
A(i,jcur) = 0;
for(j=jcur+1;j<A.n;j++) A(i,j) -= A(icur,j)*scale;
for(j=0;j<B.n;j++) B(i,j) -= B(icur,j)*scale;
}
}
}
bool LBackSubstitute(const MatrixTemplate<T>& a, const VectorTemplate<T>& b, VectorTemplate<T>& x)
{
Assert(a.isSquare());
Assert(a.n == b.n);
if(x.empty()) x.resize(a.n);
else Assert(a.n == x.n);
int n=a.n;
T aii,sum;
for(int i=0; i<n; i++) {
aii=a(i,i);
sum=b[i];
for(int j=0; j<i; j++)
sum-=a(i,j)*x[j];
if(aii == 0) {
if(!FuzzyZero(sum,(T)kBackSubZeroTolerance)) {
//cerr<<"LBackSubstitute: dividing by zero: "<<sum<<"/"<<aii<<endl;
//cerr<<MatrixPrinter(a)<<endl;
return false;
}
x[i]=0;
}
else
x[i]=sum/aii;
}
return true;
}
void MatrixTemplate<T>::copy(const MatrixTemplate<T2>& a)
{
CHECKRESIZE(a.m,a.n);
MatrixIterator<T> k=begin();
MatrixIterator<T2> ak=a.begin();
for(int i=0;i<m;i++,k.nextRow(),ak.nextRow())
for(int j=0;j<n;j++,k.nextCol(),ak.nextCol())
*k = (T)*ak;
}
void U1BackSubstitute(const MatrixTemplate<T>& a, const VectorTemplate<T>& b, VectorTemplate<T>& x)
{
Assert(a.isSquare());
Assert(a.n == b.n);
if(x.empty()) x.resize(a.n);
else Assert(a.n == x.n);
int n=a.n;
T sum;
for(int i=n-1; i>=0; i--) {
sum=b(i);
for(int j=i+1; j<n; j++)
sum-=a(i,j)*x[j];
x[i]=sum;
}
}
void L1BackSubstitute(const MatrixTemplate<T>& a, const VectorTemplate<T>& b, VectorTemplate<T>& x)
{
Assert(a.isSquare());
Assert(a.n == b.n);
if(x.empty()) x.resize(a.n);
else Assert(a.n == x.n);
int n=a.n;
T sum;
for(int i=0; i<n; i++) {
sum=b[i];
for(int j=0; j<i; j++)
sum-=a(i,j)*x[j];
x[i]=sum;
}
}
int RowEchelonDecompose(MatrixTemplate<T>& A,MatrixTemplate<T>& B,Real zeroTol)
{
if(!B.isEmpty())
Assert(B.m == A.m);
int m=A.m,n=A.n;
int p=B.n;
int i,j,icur=0,jcur;
T temp;
for(jcur=0;jcur<n;jcur++) {
//find pivot element in col jcur from rows icur..m
Real big=Zero;
int ipivot=-1;
for(i=icur;i<m;i++) {
if(Abs(A(i,jcur)) > big) {
ipivot = i;
big = Abs(A(i,jcur));
}
}
if(!FuzzyZero(big,zeroTol)) { //nonzero pivot found
//exchange rows ipivot,icur
if(ipivot != icur) {
for(j=jcur;j<n;j++) SWAP(A(ipivot,j),A(icur,j));
for(j=0;j<p;j++) SWAP(B(ipivot,j),B(icur,j));
}
//eliminate rows below icur
T scale;
for(i=icur+1;i<m;i++) {
//set row(ai) = row(ai)-aiJ/aIJ*row(aI)
scale = A(i,jcur)/A(icur,jcur);
for(j=jcur;j<n;j++) A(i,j) -= A(icur,j)*scale;
for(j=0;j<p;j++) B(i,j) -= B(icur,j)*scale;
A(i,jcur)=Zero;
}
icur++;
}
else {
//either zero pivot, or very small one
//set to zero to reduce numerical difficulties later
for(i=icur;i<m;i++) A(i,jcur)=Zero;
}
}
return Max(m-icur,n-jcur);
}