public PZMath_permutation(PZMath_permutation p)
{
size = p.Size;
data = new PZMath_vector(size);
for (int i = 0; i < size; i ++)
data[i] = p[i];
}
/// <summary>
/// LU decomposition and LU inverse example
/// </summary>
public static void LUExample()
{
double[,] m = {{2, 3, 3, 5},
{6, 6, 8, 9},
{10, 11, 12, 13},
{14, 15, 17, 17}};
PZMath_matrix matrix = new PZMath_matrix(m);
matrix.ScreenWriteLine();
// LU decomposition
// correct answer:
// L =
// 1.0000 0 0 0
// 0.1429 1.0000 0 0
// 0.4286 -0.5000 1.0000 0
// 0.7143 0.3333 -0.3333 1.0000
// U =
// 14.0000 15.0000 17.0000 17.0000
// 0 0.8571 0.5714 2.5714
// 0 0 1.0000 3.0000
// 0 0 0 1.0000
// P =
// 0 0 0 1
// 1 0 0 0
// 0 1 0 0
// 0 0 1 0
PZMath_permutation p = new PZMath_permutation(matrix.RowCount);
PZMath_matrix LU = matrix.LUDecomp(p);
LU.ScreenWriteLine();
// LU Invert
// correct answer:
// inv(m) =
// -2.0833 0.6667 3.5000 -2.4167
// 1.0000 -1.0000 -1.0000 1.0000
// 1.0000 0 -3.0000 2.0000
// -0.1667 0.3333 1.0000 -0.8333
PZMath_matrix inverse = matrix.LUInvert();
inverse.ScreenWriteLine();
}
/// <summary>
/// LU Invert
/// </summary>
/// <returns></returns>
public PZMath_matrix LUInvert()
{
PZMath_permutation p = new PZMath_permutation(row);
PZMath_matrix inverse = new PZMath_matrix(row, col);
PZMath_matrix LU = this.LUDecomp(p);
PZMath_linalg.LUInvert(LU, p, inverse);
return inverse;
}
/// <summary>
/// determination from LU decomp
/// </summary>
/// <returns></returns>
public double LUDet()
{
PZMath_permutation p = new PZMath_permutation(row);
PZMath_matrix LU = new PZMath_matrix(this);
int signum;
PZMath_linalg.LUDecomp(LU, p, out signum);
return PZMath_linalg.LUDet(LU, signum);
}
/// <summary>
/// LU decomposition, P A = L U, return LU
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public PZMath_matrix LUDecomp(PZMath_permutation p)
{
PZMath_matrix LU = new PZMath_matrix(this);
int signum;
PZMath_linalg.LUDecomp(LU, p, out signum);
return LU;
}
public static int QRPTDecomp(PZMath_matrix A, PZMath_vector tau, PZMath_permutation p, out int signum, PZMath_vector norm)
{
int M = A.RowCount;
int N = A.ColumnCount;
signum = 0;
if (tau.Size != System.Math.Min(M, N))
{
PZMath_errno.ERROR("size of tau must be MIN(M,N)", PZMath_errno.PZMath_EBADLEN);
}
else if (p.Size != N)
{
PZMath_errno.ERROR("permutation size must be N", PZMath_errno.PZMath_EBADLEN);
}
else if (norm.Size != N)
{
PZMath_errno.ERROR("norm size must be N", PZMath_errno.PZMath_EBADLEN);
}
else
{
int i;
signum = 1;
p.Init(); /* set to identity */
/* Compute column norms and store in workspace */
for (i = 0; i < N; i++)
{
PZMath_vector c = A.Column(i);
double x = PZMath_blas.Dnrm2(c);
norm[i] = x;
}
for (i = 0; i < System.Math.Min(M, N); i++)
{
/* Bring the column of largest norm into the pivot position */
double max_norm = norm[i];
int j;
int kmax = i;
for (j = i + 1; j < N; j++)
{
double x = norm[j];
if (x > max_norm)
{
max_norm = x;
kmax = j;
}
}
if (kmax != i)
{
A.SwapColumns(i, kmax);
p.Swap(i, kmax);
norm.Swap(i, kmax);
signum = - signum;
}
/* Compute the Householder transformation to reduce the j-th
column of the matrix to a multiple of the j-th unit vector */
{
PZMath_vector c_full = A.Column(i);
PZMath_vector c = c_full.SubVector(i, M - i);
double tau_i = HouseholderTransform(c);
tau[i] = tau_i;
/* Apply the transformation to the remaining columns */
if (i + 1 < N)
{
PZMath_matrix m = A.Submatrix(i, i + 1, M - i, N - (i + 1));
HouseholderHM (tau_i, c, m);
}
}
/* Update the norms of the remaining columns too */
if (i + 1 < M)
{
for (j = i + 1; j < N; j++)
{
double x = norm[j];
if (x > 0.0)
{
double y = 0;
double temp= A[i, j] / x;
if (System.Math.Abs(temp) >= 1)
y = 0.0;
else
y = x * System.Math.Sqrt(1 - temp * temp);
/* recompute norm to prevent loss of accuracy */
if (System.Math.Abs(y / x) < System.Math.Sqrt(20.0) * PZMath_machine.PZMath_SQRT_DBL_EPSILON)
{
PZMath_vector c_full = A.Column(j);
PZMath_vector c = c_full.SubVector(i + 1, M - (i + 1));
y = PZMath_blas.Dnrm2(c);
}
norm[j] = y;
}
}
}
}
}
return PZMath_errno.PZMath_SUCCESS;
}
// These functions solve the square system A x = b in-place using the LU decomposition of A into (LU,p).
// On input x should contain the right-hand side b,
// which is replaced by the solution on output.
public static int LUSVX(PZMath_matrix LU, PZMath_permutation p, PZMath_vector x)
{
if (LU.ColumnCount != LU.RowCount)
{
PZMath_errno.ERROR("PZMath_linalg::LUSVX(), LU matrix must be square", PZMath_errno.PZMath_ENOTSQR);
}
else if (LU.ColumnCount != p.Size)
{
PZMath_errno.ERROR("PZMath_linalg::LUSVX(), permutation length must match matrix size", PZMath_errno.PZMath_EBADLEN);
}
else if (LU.ColumnCount != x.Size)
{
PZMath_errno.ERROR("PZMath_linalg::LUSVX(), matrix size must match solution/rhs size", PZMath_errno.PZMath_EBADLEN);
}
else
{
/* Apply permutation to RHS */
p.Permute(x);
// todo here
/* Solve for c using forward-substitution, L c = P b */
PZMath_blas.Dtrsv(CBLAS_UPLO.CblasLower, CBLAS_TRANSPOSE.CblasNoTrans, CBLAS_DIAG.CblasUnit, LU, x);
/* Perform back-substitution, U x = c */
PZMath_blas.Dtrsv(CBLAS_UPLO.CblasUpper, CBLAS_TRANSPOSE.CblasNoTrans, CBLAS_DIAG.CblasNonUnit, LU, x);
}
return PZMath_errno.PZMath_SUCCESS;
}
// These functions compute the inverse of a matrix A from its LU decomposition (LU,p),
// storing the result in the matrix inverse.
// The inverse is computed by solving the system A x = b for each column of the identity matrix.
// It is preferable to avoid direct use of the inverse whenever possible,
// as the linear solver functions can obtain the same result more efficiently and reliably
// (consult any introductory textbook on numerical linear algebra for details).
public static int LUInvert(PZMath_matrix LU, PZMath_permutation p, PZMath_matrix inverse)
{
int i;
int n = LU.RowCount;
int status = PZMath_errno.PZMath_SUCCESS;
inverse.SetIdentity();
for (i = 0; i < n; i++)
{
PZMath_vector c = inverse.Column(i);
int status_i = LUSVX(LU, p, c);
if (status_i == PZMath_errno.PZMath_SUCCESS)
status = status_i;
}
return status;
}
// These functions factorize the square matrix A into the LU decomposition PA = LU.
// On output the diagonal and upper triangular part of the input matrix A contain the matrix U.
// The lower triangular part of the input matrix (excluding the diagonal) contains L.
// The diagonal elements of L are unity, and are not stored.
// The permutation matrix P is encoded in the permutation p.
// The j-th column of the matrix P is given by the k-th column of the identity matrix,
// where k = p_j the j-th element of the permutation vector.
// The sign of the permutation is given by signum.
// It has the value (-1)^n, where n is the number of interchanges in the permutation.
// The algorithm used in the decomposition is Gaussian Elimination with partial pivoting
// (Golub & Van Loan, Matrix Computations, Algorithm 3.4.1).
public static int LUDecomp(PZMath_matrix A, PZMath_permutation p, out int signum)
{
if (A.ColumnCount != A.RowCount)
{
PZMath_errno.ERROR("PZMath_linalg::LUDecomp(), LU decomposition requires square matrix", PZMath_errno.PZMath_ENOTSQR);
signum = 1;
return PZMath_errno.PZMath_ENOTSQR;
}
else if (p.Size != A.ColumnCount)
{
PZMath_errno.ERROR("PZMath_linalg::LUDecomp(), permutation length must match matrix size", PZMath_errno.PZMath_EBADLEN);
signum = 1;
return PZMath_errno.PZMath_EBADLEN;
}
else
{
int N = A.ColumnCount;
int i, j, k;
signum = 1;
p.Init();
for (j = 0; j < N - 1; j++)
{
/* Find maximum in the j-th column */
double ajj;
double max = System.Math.Abs(A[j, j]);
int i_pivot = j;
for (i = j + 1; i < N; i++)
{
double aij = System.Math.Abs(A[i, j]);
if (aij > max)
{
max = aij;
i_pivot = i;
}
}
if (i_pivot != j)
{
A.SwapRows(j, i_pivot);
p.Swap(j, i_pivot);
signum = -(signum);
}
ajj = A[j, j];
if (ajj != 0.0)
{
for (i = j + 1; i < N; i++)
{
double aij = A[i, j] / ajj;
A[i, j] = aij;
for (k = j + 1; k < N; k++)
{
double aik = A[i, k];
double ajk = A[j, k];
A[i, k] = aik - aij * ajk;
}
}
}
}
return PZMath_errno.PZMath_SUCCESS;
}
}
// calculate covar
public void Covar(double epsrel, PZMath_matrix covar)
{
double tolr;
int i, j, k;
int kmax = 0;
PZMath_matrix r;
PZMath_vector tau;
PZMath_vector norm;
PZMath_permutation perm;
int m = J.RowCount;
int n = J.ColumnCount ;
if (m < n)
PZMath_errno.ERROR ("PZMath_multifit_fdfsolver::Covar(), Jacobian be rectangular M x N with M >= N", PZMath_errno.PZMath_EBADLEN);
if (covar.ColumnCount != covar.RowCount || covar.RowCount != n)
PZMath_errno.ERROR ("PZMath_multifit_fdfsolver::Covar(), covariance matrix must be square and match second dimension of jacobian", PZMath_errno.PZMath_EBADLEN);
r = new PZMath_matrix(m, n);
tau = new PZMath_vector(n);
perm = new PZMath_permutation(n);
norm = new PZMath_vector(n);
{
int signum = 0;
r.MemCopyFrom(J);
PZMath_linalg.QRPTDecomp(r, tau, perm, out signum, norm);
}
/* Form the inverse of R in the full upper triangle of R */
tolr = epsrel * System.Math.Abs(r[0, 0]);
for (k = 0 ; k < n ; k++)
{
double rkk = r[k, k];
if (System.Math.Abs(rkk) <= tolr)
{
break;
}
r[k, k] = 1.0 / rkk;
for (j = 0; j < k ; j++)
{
double t = r[j, k] / rkk;
r[j, k] = 0.0;
for (i = 0; i <= j; i++)
{
double rik = r[i, k];
double rij = r[i, j];
r[i, k] = rik - t * rij;
}
}
kmax = k;
}
/* Form the full upper triangle of the inverse of R^T R in the full
upper triangle of R */
for (k = 0; k <= kmax ; k++)
{
for (j = 0; j < k; j++)
{
double rjk = r[j, k];
for (i = 0; i <= j ; i++)
{
double rij = r[i, j];
double rik = r[i, k];
r[i, j] = rij + rjk * rik;
}
}
{
double t = r[k, k];
for (i = 0; i <= k; i++)
{
double rik = r[i, k];
r[i, k] = t * rik;
}
}
}
/* Form the full lower triangle of the covariance matrix in the
strict lower triangle of R and in w */
for (j = 0 ; j < n ; j++)
{
int pj = (int) perm[j];
for (i = 0; i <= j; i++)
{
int pi = (int) perm[i];
double rij;
if (j > kmax)
{
r[i, j] = 0.0;
rij = 0.0 ;
}
else
{
rij = r[i, j];
}
if (pi > pj)
{
r[pi, pj] = rij;
}
else if (pi < pj)
{
r[pj, pi] = rij;
}
}
{
double rjj = r[j, j];
covar[pj, pj] = rjj;
}
}
/* symmetrize the covariance matrix */
for (j = 0 ; j < n ; j++)
{
for (i = 0; i < j ; i++)
{
double rji = r[j, i];
covar[j, i] = rji;
covar[i, j] = rji;
}
}
}