public void ConstructorException2()
{
MatrixF m = new MatrixF(new float[,] {{ 1, 2 },
{ 3, 4 },
{ 5, 6 }});
new EigenvalueDecompositionF(m);
}
public void DeterminantException()
{
MatrixF a = new MatrixF(new float[,] { { 1, 2 }, { 5, 6 }, { 0, 1 } });
LUDecompositionF d = new LUDecompositionF(a);
float det = d.Determinant;
}
public void Test1()
{
MatrixF a = new MatrixF(new float[,] { { 2, -1, 0},
{ -1, 2, -1},
{ 0, -1, 2} });
CholeskyDecompositionF d = new CholeskyDecompositionF(a);
Assert.AreEqual(true, d.IsSymmetricPositiveDefinite);
MatrixF l = d.L;
Assert.AreEqual(0, l[0, 1]);
Assert.AreEqual(0, l[0, 2]);
Assert.AreEqual(0, l[1, 2]);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, l * l.Transposed));
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, l * l.Transposed));
// Check solving of linear equations.
MatrixF x = new MatrixF(new float[,] { { 1, 2},
{ 3, 4},
{ 5, 6} });
MatrixF b = a * x;
Assert.IsTrue(MatrixF.AreNumericallyEqual(x, d.SolveLinearEquations(b)));
}
public void TestRandomA()
{
RandomHelper.Random = new Random(1);
for (int i = 0; i < 100; i++)
{
// Create A.
MatrixF a = new MatrixF(3, 3);
RandomHelper.Random.NextMatrixF(a, 0, 1);
LUDecompositionF d = new LUDecompositionF(a);
if (d.IsNumericallySingular == false)
{
// Check solving of linear equations.
MatrixF b = new MatrixF(3, 2);
RandomHelper.Random.NextMatrixF(b, 0, 1);
MatrixF x = d.SolveLinearEquations(b);
MatrixF b2 = a * x;
Assert.IsTrue(MatrixF.AreNumericallyEqual(b, b2, 0.01f));
MatrixF aPermuted = d.L * d.U;
Assert.IsTrue(MatrixF.AreNumericallyEqual(aPermuted, a.GetSubmatrix(d.PivotPermutationVector, 0, 2)));
}
}
}
public void TestMatricesWithoutFullRank()
{
MatrixF a = new MatrixF(3, 3);
SingularValueDecompositionF svd = new SingularValueDecompositionF(a);
Assert.AreEqual(0, svd.NumericalRank);
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
float condNumber = svd.ConditionNumber;
a = new MatrixF(new float[,] { { 1, 2, 3 }, { 4, 5, 6 }, { 4, 5, 6 } });
svd = new SingularValueDecompositionF(a);
Assert.AreEqual(2, svd.NumericalRank);
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, svd.U * svd.S * svd.V.Transposed));
svd = new SingularValueDecompositionF(a.Transposed);
Assert.AreEqual(2, svd.NumericalRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a.Transposed, svd.U * svd.S * svd.V.Transposed));
Assert.IsTrue(MatrixF.AreNumericallyEqual(a.Transposed, svd.U * svd.S * svd.V.Transposed)); // Repeat to test with cached values.
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
condNumber = svd.ConditionNumber;
a = new MatrixF(new float[,] { { 1, 2 }, { 1, 2 }, { 1, 2 } });
svd = new SingularValueDecompositionF(a);
Assert.AreEqual(1, svd.NumericalRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, svd.U * svd.S * svd.V.Transposed));
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
condNumber = svd.ConditionNumber;
}
//--------------------------------------------------------------
/// <summary>
/// Creates the principal component analysis for the given list of points.
/// </summary>
/// <param name="points">
/// The list of data points. All points must have the same
/// <see cref="VectorF.NumberOfElements"/>.
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="points"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="points"/> is empty.
/// </exception>
public PrincipalComponentAnalysisF(IList<VectorF> points)
{
if (points == null)
throw new ArgumentNullException("points");
if (points.Count == 0)
throw new ArgumentException("The list of points is empty.");
// Compute covariance matrix.
MatrixF covarianceMatrix = StatisticsHelper.ComputeCovarianceMatrix(points);
// Perform Eigenvalue decomposition.
EigenvalueDecompositionF evd = new EigenvalueDecompositionF(covarianceMatrix);
int numberOfElements = evd.RealEigenvalues.NumberOfElements;
Variances = new VectorF(numberOfElements);
V = new MatrixF(numberOfElements, numberOfElements);
// Sort eigenvalues by decreasing value.
// Since covarianceMatrix is symmetric, we have no imaginary eigenvalues.
for (int i = 0; i < Variances.NumberOfElements; i++)
{
int index = evd.RealEigenvalues.IndexOfLargestElement;
Variances[i] = evd.RealEigenvalues[index];
V.SetColumn(i, evd.V.GetColumn(index));
evd.RealEigenvalues[index] = float.NegativeInfinity;
}
}
public void SolveLinearEquationsException1()
{
// Create A.
MatrixF a = new MatrixF(new float[,] { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, -9 } });
CholeskyDecompositionF decomp = new CholeskyDecompositionF(a);
decomp.SolveLinearEquations(null);
}
public void AddMatrix()
{
MatrixF m1 = new MatrixF(3, 4, rowMajor, MatrixOrder.RowMajor);
MatrixF m2 = new MatrixF(3, 4, rowMajor, MatrixOrder.RowMajor) * (-3);
MatrixF result = MatrixF.Add(m1, m2);
for (int i = 0; i < 12; i++)
Assert.AreEqual(-rowMajor[i] * 2, result[i]);
}
public void Test2()
{
MatrixF a = new MatrixF(new float[,] {{ 0, 1, 2 },
{ 1, 4, 3 },
{ 2, 3, 5}});
EigenvalueDecompositionF d = new EigenvalueDecompositionF(a);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, d.V * d.D * d.V.Transposed));
}
public void Test1()
{
MatrixF a = new MatrixF(new float[,] {{ 1, -1, 4 },
{ 3, 2, -1 },
{ 2, 1, -1}});
EigenvalueDecompositionF d = new EigenvalueDecompositionF(a);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a * d.V, d.V * d.D));
}
public void SolveLinearEquationsException3()
{
// Create A.
MatrixF a = new MatrixF(new float[,] { { 1, 2, 3 }, { 4, 5, 6 }, { 1, 2, 3 } });
MatrixF b = new MatrixF(new float[,] { { 1, 2, 3 }, { 4, 5, 6 }, {7, 8, -9 } });
QRDecompositionF decomp = new QRDecompositionF(a);
decomp.SolveLinearEquations(b);
}
//--------------------------------------------------------------
/// <summary>
/// Creates the eigenvalue decomposition of the given matrix.
/// </summary>
/// <param name="matrixA">The square matrix A.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is non-square (rectangular).
/// </exception>
public EigenvalueDecompositionF(MatrixF matrixA)
{
if (matrixA == null)
throw new ArgumentNullException("matrixA");
if (matrixA.IsSquare == false)
throw new ArgumentException("The matrix A must be square.", "matrixA");
_n = matrixA.NumberOfColumns;
_d = new VectorF(_n);
_e = new VectorF(_n);
_isSymmetric = matrixA.IsSymmetric;
if (_isSymmetric)
{
_v = matrixA.Clone();
// Tridiagonalize.
ReduceToTridiagonal();
// Diagonalize.
TridiagonalToQL();
}
else
{
_v = new MatrixF(_n, _n);
// Abort if A contains NaN values.
// If we continue with NaN values, we run into an infinite loop.
for (int i = 0; i < _n; i++)
{
for (int j = 0; j < _n; j++)
{
if (Numeric.IsNaN(matrixA[i, j]))
{
_e.Set(float.NaN);
_v.Set(float.NaN);
_d.Set(float.NaN);
return;
}
}
}
// Storage of nonsymmetric Hessenberg form.
MatrixF matrixH = matrixA.Clone();
// Working storage for nonsymmetric algorithm.
float[] ort = new float[_n];
// Reduce to Hessenberg form.
ReduceToHessenberg(matrixH, ort);
// Reduce Hessenberg to real Schur form.
HessenbergToRealSchur(matrixH);
}
}
public void Test1()
{
MatrixF A = new MatrixF(new float[,] { { 4 } });
VectorF b = new VectorF(new float[] { 20 });
SorMethodF solver = new SorMethodF();
VectorF x = solver.Solve(A, null, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(new VectorF(1, 5), x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void Determinant()
{
MatrixF a = new MatrixF(new float[,] { { 1, 2, 3, 4 }, { 5, 6, 7, 8 }, { 0, 1, 2, 0 }, { 1, 0, 1, 0 } });
LUDecompositionF d = new LUDecompositionF(a);
Assert.AreEqual(false, d.IsNumericallySingular);
Assert.IsTrue(Numeric.AreEqual(-24, d.Determinant));
MatrixF aPermuted = d.L * d.U;
Assert.IsTrue(MatrixF.AreNumericallyEqual(aPermuted, a.GetSubmatrix(d.PivotPermutationVector, 0, 3)));
}
public void SolveWithDefaultInitialGuess()
{
MatrixF A = new MatrixF(new float[,] { { 4 } });
VectorF b = new VectorF(new float[] { 20 });
JacobiMethodF solver = new JacobiMethodF();
VectorF x = solver.Solve(A, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(new VectorF(1, 5), x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void Test2()
{
MatrixF A = new MatrixF(new float[,] { { 1, 0 },
{ 0, 1 }});
VectorF b = new VectorF(new float[] { 20, 28 });
JacobiMethodF solver = new JacobiMethodF();
VectorF x = solver.Solve(A, null, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(b, x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void Test4()
{
MatrixF A = new MatrixF(new float[,] { { -12, 2 },
{ 2, 3 }});
VectorF b = new VectorF(new float[] { 20, 28 });
SorMethodF solver = new SorMethodF();
VectorF x = solver.Solve(A, null, b);
VectorF solution = MatrixF.SolveLinearEquations(A, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(solution, x));
}
public void Test3()
{
MatrixF A = new MatrixF(new float[,] { { 2, 0 },
{ 0, 2 }});
VectorF b = new VectorF(new float[] { 20, 28 });
GaussSeidelMethodF solver = new GaussSeidelMethodF();
VectorF x = solver.Solve(A, null, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(b / 2, x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void SolveLinearEquationsException3()
{
// Create A.
MatrixF a = new MatrixF(new float[, ] {
{ 1, 2, 3 }, { 4, 5, 6 }, { 1, 2, 3 }
});
MatrixF b = new MatrixF(new float[, ] {
{ 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, -9 }
});
CholeskyDecompositionF decomp = new CholeskyDecompositionF(a);
decomp.SolveLinearEquations(b);
}
public void Test3()
{
MatrixF A = new MatrixF(new float[, ] {
{ 2, 0 },
{ 0, 2 }
});
VectorF b = new VectorF(new float[] { 20, 28 });
GaussSeidelMethodF solver = new GaussSeidelMethodF();
VectorF x = solver.Solve(A, null, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(b / 2, x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void Test2()
{
MatrixF A = new MatrixF(new float[, ] {
{ 1, 0 },
{ 0, 1 }
});
VectorF b = new VectorF(new float[] { 20, 28 });
JacobiMethodF solver = new JacobiMethodF();
VectorF x = solver.Solve(A, null, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(b, x));
Assert.AreEqual(2, solver.NumberOfIterations);
}
public void Test5()
{
MatrixF A = new MatrixF(new float[,] { { -21, 2, -4, 0 },
{ 2, 3, 0.1f, -1 },
{ 2, 10, 111.1f, -11 },
{ 23, 112, 111.1f, -143 }});
VectorF b = new VectorF(new float[] { 20, 28, -12, 0.1f });
GaussSeidelMethodF solver = new GaussSeidelMethodF();
VectorF x = solver.Solve(A, null, b);
VectorF solution = MatrixF.SolveLinearEquations(A, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(solution, x));
}
public void Determinant()
{
MatrixF a = new MatrixF(new float[, ] {
{ 1, 2, 3, 4 }, { 5, 6, 7, 8 }, { 0, 1, 2, 0 }, { 1, 0, 1, 0 }
});
LUDecompositionF d = new LUDecompositionF(a);
Assert.AreEqual(false, d.IsNumericallySingular);
Assert.IsTrue(Numeric.AreEqual(-24, d.Determinant()));
MatrixF aPermuted = d.L * d.U;
Assert.IsTrue(MatrixF.AreNumericallyEqual(aPermuted, a.GetSubmatrix(d.PivotPermutationVector, 0, 3)));
}
public void Test4()
{
MatrixF A = new MatrixF(new float[, ] {
{ -12, 2 },
{ 2, 3 }
});
VectorF b = new VectorF(new float[] { 20, 28 });
SorMethodF solver = new SorMethodF();
VectorF x = solver.Solve(A, null, b);
VectorF solution = MatrixF.SolveLinearEquations(A, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(solution, x));
}
public static void NextMatrixF(this Random random, MatrixF matrix, float min, float max)
{
if (random == null)
{
random = Random;
}
for (int r = 0; r < matrix.NumberOfRows; r++)
{
for (int c = 0; c < matrix.NumberOfColumns; c++)
{
matrix[r, c] = NextFloat(random, min, max);
}
}
}
/// <summary>
/// Creates the Cholesky decomposition of the given matrix.
/// </summary>
/// <param name="matrixA">The square matrix A.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
public CholeskyDecompositionF(MatrixF matrixA)
{
// Note: A different algorithm can be found in the Numerical Recipes book.
if (matrixA == null)
{
throw new ArgumentNullException("matrixA");
}
int n = matrixA.NumberOfRows;
_isSymmetricPositiveDefinite = true;
// Is matrix square?
if (matrixA.NumberOfColumns != n)
{
// Not square!
n = Math.Min(n, matrixA.NumberOfColumns);
_isSymmetricPositiveDefinite = false;
}
_l = new MatrixF(n, n);
for (int j = 0; j < n; j++)
{
float d = 0;
for (int k = 0; k < j; k++)
{
float s = 0;
for (int i = 0; i < k; i++)
{
s += _l[k, i] * _l[j, i];
}
s = (matrixA[j, k] - s) / _l[k, k];
L[j, k] = s;
d = d + s * s;
_isSymmetricPositiveDefinite = _isSymmetricPositiveDefinite && (matrixA[k, j] == matrixA[j, k]);
}
d = matrixA[j, j] - d;
_isSymmetricPositiveDefinite = _isSymmetricPositiveDefinite && (d > 0);
_l[j, j] = (float)Math.Sqrt(Math.Max(d, 0));
//for (int k = j + 1; k < n; k++)
// _l[j, k] = 0; // Not needed. Already initialized with 0.
}
}
/// <summary>
/// Solves the specified linear system of equations <i>Ax=b</i>.
/// </summary>
/// <param name="matrixA">The matrix A.</param>
/// <param name="initialX">
/// The initial guess for x. If this value is <see langword="null"/>, a zero vector will be used
/// as initial guess.
/// </param>
/// <param name="vectorB">The vector b.</param>
/// <returns>The solution vector x.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="vectorB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is not a square matrix.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of elements of <paramref name="initialX"/> does not match.
/// </exception>
public override VectorF Solve(MatrixF matrixA, VectorF initialX, VectorF vectorB)
{
// TODO: We can possible improve the method by reordering after each step.
// This can be done randomly or we sort by the "convergence" of the elements.
// See book Physics-Based Animation.
NumberOfIterations = 0;
if (matrixA == null)
throw new ArgumentNullException("matrixA");
if (vectorB == null)
throw new ArgumentNullException("vectorB");
if (matrixA.IsSquare == false)
throw new ArgumentException("Matrix A must be a square matrix.", "matrixA");
if (matrixA.NumberOfRows != vectorB.NumberOfElements)
throw new ArgumentException("The number of rows of A and b do not match.");
if (initialX != null && initialX.NumberOfElements != vectorB.NumberOfElements)
throw new ArgumentException("The number of elements of the initial guess for x and b do not match.");
VectorF xOld = initialX ?? new VectorF(vectorB.NumberOfElements);
VectorF xNew = new VectorF(vectorB.NumberOfElements);
bool isConverged = false;
// Make iterations until max iteration count or the result has converged.
for (int i = 0; i < MaxNumberOfIterations && !isConverged; i++)
{
for (int j = 0; j < vectorB.NumberOfElements; j++)
{
float delta = 0;
for (int k = 0; k < j; k++)
delta += matrixA[j, k] * xOld[k];
for (int k = j + 1; k < vectorB.NumberOfElements; k++)
delta += matrixA[j, k] * xOld[k];
xNew[j] = (vectorB[j] - delta) / matrixA[j, j];
}
// Test convergence
isConverged = VectorF.AreNumericallyEqual(xOld, xNew, Epsilon);
xOld = xNew.Clone();
NumberOfIterations = i + 1;
}
return xNew;
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <summary>
/// Solves the equation <c>A * X = B</c>.
/// </summary>
/// <param name="matrixB">The matrix B with as many rows as A and any number of columns.</param>
/// <returns>X, so that <c>A * X = B</c>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of rows does not match.
/// </exception>
/// <exception cref="MathematicsException">
/// The matrix A is numerically singular.
/// </exception>
public MatrixF SolveLinearEquations(MatrixF matrixB)
{
if (matrixB == null)
{
throw new ArgumentNullException("matrixB");
}
if (matrixB.NumberOfRows != _m)
{
throw new ArgumentException("The number of rows does not match.", "matrixB");
}
if (IsNumericallySingular)
{
throw new MathematicsException("The original matrix A is numerically singular.");
}
// Copy right hand side with pivoting
MatrixF x = matrixB.GetSubmatrix(_pivotVector, 0, matrixB.NumberOfColumns - 1);
// 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 < matrixB.NumberOfColumns; 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 < matrixB.NumberOfColumns; j++)
{
x[k, j] /= _lu[k, k];
}
for (int i = 0; i < k; i++)
{
for (int j = 0; j < matrixB.NumberOfColumns; j++)
{
x[i, j] -= x[k, j] * _lu[i, k];
}
}
}
return(x);
}
public void TestRandomRegularA()
{
RandomHelper.Random = new Random(1);
for (int i = 0; i < 100; i++)
{
VectorF column1 = new VectorF(3);
RandomHelper.Random.NextVectorF(column1, 1, 2);
VectorF column2 = new VectorF(3);
RandomHelper.Random.NextVectorF(column2, 1, 2);
// Make linearly independent.
if (column1 / column1[0] == column2 / column2[0])
{
column2[0]++;
}
// Create linearly independent third column.
VectorF column3 = column1 + column2;
column3[1]++;
// Create A.
MatrixF a = new MatrixF(3, 3);
a.SetColumn(0, column1);
a.SetColumn(1, column2);
a.SetColumn(2, column3);
LUDecompositionF d = new LUDecompositionF(a);
MatrixF aPermuted = d.L * d.U;
Assert.IsTrue(MatrixF.AreNumericallyEqual(aPermuted, a.GetSubmatrix(d.PivotPermutationVector, 0, 2)));
aPermuted = d.L * d.U; // Repeat with to test cached values.
Assert.IsTrue(MatrixF.AreNumericallyEqual(aPermuted, a.GetSubmatrix(d.PivotPermutationVector, 0, 2)));
Assert.AreEqual(false, d.IsNumericallySingular);
// Check solving of linear equations.
MatrixF b = new MatrixF(3, 2);
RandomHelper.Random.NextMatrixF(b, 0, 1);
MatrixF x = d.SolveLinearEquations(b);
MatrixF b2 = a * x;
Assert.IsTrue(MatrixF.AreNumericallyEqual(b, b2, 0.01f));
}
}
public void TestMatricesWithFullRank()
{
MatrixF a = new MatrixF(new float[,] { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, -9 } });
QRDecompositionF qr = new QRDecompositionF(a);
Assert.AreEqual(true, qr.HasNumericallyFullRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, qr.Q * qr.R));
qr = new QRDecompositionF(a.Transposed);
Assert.AreEqual(true, qr.HasNumericallyFullRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a.Transposed, qr.Q * qr.R));
a = new MatrixF(new float[,] { { 1, 2 }, { 4, 5 }, { 4, 5 } });
qr = new QRDecompositionF(a);
Assert.AreEqual(true, qr.HasNumericallyFullRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, qr.Q * qr.R));
MatrixF h = qr.H; // Just call this one to see if it runs through.
}
public void Test5()
{
MatrixF A = new MatrixF(new float[, ] {
{ -21, 2, -4, 0 },
{ 2, 3, 0.1f, -1 },
{ 2, 10, 111.1f, -11 },
{ 23, 112, 111.1f, -143 }
});
VectorF b = new VectorF(new float[] { 20, 28, -12, 0.1f });
SorMethodF solver = new SorMethodF();
VectorF x = solver.Solve(A, null, b);
VectorF solution = MatrixF.SolveLinearEquations(A, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(solution, x));
}
/// <summary>
/// Solves the equation <c>A * X = B</c>.
/// </summary>
/// <param name="matrixB">The matrix B with as many rows as A and any number of columns.</param>
/// <returns>X, so that <c>A * X = B</c>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of rows does not match.
/// </exception>
/// <exception cref="MathematicsException">
/// The matrix A is not symmetric and positive definite.
/// </exception>
public MatrixF SolveLinearEquations(MatrixF matrixB)
{
if (matrixB == null)
{
throw new ArgumentNullException("matrixB");
}
if (matrixB.NumberOfRows != L.NumberOfRows)
{
throw new ArgumentException("The number of rows does not match.", "matrixB");
}
if (IsSymmetricPositiveDefinite == false)
{
throw new MathematicsException("The original matrix A is not symmetric and positive definite.");
}
// Initialize x as a copy of B.
MatrixF x = matrixB.Clone();
// Solve L*Y = B.
for (int k = 0; k < L.NumberOfRows; k++)
{
for (int j = 0; j < matrixB.NumberOfColumns; j++)
{
for (int i = 0; i < k; i++)
{
x[k, j] -= x[i, j] * L[k, i];
}
x[k, j] /= L[k, k];
}
}
// Solve transpose(L) * X = Y.
for (int k = L.NumberOfRows - 1; k >= 0; k--)
{
for (int j = 0; j < matrixB.NumberOfColumns; j++)
{
for (int i = k + 1; i < L.NumberOfRows; i++)
{
x[k, j] -= x[i, j] * L[i, k];
}
x[k, j] /= L[k, k];
}
}
return(x);
}
/// <summary>
/// Called when <see cref="ScatteredInterpolationF.Setup"/> is called.
/// </summary>
/// <remarks>
/// Here internal values can be computed from the registered reference pairs if required. It is
/// assured that the reference data pairs have valid dimensions: All x values have the same
/// number of elements and all y values have the same number of elements. All reference data
/// values are not <see langword="null"/>.
/// </remarks>
/// <exception cref="MathematicsException">
/// Cannot compute regression - try to choose different reference data pairs or another basis
/// function.
/// </exception>
protected override void OnSetup()
{
// Compute weights:
// Make matrix Φ (also known as G).
int numberOfPairs = Count;
MatrixF G = new MatrixF(numberOfPairs, numberOfPairs);
for (int r = 0; r < numberOfPairs; r++) // rows
{
for (int c = 0; c < numberOfPairs; c++) // columns
{
float radialArgument = DistanceFunction(GetX(r), GetX(c), c);
G[r, c] = BasisFunction(radialArgument, c);
}
}
// We look for the matrix W that contains the weights.
// Each row resembles an n-dimensional weight.
MatrixF W;
// Make the matrix Y with the reference values, such that ideally Y = G * W.
MatrixF Y = new MatrixF(numberOfPairs, GetY(0).NumberOfElements);
for (int r = 0; r < numberOfPairs; r++)
{
Y.SetRow(r, GetY(r));
}
// Compute W as the least squares solution.
try
{
W = MatrixF.SolveLinearEquations(G, Y);
}
catch (MathematicsException exception)
{
throw new MathematicsException("Cannot compute regression - try to choose different reference data pairs or another basis function.", exception);
}
// The rows of W are the weights.
_weights = new VectorF[numberOfPairs];
for (int r = 0; r < numberOfPairs; r++)
{
_weights[r] = W.GetRow(r);
}
}
public Bone(int boneIndex, int parentIndex, Bone parent, string name,
CoordinateF defaultPosition, CoordinateF defaultAngles,
CoordinateF defaultPositionScale, CoordinateF defaultAnglesScale)
{
BoneIndex = boneIndex;
ParentIndex = parentIndex;
Parent = parent;
Name = name;
DefaultPosition = defaultPosition;
DefaultAngles = defaultAngles;
DefaultPositionScale = defaultPositionScale;
DefaultAnglesScale = defaultAnglesScale;
Transform = QuaternionF.EulerAngles(DefaultAngles).GetMatrix().Translate(defaultPosition);
if (parent != null)
{
Transform *= parent.Transform;
}
}
//--------------------------------------------------------------
/// <summary>
/// Creates the QR decomposition of the given matrix.
/// </summary>
/// <param name="matrixA">
/// The matrix A. (Can be rectangular. NumberOfRows must be ≥ NumberOfColumns.)
/// </param>
/// <remarks>
/// The QR decomposition is computed by Householder reflections.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of rows must be greater than or equal to the number of columns.
/// </exception>
public QRDecompositionF(MatrixF matrixA)
{
if (matrixA == null)
throw new ArgumentNullException("matrixA");
if (matrixA.NumberOfRows < matrixA.NumberOfColumns)
throw new ArgumentException("The number of rows must be greater than or equal to the number of columns.", "matrixA");
// Initialize.
_qr = matrixA.Clone();
_m = matrixA.NumberOfRows;
_n = matrixA.NumberOfColumns;
_rDiagonal = new float[_n];
// Main loop.
for (int k = 0; k < _n; k++)
{
// Compute 2-norm of k-th column without under/overflow.
float norm = 0;
for (int i = k; i < _m; i++)
norm = MathHelper.Hypotenuse(norm, _qr[i, k]);
if (norm != 0) // TODO: Maybe a comparison with an epsilon tolerance is required here!?
{
// Form k-th Householder vector.
if (_qr[k, k] < 0)
norm = -norm;
for (int i = k; i < _m; i++)
_qr[i, k] /= norm;
_qr[k, k] += 1;
// Apply transformation to remaining columns.
for (int j = k + 1; j < _n; j++)
{
float s = 0;
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];
}
}
_rDiagonal[k] = -norm;
}
}
public void TestRandomRegularA()
{
RandomHelper.Random = new Random(1);
for (int i = 0; i < 100; i++)
{
VectorF column1 = new VectorF(3);
RandomHelper.Random.NextVectorF(column1, 1, 2);
VectorF column2 = new VectorF(3);
RandomHelper.Random.NextVectorF(column2, 1, 2);
// Make linearly independent.
if (column1 / column1[0] == column2 / column2[0])
{
column2[0]++;
}
// Create linearly independent third column.
VectorF column3 = column1 + column2;
column3[1]++;
// Create A.
MatrixF a = new MatrixF(3, 3);
a.SetColumn(0, column1);
a.SetColumn(1, column2);
a.SetColumn(2, column3);
QRDecompositionF d = new QRDecompositionF(a);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, d.Q * d.R));
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, d.Q * d.R)); // Second time with the cached values.
Assert.AreEqual(true, d.HasNumericallyFullRank);
// Check solving of linear equations.
MatrixF b = new MatrixF(3, 2);
RandomHelper.Random.NextMatrixF(b, 0, 1);
MatrixF x = d.SolveLinearEquations(b);
MatrixF b2 = a * x;
Assert.IsTrue(MatrixF.AreNumericallyEqual(b, b2, 0.01f));
MatrixF h = d.H; // Just call this one to see if it runs through.
h = d.H; // Call it secont time to cover code with internal caching.
}
}
/// <summary>
/// Creates a new NeuralNetwork based on an instance.
/// </summary>
/// <param name="_nn"></param>
/// <returns></returns>
public static NeuralNetwork Copy(NeuralNetwork _nn)
{
NeuralNetwork nn2 = new NeuralNetwork(
_nn.inputs, _nn.hiddenNodes, _nn.outputs, _nn.numHiddenLayers);
List <MatrixF> weights = new List <MatrixF>();
List <MatrixF> bias = new List <MatrixF>();
for (int i = 0; i < _nn.weightMatrixes.Count; i++)
{
weights.Add(MatrixF.Copy(_nn.weightMatrixes[i]));
}
for (int i = 0; i < _nn.biasMatrixes.Count; i++)
{
bias.Add(MatrixF.Copy(_nn.biasMatrixes[i]));
}
nn2.weightMatrixes = weights;
nn2.biasMatrixes = bias;
return(nn2);
}
//--------------------------------------------------------------
/// <summary>
/// Creates the Cholesky decomposition of the given matrix.
/// </summary>
/// <param name="matrixA">The square matrix A.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
public CholeskyDecompositionF(MatrixF matrixA)
{
// Note: A different algorithm can be found in the Numerical Recipes book.
if (matrixA == null)
throw new ArgumentNullException("matrixA");
int n = matrixA.NumberOfRows;
_isSymmetricPositiveDefinite = true;
// Is matrix square?
if (matrixA.NumberOfColumns != n)
{
// Not square!
n = Math.Min(n, matrixA.NumberOfColumns);
_isSymmetricPositiveDefinite = false;
}
_l = new MatrixF(n, n);
for (int j = 0; j < n; j++)
{
float d = 0;
for (int k = 0; k < j; k++)
{
float s = 0;
for (int i = 0; i < k; i++)
s += _l[k, i] * _l[j, i];
s = (matrixA[j, k] - s) / _l[k, k];
L[j, k] = s;
d = d + s * s;
_isSymmetricPositiveDefinite = _isSymmetricPositiveDefinite && (matrixA[k, j] == matrixA[j, k]);
}
d = matrixA[j, j] - d;
_isSymmetricPositiveDefinite = _isSymmetricPositiveDefinite && (d > 0);
_l[j, j] = (float) Math.Sqrt(Math.Max(d, 0));
//for (int k = j + 1; k < n; k++)
// _l[j, k] = 0; // Not needed. Already initialized with 0.
}
}
/// <summary>
/// Solves the specified linear system of equations <i>Ax=b</i>.
/// </summary>
/// <param name="matrixA">The matrix A.</param>
/// <param name="initialX">
/// The initial guess for x. If this value is <see langword="null"/>, a zero vector will be used
/// as initial guess.
/// </param>
/// <param name="vectorB">The vector b.</param>
/// <returns>The solution vector x.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="vectorB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is not a square matrix.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of elements of <paramref name="initialX"/> does not match.
/// </exception>
public override VectorF Solve(MatrixF matrixA, VectorF initialX, VectorF vectorB)
{
NumberOfIterations = 0;
if (matrixA == null)
throw new ArgumentNullException("matrixA");
if (vectorB == null)
throw new ArgumentNullException("vectorB");
if (matrixA.IsSquare == false)
throw new ArgumentException("Matrix A must be a square matrix.", "matrixA");
if (matrixA.NumberOfRows != vectorB.NumberOfElements)
throw new ArgumentException("The number of rows of A and b do not match.");
if (initialX != null && initialX.NumberOfElements != vectorB.NumberOfElements)
throw new ArgumentException("The number of elements of the initial guess for x and b do not match.");
VectorF xOld = initialX ?? new VectorF(vectorB.NumberOfElements);
VectorF xNew = new VectorF(vectorB.NumberOfElements);
bool isConverged = false;
// Make iterations until max iteration count or the result has converged.
for (int i = 0; i < MaxNumberOfIterations && !isConverged; i++)
{
for (int j=0; j<vectorB.NumberOfElements; j++)
{
float delta = 0;
for (int k=0; k < j; k++)
delta += matrixA[j, k] * xNew[k];
for (int k=j+1; k < vectorB.NumberOfElements; k++)
delta += matrixA[j, k] * xOld[k];
delta = (vectorB[j] - delta) / matrixA[j, j];
xNew[j] = xOld[j] + RelaxationFactor * (delta - xOld[j]);
}
// Test convergence
isConverged = VectorF.AreNumericallyEqual(xOld, xNew, Epsilon);
xOld = xNew.Clone();
NumberOfIterations = i + 1;
}
return xNew;
}
public void Test6()
{
MatrixF A = new MatrixF(new float[, ] {
{ -21, 2, -4, 0 },
{ 2, 3, 0.1f, -1 },
{ 2, 10, 111.1f, -11 },
{ 23, 112, 111.1f, -143 }
});
VectorF b = new VectorF(new float[] { 20, 28, -12, 0.1f });
JacobiMethodF solver = new JacobiMethodF();
solver.MaxNumberOfIterations = 10;
VectorF x = solver.Solve(A, null, b);
VectorF solution = MatrixF.SolveLinearEquations(A, b);
Assert.IsFalse(VectorF.AreNumericallyEqual(solution, x));
Assert.AreEqual(10, solver.NumberOfIterations);
}
public void Test7()
{
MatrixF A = new MatrixF(new float[, ] {
{ -21, 2, -4, 0 },
{ 2, 3, 0.1f, -1 },
{ 2, 10, 111.1f, -11 },
{ 23, 112, 111.1f, -143 }
});
VectorF b = new VectorF(new float[] { 20, 28, -12, 0.1f });
SorMethodF solver = new SorMethodF();
solver.Epsilon = 0.1f;
VectorF x = solver.Solve(A, null, b);
VectorF solution = MatrixF.SolveLinearEquations(A, b);
Assert.IsTrue(VectorF.AreNumericallyEqual(solution, x, 0.1f));
Assert.IsFalse(VectorF.AreNumericallyEqual(solution, x));
Assert.Greater(12, solver.NumberOfIterations); // For normal accuracy (EpsilonF) we need 12 iterations.
}
public void TestMatricesWithFullRank()
{
MatrixF a = new MatrixF(new float[,] { { 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, -9 } });
SingularValueDecompositionF svd = new SingularValueDecompositionF(a);
Assert.AreEqual(3, svd.NumericalRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, svd.U * svd.S * svd.V.Transposed));
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
float condNumber = svd.ConditionNumber;
svd = new SingularValueDecompositionF(a.Transposed);
Assert.AreEqual(3, svd.NumericalRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a.Transposed, svd.U * svd.S * svd.V.Transposed));
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
condNumber = svd.ConditionNumber;
a = new MatrixF(new float[,] { { 1, 2 }, { 4, 5 }, { 4, 5 } });
svd = new SingularValueDecompositionF(a);
Assert.AreEqual(2, svd.NumericalRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, svd.U * svd.S * svd.V.Transposed));
Assert.AreEqual(svd.SingularValues[0], svd.Norm2);
condNumber = svd.ConditionNumber;
}
/// <summary>
/// Returns outputs based on inputs going through the neural network
/// </summary>
/// <param name="_inputs"></param>
/// <returns></returns>
public float[] FeedForward(float[] _inputs)
{
// create matrix from inputs
List <MatrixF> m = new List <MatrixF>();
m.Add(MatrixF.FromArray(_inputs));
// calculate hidden layers outputs
for (int i = 0; i < numHiddenLayers; i++)
{
m.Add(MatrixF.DotProduct(weightMatrixes[i], m[i]));
m[i + 1].Add(biasMatrixes[i]);
Activate(m[i + 1]);
}
// calculate output layer output
m.Add(MatrixF.DotProduct(weightMatrixes[weightMatrixes.Count - 1], m[m.Count - 1]));
m[m.Count - 1].Add(biasMatrixes[biasMatrixes.Count - 1]);
Activate(m[m.Count - 1]);
return(MatrixF.ToArray(m[m.Count - 1]));
}
public void TestWithNaNValues()
{
MatrixF a = new MatrixF(new [,] {{ 0, float.NaN, 2 },
{ 1, 4, 3 },
{ 2, 3, 5}});
var d = new EigenvalueDecompositionF(a);
foreach (var element in d.RealEigenvalues.ToList())
Assert.IsNaN(element);
foreach (var element in d.ImaginaryEigenvalues.ToList())
Assert.IsNaN(element);
foreach (var element in d.V.ToList(MatrixOrder.RowMajor))
Assert.IsNaN(element);
d = new EigenvalueDecompositionF(new MatrixF(4, 4, float.NaN));
foreach (var element in d.RealEigenvalues.ToList())
Assert.IsNaN(element);
foreach (var element in d.ImaginaryEigenvalues.ToList())
Assert.IsNaN(element);
foreach (var element in d.V.ToList(MatrixOrder.RowMajor))
Assert.IsNaN(element);
}
public void TestMatricesWithFullRank()
{
MatrixF a = new MatrixF(new float[, ] {
{ 1, 2, 3 }, { 4, 5, 6 }, { 7, 8, -9 }
});
QRDecompositionF qr = new QRDecompositionF(a);
Assert.AreEqual(true, qr.HasNumericallyFullRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, qr.Q * qr.R));
qr = new QRDecompositionF(a.Transposed);
Assert.AreEqual(true, qr.HasNumericallyFullRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a.Transposed, qr.Q * qr.R));
a = new MatrixF(new float[, ] {
{ 1, 2 }, { 4, 5 }, { 4, 5 }
});
qr = new QRDecompositionF(a);
Assert.AreEqual(true, qr.HasNumericallyFullRank);
Assert.IsTrue(MatrixF.AreNumericallyEqual(a, qr.Q * qr.R));
MatrixF h = qr.H; // Just call this one to see if it runs through.
}
public void ToMatrixF()
{
Matrix22F m22 = new Matrix22F(1, 2, 3, 4);
MatrixF m = m22.ToMatrixF();
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
Assert.AreEqual(i * 2 + j + 1, m[i, j]);
}
}
m = m22;
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
Assert.AreEqual(i * 2 + j + 1, m[i, j]);
}
}
}
public void ToMatrixF()
{
Matrix m33 = new Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9);
MatrixF m = m33.ToMatrixF();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Assert.AreEqual(i * 3 + j + 1, m[i, j]);
}
}
m = m33;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Assert.AreEqual(i * 3 + j + 1, m[i, j]);
}
}
}
public void TestWithNaNValues()
{
MatrixF a = new MatrixF(new[, ] {
{ 0, float.NaN, 2 },
{ 1, 4, 3 },
{ 2, 3, 5 }
});
var d = new SingularValueDecompositionF(a);
foreach (var element in d.SingularValues.ToList())
{
Assert.IsNaN(element);
}
foreach (var element in d.U.ToList(MatrixOrder.RowMajor))
{
Assert.IsNaN(element);
}
foreach (var element in d.V.ToList(MatrixOrder.RowMajor))
{
Assert.IsNaN(element);
}
d = new SingularValueDecompositionF(new MatrixF(4, 3, float.NaN));
foreach (var element in d.SingularValues.ToList())
{
Assert.IsNaN(element);
}
foreach (var element in d.U.ToList(MatrixOrder.RowMajor))
{
Assert.IsNaN(element);
}
foreach (var element in d.V.ToList(MatrixOrder.RowMajor))
{
Assert.IsNaN(element);
}
}
public void Absolute()
{
float[] values = new float[] { -1.0f, -2.0f, -3.0f,
-4.0f, -5.0f, -6.0f,
-7.0f, -8.0f, -9.0f };
MatrixF m = new MatrixF(3, 3, values, MatrixOrder.RowMajor);
MatrixF absolute = m.Clone();
absolute.Absolute();
for (int i = 0; i < absolute.NumberOfRows; i++)
for (int j = 0; j < absolute.NumberOfColumns; j++)
Assert.AreEqual(i * absolute.NumberOfColumns + j + 1, absolute[i, j]);
absolute = MatrixF.Absolute(m);
for (int i = 0; i < absolute.NumberOfRows; i++)
for (int j = 0; j < absolute.NumberOfColumns; j++)
Assert.AreEqual(i * absolute.NumberOfColumns + j + 1, absolute[i, j]);
values = new float[] { 1.0f, 2.0f, 3.0f,
4.0f, 5.0f, 6.0f,
7.0f, 8.0f, 9.0f };
m = new MatrixF(3, 3, values, MatrixOrder.RowMajor);
absolute = m.Clone();
absolute.Absolute();
for (int i = 0; i < absolute.NumberOfRows; i++)
for (int j = 0; j < absolute.NumberOfColumns; j++)
Assert.AreEqual(i * absolute.NumberOfColumns + j + 1, absolute[i, j]);
absolute = MatrixF.Absolute(m);
for (int i = 0; i < absolute.NumberOfRows; i++)
for (int j = 0; j < absolute.NumberOfColumns; j++)
Assert.AreEqual(i * absolute.NumberOfColumns + j + 1, absolute[i, j]);
Assert.IsNull(MatrixF.Absolute(null));
}
/// <summary>
/// Creates a new Neural Network
/// </summary>
/// <param name="_inputs">number of inputs</param>
/// <param name="_outputs">number of output</param>
/// <param name="_hiddenNodes">number of neurons per hidden layer</param>
public NeuralNetwork(int _inputs, int _hiddenNodes, int _outputs, int _numHiddenLayers = 1)
{
inputs = _inputs;
outputs = _outputs;
hiddenNodes = _hiddenNodes;
numHiddenLayers = _numHiddenLayers;
// matrix of weights to use with layers
weightMatrixes = new List <MatrixF>();
weightMatrixes.Add(MatrixF.RandomMatrix(hiddenNodes, inputs));
for (int i = 0; i < _numHiddenLayers - 1; i++)
{
weightMatrixes.Add(MatrixF.RandomMatrix(hiddenNodes, hiddenNodes));
}
weightMatrixes.Add(MatrixF.RandomMatrix(outputs, hiddenNodes));
// very tall matrix of bias to use with layers
biasMatrixes = new List <MatrixF>();
for (int i = 0; i < _numHiddenLayers; i++)
{
biasMatrixes.Add(MatrixF.RandomMatrix(hiddenNodes, 1));
}
biasMatrixes.Add(MatrixF.RandomMatrix(outputs, 1));
}
// This method shows how to solve linear systems of equations.
private void SolveLinearSystems()
{
var debugRenderer = GraphicsScreen.DebugRenderer2D;
debugRenderer.DrawText("----- SolveLinearSystems Example:");
// ----- Part 1: We want to solve following system of equations:
{
// 3x + 4y - 10z = 6
// -7x + 9y = 0
// x - 2y + 3z = -8
// We will represent this linear system using matrices and vectors:
// A * x = b
// where the vector x contains the unknown variables.
// Define the coefficient matrix A:
Matrix33F A = new Matrix33F(3, 4, -10,
-7, 9, 0,
1, -2, 3);
// Define the result vector b:
Vector3F b = new Vector3F(6, 0, -8);
// x can be computed with x = A^-1 * b.
Vector3F x = A.Inverse * b;
// Note: A.Inverse will throw an exception if A is not invertible.
// Check the result.
if (Vector3F.AreNumericallyEqual(A * x, b))
{
debugRenderer.DrawText("Solution is correct.\n"); // This message is written.
}
else
{
debugRenderer.DrawText("Solution is incorrect.\n");
}
}
// ----- Part 2: We want to solve following system of equations:
{
// 3x + 4y = 6
// -7x + 9y = 1
// x - 2y = -1
// This linear system is overdetermined - there are more equations than unknown.
// We can use MatrixF.SolveLinearEquations() to find an approximate solution
// using the least-squares method.
// Define the coefficient matrix A:
MatrixF A = new MatrixF(
new float[3, 2]
{
{ 3, 4 },
{ -7, 9 },
{ 1, -2 }
});
// Define the result vector b.
// (MatrixF.SolveLinearEquations() takes b as MatrixF not VectorF!)
MatrixF b = new MatrixF(
new float[3, 1]
{
{ 6 },
{ 1 },
{ -1 }
});
// Next we compute x.
// MatrixF.SolveLinearEquations() computes the exact result if possible.
// For overdetermined systems (A has more rows than columns) the least-
// squares solution is computed.
MatrixF x = MatrixF.SolveLinearEquations(A, b);
// The result x is an approximate solution for the above overdetermined linear system.
debugRenderer.DrawText("Result: x = " + x[0, 0] + ", y = " + x[1, 0] + "\n");
}
}
//--------------------------------------------------------------
/// <summary>
/// Returns the least squares solution for the equation <c>A * X = B</c>.
/// </summary>
/// <param name="matrixB">The matrix B with as many rows as A and any number of columns.</param>
/// <returns>X with the least squares solution.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of rows does not match.
/// </exception>
/// <exception cref="MathematicsException">
/// The matrix A does not have full rank.
/// </exception>
public MatrixF SolveLinearEquations(MatrixF matrixB)
{
if (matrixB == null)
throw new ArgumentNullException("matrixB");
if (matrixB.NumberOfRows != _m)
throw new ArgumentException("The number of rows does not match.", "matrixB");
if (HasNumericallyFullRank == false)
throw new MathematicsException("The matrix does not have full rank.");
// Copy right hand side
MatrixF x = matrixB.Clone();
// Compute Y = transpose(Q)*B
for (int k = 0; k < _n; k++)
{
for (int j = 0; j < matrixB.NumberOfColumns; j++)
{
float s = 0;
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 < matrixB.NumberOfColumns; j++)
x[k, j] /= _rDiagonal[k];
for (int i = 0; i < k; i++)
for (int j = 0; j < matrixB.NumberOfColumns; j++)
x[i, j] -= x[k, j] * _qr[i, k];
}
return x.GetSubmatrix(0, _n - 1, 0, matrixB.NumberOfColumns - 1);
}
/// <summary>
/// Solves the specified linear system of equations <i>Ax=b</i>.
/// </summary>
/// <param name="matrixA">The matrix A.</param>
/// <param name="initialX">
/// The initial guess for x. If this value is <see langword="null"/>, a zero vector will be used
/// as initial guess.
/// </param>
/// <param name="vectorB">The vector b.</param>
/// <returns>The solution vector x.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixA"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="vectorB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// <paramref name="matrixA"/> is not a square matrix.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of elements of <paramref name="initialX"/> does not match.
/// </exception>
public override VectorF Solve(MatrixF matrixA, VectorF initialX, VectorF vectorB)
{
// TODO: We can possible improve the method by reordering after each step.
// This can be done randomly or we sort by the "convergence" of the elements.
// See book Physics-Based Animation.
NumberOfIterations = 0;
if (matrixA == null)
{
throw new ArgumentNullException("matrixA");
}
if (vectorB == null)
{
throw new ArgumentNullException("vectorB");
}
if (matrixA.IsSquare == false)
{
throw new ArgumentException("Matrix A must be a square matrix.", "matrixA");
}
if (matrixA.NumberOfRows != vectorB.NumberOfElements)
{
throw new ArgumentException("The number of rows of A and b do not match.");
}
if (initialX != null && initialX.NumberOfElements != vectorB.NumberOfElements)
{
throw new ArgumentException("The number of elements of the initial guess for x and b do not match.");
}
VectorF xOld = initialX ?? new VectorF(vectorB.NumberOfElements);
VectorF xNew = new VectorF(vectorB.NumberOfElements);
bool isConverged = false;
// Make iterations until max iteration count or the result has converged.
for (int i = 0; i < MaxNumberOfIterations && !isConverged; i++)
{
for (int j = 0; j < vectorB.NumberOfElements; j++)
{
float delta = 0;
for (int k = 0; k < j; k++)
{
delta += matrixA[j, k] * xNew[k];
}
for (int k = j + 1; k < vectorB.NumberOfElements; k++)
{
delta += matrixA[j, k] * xOld[k];
}
xNew[j] = (vectorB[j] - delta) / matrixA[j, j];
}
// Test convergence
isConverged = VectorF.AreNumericallyEqual(xOld, xNew, Epsilon);
xOld = xNew.Clone();
NumberOfIterations = i + 1;
}
return(xNew);
}
public RotationMatrix(Vector3 rotation)
{
X = MatrixF.CreateRotationX(rotation.X);
Y = MatrixF.CreateRotationY(rotation.Y);
Z = MatrixF.CreateRotationZ(rotation.Z);
}
public static void ComputeBoundingCapsule(IList <Vector3> points, out float radius, out float height, out Pose pose)
{
if (points == null)
{
throw new ArgumentNullException("points");
}
// Covariance matrix.
MatrixF cov = null;
// ReSharper disable EmptyGeneralCatchClause
try
{
if (points.Count > 4)
{
// Reduce point list to convex hull.
DcelMesh dcelMesh = CreateConvexHull(points);
TriangleMesh mesh = dcelMesh.ToTriangleMesh();
// Use reduced point list - if we have found a useful one. (Line objects
// have not useful triangle mesh.)
if (mesh.Vertices.Count > 0)
{
points = mesh.Vertices;
}
cov = ComputeCovarianceMatrixFromSurface(mesh);
}
}
catch
{
}
// ReSharper restore EmptyGeneralCatchClause
// If anything happens in the convex hull creation, we can still go on including the
// interior points and compute the covariance matrix for the points instead of the
// surface.
if (cov == null || Numeric.IsNaN(cov.Determinant()))
{
cov = ComputeCovarianceMatrixFromPoints(points);
}
// Perform Eigenvalue decomposition.
EigenvalueDecompositionF evd = new EigenvalueDecompositionF(cov);
// v transforms from local coordinate space of the capsule into world space.
var v = evd.V.ToMatrix();
Debug.Assert(v.GetColumn(0).IsNumericallyNormalized);
Debug.Assert(v.GetColumn(1).IsNumericallyNormalized);
Debug.Assert(v.GetColumn(2).IsNumericallyNormalized);
// v is like a rotation matrix, but the coordinate system is not necessarily right handed.
// --> Make sure it is right-handed.
v.SetColumn(2, Vector3.Cross(v.GetColumn(0), v.GetColumn(1)));
// Make local Y the largest axis. (Y is the long capsule axis.)
Vector3 eigenValues = evd.RealEigenvalues.ToVector3();
int largestComponentIndex = eigenValues.IndexOfLargestComponent;
if (largestComponentIndex != 1)
{
// Swap two columns to create a right handed rotation matrix.
Vector3 colLargest = v.GetColumn(largestComponentIndex);
Vector3 col1 = v.GetColumn(1);
v.SetColumn(1, colLargest);
v.SetColumn(largestComponentIndex, col1);
v.SetColumn(2, Vector3.Cross(v.GetColumn(0), v.GetColumn(1)));
}
// Compute capsule for the orientation given by v.
Vector3 center;
ComputeBoundingCapsule(points, v, out radius, out height, out center);
pose = new Pose(center, v);
}
//--------------------------------------------------------------
/// <summary>
/// Solves the equation <c>A * X = B</c>.
/// </summary>
/// <param name="matrixB">The matrix B with as many rows as A and any number of columns.</param>
/// <returns>X, so that <c>A * X = B</c>.</returns>
/// <exception cref="ArgumentNullException">
/// <paramref name="matrixB"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// The number of rows does not match.
/// </exception>
/// <exception cref="MathematicsException">
/// The matrix A is not symmetric and positive definite.
/// </exception>
public MatrixF SolveLinearEquations(MatrixF matrixB)
{
if (matrixB == null)
throw new ArgumentNullException("matrixB");
if (matrixB.NumberOfRows != L.NumberOfRows)
throw new ArgumentException("The number of rows does not match.", "matrixB");
if (IsSymmetricPositiveDefinite == false)
throw new MathematicsException("The original matrix A is not symmetric and positive definite.");
// Initialize x as a copy of B.
MatrixF x = matrixB.Clone();
// Solve L*Y = B.
for (int k = 0; k < L.NumberOfRows; k++)
{
for (int j = 0; j < matrixB.NumberOfColumns; j++)
{
for (int i = 0; i < k; i++)
x[k, j] -= x[i, j] * L[k, i];
x[k, j] /= L[k, k];
}
}
// Solve transpose(L) * X = Y.
for (int k = L.NumberOfRows - 1; k >= 0; k--)
{
for (int j = 0; j < matrixB.NumberOfColumns; j++)
{
for (int i = k + 1; i < L.NumberOfRows; i++)
x[k, j] -= x[i, j] * L[i, k];
x[k, j] /= L[k, k];
}
}
return x;
}
//--------------------------------------------------------------
/// <summary>
/// Called when <see cref="ScatteredInterpolationF.Setup"/> is called.
/// </summary>
/// <remarks>
/// Here internal values can be computed from the registered reference pairs if required. It is
/// assured that the reference data pairs have valid dimensions: All x values have the same
/// number of elements and all y values have the same number of elements. All reference data
/// values are not <see langword="null"/>.
/// </remarks>
/// <exception cref="MathematicsException">
/// Cannot compute regression - try to choose different reference data pairs or another basis
/// function.
/// </exception>
protected override void OnSetup()
{
// Compute weights:
// Make matrix Φ (also known as G).
int numberOfPairs = Count;
MatrixF G = new MatrixF(numberOfPairs, numberOfPairs);
for (int r = 0; r < numberOfPairs; r++) // rows
{
for (int c = 0; c < numberOfPairs; c++) // columns
{
float radialArgument = DistanceFunction(GetX(r), GetX(c), c);
G[r, c] = BasisFunction(radialArgument, c);
}
}
// We look for the matrix W that contains the weights.
// Each row resembles an n-dimensional weight.
MatrixF W;
// Make the matrix Y with the reference values, such that ideally Y = G * W.
MatrixF Y = new MatrixF(numberOfPairs, GetY(0).NumberOfElements);
for (int r = 0; r < numberOfPairs; r++)
Y.SetRow(r, GetY(r));
// Compute W as the least squares solution.
try
{
W = MatrixF.SolveLinearEquations(G, Y);
}
catch (MathematicsException exception)
{
throw new MathematicsException("Cannot compute regression - try to choose different reference data pairs or another basis function.", exception);
}
// The rows of W are the weights.
_weights = new VectorF[numberOfPairs];
for (int r = 0; r < numberOfPairs; r++)
_weights[r] = W.GetRow(r);
}
/// <overloads>
/// <summary>
/// Solves the specified linear system of equations <i>A * x = b</i>.
/// </summary>
/// </overloads>
///
/// <summary>
/// Solves the specified linear system of equations <i>A * x = b</i>.
/// </summary>
/// <param name="matrixA">The matrix A.</param>
/// <param name="vectorB">The vector b.</param>
/// <returns>The solution vector x.</returns>
/// <remarks>
/// A zero vector is used as initial guess for x.
/// </remarks>
public VectorF Solve(MatrixF matrixA, VectorF vectorB)
{
return(Solve(matrixA, null, vectorB));
}
/// <summary>
/// Called when <see cref="IKSolver.Solve"/> is called.
/// </summary>
/// <param name="deltaTime">The current time step (in seconds).</param>
protected override void OnSolve(float deltaTime)
{
if (NumberOfIterations <= 0)
{
return;
}
if (_isDirty)
{
if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex))
{
throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain.");
}
}
_isDirty = false;
var skeleton = SkeletonPose.Skeleton;
bool requiresBlending = RequiresBlending();
float maxRotationAngle;
bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle);
if (requiresBlending || requiresLimiting)
{
// Remember original bone transforms for interpolation with the result at the end.
// Transforms are stored from tip to root (reverse order!).
_originalTransforms.Clear();
int boneIndex = TipBoneIndex;
while (true)
{
_originalTransforms.Add(SkeletonPose.GetBoneTransform(boneIndex));
if (boneIndex == RootBoneIndex)
{
break;
}
boneIndex = skeleton.GetParent(boneIndex);
}
}
int numberOfBones = SkeletonPose.GetNumberOfBones(RootBoneIndex, TipBoneIndex);
// The transposed jacobian matrix.
var jacobianTransposed = new MatrixF(numberOfBones, 6);
// The force vector (3 linear and 3 angular (torque) entries).
VectorF force = new VectorF(6);
// The rotation axes of the bones.
Vector3F[] axes = new Vector3F[numberOfBones];
float toleranceSquared = AllowedDeviation * AllowedDeviation;
// In each iteration we compute the jacobian matrix, compute the bone velocities
// an make an euler integration step.
for (int iteration = 0; iteration < NumberOfIterations; iteration++)
{
var tipBoneAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex);
var tipAbsolute = tipBoneAbsolute.ToParentPosition(TipOffset);
var targetToTip = tipAbsolute - Target;
if (targetToTip.LengthSquared < toleranceSquared)
{
if (iteration == 0)
{
return;
}
break;
}
// Loop from tip to root and fill Jacobian.
// (See description of Jacobian Transpose method to see how the rotation axes and
// Jacobian entries must look like).
var currentBoneIndex = TipBoneIndex;
int exitBoneIndex = RootBoneIndex >= 0 ? skeleton.GetParent(RootBoneIndex) : -1;
int i = numberOfBones;
do
{
i--;
// Compute rotation axis.
Vector3F currentJointAbsolute = SkeletonPose.GetBonePoseAbsolute(currentBoneIndex).Translation;
Vector3F jointToTarget = Target - currentJointAbsolute;
Vector3F jointToTip = tipAbsolute - currentJointAbsolute;
axes[i] = Vector3F.Cross(jointToTarget, jointToTip);
if (!axes[i].TryNormalize())
{
axes[i] = Vector3F.UnitX; // TODO: What should we really do in this case?
}
Vector3F jacobianColumnUpperPart = Vector3F.Cross(jointToTip, axes[i]);
// Fill J.
jacobianTransposed[i, 0] = jacobianColumnUpperPart.X;
jacobianTransposed[i, 1] = jacobianColumnUpperPart.Y;
jacobianTransposed[i, 2] = jacobianColumnUpperPart.Z;
jacobianTransposed[i, 3] = axes[i].X;
jacobianTransposed[i, 4] = axes[i].Y;
jacobianTransposed[i, 5] = axes[i].Z;
currentBoneIndex = skeleton.GetParent(currentBoneIndex);
} while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0);
Debug.Assert(i == 0);
// Set the force.
force[0] = targetToTip.X;
force[1] = targetToTip.Y;
force[2] = targetToTip.Z;
force[3] = 0;
force[4] = 0;
force[5] = 0;
// Compute pseudo velocities.
VectorF velocities = jacobianTransposed * force; // TODO: Garbage!
// Euler integration step.
currentBoneIndex = TipBoneIndex;
i = numberOfBones;
do
{
i--;
// Rotation axis for this bone.
Vector3F axis = axes[i];
// Angle is computed using Euler integration with an arbitrary step size.
float angle = velocities[i] * StepSize;
// Apply rotation.
QuaternionF rotationChange = QuaternionF.CreateRotation(axis, angle);
SkeletonPose.RotateBoneAbsolute(currentBoneIndex, rotationChange);
currentBoneIndex = skeleton.GetParent(currentBoneIndex);
} while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0);
// Call delegate that checks bone limits.
if (LimitBoneTransforms != null)
{
LimitBoneTransforms();
}
}
if (requiresBlending || requiresLimiting)
{
// Apply weight and the angular velocity limit.
int boneIndex = TipBoneIndex;
int i = 0;
while (true)
{
var originalTransform = _originalTransforms[i];
var targetTransform = SkeletonPose.GetBoneTransform(boneIndex);
// Apply weight.
if (requiresBlending)
{
BlendBoneTransform(ref originalTransform, ref targetTransform);
}
// Apply angular velocity limit.
if (requiresLimiting)
{
LimitBoneTransform(ref originalTransform, ref targetTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(boneIndex, targetTransform);
if (boneIndex == RootBoneIndex)
{
break;
}
boneIndex = skeleton.GetParent(boneIndex);
i++;
}
}
_originalTransforms.Clear();
}
/// <summary> /// Solves the specified linear system of equations <i>A * x = b</i> using an initial guess. /// </summary> /// <param name="matrixA">The matrix A.</param> /// <param name="initialX"> /// The initial guess for x. If this value is <see langword="null"/>, a zero vector will be used /// as initial guess. /// </param> /// <param name="vectorB">The vector b.</param> /// <returns>The solution vector x.</returns> public abstract VectorF Solve(MatrixF matrixA, VectorF initialX, VectorF vectorB);
