/// <summary>
/// Cholesky algorithm for symmetric and positive definite matrix
/// </summary>
/// <param name="mat3d">A Square, symmetric matrix.</param>
public CholeskyDecomposition(Matrix3d mat3d)
{
// Initialize.
double[,] A = mat3d.ToArray();
n = 3;
L = new double[n, 3];
isspd = (mat3d.ColumnLength() == n);
// Main loop.
for (int j = 0; j < n; j++)
{
double d = 0.0f;
for (int k = 0; k < j; k++)
{
double s = 0.0f;
for (int i = 0; i < k; i++)
{
s += L[k, i] * L[j, i];
}
L[j, k] = s = (A[j, k] - s) / L[k, k];
d = d + s * s;
isspd = isspd & (A[k, j] == A[j, k]);
}
d = A[j, j] - d;
isspd = isspd & (d > 0);
L[j, j] = Convert.ToSingle(Math.Sqrt(Math.Max(d, 0)));
for (int k = j + 1; k < n; k++)
{
L[j, k] = 0.0f;
}
}
}
/* ------------------------
* Constructor
* ------------------------ */
///<summary> Check for symmetry, then construct the eigenvalue decomposition</summary>
///<param name="mat3D">Square matrix</param>
///<returns>Structure to access D and V.</returns>
public EigenvalueDecomposition(Matrix3d mat3D)
{
double[,] A = mat3D.ToArray();
this.n = mat3D.ColumnLength();
int m = mat3D.RowLength();
Varray = new double[n, m];
d = new double[n];
e = new double[n];
issymmetric = true;
for (int j = 0; (j < n) & issymmetric; j++)
{
for (int i = 0; (i < n) & issymmetric; i++)
{
issymmetric = (A[i, j] == A[j, i]);
}
}
if (issymmetric)
{
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
Varray[i, j] = A[i, j];
}
}
// Tridiagonalize.
tred2();
// Diagonalize.
tql2();
}
else
{
H = new double[n, m];
ort = new double[n];
for (int j = 0; j < n; j++)
{
for (int i = 0; i < n; i++)
{
H[i, j] = A[i, j];
}
}
// Reduce to Hessenberg form.
orthes();
// Reduce Hessenberg to real Schur form.
hqr2();
}
}
public static DataTable ToDataTable(this Matrix3d mat)
{
DataTable dt = new DataTable();
for (int i = 0; i < mat.ColumnLength(); i++)
{
dt.Columns.Add("col" + i, typeof(double));
}
for (int iRow = 0; iRow < 3; iRow++)
{
DataRow dr = dt.NewRow();
for (int iCol = 0; iCol < mat.ColumnLength(); iCol++)
{
dr[iCol] = (double)mat[iCol, iRow];
}
dt.Rows.Add(dr);
}
return(dt);
}
///<Sumary>Least squares solution of A*X = B</Sumary>
///<param name="B">A Matrix with as many rows as A and any number of columns.</param>
///<returns> X that minimizes the two norm of Q*R*X-B.</returns>
public Matrix3d Solve(Matrix3d B)
{
if (B.RowLength() != m)
{
throw new ArgumentException("Matrix row dimensions must agree.");
}
if (!IsFullRank())
{
throw new Exception("Matrix is rank deficient.");
}
// Copy right hand side
int nx = B.ColumnLength();
double[,] X = B.ToArray();
// Compute Y = transpose(Q)*B
for (int k = 0; k < n; k++)
{
for (int j = 0; j < nx; j++)
{
double s = 0.0f;
for (int i = k; i < m; i++)
{
s += QR[i, k] * X[i, j];
}
s = -s / QR[k, k];
for (int i = k; i < m; i++)
{
X[i, j] += s * QR[i, k];
}
}
}
// Solve R*X = Y;
for (int k = n - 1; k >= 0; k--)
{
for (int j = 0; j < nx; j++)
{
X[k, j] /= Rdiag[k];
}
for (int i = 0; i < k; i++)
{
for (int j = 0; j < nx; j++)
{
X[i, j] -= X[k, j] * QR[i, k];
}
}
}
Matrix3d mat = new Matrix3d();
mat = mat.FromDoubleArray(X);
return(mat.GetMatrix(0, n - 1, 0, nx - 1));
}
/* ------------------------
* Constructor
* ------------------------ */
///<summary>QR Decomposition, computed by Householder reflections.</summary>
///<param name="A">Rectangular matrix</param>
///<returns>Structure to access R and the Householder vectors and compute Q.</returns>
public QRDecomposition(Matrix3d A)
{
// Initialize.
QR = A.ToArray();
m = A.RowLength();
n = A.ColumnLength();
Rdiag = new double[n];
// Main loop.
for (int k = 0; k < n; k++)
{
// Compute 2-norm of k-th column without under/overflow.
double nrm = 0;
for (int i = k; i < m; i++)
{
nrm = Maths.Hypot(nrm, QR[i, k]);
}
if (nrm != 0)
{
// Form k-th Householder vector.
if (QR[k, k] < 0)
{
nrm = -nrm;
}
for (int i = k; i < m; i++)
{
QR[i, k] /= nrm;
}
QR[k, k] += 1.0f;
// Apply transformation to remaining columns.
for (int j = k + 1; j < n; j++)
{
double s = 0.0f;
for (int i = k; i < m; i++)
{
s += QR[i, k] * QR[i, j];
}
s = -s / QR[k, k];
for (int i = k; i < m; i++)
{
QR[i, j] += s * QR[i, k];
}
}
}
Rdiag[k] = -nrm;
}
}
///<summary>Solve A*X = B</summary>
///<param name="B">A Matrix with as many rows as A and any number of columns.</param>
///<returns>X so that L*L'*X = B</returns>
///<exception cref="ArgumentException">System.ArgumentException Matrix row dimensions must agree.</exception>
///<exception cref="Exception">System.Exception Matrix is not symmetric positive definite.</exception>
public Matrix3d Solve(Matrix3d B)
{
if (B.RowLength() != n)
{
throw new ArgumentException("Matrix row dimensions must agree.");
}
if (!isspd)
{
throw new Exception("Matrix is not symmetric positive definite.");
}
// Copy right hand side.
double[,] X = B.ToArray();
int nx = B.ColumnLength();
// Solve L*Y = B;
for (int k = 0; k < n; k++)
{
for (int j = 0; j < nx; j++)
{
for (int i = 0; i < k; i++)
{
X[k, j] -= X[i, j] * L[k, i];
}
X[k, j] /= L[k, k];
}
}
// Solve L'*X = Y;
for (int k = n - 1; k >= 0; k--)
{
for (int j = 0; j < nx; j++)
{
for (int i = k + 1; i < n; i++)
{
X[k, j] -= X[i, j] * L[i, k];
}
X[k, j] /= L[k, k];
}
}
Matrix3d mat = new Matrix3d();
mat = mat.FromDoubleArray(X);
return(mat);
}
/** Solve A*X = B
* @param B A Matrix with as many rows as A and any number of columns.
* @return X so that L*U*X = B(piv,:)
* @exception System.ArgumentException Matrix row dimensions must agree.
* @exception System.Exception Matrix is singular.
*/
public Matrix3d solve(Matrix3d B)
{
if (B.RowLength() != m)
{
throw new ArgumentException("Matrix row dimensions must agree.");
}
if (!this.IsNonsingular())
{
throw new Exception("Matrix is singular.");
}
// Copy right hand side with pivoting
int nx = B.ColumnLength();
Matrix3d Xmat = B.GetMatrix(piv, 0, nx - 1);
double[,] X = Xmat.ToArray();
// Solve L*Y = B(piv,:)
for (int k = 0; k < n; k++)
{
for (int i = k + 1; i < n; i++)
{
for (int j = 0; j < nx; j++)
{
X[i, j] -= X[k, j] * LU[i, k];
}
}
}
// Solve U*X = Y;
for (int k = n - 1; k >= 0; k--)
{
for (int j = 0; j < nx; j++)
{
X[k, j] /= LU[k, k];
}
for (int i = 0; i < k; i++)
{
for (int j = 0; j < nx; j++)
{
X[i, j] -= X[k, j] * LU[i, k];
}
}
}
return(Xmat);
}
/// <summary>
/// Converts matrix to a string
/// </summary>
/// <param name="matrixFrontCap">placed at the beginning of the string</param>
/// <param name="rowFrontCap">placed at the beginning of each row</param>
/// <param name="rowDelimiter">delimiter for rows</param>
/// <param name="columnDelimiter">delimiter for Columns</param>
/// <param name="rowEndCap">placed at the end of each row</param>
/// <param name="matrixEndCap">placed at the beginning of the string</param>
/// <returns></returns>
public static string ToString(this Matrix3d mat, string matrixFrontCap, string rowFrontCap, string rowDelimiter, string columnDelimiter, string rowEndCap, string matrixEndCap)
{
StringBuilder sb = new StringBuilder(); // start on new line.
sb.Append(matrixFrontCap);
int n = mat.ColumnLength();
int m = mat.RowLength();
for (int i = 0; i < m; i++)
{
sb.Append(rowFrontCap);
for (int j = 0; j < n; j++)
{
sb.Append(mat[i, j].ToString("R")); //Round-trip is necessary
sb.Append((j < n - 1) ? columnDelimiter : "");
}
sb.Append(rowEndCap);
sb.Append((i < m - 1) ? rowDelimiter : "");
}
sb.Append(matrixEndCap);
return(sb.ToString());
}
public Matrix3d LUDecompose(Matrix3d A)
{
// Use a "left-looking", dot-product, Crout/Doolittle algorithm.
LU = A.ToArray();
m = A.RowLength();
n = A.ColumnLength();
piv = new int[m];
for (int i = 0; i < m; i++)
{
piv[i] = i;
}
pivsign = 1;
//double[] LUrowi;
//double[] LUcolj = new double[m];
// Outer loop.
for (int j = 0; j < n; j++)
{
// Make a copy of the j-th column to localize references.
//for (int i = 0; i < m; i++)
//{
// LUcolj[i] = LU[i,j];
//}
// Apply previous transformations.
for (int i = 0; i < m; i++)
{
// Most of the time is spent in the following dot product.
int kmax = Math.Min(i, j);
double s = 0.0f;
for (int k = 0; k < kmax; k++)
{
s += LU[i, k] * LU[k, i];
}
LU[j, i] = LU[i, j] -= s;
}
// Find pivot and exchange if necessary.
int p = j;
for (int i = j + 1; i < m; i++)
{
if (Math.Abs(LU[i, j]) > Math.Abs(LU[p, j]))
{
p = i;
}
}
if (p != j)
{
for (int k = 0; k < n; k++)
{
double t = LU[p, k]; LU[p, k] = LU[j, k]; LU[j, k] = t;
}
int k2 = piv[p]; piv[p] = piv[j]; piv[j] = k2;
pivsign = -pivsign;
}
// Compute multipliers.
if (j < m && LU[j, j] != 0)
{
for (int i = j + 1; i < m; i++)
{
LU[i, j] /= LU[j, j];
}
}
}
Matrix3d mat = new Matrix3d();
mat = mat.FromDoubleArray(LU);
return(mat);
}
