public static Point3D PointOntoCircle(Circle3D circle, Point3D point)
{
var plane = new Plane3D(circle.Origin, circle.Normal);
var point3D = PointOntoPlane(plane, point);
Distance3D.Between(point, point3D);
var vector3D = circle.Origin - point3D;
vector3D.Normalise();
return new Point3D();
}
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 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;
}
private static double PointToPlane(Plane3D plane, Point3D point)
{
return Vector.Dot(plane.Normal, point - plane.Point);
}
public static double PointToCircle(Circle3D circle, Point3D point)
{
var plane = new Plane3D(circle.Origin, circle.Normal);
var point3D = Project3D.PointOntoPlane(plane, point);
Between(point, point3D);
var vector3D = circle.Origin - point3D;
vector3D.Normalise();
var p = new Point3D();
return PointToPoint(point, p);
}
public static Point3D PointOntoPlane(Plane3D plane, Point3D point)
{
var vec = plane.Point - point;
var vector3D = Vector3D.Cross(Vector3D.Cross(vec, plane.Normal), plane.Normal).Normalised();
return plane.Point + (Vector3D) ((Point3D) (vector3D*Vector.Dot(vec, vector3D)));
}
