/// <summary>
/// Places the values in the this GMatrix into the matrix m1;
/// m1 should be at least as large as this GMatrix.
/// </summary>
/// <remarks>
/// Places the values in the this GMatrix into the matrix m1;
/// m1 should be at least as large as this GMatrix.
/// </remarks>
/// <param name="m1">The matrix that will hold the new values</param>
public void Get(GMatrix m1)
{
int i;
int j;
int nc;
int nr;
if (nCol < m1.nCol)
{
nc = nCol;
}
else
{
nc = m1.nCol;
}
if (nRow < m1.nRow)
{
nr = nRow;
}
else
{
nr = m1.nRow;
}
for (i = 0; i < nr; i++)
{
for (j = 0; j < nc; j++)
{
m1.values[i][j] = values[i][j];
}
}
for (i = nr; i < m1.nRow; i++)
{
for (j = 0; j < m1.nCol; j++)
{
m1.values[i][j] = 0.0;
}
}
for (j = nc; j < m1.nCol; j++)
{
for (i = 0; i < nr; i++)
{
m1.values[i][j] = 0.0;
}
}
}
/// <summary>
/// Sets the value of this matrix to the result of multiplying
/// the two argument matrices together (this = m1 * m2).
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the result of multiplying
/// the two argument matrices together (this = m1 * m2).
/// </remarks>
/// <param name="m1">the first matrix</param>
/// <param name="m2">the second matrix</param>
public void Mul(GMatrix m1, GMatrix m2)
{
int i;
int j;
int k;
if (m1.nCol != m2.nRow || nRow != m1.nRow || nCol != m2.nCol)
{
throw new MismatchedSizeException("GMatrix.mul(GMatrix, GMatrix) dimension mismatch ");
}
//double[][] tmp = new double[nRow][nCol];
double[][] tmp = new double[][] { new double[7], new double[7], new double[7], new
double[7], new double[7], new double[7], new double[7] };
for (i = 0; i < m1.nRow; i++)
{
for (j = 0; j < m2.nCol; j++)
{
tmp[i][j] = 0.0;
for (k = 0; k < m1.nCol; k++)
{
tmp[i][j] += m1.values[i][k] * m2.values[k][j];
}
}
}
values = tmp;
}
private static void Update_u_split(int topr, int bottomr, GMatrix u, double
[] cosl, double[] sinl, GMatrix t, GMatrix m)
{
int j;
double utemp;
for (j = 0; j < u.nCol; j++)
{
utemp = u.values[topr][j];
u.values[topr][j] = cosl[0] * utemp - sinl[0] * u.values[bottomr][j];
u.values[bottomr][j] = sinl[0] * utemp + cosl[0] * u.values[bottomr][j];
}
System.Console.Out.WriteLine("\nm=");
CheckMatrix(m);
System.Console.Out.WriteLine("\nu=");
CheckMatrix(t);
m.Mul(t, m);
System.Console.Out.WriteLine("\nt*m=");
CheckMatrix(m);
}
private static void Update_v_split(int topr, int bottomr, GMatrix v, double
[] cosr, double[] sinr, GMatrix t, GMatrix m)
{
int j;
double vtemp;
for (j = 0; j < v.nRow; j++)
{
vtemp = v.values[j][topr];
v.values[j][topr] = cosr[0] * vtemp - sinr[0] * v.values[j][bottomr];
v.values[j][bottomr] = sinr[0] * vtemp + cosr[0] * v.values[j][bottomr];
}
System.Console.Out.WriteLine("topr =" + topr);
System.Console.Out.WriteLine("bottomr =" + bottomr);
System.Console.Out.WriteLine("cosr =" + cosr[0]);
System.Console.Out.WriteLine("sinr =" + sinr[0]);
System.Console.Out.WriteLine("\nm =");
CheckMatrix(m);
System.Console.Out.WriteLine("\nv =");
CheckMatrix(t);
m.Mul(m, t);
System.Console.Out.WriteLine("\nt*m =");
CheckMatrix(m);
}
private static void Print_m(GMatrix m, GMatrix u, GMatrix
v)
{
GMatrix mtmp = new GMatrix(m.nCol, m.nRow);
mtmp.Mul(u, mtmp);
mtmp.Mul(mtmp, v);
System.Console.Out.WriteLine("\n m = \n" + GMatrix.ToString(mtmp));
}
private static string ToString(GMatrix m)
{
StringBuilder buffer = new StringBuilder(m.nRow * m.nCol * 8);
int i;
int j;
for (i = 0; i < m.nRow; i++)
{
for (j = 0; j < m.nCol; j++)
{
if (Math.Abs(m.values[i][j]) < .000000001)
{
buffer.Append("0.0000 ");
}
else
{
buffer.Append(m.values[i][j]).Append(" ");
}
}
buffer.Append("\n");
}
return buffer.ToString();
}
/// <summary>
/// Sets the value of this matrix to the matrix difference
/// of matrices m1 and m2 (this = m1 - m2).
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix difference
/// of matrices m1 and m2 (this = m1 - m2).
/// </remarks>
/// <param name="m1">the first matrix</param>
/// <param name="m2">the second matrix</param>
public void Sub(GMatrix m1, GMatrix m2)
{
int i;
int j;
if (m2.nRow != m1.nRow)
{
throw new MismatchedSizeException("GMatrix.sub(GMatrix, GMatrix): row dimension mismatch");
}
if (m2.nCol != m1.nCol)
{
throw new MismatchedSizeException("GMatrix.sub(GMatrix, GMatrix): column dimension mismatch");
}
if (nRow != m1.nRow || nCol != m1.nCol)
{
throw new MismatchedSizeException("GMatrix.sub(GMatrix, GMatrix): input matrix dimensions do not match dimensions for this matrix");
}
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
values[i][j] = m1.values[i][j] - m2.values[i][j];
}
}
}
private static void Chase_up(double[] s, double[] e, int k, GMatrix v)
{
double f;
double g;
double r;
double[] cosr = new double[1];
double[] sinr = new double[1];
int i;
GMatrix t = new GMatrix(v.nRow, v.nCol);
GMatrix m = new GMatrix(v.nRow, v.nCol);
f = e[k];
g = s[k];
for (i = k; i > 0; i--)
{
r = Compute_rot(f, g, sinr, cosr);
f = -e[i - 1] * sinr[0];
g = s[i - 1];
s[i] = r;
e[i - 1] = e[i - 1] * cosr[0];
Update_v_split(i, k + 1, v, cosr, sinr, t, m);
}
s[i + 1] = Compute_rot(f, g, sinr, cosr);
Update_v_split(i, k + 1, v, cosr, sinr, t, m);
}
/// <summary>Sets the value of this matrix to the values found in matrix m1.</summary>
/// <remarks>Sets the value of this matrix to the values found in matrix m1.</remarks>
/// <param name="m1">the source matrix</param>
public void Set(GMatrix m1)
{
int i;
int j;
if (nRow < m1.nRow || nCol < m1.nCol)
{
nRow = m1.nRow;
nCol = m1.nCol;
//values = new double[nRow][nCol];
values = new double[][] { new double[7], new double[7], new double[7], new double
[7], new double[7], new double[7], new double[7] };
}
for (i = 0; i < Math.Min(nRow, m1.nRow); i++)
{
for (j = 0; j < Math.Min(nCol, m1.nCol); j++)
{
values[i][j] = m1.values[i][j];
}
}
for (i = m1.nRow; i < nRow; i++)
{
// pad rest or matrix with zeros
for (j = m1.nCol; j < nCol; j++)
{
values[i][j] = 0.0;
}
}
}
/// <summary>
/// Sets the value of this matrix to the matrix difference of itself
/// and matrix m1 (this = this - m1).
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix difference of itself
/// and matrix m1 (this = this - m1).
/// </remarks>
/// <param name="m1">the other matrix</param>
public void Sub(GMatrix m1)
{
int i;
int j;
if (nRow != m1.nRow)
{
throw new MismatchedSizeException("GMatrix.sub(GMatrix): row dimension mismatch");
}
if (nCol != m1.nCol)
{
throw new MismatchedSizeException("GMatrix.sub(GMatrix): column dimension mismatch");
}
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
values[i][j] = values[i][j] - m1.values[i][j];
}
}
}
/// <summary>
/// Sets the value of this matrix equal to the negation of
/// of the GMatrix parameter.
/// </summary>
/// <remarks>
/// Sets the value of this matrix equal to the negation of
/// of the GMatrix parameter.
/// </remarks>
/// <param name="m1">The source matrix</param>
public void Negate(GMatrix m1)
{
int i;
int j;
if (nRow != m1.nRow || nCol != m1.nCol)
{
throw new MismatchedSizeException("GMatrix.negate(GMatrix, GMatrix): input matrix dimensions do not match dimensions for this matrix");
}
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
values[i][j] = -m1.values[i][j];
}
}
}
/// <summary>
/// Multiplies matrix m1 times the transpose of matrix m2, and
/// places the result into this.
/// </summary>
/// <remarks>
/// Multiplies matrix m1 times the transpose of matrix m2, and
/// places the result into this.
/// </remarks>
/// <param name="m1">The matrix on the left hand side of the multiplication</param>
/// <param name="m2">The matrix on the right hand side of the multiplication</param>
public void MulTransposeRight(GMatrix m1, GMatrix m2)
{
int i;
int j;
int k;
if (m1.nCol != m2.nCol || nCol != m2.nRow || nRow != m1.nRow)
{
throw new MismatchedSizeException("GMatrix.mulTransposeRight matrix dimension mismatch");
}
if (m1 == this || m2 == this)
{
//double[][] tmp = new double[nRow][nCol];
double[][] tmp = new double[][] { new double[7], new double[7], new double[7], new
double[7], new double[7], new double[7], new double[7] };
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
tmp[i][j] = 0.0;
for (k = 0; k < m1.nCol; k++)
{
tmp[i][j] += m1.values[i][k] * m2.values[j][k];
}
}
}
values = tmp;
}
else
{
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
values[i][j] = 0.0;
for (k = 0; k < m1.nCol; k++)
{
values[i][j] += m1.values[i][k] * m2.values[j][k];
}
}
}
}
}
/// <summary>
/// Constructs a new GMatrix and copies the initial values
/// from the parameter matrix.
/// </summary>
/// <remarks>
/// Constructs a new GMatrix and copies the initial values
/// from the parameter matrix.
/// </remarks>
/// <param name="matrix">the source of the initial values of the new GMatrix</param>
public GMatrix(GMatrix matrix)
{
nRow = matrix.nRow;
nCol = matrix.nCol;
//values = new double[nRow][nCol];
values = new double[][] { new double[7], new double[7], new double[7], new double
[7], new double[7], new double[7], new double[7] };
int i;
int j;
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
values[i][j] = matrix.values[i][j];
}
}
}
/// <summary>General invert routine.</summary>
/// <remarks>
/// General invert routine. Inverts m1 and places the result in "this".
/// Note that this routine handles both the "this" version and the
/// non-"this" version.
/// Also note that since this routine is slow anyway, we won't worry
/// about allocating a little bit of garbage.
/// </remarks>
internal void InvertGeneral(GMatrix m1)
{
int size = m1.nRow * m1.nCol;
double[] temp = new double[size];
double[] result = new double[size];
int[] row_perm = new int[m1.nRow];
int[] even_row_exchange = new int[1];
int i;
int j;
// Use LU decomposition and backsubstitution code specifically
// for floating-point nxn matrices.
if (m1.nRow != m1.nCol)
{
// Matrix is either under or over determined
throw new MismatchedSizeException("cannot invert non square matrix");
}
// Copy source matrix to temp
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
temp[i * nCol + j] = m1.values[i][j];
}
}
// Calculate LU decomposition: Is the matrix singular?
if (!LuDecomposition(m1.nRow, temp, row_perm, even_row_exchange))
{
// Matrix has no inverse
throw new SingularMatrixException("cannot invert matrix");
}
// Perform back substitution on the identity matrix
for (i = 0; i < size; i++)
{
result[i] = 0.0;
}
for (i = 0; i < nCol; i++)
{
result[i + i * nCol] = 1.0;
}
LuBacksubstitution(m1.nRow, temp, row_perm, result);
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
values[i][j] = result[i * nCol + j];
}
}
}
/// <summary>
/// Finds the singular value decomposition (SVD) of this matrix
/// such that this = U*W*transpose(V); and returns the rank of
/// this matrix; the values of U,W,V are all overwritten.
/// </summary>
/// <remarks>
/// Finds the singular value decomposition (SVD) of this matrix
/// such that this = U*W*transpose(V); and returns the rank of
/// this matrix; the values of U,W,V are all overwritten. Note
/// that the matrix V is output as V, and
/// not transpose(V). If this matrix is mxn, then U is mxm, W
/// is a diagonal matrix that is mxn, and V is nxn. Using the
/// notation W = diag(w), then the inverse of this matrix is:
/// inverse(this) = V*diag(1/w)*tranpose(U), where diag(1/w)
/// is the same matrix as W except that the reciprocal of each
/// of the diagonal components is used.
/// </remarks>
/// <param name="U">The computed U matrix in the equation this = U*W*transpose(V)</param>
/// <param name="W">The computed W matrix in the equation this = U*W*transpose(V)</param>
/// <param name="V">The computed V matrix in the equation this = U*W*transpose(V)</param>
/// <returns>The rank of this matrix.</returns>
public int Svd(GMatrix U, GMatrix W, GMatrix V)
{
// check for consistancy in dimensions
if (nCol != V.nCol || nCol != V.nRow)
{
throw new MismatchedSizeException("GMatrix.SVD: dimension mismatch with V matrix");
}
if (nRow != U.nRow || nRow != U.nCol)
{
throw new MismatchedSizeException("GMatrix.SVD: dimension mismatch with U matrix");
}
if (nRow != W.nRow || nCol != W.nCol)
{
throw new MismatchedSizeException("GMatrix.SVD: dimension mismatch with W matrix");
}
// Fix ArrayIndexOutOfBounds for 2x2 matrices, which partially
// addresses bug 4348562 for J3D 1.2.1.
//
// Does *not* fix the following problems reported in 4348562,
// which will wait for J3D 1.3:
//
// 1) no output of W
// 2) wrong transposition of U
// 3) wrong results for 4x4 matrices
// 4) slow performance
if (nRow == 2 && nCol == 2)
{
if (values[1][0] == 0.0)
{
U.SetIdentity();
V.SetIdentity();
if (values[0][1] == 0.0)
{
return 2;
}
double[] sinl = new double[1];
double[] sinr = new double[1];
double[] cosl = new double[1];
double[] cosr = new double[1];
double[] single_values = new double[2];
single_values[0] = values[0][0];
single_values[1] = values[1][1];
Compute_2X2(values[0][0], values[0][1], values[1][1], single_values, sinl, cosl,
sinr, cosr, 0);
Update_u(0, U, cosl, sinl);
Update_v(0, V, cosr, sinr);
return 2;
}
}
// else call computeSVD() and check for 2x2 there
return ComputeSVD(this, U, W, V);
}
private static void Chase_across(double[] s, double[] e, int k, GMatrix u
)
{
double f;
double g;
double r;
double[] cosl = new double[1];
double[] sinl = new double[1];
int i;
GMatrix t = new GMatrix(u.nRow, u.nCol);
GMatrix m = new GMatrix(u.nRow, u.nCol);
g = e[k];
f = s[k + 1];
for (i = k; i < u.nCol - 2; i++)
{
r = Compute_rot(f, g, sinl, cosl);
g = -e[i + 1] * sinl[0];
f = s[i + 2];
s[i + 1] = r;
e[i + 1] = e[i + 1] * cosl[0];
Update_u_split(k, i + 1, u, cosl, sinl, t, m);
}
s[i + 1] = Compute_rot(f, g, sinl, cosl);
Update_u_split(k, i + 1, u, cosl, sinl, t, m);
}
/// <summary>Places the matrix values of the transpose of matrix m1 into this matrix.
/// </summary>
/// <remarks>Places the matrix values of the transpose of matrix m1 into this matrix.
/// </remarks>
/// <param name="m1">the matrix to be transposed (but not modified)</param>
public void Transpose(GMatrix m1)
{
int i;
int j;
if (nRow != m1.nCol || nCol != m1.nRow)
{
throw new MismatchedSizeException("GMatrix.transpose(GMatrix) mismatch in matrix dimensions");
}
if (m1 != this)
{
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
values[i][j] = m1.values[j][i];
}
}
}
else
{
Transpose();
}
}
private static void CheckMatrix(GMatrix m)
{
int i;
int j;
for (i = 0; i < m.nRow; i++)
{
for (j = 0; j < m.nCol; j++)
{
if (Math.Abs(m.values[i][j]) < 0.0000000001)
{
System.Console.Out.Write(" 0.0 ");
}
else
{
System.Console.Out.Write(" " + m.values[i][j]);
}
}
System.Console.Out.Write("\n");
}
}
internal static int ComputeSVD(GMatrix mat, GMatrix U, GMatrix
W, GMatrix V)
{
int i;
int j;
int k;
int nr;
int nc;
int si;
int converged;
int rank;
double cs;
double sn;
double r;
double mag;
double scale;
double t;
int eLength;
int sLength;
int vecLength;
GMatrix tmp = new GMatrix(mat.nRow, mat.nCol);
GMatrix u = new GMatrix(mat.nRow, mat.nCol);
GMatrix v = new GMatrix(mat.nRow, mat.nCol);
GMatrix m = new GMatrix(mat);
// compute the number of singular values
if (m.nRow >= m.nCol)
{
sLength = m.nCol;
eLength = m.nCol - 1;
}
else
{
sLength = m.nRow;
eLength = m.nRow;
}
if (m.nRow > m.nCol)
{
vecLength = m.nRow;
}
else
{
vecLength = m.nCol;
}
double[] vec = new double[vecLength];
double[] single_values = new double[sLength];
double[] e = new double[eLength];
rank = 0;
U.SetIdentity();
V.SetIdentity();
nr = m.nRow;
nc = m.nCol;
// householder reduction
for (si = 0; si < sLength; si++)
{
// for each singular value
if (nr > 1)
{
// zero out column
// compute reflector
mag = 0.0;
for (i = 0; i < nr; i++)
{
mag += m.values[i + si][si] * m.values[i + si][si];
}
mag = Math.Sqrt(mag);
if (m.values[si][si] == 0.0)
{
vec[0] = mag;
}
else
{
vec[0] = m.values[si][si] + D_sign(mag, m.values[si][si]);
}
for (i = 1; i < nr; i++)
{
vec[i] = m.values[si + i][si];
}
scale = 0.0;
for (i = 0; i < nr; i++)
{
scale += vec[i] * vec[i];
}
scale = 2.0 / scale;
for (j = si; j < m.nRow; j++)
{
for (k = si; k < m.nRow; k++)
{
u.values[j][k] = -scale * vec[j - si] * vec[k - si];
}
}
for (i = si; i < m.nRow; i++)
{
u.values[i][i] += 1.0;
}
// compute s
t = 0.0;
for (i = si; i < m.nRow; i++)
{
t += u.values[si][i] * m.values[i][si];
}
m.values[si][si] = t;
// apply reflector
for (j = si; j < m.nRow; j++)
{
for (k = si + 1; k < m.nCol; k++)
{
tmp.values[j][k] = 0.0;
for (i = si; i < m.nCol; i++)
{
tmp.values[j][k] += u.values[j][i] * m.values[i][k];
}
}
}
for (j = si; j < m.nRow; j++)
{
for (k = si + 1; k < m.nCol; k++)
{
m.values[j][k] = tmp.values[j][k];
}
}
// update U matrix
for (j = si; j < m.nRow; j++)
{
for (k = 0; k < m.nCol; k++)
{
tmp.values[j][k] = 0.0;
for (i = si; i < m.nCol; i++)
{
tmp.values[j][k] += u.values[j][i] * U.values[i][k];
}
}
}
for (j = si; j < m.nRow; j++)
{
for (k = 0; k < m.nCol; k++)
{
U.values[j][k] = tmp.values[j][k];
}
}
nr--;
}
if (nc > 2)
{
// zero out row
mag = 0.0;
for (i = 1; i < nc; i++)
{
mag += m.values[si][si + i] * m.values[si][si + i];
}
// generate the reflection vector, compute the first entry and
// copy the rest from the row to be zeroed
mag = Math.Sqrt(mag);
if (m.values[si][si + 1] == 0.0)
{
vec[0] = mag;
}
else
{
vec[0] = m.values[si][si + 1] + D_sign(mag, m.values[si][si + 1]);
}
for (i = 1; i < nc - 1; i++)
{
vec[i] = m.values[si][si + i + 1];
}
// use reflection vector to compute v matrix
scale = 0.0;
for (i = 0; i < nc - 1; i++)
{
scale += vec[i] * vec[i];
}
scale = 2.0 / scale;
for (j = si + 1; j < nc; j++)
{
for (k = si + 1; k < m.nCol; k++)
{
v.values[j][k] = -scale * vec[j - si - 1] * vec[k - si - 1];
}
}
for (i = si + 1; i < m.nCol; i++)
{
v.values[i][i] += 1.0;
}
t = 0.0;
for (i = si; i < m.nCol; i++)
{
t += v.values[i][si + 1] * m.values[si][i];
}
m.values[si][si + 1] = t;
// apply reflector
for (j = si + 1; j < m.nRow; j++)
{
for (k = si + 1; k < m.nCol; k++)
{
tmp.values[j][k] = 0.0;
for (i = si + 1; i < m.nCol; i++)
{
tmp.values[j][k] += v.values[i][k] * m.values[j][i];
}
}
}
for (j = si + 1; j < m.nRow; j++)
{
for (k = si + 1; k < m.nCol; k++)
{
m.values[j][k] = tmp.values[j][k];
}
}
// update V matrix
for (j = 0; j < m.nRow; j++)
{
for (k = si + 1; k < m.nCol; k++)
{
tmp.values[j][k] = 0.0;
for (i = si + 1; i < m.nCol; i++)
{
tmp.values[j][k] += v.values[i][k] * V.values[j][i];
}
}
}
for (j = 0; j < m.nRow; j++)
{
for (k = si + 1; k < m.nCol; k++)
{
V.values[j][k] = tmp.values[j][k];
}
}
nc--;
}
}
for (i = 0; i < sLength; i++)
{
single_values[i] = m.values[i][i];
}
for (i = 0; i < eLength; i++)
{
e[i] = m.values[i][i + 1];
}
// Fix ArrayIndexOutOfBounds for 2x2 matrices, which partially
// addresses bug 4348562 for J3D 1.2.1.
//
// Does *not* fix the following problems reported in 4348562,
// which will wait for J3D 1.3:
//
// 1) no output of W
// 2) wrong transposition of U
// 3) wrong results for 4x4 matrices
// 4) slow performance
if (m.nRow == 2 && m.nCol == 2)
{
double[] cosl = new double[1];
double[] cosr = new double[1];
double[] sinl = new double[1];
double[] sinr = new double[1];
Compute_2X2(single_values[0], e[0], single_values[1], single_values, sinl, cosl,
sinr, cosr, 0);
Update_u(0, U, cosl, sinl);
Update_v(0, V, cosr, sinr);
return 2;
}
// compute_qr causes ArrayIndexOutOfBounds for 2x2 matrices
Compute_qr(0, e.Length - 1, single_values, e, U, V);
// compute rank = number of non zero singular values
rank = single_values.Length;
// sort by order of size of single values
// and check for zero's
return rank;
}
private static void Print_svd(double[] s, double[] e, GMatrix u, GMatrix
v)
{
int i;
GMatrix mtmp = new GMatrix(u.nCol, v.nRow);
System.Console.Out.WriteLine(" \ns = ");
for (i = 0; i < s.Length; i++)
{
System.Console.Out.WriteLine(" " + s[i]);
}
System.Console.Out.WriteLine(" \ne = ");
for (i = 0; i < e.Length; i++)
{
System.Console.Out.WriteLine(" " + e[i]);
}
System.Console.Out.WriteLine(" \nu = \n" + u.ToString());
System.Console.Out.WriteLine(" \nv = \n" + v.ToString());
mtmp.SetIdentity();
for (i = 0; i < s.Length; i++)
{
mtmp.values[i][i] = s[i];
}
for (i = 0; i < e.Length; i++)
{
mtmp.values[i][i + 1] = e[i];
}
System.Console.Out.WriteLine(" \nm = \n" + mtmp.ToString());
mtmp.MulTransposeLeft(u, mtmp);
mtmp.MulTransposeRight(mtmp, v);
System.Console.Out.WriteLine(" \n u.transpose*m*v.transpose = \n" + mtmp.ToString
());
}
/// <summary>Copies a sub-matrix derived from this matrix into the target matrix.</summary>
/// <remarks>
/// Copies a sub-matrix derived from this matrix into the target matrix.
/// The upper left of the sub-matrix is located at (rowSource, colSource);
/// the lower right of the sub-matrix is located at
/// (lastRowSource,lastColSource). The sub-matrix is copied into the
/// the target matrix starting at (rowDest, colDest).
/// </remarks>
/// <param name="rowSource">the top-most row of the sub-matrix</param>
/// <param name="colSource">the left-most column of the sub-matrix</param>
/// <param name="numRow">the number of rows in the sub-matrix</param>
/// <param name="numCol">the number of columns in the sub-matrix</param>
/// <param name="rowDest">
/// the top-most row of the position of the copied
/// sub-matrix within the target matrix
/// </param>
/// <param name="colDest">
/// the left-most column of the position of the copied
/// sub-matrix within the target matrix
/// </param>
/// <param name="target">the matrix into which the sub-matrix will be copied</param>
public void CopySubMatrix(int rowSource, int colSource, int numRow, int numCol, int
rowDest, int colDest, GMatrix target)
{
int i;
int j;
if (this != target)
{
for (i = 0; i < numRow; i++)
{
for (j = 0; j < numCol; j++)
{
target.values[rowDest + i][colDest + j] = values[rowSource + i][colSource + j];
}
}
}
else
{
//double[][] tmp = new double[numRow][numCol];
double[][] tmp = new double[][] { new double[7], new double[7], new double[7], new
double[7], new double[7], new double[7], new double[7] };
for (i = 0; i < numRow; i++)
{
for (j = 0; j < numCol; j++)
{
tmp[i][j] = values[rowSource + i][colSource + j];
}
}
for (i = 0; i < numRow; i++)
{
for (j = 0; j < numCol; j++)
{
target.values[rowDest + i][colDest + j] = tmp[i][j];
}
}
}
}
private static void Update_u(int index, GMatrix u, double[] cosl, double[]
sinl)
{
int j;
double utemp;
for (j = 0; j < u.nCol; j++)
{
utemp = u.values[index][j];
u.values[index][j] = cosl[0] * utemp + sinl[0] * u.values[index + 1][j];
u.values[index + 1][j] = -sinl[0] * utemp + cosl[0] * u.values[index + 1][j];
}
}
internal static void Compute_qr(int start, int end, double[] s, double[] e, GMatrix
u, GMatrix v)
{
int i;
int j;
int k;
int n;
int sl;
bool converged;
double shift;
double r;
double utemp;
double vtemp;
double f;
double g;
double[] cosl = new double[1];
double[] cosr = new double[1];
double[] sinl = new double[1];
double[] sinr = new double[1];
GMatrix m = new GMatrix(u.nCol, v.nRow);
int MaxInterations = 2;
double ConvergeTol = 4.89E-15;
double c_b48 = 1.0;
double c_b71 = -1.0;
converged = false;
f = 0.0;
g = 0.0;
for (k = 0; k < MaxInterations && !converged; k++)
{
for (i = start; i <= end; i++)
{
// if at start of iterfaction compute shift
if (i == start)
{
if (e.Length == s.Length)
{
sl = end;
}
else
{
sl = end + 1;
}
shift = Compute_shift(s[sl - 1], e[end], s[sl]);
f = (Math.Abs(s[i]) - shift) * (D_sign(c_b48, s[i]) + shift / s[i]);
g = e[i];
}
r = Compute_rot(f, g, sinr, cosr);
if (i != start)
{
e[i - 1] = r;
}
f = cosr[0] * s[i] + sinr[0] * e[i];
e[i] = cosr[0] * e[i] - sinr[0] * s[i];
g = sinr[0] * s[i + 1];
s[i + 1] = cosr[0] * s[i + 1];
// if (debug) print_se(s,e);
Update_v(i, v, cosr, sinr);
r = Compute_rot(f, g, sinl, cosl);
s[i] = r;
f = cosl[0] * e[i] + sinl[0] * s[i + 1];
s[i + 1] = cosl[0] * s[i + 1] - sinl[0] * e[i];
if (i < end)
{
// if not last
g = sinl[0] * e[i + 1];
e[i + 1] = cosl[0] * e[i + 1];
}
//if (debug) print_se(s,e);
Update_u(i, u, cosl, sinl);
}
// if extra off diagonal perform one more right side rotation
if (s.Length == e.Length)
{
r = Compute_rot(f, g, sinr, cosr);
f = cosr[0] * s[i] + sinr[0] * e[i];
e[i] = cosr[0] * e[i] - sinr[0] * s[i];
s[i + 1] = cosr[0] * s[i + 1];
Update_v(i, v, cosr, sinr);
}
// check for convergence on off diagonals and reduce
while ((end - start > 1) && (Math.Abs(e[end]) < ConvergeTol))
{
end--;
}
// check if need to split
for (n = end - 2; n > start; n--)
{
if (Math.Abs(e[n]) < ConvergeTol)
{
// split
Compute_qr(n + 1, end, s, e, u, v);
// do lower matrix
end = n - 1;
// do upper matrix
// check for convergence on off diagonals and reduce
while ((end - start > 1) && (Math.Abs(e[end]) < ConvergeTol))
{
end--;
}
}
}
if ((end - start <= 1) && (Math.Abs(e[start + 1]) < ConvergeTol))
{
converged = true;
}
}
// check if zero on the diagonal
if (Math.Abs(e[1]) < ConvergeTol)
{
Compute_2X2(s[start], e[start], s[start + 1], s, sinl, cosl, sinr, cosr, 0);
e[start] = 0.0;
e[start + 1] = 0.0;
}
i = start;
Update_u(i, u, cosl, sinl);
Update_v(i, v, cosr, sinr);
return;
}
private static void Update_v(int index, GMatrix v, double[] cosr, double[]
sinr)
{
int j;
double vtemp;
for (j = 0; j < v.nRow; j++)
{
vtemp = v.values[j][index];
v.values[j][index] = cosr[0] * vtemp + sinr[0] * v.values[j][index + 1];
v.values[j][index + 1] = -sinr[0] * vtemp + cosr[0] * v.values[j][index + 1];
}
}
public virtual bool EpsilonEquals(GMatrix m1, float epsilon)
{
return EpsilonEquals(m1, (double)epsilon);
}
/// <summary>
/// Returns true if all of the data members of GMatrix m1 are
/// equal to the corresponding data members in this GMatrix.
/// </summary>
/// <remarks>
/// Returns true if all of the data members of GMatrix m1 are
/// equal to the corresponding data members in this GMatrix.
/// </remarks>
/// <param name="m1">The matrix with which the comparison is made.</param>
/// <returns>true or false</returns>
public virtual bool Equals(GMatrix m1)
{
try
{
int i;
int j;
if (nRow != m1.nRow || nCol != m1.nCol)
{
return false;
}
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
if (values[i][j] != m1.values[i][j])
{
return false;
}
}
}
return true;
}
catch (ArgumentNullException)
{
return false;
}
}
/// <summary>
/// Returns true if the L-infinite distance between this matrix
/// and matrix m1 is less than or equal to the epsilon parameter,
/// otherwise returns false.
/// </summary>
/// <remarks>
/// Returns true if the L-infinite distance between this matrix
/// and matrix m1 is less than or equal to the epsilon parameter,
/// otherwise returns false. The L-infinite
/// distance is equal to
/// MAX[i=0,1,2, . . .n ; j=0,1,2, . . .n ; abs(this.m(i,j) - m1.m(i,j)]
/// </remarks>
/// <param name="m1">The matrix to be compared to this matrix</param>
/// <param name="epsilon">the threshold value</param>
public virtual bool EpsilonEquals(GMatrix m1, double epsilon)
{
int i;
int j;
double diff;
if (nRow != m1.nRow || nCol != m1.nCol)
{
return false;
}
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
diff = values[i][j] - m1.values[i][j];
if ((diff < 0 ? -diff : diff) > epsilon)
{
return false;
}
}
}
return true;
}
/// <summary>Inverts matrix m1 and places the new values into this matrix.</summary>
/// <remarks>
/// Inverts matrix m1 and places the new values into this matrix. Matrix
/// m1 is not modified.
/// </remarks>
/// <param name="m1">the matrix to be inverted</param>
public void Invert(GMatrix m1)
{
InvertGeneral(m1);
}
/// <summary>
/// LU Decomposition: this matrix must be a square matrix and the
/// LU GMatrix parameter must be the same size as this matrix.
/// </summary>
/// <remarks>
/// LU Decomposition: this matrix must be a square matrix and the
/// LU GMatrix parameter must be the same size as this matrix.
/// The matrix LU will be overwritten as the combination of a
/// lower diagonal and upper diagonal matrix decompostion of this
/// matrix; the diagonal
/// elements of L (unity) are not stored. The GVector parameter
/// records the row permutation effected by the partial pivoting,
/// and is used as a parameter to the GVector method LUDBackSolve
/// to solve sets of linear equations.
/// This method returns +/- 1 depending on whether the number
/// of row interchanges was even or odd, respectively.
/// </remarks>
/// <param name="Lu">
/// The matrix into which the lower and upper decompositions
/// will be placed.
/// </param>
/// <param name="permutation">
/// The row permutation effected by the partial
/// pivoting
/// </param>
/// <returns>
/// +-1 depending on whether the number of row interchanges
/// was even or odd respectively
/// </returns>
public int Lud(GMatrix Lu, GVector permutation)
{
int size = Lu.nRow * Lu.nCol;
double[] temp = new double[size];
int[] even_row_exchange = new int[1];
int[] row_perm = new int[Lu.nRow];
int i;
int j;
if (nRow != nCol)
{
throw new MismatchedSizeException("cannot perform LU decomposition on a non square matrix");
}
if (nRow != Lu.nRow)
{
throw new MismatchedSizeException("LU must have same dimensions as this matrix");
}
if (nCol != Lu.nCol)
{
throw new MismatchedSizeException("LU must have same dimensions as this matrix");
}
if (Lu.nRow != permutation.GetSize())
{
throw new MismatchedSizeException("row permutation must be same dimension as matrix");
}
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
temp[i * nCol + j] = values[i][j];
}
}
// Calculate LU decomposition: Is the matrix singular?
if (!LuDecomposition(Lu.nRow, temp, row_perm, even_row_exchange))
{
// Matrix has no inverse
throw new SingularMatrixException("cannot invert matrix");
}
for (i = 0; i < nRow; i++)
{
for (j = 0; j < nCol; j++)
{
Lu.values[i][j] = temp[i * nCol + j];
}
}
for (i = 0; i < Lu.nRow; i++)
{
permutation.values[i] = (double)row_perm[i];
}
return even_row_exchange[0];
}
