private TransformationMatrix3D(Matrix mat)
     : base(4)
 {
     if (mat.Rows != 4 || mat.Columns != 4)
     {
         throw new MatrixException("Matrix is not the correct size to convert to a TransformationMatrix3D");
     }
     var squareMatrix = new SquareMatrix(3);
     for (var i = 0; i < 3; i++)
     {
         for (var j = 0; j < 3; j++)
         {
             squareMatrix[i, j] = mat[i, j];
         }
     }
     if (!squareMatrix.IsRotationMatrix())
     {
         throw new MatrixException("Matrix does not contain a valid rotation");
     }
     for (var k = 0; k < 4; k++)
     {
         for (var l = 0; l < 4; l++)
         {
             base[k, l] = mat[k, l];
         }
     }
 }
Esempio n. 2
0
 public static SquareMatrix Identity(int size)
 {
     var squareMatrix = new SquareMatrix(size);
     for (var i = 0; i < size; i++)
     {
         squareMatrix[i, i] = 1.0;
     }
     return squareMatrix;
 }
Esempio n. 3
0
 public double Determinant()
 {
     var squareMatrix = new SquareMatrix(this);
     var num = squareMatrix.MakeRowEchelon();
     double result;
     for (var i = 0; i < Size; i++)
     {
         if (squareMatrix.IsRowZero(i))
         {
             result = 0.0;
             return result;
         }
     }
     result = num;
     return result;
 }
Esempio n. 4
0
 public SquareMatrix Inverse()
 {
     var matrix = Augment(Identity(Size));
     matrix.MakeRowEchelon();
     var squareMatrix = new SquareMatrix(Size);
     var squareMatrix2 = new SquareMatrix(Size);
     for (var i = 0; i < Size; i++)
     {
         squareMatrix.SetColumn(i, matrix.GetColumn(i));
         squareMatrix2.SetColumn(i, matrix.GetColumn(i + Size));
     }
     for (var j = 0; j < squareMatrix.Rows; j++)
     {
         if (squareMatrix.IsRowZero(j))
         {
             throw new MatrixException("Matrix is singular");
         }
     }
     for (var k = Size - 1; k > 0; k--)
     {
         for (var l = k - 1; l >= 0; l--)
         {
             var scalar = -squareMatrix[l, k];
             squareMatrix.AddRowTimesScalar(l, k, scalar);
             squareMatrix2.AddRowTimesScalar(l, k, scalar);
         }
     }
     return squareMatrix2;
 }
Esempio n. 5
0
 public SquareMatrix(SquareMatrix mat)
     : base(mat)
 {
 }
Esempio n. 6
0
 public SquareMatrix Power(int power)
 {
     Matrix matrix = new SquareMatrix(this);
     for (var i = 1; i < power; i++)
     {
         matrix = this*matrix;
     }
     return new SquareMatrix(matrix);
 }
Esempio n. 7
0
 public SquareMatrix Minor(int i, int j)
 {
     var squareMatrix = new SquareMatrix(Size - 1);
     var num = 0;
     for (var k = 0; k < Rows; k++)
     {
         if (k != i)
         {
             var num2 = 0;
             for (var l = 0; l < Columns; l++)
             {
                 if (l != j)
                 {
                     squareMatrix[num, num2] = base[k, l];
                     num2++;
                 }
             }
             num++;
         }
     }
     return squareMatrix;
 }
Esempio n. 8
0
 public void LUDecomposition(out SquareMatrix l, out SquareMatrix u)
 {
     l = new SquareMatrix(Size);
     u = new SquareMatrix(Size);
     if (Math.Abs(base[0, 0] - 0.0) < 0.0001)
     {
         throw new MatrixException("Unable to decompose matrix");
     }
     l.SetColumn(0, GetColumn(0));
     u.SetRow(0, GetRow(0));
     u.MultiplyRow(0, 1.0/base[0, 0]);
     for (var i = 1; i < Size; i++)
     {
         var array = new Vector[Size];
         var array2 = new Vector[Size];
         for (var j = 1; j < Size; j++)
         {
             array[j] = new Vector(i);
             array2[j] = new Vector(i);
             var row = l.GetRow(j);
             var column = u.GetColumn(j);
             for (var k = 0; k < i; k++)
             {
                 array[j][k] = row[k];
                 array2[j][k] = column[k];
             }
         }
         for (var m = i; m < Size; m++)
         {
             l[m, i] = base[m, i] - Vector.Dot(array[m], array2[i]);
             if (m == i)
             {
                 u[i, m] = 1.0;
             }
             else
             {
                 if (Math.Abs(l[i, i] - 0.0) < 0.0001)
                 {
                     throw new MatrixException("Unable to decompose matrix");
                 }
                 u[i, m] = (base[i, m] - Vector.Dot(array[i], array2[m]))/l[i, i];
             }
         }
     }
 }
Esempio n. 9
0
 public Matrix PseudoInverse()
 {
     var sVD = new SVD(this);
     Matrix matrix = new SquareMatrix(Columns);
     for (var i = 0; i < sVD.W.Rows; i++)
     {
         if (sVD.W[i] > 1E-05)
         {
             matrix[i, i] = 1.0/sVD.W[i];
         }
     }
     return sVD.V*matrix*sVD.U.Transpose();
 }
Esempio n. 10
0
 public Vector Solve(ErrorFunction errorFunction, Vector initialGuess)
 {
     if (initialGuess.Size != NumVariables)
     {
         throw new MatrixException("Size of the initial guess vector is not correct");
     }
     var vector = new Vector(initialGuess);
     NumStepsToConverge = 0;
     Vector result;
     for (var i = 0; i < 20; i++)
     {
         var matrix = CalculateJacobian(errorFunction, vector);
         var vec = errorFunction(vector);
         var matrix2 = matrix.Transpose();
         var squareMatrix = new SquareMatrix(matrix2*matrix);
         var vec2 = matrix2*vec;
         var vector2 = squareMatrix.PseudoInverse()*vec2;
         vector -= vector2;
         if (IsDone(vector2))
         {
             NumStepsToConverge = i + 1;
             result = vector;
             return result;
         }
     }
     result = vector;
     return result;
 }
Esempio n. 11
0
 public Plane3D FitPlaneToPoints(Collection<Point3D> points)
 {
     if (points == null)
     {
         throw new NullReferenceException();
     }
     if (points.Count < 3)
     {
         throw new MatrixException("Not enough points to fit a plane");
     }
     var point3D = Centroid(points);
     var num = 0.0;
     var num2 = 0.0;
     var num3 = 0.0;
     var num4 = 0.0;
     var num5 = 0.0;
     var num6 = 0.0;
     foreach (var current in points)
     {
         var num7 = current.X - point3D.X;
         var num8 = current.Y - point3D.Y;
         var num9 = current.Z - point3D.Z;
         num += num7*num7;
         num2 += num8*num8;
         num3 += num9*num9;
         num4 += num7*num8;
         num5 += num8*num9;
         num6 += num7*num9;
     }
     var mat = new SquareMatrix(3, new[]
     {
         num,
         num4,
         num6,
         num4,
         num2,
         num5,
         num6,
         num5,
         num3
     });
     var sVD = new SVD(mat);
     var normal = new Vector3D(sVD.U.GetColumn(sVD.SmallestSingularIndex));
     var plane3D = new Plane3D(point3D, normal);
     CalculateErrors(points, plane3D);
     return plane3D;
 }
Esempio n. 12
0
 public Line3D FitLineToPoints(Collection<Point3D> points)
 {
     if (points == null)
     {
         throw new MatrixNullReference();
     }
     var point3D = Centroid(points);
     var num = 0.0;
     var num2 = 0.0;
     var num3 = 0.0;
     var num4 = 0.0;
     var num5 = 0.0;
     var num6 = 0.0;
     foreach (var current in points)
     {
         var num7 = current.X - point3D.X;
         var num8 = current.Y - point3D.Y;
         var num9 = current.Z - point3D.Z;
         num += num7*num7;
         num2 += num8*num8;
         num3 += num9*num9;
         num4 += num7*num8;
         num5 += num8*num9;
         num6 += num7*num9;
     }
     var mat = new SquareMatrix(3, new[]
     {
         num2 + num3,
         -num4,
         -num6,
         -num4,
         num3 + num,
         -num5,
         -num6,
         -num5,
         num + num2
     });
     var sVD = new SVD(mat);
     var direction = new Vector3D(sVD.U.GetColumn(sVD.SmallestSingularIndex));
     var line3D = new Line3D(point3D, direction);
     CalculateErrors(points, line3D);
     return line3D;
 }
Esempio n. 13
0
 public SVD(Matrix mat)
 {
     var i = 0;
     if (mat.Rows < mat.Columns)
     {
         throw new MatrixException("Matrix must have rows >= columns");
     }
     var rows = mat.Rows;
     var columns = mat.Columns;
     var vector = new Vector(columns);
     var num = 0.0;
     var num2 = 0.0;
     var num3 = 0.0;
     var num4 = 0;
     _u = new Matrix(mat);
     _v = new SquareMatrix(columns);
     _w = new Vector(columns);
     for (var j = 0; j < columns; j++)
     {
         i = j + 1;
         vector[j] = num*num2;
         var num5 = num2 = (num = 0.0);
         if (j < rows)
         {
             for (var k = j; k < rows; k++)
             {
                 num += Math.Abs(_u[k, j]);
             }
             if (Math.Abs(num - 0.0) > 0.0001)
             {
                 for (var l = j; l < rows; l++)
                 {
                     Matrix u;
                     int row;
                     int column;
                     (u = _u)[row = l, column = j] = u[row, column]/num;
                     num5 += _u[l, j]*_u[l, j];
                 }
                 var num6 = _u[j, j];
                 num2 = ((num6 < 0.0) ? Math.Sqrt(num5) : (-Math.Sqrt(num5)));
                 var num7 = num6*num2 - num5;
                 _u[j, j] = num6 - num2;
                 if (j != columns - 1)
                 {
                     for (var m = i; m < columns; m++)
                     {
                         num5 = 0.0;
                         for (var n = j; n < rows; n++)
                         {
                             num5 += _u[n, j]*_u[n, m];
                         }
                         num6 = num5/num7;
                         for (var num8 = j; num8 < rows; num8++)
                         {
                             Matrix u2;
                             int num9;
                             int column2;
                             (u2 = _u)[num9 = num8, column2 = m] = u2[num9, column2] + num6*_u[num8, j];
                         }
                     }
                 }
                 for (var num10 = j; num10 < rows; num10++)
                 {
                     Matrix u;
                     int row;
                     int column;
                     (u = _u)[row = num10, column = j] = u[row, column]*num;
                 }
             }
         }
         _w[j] = num*num2;
         num5 = (num2 = (num = 0.0));
         if (j < rows && j != columns - 1)
         {
             for (var num11 = i; num11 < columns; num11++)
             {
                 num += Math.Abs(_u[j, num11]);
             }
             if (Math.Abs(num - 0.0) > 0.0001)
             {
                 for (var num12 = i; num12 < columns; num12++)
                 {
                     Matrix u;
                     int row;
                     int column;
                     (u = _u)[row = j, column = num12] = u[row, column]/num;
                     num5 += _u[j, num12]*_u[j, num12];
                 }
                 var num6 = _u[j, i];
                 num2 = ((num6 < 0.0) ? Math.Sqrt(num5) : (-Math.Sqrt(num5)));
                 var num7 = num6*num2 - num5;
                 _u[j, i] = num6 - num2;
                 for (var num13 = i; num13 < columns; num13++)
                 {
                     vector[num13] = _u[j, num13]/num7;
                 }
                 if (j != rows - 1)
                 {
                     for (var num14 = i; num14 < rows; num14++)
                     {
                         num5 = 0.0;
                         for (var num15 = i; num15 < columns; num15++)
                         {
                             num5 += _u[num14, num15]*_u[j, num15];
                         }
                         for (var num16 = i; num16 < columns; num16++)
                         {
                             Matrix u3;
                             int row2;
                             int column3;
                             (u3 = _u)[row2 = num14, column3 = num16] = u3[row2, column3] + num5*vector[num16];
                         }
                     }
                 }
                 for (var num17 = i; num17 < columns; num17++)
                 {
                     Matrix u;
                     int row;
                     int column;
                     (u = _u)[row = j, column = num17] = u[row, column]*num;
                 }
             }
         }
         num3 = Math.Max(num3, Math.Abs(_w[j]) + Math.Abs(vector[j]));
     }
     for (var num18 = columns - 1; num18 >= 0; num18--)
     {
         if (num18 < columns - 1)
         {
             if (Math.Abs(num2 - 0.0) > 0.0001)
             {
                 for (var num19 = i; num19 < columns; num19++)
                 {
                     _v[num19, num18] = _u[num18, num19]/_u[num18, i]/num2;
                 }
                 for (var num20 = i; num20 < columns; num20++)
                 {
                     var num5 = 0.0;
                     for (var num21 = i; num21 < columns; num21++)
                     {
                         num5 += _u[num18, num21]*_v[num21, num20];
                     }
                     for (var num22 = i; num22 < columns; num22++)
                     {
                         int num9;
                         Matrix v;
                         int row3;
                         (v = _v)[row3 = num22, num9 = num20] = v[row3, num9] + num5*_v[num22, num18];
                     }
                 }
             }
             for (var num23 = i; num23 < columns; num23++)
             {
                 _v[num18, num23] = (_v[num23, num18] = 0.0);
             }
         }
         _v[num18, num18] = 1.0;
         num2 = vector[num18];
         i = num18;
     }
     for (var num24 = columns - 1; num24 >= 0; num24--)
     {
         i = num24 + 1;
         num2 = _w[num24];
         if (num24 < columns - 1)
         {
             for (var num25 = i; num25 < columns; num25++)
             {
                 _u[num24, num25] = 0.0;
             }
         }
         Matrix u2;
         int num9;
         int column2;
         if (Math.Abs(num2 - 0.0) > 0.0001)
         {
             num2 = 1.0/num2;
             if (num24 != columns - 1)
             {
                 for (var num26 = i; num26 < columns; num26++)
                 {
                     var num5 = 0.0;
                     for (var num27 = i; num27 < rows; num27++)
                     {
                         num5 += _u[num27, num24]*_u[num27, num26];
                     }
                     var num6 = num5/_u[num24, num24]*num2;
                     for (var num28 = num24; num28 < rows; num28++)
                     {
                         (u2 = _u)[num9 = num28, column2 = num26] = u2[num9, column2] + num6*_u[num28, num24];
                     }
                 }
             }
             for (var num29 = num24; num29 < rows; num29++)
             {
                 Matrix u;
                 int row;
                 int column;
                 (u = _u)[row = num29, column = num24] = u[row, column]*num2;
             }
         }
         else
         {
             for (var num30 = num24; num30 < rows; num30++)
             {
                 _u[num30, num24] = 0.0;
             }
         }
         (u2 = _u)[num9 = num24, column2 = num24] = u2[num9, column2] + 1.0;
     }
     for (var num31 = columns - 1; num31 >= 0; num31--)
     {
         for (var num32 = 0; num32 < 30; num32++)
         {
             var num33 = 1;
             for (i = num31; i >= 0; i--)
             {
                 num4 = i - 1;
                 if (Math.Abs(Math.Abs(vector[i]) + num3 - num3) < 0.0001)
                 {
                     num33 = 0;
                     break;
                 }
                 if (Math.Abs(Math.Abs(_w[num4]) + num3 - num3) < 0.0001)
                 {
                     break;
                 }
             }
             double num5;
             double num6;
             double num7;
             double num35;
             double num37;
             double num38;
             if (num33 != 0)
             {
                 num5 = 1.0;
                 for (var num34 = i; num34 <= num31; num34++)
                 {
                     num6 = num5*vector[num34];
                     if (Math.Abs(Math.Abs(num6) + num3 - num3) > 0.0001)
                     {
                         num2 = _w[num34];
                         num7 = Pythag(num6, num2);
                         _w[num34] = (float) num7;
                         num7 = 1.0/num7;
                         num35 = num2*num7;
                         num5 = -num6*num7;
                         for (var num36 = 0; num36 < rows; num36++)
                         {
                             num37 = _u[num36, num4];
                             num38 = _u[num36, num34];
                             _u[num36, num4] = num37*num35 + num38*num5;
                             _u[num36, num34] = num38*num35 - num37*num5;
                         }
                     }
                 }
             }
             num38 = _w[num31];
             if (i == num31)
             {
                 if (num38 < 0.0)
                 {
                     _w[num31] = -num38;
                     for (var num39 = 0; num39 < columns; num39++)
                     {
                         _v[num39, num31] = -_v[num39, num31];
                     }
                 }
                 break;
             }
             if (num32 >= 30)
             {
                 throw new MatrixException("No convergence after 30 iterations");
             }
             var num40 = _w[i];
             num4 = num31 - 1;
             num37 = _w[num4];
             num2 = vector[num4];
             num7 = vector[num31];
             num6 = ((num37 - num38)*(num37 + num38) + (num2 - num7)*(num2 + num7))/(2.0*num7*num37);
             num2 = Pythag(num6, 1.0);
             var num41 = (num6 >= 0.0) ? num2 : (-num2);
             num6 = ((num40 - num38)*(num40 + num38) + num7*(num37/(num6 + num41) - num7))/num40;
             num5 = (num35 = 1.0);
             for (var num42 = i; num42 <= num4; num42++)
             {
                 var num43 = num42 + 1;
                 num2 = vector[num43];
                 num37 = _w[num43];
                 num7 = num5*num2;
                 num2 = num35*num2;
                 num38 = Pythag(num6, num7);
                 vector[num42] = num38;
                 num35 = num6/num38;
                 num5 = num7/num38;
                 num6 = num40*num35 + num2*num5;
                 num2 = num2*num35 - num40*num5;
                 num7 = num37*num5;
                 num37 *= num35;
                 for (var num44 = 0; num44 < columns; num44++)
                 {
                     num40 = _v[num44, num42];
                     num38 = _v[num44, num43];
                     _v[num44, num42] = num40*num35 + num38*num5;
                     _v[num44, num43] = num38*num35 - num40*num5;
                 }
                 num38 = Pythag(num6, num7);
                 _w[num42] = num38;
                 if (Math.Abs(num38 - 0.0) > 0.0001)
                 {
                     num38 = 1.0/num38;
                     num35 = num6*num38;
                     num5 = num7*num38;
                 }
                 num6 = num35*num2 + num5*num37;
                 num40 = num35*num37 - num5*num2;
                 for (var num45 = 0; num45 < rows; num45++)
                 {
                     num37 = _u[num45, num42];
                     num38 = _u[num45, num43];
                     _u[num45, num42] = num37*num35 + num38*num5;
                     _u[num45, num43] = num38*num35 - num37*num5;
                 }
             }
             vector[i] = 0.0;
             vector[num31] = num6;
             _w[num31] = num40;
         }
     }
 }