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;
}
public Circle3D FitCircleToPoints(Collection<Point3D> points)
{
if (points == null)
{
throw new MatrixNullReference();
}
if (points.Count < 3)
{
throw new ArgumentException("Need at least 3 points to fit circle");
}
_solver = new NRSolver(points.Count*3 + 1, 7);
_measuredPoints = points;
var leastSquaresFit3D = new LeastSquaresFit3D();
var plane3D = leastSquaresFit3D.FitPlaneToPoints(points);
var initialGuess = new Circle3D
{
Origin = leastSquaresFit3D.Centroid(points),
Normal = plane3D.Normal,
Radius = leastSquaresFit3D.AverageError
};
var vector = _solver.Solve(Circle3DErrorFunction, VectorFromCircle3D(initialGuess));
var circle3D = new Circle3D
{
Origin = new Point3D(vector[0], vector[1], vector[2]),
Normal = new Vector3D(vector[3], vector[4], vector[5]),
Radius = vector[6]
};
CalculateErrors(points, circle3D);
return circle3D;
}
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 Point3D Centroid(Collection<Point3D> points)
{
var leastSquaresFit3D = new LeastSquaresFit3D();
return leastSquaresFit3D.Centroid(points);
}