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