/// <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]; } }