/// <summary>
/// Run example
/// </summary>
public void Run()
{
// Format matrix output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Solve next system of linear equations (Ax=b):
// 5*x + 2*y - 4*z = -7
// 3*x - 7*y + 6*z = 38
// 4*x + 1*y + 5*z = 43
// Create matrix "A" with coefficients
var matrixA = new DenseMatrix(new[,] { { 5.00, 2.00, -4.00 }, { 3.00, -7.00, 6.00 }, { 4.00, 1.00, 5.00 } });
Console.WriteLine(@"Matrix 'A' with coefficients");
Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Create vector "b" with the constant terms.
var vectorB = new DenseVector(new[] { -7.0, 38.0, 43.0 });
Console.WriteLine(@"Vector 'b' with the constant terms");
Console.WriteLine(vectorB.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 1. Solve linear equations using LU decomposition
var resultX = matrixA.LU().Solve(vectorB);
Console.WriteLine(@"1. Solution using LU decomposition");
Console.WriteLine(resultX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Solve linear equations using QR decomposition
resultX = matrixA.QR().Solve(vectorB);
Console.WriteLine(@"2. Solution using QR decomposition");
Console.WriteLine(resultX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Solve linear equations using SVD decomposition
matrixA.Svd(true).Solve(vectorB, resultX);
Console.WriteLine(@"3. Solution using SVD decomposition");
Console.WriteLine(resultX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 4. Solve linear equations using Gram-Shmidt decomposition
matrixA.GramSchmidt().Solve(vectorB, resultX);
Console.WriteLine(@"4. Solution using Gram-Shmidt decomposition");
Console.WriteLine(resultX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 5. Verify result. Multiply coefficient matrix "A" by result vector "x"
var reconstructVecorB = matrixA * resultX;
Console.WriteLine(@"5. Multiply coefficient matrix 'A' by result vector 'x'");
Console.WriteLine(reconstructVecorB.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// To use Cholesky or Eigenvalue decomposition coefficient matrix must be
// symmetric (for Evd and Cholesky) and positive definite (for Cholesky)
// Multipy matrix "A" by its transpose - the result will be symmetric and positive definite matrix
var newMatrixA = matrixA.TransposeAndMultiply(matrixA);
Console.WriteLine(@"Symmetric positive definite matrix");
Console.WriteLine(newMatrixA.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 6. Solve linear equations using Cholesky decomposition
newMatrixA.Cholesky().Solve(vectorB, resultX);
Console.WriteLine(@"6. Solution using Cholesky decomposition");
Console.WriteLine(resultX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 7. Solve linear equations using eigen value decomposition
newMatrixA.Evd().Solve(vectorB, resultX);
Console.WriteLine(@"7. Solution using eigen value decomposition");
Console.WriteLine(resultX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 8. Verify result. Multiply new coefficient matrix "A" by result vector "x"
reconstructVecorB = newMatrixA * resultX;
Console.WriteLine(@"8. Multiply new coefficient matrix 'A' by result vector 'x'");
Console.WriteLine(reconstructVecorB.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
public void LUFailsWithNonSquareMatrix()
{
var matrix = new DenseMatrix(3, 2);
Assert.That(() => matrix.LU(), Throws.ArgumentException);
}
/// <summary>
/// Run example
/// </summary>
/// <seealso cref="http://en.wikipedia.org/wiki/LU_decomposition">LU decomposition</seealso>
/// <seealso cref="http://en.wikipedia.org/wiki/Invertible_matrix">Invertible matrix</seealso>
public void Run()
{
// Format matrix output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Create square matrix
var matrix = new DenseMatrix(new[,] { { 1.0, 2.0 }, { 3.0, 4.0 } });
Console.WriteLine(@"Initial square matrix");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Perform LU decomposition
var lu = matrix.LU();
Console.WriteLine(@"Perform LU decomposition");
// 1. Lower triangular factor
Console.WriteLine(@"1. Lower triangular factor");
Console.WriteLine(lu.L.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Upper triangular factor
Console.WriteLine(@"2. Upper triangular factor");
Console.WriteLine(lu.U.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Permutations applied to LU factorization
Console.WriteLine(@"3. Permutations applied to LU factorization");
for (var i = 0; i < lu.P.Dimension; i++)
{
if (lu.P[i] > i)
{
Console.WriteLine(@"Row {0} permuted with row {1}", lu.P[i], i);
}
}
Console.WriteLine();
// 4. Reconstruct initial matrix: PA = L * U
var reconstruct = lu.L * lu.U;
// The rows of the reconstructed matrix should be permuted to get the initial matrix
reconstruct.PermuteRows(lu.P.Inverse());
Console.WriteLine(@"4. Reconstruct initial matrix: PA = L*U");
Console.WriteLine(reconstruct.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 5. Get the determinant of the matrix
Console.WriteLine(@"5. Determinant of the matrix");
Console.WriteLine(lu.Determinant);
Console.WriteLine();
// 6. Get the inverse of the matrix
var matrixInverse = lu.Inverse();
Console.WriteLine(@"6. Inverse of the matrix");
Console.WriteLine(matrixInverse.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 7. Matrix multiplied by its inverse
var identity = matrix * matrixInverse;
Console.WriteLine(@"7. Matrix multiplied by its inverse ");
Console.WriteLine(identity.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
private void solveActuator2CartesianDisp(double[] adisp)
{
bool check = false;
DenseVector cartDisp = new DenseVector(6);
DenseVector newAct = new DenseVector(adisp);
DenseVector actError = (DenseVector)newAct.Subtract(actuatorDisp);
cartesianDisp.CopyTo(cartDisp);
int iterations = 0;
while (check == false)
{
List2String l2s = new List2String();
DenseMatrix JacobianMatrix = new DenseMatrix(6, 6);
for (int i = 0; i < 6; i++)
{
DenseVector DL_Dd = actuators[i].calcNewDiffs(cartDisp.Values);
JacobianMatrix.SetRow(i, DL_Dd);
}
DenseVector diffCart = (DenseVector)JacobianMatrix.LU().Solve(actError);
log.Debug("Cartesian differences " + l2s.ToString(diffCart.Values));
cartDisp = (DenseVector)cartDisp.Add(diffCart);
setCartesianDisp(cartDisp.Values);
log.Debug("New cartesian estimate " + this);
actError = (DenseVector)newAct.Subtract(actuatorDisp);
log.Debug("Actuator error " + l2s.ToString(actError.Values));
check = withinErrorWindow(actError);
if (iterations > 20)
{
check = true;
log.Error("Calculations for " + label + " won't converge with " + this);
}
iterations++;
}
}
void Solve(double[,] A, double[] b, int nW)
{
var matrixA = new DenseMatrix(A);
var vectorB = new DenseVector(b);
Vector<double> resultX = matrixA.LU().Solve(vectorB);
for (int nPos = 0; nPos < resultX.Count; nPos++)
{
m_Weights[nPos, nW] = resultX[nPos];
}
}
//public static int solve(double[,] A, double[] fitz, CenterArrayNode CenterNode, IRBFPolynomial Poly)
//{
// System.Diagnostics.Debug.Assert(fitz.Length == A.GetLength(0));
// System.Diagnostics.Debug.Assert(A.GetLength(0) == A.GetLength(1));
// int[] pivot = new int[A.GetLength(0)];
// int i;//,j,k;
// //double[] d = new double[A.Length];
// //for (j = 0; j < A.GetLength(0); j++)
// // for (k = 0; k < A.GetLength(1); k++)
// // d[k + j * A.GetLength(0)] = A[j, k];
// //DumpD(d, A.GetLength(1), A.GetLength(0), "c:\\before.csv");
// double[,] fit = new double[fitz.Length, 1];
// for (i = 0; i < fitz.GetLength(0); i++)
// fit[i, 0] = fitz[i];
// double[,] w2 = BLAS.SimultaneousSolver(A, fit);
// //var matrixA = new DenseMatrix(A);
// //var vectorB = new DenseVector(fitz);
// //Vector<double> resultX = matrixA.LU().Solve(vectorB);
// //MCroutPPS.Decomp(Tolerance, ref d, ref pivot, ref error, A.GetLength(1), A.GetLength(0));
// //DumpD(d, A.GetLength(1), A.GetLength(0), "c:\\after.csv");
// //if (error <= 0)
// //{
// // return error;
// //}
// //double[] w = new double[A.GetLength(0)];// create weights vector
// double[] w = new double[A.GetLength(0)];
// for (i = 0; i < w2.GetLength(0); i++)
// w[i] = w2[i, 0];
// //MCroutPPS.Desolv(ref d, ref w, ref fitz, ref pivot, ref error, A.GetLength(0));
// //DumpW(w, "c:\\w.csv");
// i = 0;
// foreach (double weight in w)
// {
// if (i < CenterNode.Centers.Count)
// CenterNode[i].w = weight; // set the center's weight
// else
// Poly[i - CenterNode.Centers.Count] = weight;
// //polycofs[i - Centers.Count] = weight; //store the polynomial coefficients
// ++i;
// }
// return 0;
//}
public static int solve(double[,] A, double[] fitz, ICenter[] Centers, IRBFPolynomial Poly)
{
var matrixA = new DenseMatrix(A);
var vectorB = new DenseVector(fitz);
Vector<double> resultX = matrixA.LU().Solve(vectorB);
List<double> w2 = new List<double>(resultX.ToArray());
int i = 0;
w2.ForEach((double weight) =>
{
if (i < Centers.Length)
Centers[i].w = weight; // set the center's weight
else
Poly[i - Centers.Length] = weight;//store the polynomial coefficients
++i;
});
return 0;
}
/// <summary>
/// using the start, end and passed geo fit points will slide the points along their normals to achieve a geodesic
/// </summary>
/// <param name="g">the curve object to spline</param>
/// <param name="start">the starting point of this segment</param>
/// <param name="end">the ending point of this segment</param>
/// <param name="sFits">the s-positons of each geo-point</param>
/// <param name="uFits">the u-positons of each geo-point</param>
/// <returns>true if successful, false otherwise</returns>
static bool FitGeo(MouldCurve g, IFitPoint start, IFitPoint end, double[] sFits, Vect2[] uFits)
{
int NumFits = sFits.Length;
int INC = NumFits - 1;
int i;
Vect2[] uNor = new Vect2[NumFits];
Vect3 xyz = new Vect3(), dxu = new Vect3(), dxv = new Vect3();
Vect2 ut = new Vect2(), un = new Vect2();
double a11, a12, a22, det;
//calculate insurface normals at each fitpoint
uNor[0] = new Vect2();//fixed endpoint doesnt need normal
for (i = 1; i < INC; i++)
{
g.Surface.xVec(uFits[i], ref xyz, ref dxu, ref dxv);
//covariant metric tensor components
a11 = dxu.Norm;
a12 = dxu.Dot(dxv);
a22 = dxv.Norm;
det = Math.Sqrt(a11 * a22 - a12 * a12);
//tangent(secant) vector u components
ut = uFits[i + 1] - uFits[i - 1];
//contravariant normal vector components in the surface plane
un[0] = (a12 * ut[0] + a22 * ut[1]) / det;
un[1] = -(a11 * ut[0] + a12 * ut[1]) / det;
//store unit normal u components
un.Magnitude = 1;
uNor[i] = new Vect2(un);
}
uNor[INC] = new Vect2();//fixed endpoint doesnt need normal
Vect3 xPrev = new Vect3();
Vect3 ddxu = new Vect3(), ddxv = new Vect3(), dduv = new Vect3();
Vect3[] xNor = new Vect3[NumFits];
Vect3[] dxNor = new Vect3[NumFits];
Vect3[] xTan = new Vect3[NumFits];
double[] xLen = new double[NumFits];
bool Conver = false;
int nNwt; Vector x; DenseVector sNor;
for (nNwt = 0; nNwt < 50; nNwt++)
{
xNor[0] = new Vect3();
//update startpoint slide position
if (start is SlidePoint)
{
dxNor[0] = new Vect3();
(start as SlidePoint).Curve.xCvt((start as SlidePoint).SCurve, ref uFits[0], ref xPrev, ref xNor[0], ref dxNor[0]);
//uFits[0][0] = FitPoints[0][1];
//uFits[0][1] = FitPoints[0][2];
}
else
g.xVal(uFits[0], ref xPrev);
//update endpoint slide position
if (end is SlidePoint)
{
uFits[INC][0] = end[1];
uFits[INC][1] = end[2];
}
xLen[0] = 0;
//calc internal point vectors
for (i = 1; i < NumFits; i++)
{
g.Surface.xCvt(uFits[i], ref xyz, ref dxu, ref dxv, ref ddxu, ref ddxv, ref dduv);
a11 = uNor[i][0] * uNor[i][0];
a12 = uNor[i][0] * uNor[i][1] * 2;
a22 = uNor[i][1] * uNor[i][1];
// insurface normal x components
dxu.Scale(uNor[i][0]);
dxv.Scale(uNor[i][1]);
xNor[i] = dxu + dxv;
//insurface normal x derivatives
ddxu.Scale(a11);
dduv.Scale(a12);
ddxv.Scale(a22);
dxNor[i] = ddxu + dduv + ddxv;
//forward facing tangent vector
xTan[i] = xyz - xPrev;
xPrev.Set(xyz);
xLen[i] = xTan[i].Magnitude;//segment length
xLen[0] += xLen[i];//accumulate total length
xTan[i].Magnitude = 1;//unit tangent vector
}
//update endpoint slide position
if (end is SlidePoint)
{
(end as SlidePoint).Curve.xCvt((end as SlidePoint).SCurve, ref uFits[INC], ref xyz, ref xNor[INC], ref dxNor[INC]);
}
DenseMatrix A = new DenseMatrix(NumFits);
sNor = new DenseVector(NumFits);
double p0, pp, d, d0, dp, gm, g0, gp; int ix;
//slide startpoint
if (start is SlidePoint)
{
//mid point normal vector dotted with end point tangent vectors
pp = xNor[0].Dot(xTan[1]);
for (g0 = gp = 0, ix = 0; ix < 3; ix++)
{
//midpoint tangent and curavture variantion
dp = (xNor[0][ix] - pp * xTan[1][ix]) / xLen[1];
//mid and top point gradients
g0 += -dp * xNor[0][ix] + xTan[1][ix] * dxNor[0][ix];
gp += dp * xNor[1][ix];
}
//geodesic residual and gradients
A[0, 0] = g0;
A[0, 1] = gp;
sNor[0] = pp;
}
else//fixed start point
{
A[0, 0] = 1;
sNor[0] = 0;
}
for (i = 1; i < INC; i++)//internal points
{
//midpoint normal dotted with tangents
p0 = xNor[i].Dot(xTan[i]);// BLAS.dot(xNor[i], xTan[i]);
pp = xNor[i].Dot(xTan[i + 1]);//BLAS.dot(xNor[i], xTan[i + 1]);
for (gm = g0 = gp = 0, ix = 0; ix < 3; ix++)
{
//midpoint curvature vector
d = xTan[i + 1][ix] - xTan[i][ix];
//midpoint tangent and curavture variantion
d0 = (xNor[i][ix] - p0 * xTan[i][ix]) / xLen[i];
dp = (xNor[i][ix] - pp * xTan[i + 1][ix]) / xLen[i + 1];
//bottom, mid and top point gradients
gm += d0 * xNor[i - 1][ix];
g0 += (-d0 - dp) * xNor[i][ix] + d * dxNor[i][ix];
gp += dp * xNor[i + 1][ix];
}
A[i, i - 1] = gm;
A[i, i] = g0;
A[i, i + 1] = gp;
sNor[i] = -p0 + pp;
}
if (end is SlidePoint)//slide endpoint
{
p0 = xNor[i].Dot(xTan[i]);
for (gm = g0 = 0, ix = 0; ix < 3; ix++)
{
//midpoint tangent and curavture variantion
d0 = (xNor[i][ix] - p0 * xTan[i][ix]) / xLen[i];
//bottom, mid and top point gradients
gm += d0 * xNor[i - 1][ix];
g0 += -d0 * xNor[i][ix] - xTan[i][ix] * dxNor[i][ix];
}
A[i, i - 1] = gm;
A[i, i] = g0;
sNor[i] = -p0;
}
else//fixed endpoint
{
A[i, i] = 1;
sNor[i] = 0;
}
LU decomp = A.LU();
x = (Vector)decomp.Solve(sNor);
double Reduce = Math.Min(1, .05 / x.AbsoluteMaximum());
if( start is SlidePoint)
(start as SlidePoint).SCurve -= x[0] * Reduce;
if( end is SlidePoint)
(end as SlidePoint).SCurve -= x[INC] * Reduce;
for (i = 1; i < NumFits; i++)//increment uv points
{
uFits[i][0] -= x[i] * uNor[i][0] * Reduce;
uFits[i][1] -= x[i] * uNor[i][1] * Reduce;
}
if (nNwt < 5)
{
//keep initial (s)-increments within bounds
if (start is SlidePoint)
(start as SlidePoint).SCurve = Utilities.LimitRange(0, (start as SlidePoint).SCurve, 1);
if (end is SlidePoint)
(end as SlidePoint).SCurve = Utilities.LimitRange(0, (end as SlidePoint).SCurve, 1);
// keep initial (u)-increments within bounds
for (i = 1; i < NumFits - 1; i++)
{
uFits[i][0] = Utilities.LimitRange(0, uFits[i][0], 1);
uFits[i][1] = Utilities.LimitRange(-.125, uFits[i][1], 1.125);
}
}
double xmax = x.AbsoluteMaximum();
double smax = sNor.AbsoluteMaximum();
if (Conver = (x.AbsoluteMaximum() < 1e-8 && sNor.AbsoluteMaximum() < 1e-7))
break;
}
if (!Conver)
return false;
g.Length = xLen[0];//store length
//calculate unit length (s)-parameter values
sFits[0] = 0;
for (i = 1; i < NumFits; i++)
sFits[i] = sFits[i - 1] + xLen[i] / xLen[0];
//g.m_uvs = uFits;
g.ReSpline(sFits, uFits);
return true;
}
public void Fit(double[] sPos, double[][] xPnts)
{
if (sPos.Length < 5)
{
TriFit(sPos, xPnts);
return;
}
int nKnot = sPos.Length -2;
m_xKnot = new double[sPos.Length+2];
m_xCof = new double[sPos.Length, Deg];
int nKnt = 0;
m_xKnot[nKnt++] = sPos[0];
m_xKnot[nKnt++] = sPos[0];
m_xKnot[nKnt++] = sPos[0];
// trible up Knot end points but leave extra internal end points
for (int nOff = 2; nOff <nKnot; nOff++) m_xKnot[nKnt++] = sPos[nOff];
m_xKnot[nKnt++] = sPos[sPos.Length - 1];
m_xKnot[nKnt++] = sPos[sPos.Length - 1];
m_xKnot[nKnt++] = sPos[sPos.Length - 1];
DenseMatrix A = new DenseMatrix(sPos.Length);
int nS;
double[,] basis = null;
int iRow = 0;//, iCol = 0;
for (iRow = 0; iRow < sPos.Length; iRow++)
{
BsCil(0, sPos[iRow], m_xKnot, out nS, ref basis);
for (int j = 0; j < 4; j++)
A[iRow, nS + j] = basis[0, j];
}
LU decomp = A.LU();
DenseVector b;
for( int j =0; j < Deg; j++ )
{
b = new DenseVector(xPnts[j]);
Vector x = (Vector)decomp.Solve(b);
for (int i = 0; i < sPos.Length; i++)
m_xCof[i, j] = x[i];
}
}
