public static double Dot(Vector vec1, Vector vec2)
{
if (vec1.Size != vec2.Size)
{
throw new MatrixException("Vectors are not of equal size");
}
Matrix matrix = vec1.Transpose()*vec2;
return matrix[0, 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;
}
}
}
public static Vector Divide(Vector vec, double scalar)
{
return new Vector(vec/scalar);
}
public Vector GetRow(int row)
{
var vector = new Vector(Columns);
for (var i = 0; i < Columns; i++)
{
vector[i] = this[row, i];
}
return vector;
}
public void SetRow(int row, Vector vec)
{
for (var i = 0; i < Columns; i++)
{
this[row, i] = vec[i];
}
}
public static Vector Negate(Vector v)
{
return new Vector(-v);
}
public static Vector3D operator *(TransformationMatrix3D mat, Vector3D vec)
{
var vec2 = new Vector(4, new[]
{
vec[0],
vec[1],
vec[2],
1.0
});
Matrix matrix = mat*vec2;
return new Vector3D(matrix[0, 0], matrix[1, 0], matrix[2, 0]);
}
public Sphere3D FitSphereToPoints(Collection<Point3D> points)
{
if (points == null)
{
throw new NullReferenceException();
}
if (points.Count < 4)
{
throw new MatrixException("Need at least 4 points to fit sphere");
}
var sphere3D = new Sphere3D();
var leastSquaresFit3D = new LeastSquaresFit3D();
sphere3D.Origin = leastSquaresFit3D.Centroid(points);
sphere3D.Radius = leastSquaresFit3D.RmsError;
var matrix = new Matrix(points.Count, 4);
var vector = new Vector(points.Count);
for (var i = 0; i < 50; i++)
{
var num = 0;
foreach (var current in points)
{
var vector3D = Project3D.PointOntoSphere(sphere3D, current) - sphere3D.Origin;
vector3D.Normalise();
matrix[num, 0] = vector3D.X;
matrix[num, 1] = vector3D.Y;
matrix[num, 2] = vector3D.Z;
matrix[num, 3] = -1.0;
var value = Distance3D.PointToSphere(sphere3D, current);
vector[num] = value;
num++;
}
var vector2 = matrix.PseudoInverse()*vector;
if (vector2.Length() < 1E-06)
{
break;
}
var origin = sphere3D.Origin;
origin.X += vector2[0];
var origin2 = sphere3D.Origin;
origin2.Y += vector2[1];
var origin3 = sphere3D.Origin;
origin3.Z += vector2[2];
sphere3D.Radius -= vector2[3];
}
CalculateErrors(points, sphere3D);
return sphere3D;
}
private static Vector VectorFromCircle3D(Circle3D initialGuess)
{
var vector = new Vector(7);
vector[0] = initialGuess.Origin.X;
vector[1] = initialGuess.Origin.Y;
vector[2] = initialGuess.Origin.Z;
vector[3] = initialGuess.Normal.X;
vector[4] = initialGuess.Normal.Y;
vector[5] = initialGuess.Normal.Z;
vector[6] = initialGuess.Radius;
return vector;
}
public static Quaternion FromAxisAngle(Vector axis, double angle)
{
angle *= 0.017453292519943295;
axis.Normalise();
var num = Math.Sin(angle/2.0);
var w = Math.Cos(angle/2.0);
var quaternion = new Quaternion(axis[0]*num, axis[1]*num, axis[2]*num, w);
quaternion.Normalise();
return quaternion;
}
public Circle3D FitCircleToPoints2(Collection<Point3D> points)
{
if (points == null)
{
throw new MatrixNullReference();
}
if (points.Count < 3)
{
throw new ArgumentException("Need at least 3 points to fit circle");
}
var circle3D = new Circle3D();
var leastSquaresFit3D = new LeastSquaresFit3D();
var matrix = new Matrix(points.Count, 7);
var vector = new Vector(points.Count);
for (var i = 0; i < 50; i++)
{
circle3D.Origin = leastSquaresFit3D.Centroid(points);
circle3D.Radius = leastSquaresFit3D.RmsError;
var plane3D = leastSquaresFit3D.FitPlaneToPoints(points);
circle3D.Normal = plane3D.Normal;
var num = 0;
foreach (var current in points)
{
var p = Project3D.PointOntoPlane(plane3D, current);
var vector3D = circle3D.Origin - p;
vector3D.Normalise();
matrix[num, 0] = vector3D[0];
matrix[num, 1] = vector3D[1];
matrix[num, 2] = vector3D[2];
matrix[num, 3] = plane3D.Normal.X;
matrix[num, 4] = plane3D.Normal.Y;
matrix[num, 5] = plane3D.Normal.Z;
matrix[num, 6] = -1.0;
var value = Distance3D.PointToCircle(circle3D, current);
vector[num] = value;
num++;
}
var vector2 = matrix.PseudoInverse()*vector;
if (vector2.Length() < 1E-06)
{
break;
}
var origin = circle3D.Origin;
origin.X += vector2[0];
var origin2 = circle3D.Origin;
origin2.Y += vector2[1];
var origin3 = circle3D.Origin;
origin3.Z += vector2[2];
var normal = circle3D.Normal;
normal.X += vector2[3];
var normal2 = circle3D.Normal;
normal2.Y += vector2[4];
var normal3 = circle3D.Normal;
normal3.Z += vector2[5];
circle3D.Radius -= vector2[6];
}
CalculateErrors(points, circle3D);
return circle3D;
}
public static Vector3D Cross(Vector vec1, Vector vec2)
{
var vector3D = new Vector3D();
vector3D[0] = vec1[1]*vec2[2] - vec1[2]*vec2[1];
vector3D[1] = vec1[2]*vec2[0] - vec1[0]*vec2[2];
vector3D[2] = vec1[0]*vec2[1] - vec1[1]*vec2[0];
return vector3D;
}
public static double Angle(Vector vec1, Vector vec2)
{
var num = Dot(vec1, vec2);
var num2 = vec1.Length()*vec2.Length();
return Math.Acos(num/num2)*180.0/3.1415926535897931;
}
public Vector3D(Vector vec)
: base(3)
{
for (var i = 0; i < 3; i++)
{
base[i] = vec[i];
}
}
public static bool Equals(Vector v1, Vector v2)
{
return v1 == v2;
}
private Vector Circle3DErrorFunction(Vector vec)
{
var vector = new Vector(_solver.NumEquations);
var index = 0;
var point3D = new Point3D(vec[0], vec[1], vec[2]);
var plane = new Plane3D(point3D, new Vector3D(vec[3], vec[4], vec[5]));
foreach (var current in _measuredPoints)
{
var p = Project3D.PointOntoPlane(plane, current);
var vector3D = point3D - p;
vector3D.Normalise();
var point3D2 = new Point3D();
vector[index++] = current.X - point3D2.X;
vector[index++] = current.Y - point3D2.Y;
vector[index++] = current.Z - point3D2.Z;
}
vector[index] = new Vector3D(vec[3], vec[4], vec[5]).Length() - 1.0;
return vector;
}
public static Vector Multiply(Vector vec, double scalar)
{
return new Vector(vec*scalar);
}
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 static Vector Subtract(Vector v1, Vector v2)
{
return new Vector(v1 - v2);
}
private static bool IsDone(Vector delta)
{
bool result;
for (var i = 0; i < delta.Rows; i++)
{
if (Math.Abs(delta[i]) > 1E-07)
{
result = false;
return result;
}
}
result = true;
return result;
}
public Vector GetColumn(int column)
{
var vector = new Vector(Rows);
for (var i = 0; i < Rows; i++)
{
vector[i] = this[i, column];
}
return vector;
}
private Matrix CalculateJacobian(ErrorFunction errorFunction, Vector guess)
{
var matrix = new Matrix(NumEquations, NumVariables);
for (var i = 0; i < matrix.Columns; i++)
{
var num = (Math.Abs(guess[i]) >= 1.0) ? (Math.Abs(guess[i])*1E-07) : 1E-07;
var vector = new Vector(guess);
Vector vector2;
int index;
(vector2 = vector)[index = i] = vector2[index] + num;
var v = errorFunction(vector);
Vector vector3;
int index2;
(vector3 = vector)[index2 = i] = vector3[index2] - 2.0*num;
var v2 = errorFunction(vector);
var vec = v - v2;
matrix.SetColumn(i, vec/(2.0*num));
}
return matrix;
}
public void SetColumn(int column, Vector vec)
{
for (var i = 0; i < Rows; i++)
{
this[i, column] = vec[i];
}
}
public static Vector Add(Vector vec, double scalar)
{
return new Vector(vec + scalar);
}
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 Vector2D(Vector vec)
: base(2)
{
base[0] = vec[0];
base[1] = vec[1];
}
