public Sphere3D(Sphere3D sphere)
{
Origin = sphere.Origin;
Radius = sphere.Radius;
}
public static double PointToSphere(Sphere3D sphere, Point3D point)
{
return (point - sphere.Origin).Length() - sphere.Radius;
}
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 static Point3D PointOntoSphere(Sphere3D sphere, Point3D point)
{
var vector3D = point - sphere.Origin;
vector3D.Normalise();
return new Point3D();
}