public static void SortValues(double[] values, double[] utStore, double[] vStore, int rows, int cols)
{
// this is a selection sort, an O(N^2) sort which requires fewer swaps than an insertion sort or an O(N ln N) sort
// loop over ranks
for (int i = 0; i < cols; i++)
{
// find the next largest value
int j = i;
double t = values[i];
for (int k = i + 1; k < cols; k++)
{
if (values[k] > t)
{
j = k;
t = values[k];
}
}
// if necessary swap it with the current element
if (j != i)
{
Global.Swap(ref values[i], ref values[j]);
if (utStore != null)
{
Blas1.dSwap(utStore, i, rows, utStore, j, rows, rows);
}
if (vStore != null)
{
Blas1.dSwap(vStore, i * cols, 1, vStore, j * cols, 1, cols);
}
}
}
}
private static void SwapIndexes(double[] aStore, int[] perm, int dimension, int p, int q)
{
if (p == q)
{
return;
}
Blas1.dSwap(aStore, p, dimension, aStore, q, dimension, dimension);
Blas1.dSwap(aStore, dimension * p, 1, aStore, dimension * q, 1, dimension);
if (perm != null)
{
Global.Swap <int>(ref perm[p], ref perm[q]);
}
}
// on input:
// store contains the matrix in column-major order (must have the length dimension^2)
// permutation contains the row permutation (typically 0, 1, 2, ..., dimension - 1; must have the length dimension^2)
// parity contains the parity of the row permutation (typically 1; must be 1 or -1)
// dimension contains the dimension of the matrix (must be non-negative)
// on output:
// store is replaced by the L and U matrices, L in the lower-left triangle (with 1s along the diagonal), U in the upper-right triangle
// permutation is replaced by the row permutation, and parity be the parity of that permutation
// A = PLU
public static void LUDecompose(double[] store, int[] permutation, ref int parity, int dimension)
{
for (int d = 0; d < dimension; d++)
{
int pivotRow = -1;
double pivotValue = 0.0;
for (int r = d; r < dimension; r++)
{
int a0 = dimension * d + r;
double t = store[a0] - Blas1.dDot(store, r, dimension, store, dimension * d, 1, d);
store[a0] = t;
if (Math.Abs(t) > Math.Abs(pivotValue))
{
pivotRow = r;
pivotValue = t;
}
}
if (pivotValue == 0.0)
{
throw new DivideByZeroException();
}
if (pivotRow != d)
{
// switch rows
Blas1.dSwap(store, d, dimension, store, pivotRow, dimension, dimension);
int t = permutation[pivotRow];
permutation[pivotRow] = permutation[d];
permutation[d] = t;
parity = -parity;
}
Blas1.dScal(1.0 / pivotValue, store, dimension * d + d + 1, 1, dimension - d - 1);
for (int c = d + 1; c < dimension; c++)
{
double t = Blas1.dDot(store, d, dimension, store, dimension * c, 1, d);
store[dimension * c + d] -= t;
}
}
}
// inverts the matrix in place
// the in-place-ness makes this a bit confusing
public static void GaussJordanInvert(double[] store, int dimension)
{
// keep track of row exchanges
int[] ps = new int[dimension];
// iterate over dimensions
for (int k = 0; k < dimension; k++)
{
// look for a pivot in the kth column on any lower row
int p = k;
double q = MatrixAlgorithms.GetEntry(store, dimension, dimension, k, k);
for (int r = k + 1; r < dimension; r++)
{
double s = MatrixAlgorithms.GetEntry(store, dimension, dimension, r, k);
if (Math.Abs(s) > Math.Abs(q))
{
p = r;
q = s;
}
}
ps[k] = p;
// if no non-zero pivot is found, the matrix is singular and cannot be inverted
if (q == 0.0)
{
throw new DivideByZeroException();
}
// if the best pivot was on a lower row, swap it into the kth row
if (p != k)
{
Blas1.dSwap(store, k, dimension, store, p, dimension, dimension);
}
// divide the pivot row by the pivot element, so the diagonal element becomes unity
MatrixAlgorithms.SetEntry(store, dimension, dimension, k, k, 1.0);
Blas1.dScal(1.0 / q, store, k, dimension, dimension);
// add factors to the pivot row to zero all off-diagonal elements in the kth column
for (int r = 0; r < dimension; r++)
{
if (r == k)
{
continue;
}
double a = MatrixAlgorithms.GetEntry(store, dimension, dimension, r, k);
MatrixAlgorithms.SetEntry(store, dimension, dimension, r, k, 0.0);
Blas1.dAxpy(-a, store, k, dimension, store, r, dimension, dimension);
}
}
// unscramble exchanges
for (int k = dimension - 1; k >= 0; k--)
{
int p = ps[k];
if (p != k)
{
Blas1.dSwap(store, dimension * p, 1, store, dimension * k, 1, dimension);
}
}
}
