void CroutMatrix::lubksb(Real* B, int mini)
{
REPORT
Tracer trace("Crout(lubksb)");
if (sing) Throw(SingularException(*this));
int i, j, ii = nrows; // ii initialised : B might be all zeros
for (i = 0; i < nrows; i++)
{
int ip = indx[i]; Real temp = B[ip]; B[ip] = B[i]; B[i] = temp;
if (temp != 0.0) { ii = i; break; }
}
Real* bi; Real* ai;
i = ii + 1;
if (i < nrows)
{
bi = B + ii; ai = store + ii + i * nrows;
for (;;)
{
int ip = indx[i]; Real sum = B[ip]; B[ip] = B[i];
Real* aij = ai; Real* bj = bi; j = i - ii;
while (j--) sum -= *aij++ * *bj++;
B[i] = sum;
if (++i == nrows) break;
ai += nrows;
}
}
ai = store + nrows * nrows;
for (i = nrows - 1; i >= mini; i--)
{
Real* bj = B+i; ai -= nrows; Real* ajx = ai+i;
Real sum = *bj; Real diag = *ajx;
j = nrows - i; while(--j) sum -= *(++ajx) * *(++bj);
B[i] = sum / diag;
}
}
void QRZ(Matrix& X, UpperTriangularMatrix& U)
{
REPORT
Tracer et("QRZ(1)");
int n = X.Nrows(); int s = X.Ncols(); U.ReSize(s); U = 0.0;
Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
int j, k; int J = s; int i = s;
while (i--)
{
Real* xj0 = xi0; Real* xi = xi0; k = n;
if (k) for (;;)
{
u = u0; Real Xi = *xi; Real* xj = xj0;
j = J; while(j--) *u++ += Xi * *xj++;
if (!(--k)) break;
xi += s; xj0 += s;
}
Real sum = sqrt(*u0); *u0 = sum; u = u0+1;
if (sum == 0.0) Throw(SingularException(U));
int J1 = J-1; j = J1; while(j--) *u++ /= sum;
xj0 = xi0; xi = xi0++; k = n;
if (k) for (;;)
{
u = u0+1; Real Xi = *xi; Real* xj = xj0;
Xi /= sum; *xj++ = Xi;
j = J1; while(j--) *xj++ -= *u++ * Xi;
if (!(--k)) break;
xi += s; xj0 += s;
}
u0 += J--;
}
}
void BandLUsolverPartialPivot<TYPE>::Solve(const BaseMatrix<TYPE> &in, BaseMatrix<TYPE> &out) const
{
assert(in.Nrows() == combine.Ncols());
assert(in.Nrows() == out.Nrows() && in.Ncols() == out.Ncols());
if (LinearEquationSolver<TYPE>::IsFailed())
{
Singleton<Tracer>::Instance()->AddMessage("BandLUsolverPartialPivot::Solve");
throw SingularException(BandLUsolver<TYPE>::mat);
}
const PermuteMatrix<TYPE> &lp = BandLUsolver<TYPE>::left;
Matrix<TYPE> t(in.Nrows(), in.Ncols());
lm.Solve(c_perm(lp, in), t);
um.Solve(t, out);
}
void CroutMatrix::lubksb(Real* B, int mini)
{
REPORT
// this has been adapted from Numerical Recipes in C. The code has been
// substantially streamlined, so I do not think much of the original
// copyright remains. However there is not much opportunity for
// variation in the code, so it is still similar to the NR code.
// I follow the NR code in skipping over initial zeros in the B vector.
Tracer trace("Crout(lubksb)");
if (sing) Throw(SingularException(*this));
int i, j, ii = nrows; // ii initialised : B might be all zeros
// scan for first non-zero in B
for (i = 0; i < nrows; i++)
{
int ip = indx[i]; Real temp = B[ip]; B[ip] = B[i]; B[i] = temp;
if (temp != 0.0) { ii = i; break; }
}
Real* bi; Real* ai;
i = ii + 1;
if (i < nrows)
{
bi = B + ii; ai = store + ii + i * nrows;
for (;;)
{
int ip = indx[i]; Real sum = B[ip]; B[ip] = B[i];
Real* aij = ai; Real* bj = bi; j = i - ii;
while (j--) sum -= *aij++ * *bj++;
B[i] = sum;
if (++i == nrows) break;
ai += nrows;
}
}
ai = store + nrows * nrows;
for (i = nrows - 1; i >= mini; i--)
{
Real* bj = B+i; ai -= nrows; Real* ajx = ai+i;
Real sum = *bj; Real diag = *ajx;
j = nrows - i; while(--j) sum -= *(++ajx) * *(++bj);
B[i] = sum / diag;
}
}
void ConstantSolver<TYPE>::Solve(const BaseMatrix<TYPE> &in, BaseMatrix<TYPE> &out) const
{
if (LinearEquationSolver<TYPE>::IsFailed())
{
Singleton<Tracer>::Instance()->AddMessage("ConstantSolver::Solve");
throw SingularException(SimpleSolver<TYPE>::mat);
}
int r = SimpleSolver<TYPE>::mat.Nrows();
int c = SimpleSolver<TYPE>::mat.Ncols();
assert(r == 1 && c == 1 && c == in.Nrows());
assert(in.Ncols() == out.Ncols() && in.Nrows() == out.Nrows());
const BaseMatrix<TYPE> &m = SimpleSolver<TYPE>::mat;
for (int i = 1; i <= c; ++i)
{
for (int j = r; j >= 1; --j)
{
out(j, i) = in(j, i) / m(j, j);
}
}
}
void QRZT(Matrix& X, LowerTriangularMatrix& L)
{
REPORT
Tracer et("QZT(1)");
int n = X.Ncols(); int s = X.Nrows(); L.ReSize(s);
Real* xi = X.Store(); int k;
for (int i=0; i<s; i++)
{
Real sum = 0.0;
Real* xi0=xi; k=n; while(k--) { sum += square(*xi++); }
sum = sqrt(sum);
L.element(i,i) = sum;
if (sum==0.0) Throw(SingularException(L));
Real* xj0=xi0; k=n; while(k--) { *xj0++ /= sum; }
for (int j=i+1; j<s; j++)
{
sum=0.0;
xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
L.element(j,i) = sum;
}
}
}
void IdentitySolver<TYPE>::Solve(const BaseMatrix<TYPE> &in, BaseMatrix<TYPE> &out) const
{
if (LinearEquationSolver<TYPE>::IsFailed())
{
Singleton<Tracer>::Instance()->AddMessage("IdentitySolver::Solve");
throw SingularException(SimpleSolver<TYPE>::mat);
}
int n = SimpleSolver<TYPE>::mat.Nrows();
assert(n == in.Nrows());
assert(in.Ncols() == out.Ncols() && in.Nrows() == out.Nrows());
TYPE t = SimpleSolver<TYPE>::mat(1, 1);
int c = in.Ncols();
for (int i = 1; i <= c; ++i)
{
for (int j = n; j >= 1; --j)
{
out(j, i) = in(j, i) / t;
}
}
}
