public static Point3D PointOntoLine(Line3D line, Point3D point)
{
var vector3D = line.Origin - point;
Point3D result;
if (Math.Abs(vector3D.Length() - 0.0) < 0.001)
{
result = new Point3D(point);
}
else
{
var scalar = Vector.Dot(vector3D, line.Direction);
result = line.Origin + (Vector3D) ((Point3D) (line.Direction*scalar));
}
return result;
}
public Line3D FitLineToPoints(Collection<Point3D> points)
{
if (points == null)
{
throw new MatrixNullReference();
}
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[]
{
num2 + num3,
-num4,
-num6,
-num4,
num3 + num,
-num5,
-num6,
-num5,
num + num2
});
var sVD = new SVD(mat);
var direction = new Vector3D(sVD.U.GetColumn(sVD.SmallestSingularIndex));
var line3D = new Line3D(point3D, direction);
CalculateErrors(points, line3D);
return line3D;
}
private static double PointToLine(Line3D line, Point3D point)
{
return Vector3D.Cross(line.Direction, line.Origin - point).Length();
}
private static double LineToLine(Line3D line1, Line3D line2, out Point3D closestPoint1,
out Point3D closestPoint2)
{
var origin = line1.Origin;
var origin2 = line2.Origin;
var direction = line1.Direction;
var direction2 = line2.Direction;
var vector3D = origin - origin2;
var num = Vector.Dot(-direction, direction2);
var num2 = Vector.Dot(vector3D, direction);
var num3 = vector3D.Length()*vector3D.Length();
var num4 = Math.Abs(1.0 - num*num);
double num7;
double num8;
double d;
if (num4 > 1E-05)
{
var num5 = Vector.Dot(-vector3D, direction2);
var num6 = 1.0/num4;
num7 = (num*num5 - num2)*num6;
num8 = (num*num2 - num5)*num6;
d = num7*(num7 + num*num8 + 2.0*num2) + num8*(num*num7 + num8 + 2.0*num5) + num3;
}
else
{
num7 = -num2;
num8 = 0.0;
d = num2*num7 + num3;
}
closestPoint1 = origin + new Vector3D(num7*direction);
closestPoint2 = origin2 + new Vector3D(num8*direction2);
return Math.Sqrt(d);
}
private static double LineToLine(Line3D line1, Line3D line2)
{
Point3D point3D;
Point3D point3D2;
return LineToLine(line1, line2, out point3D, out point3D2);
}
private static double LineToCircle(Circle3D circle, Line3D point)
{
throw new NotImplementedException();
}