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];
}
}
}
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;
}
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;
}
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;
}
public SquareMatrix(SquareMatrix mat)
: base(mat)
{
}
public SquareMatrix Power(int power)
{
Matrix matrix = new SquareMatrix(this);
for (var i = 1; i < power; i++)
{
matrix = this*matrix;
}
return new SquareMatrix(matrix);
}
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;
}
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];
}
}
}
}
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();
}
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;
}
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;
}
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;
}
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;
}
}
}
