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();
}
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;
}
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);
}
public static double PointToSphere(Sphere3D sphere, Point3D point)
{
return (point - sphere.Origin).Length() - sphere.Radius;
}
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 Line3D(Point3D origin, Vector3D direction)
{
Origin = origin;
Direction = direction;
Direction.Normalise();
}
public Sphere3D()
{
Origin = new Point3D();
Radius = 0.0;
}
public Point3D Centroid(Collection<Point3D> points)
{
var count = points.Count;
var num = 0.0;
var num2 = 0.0;
var num3 = 0.0;
foreach (var current in points)
{
num += current.X;
num2 += current.Y;
num3 += current.Z;
}
var point3D = new Point3D(num/count, num2/count, num3/count);
Transform = new TransformationMatrix3D(new Vector3D(point3D.X, point3D.Y, point3D.Z), new RotationMatrix3D());
CalculateErrors(points, point3D);
return point3D;
}
public Point3D(Point3D point)
{
_position = point._position;
}
private bool Equals(Point3D other)
{
return Equals(_position, other._position);
}
public static Vector3D Subtract(Point3D p1, Point3D p2)
{
return new Vector3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
}
public static Collection<Point3D> Multiply(Collection<TransformationMatrix3D> transforms, Point3D point)
{
var collection = new Collection<Point3D>();
foreach (var current in transforms)
{
collection.Add(current*point);
}
return collection;
}
public static bool Equals(Point3D p1, Point3D p2)
{
return p1 == p2;
}
public static Point3D Add(Point3D point, Vector3D vec)
{
return new Point3D
{
X = point.X + vec.X,
Y = point.Y + vec.Y,
Z = point.Z + vec.Z
};
}
private static double PointToPlane(Plane3D plane, Point3D point)
{
return Vector.Dot(plane.Normal, point - plane.Point);
}
private static double PointToPoint(Point3D p1, Point3D p2)
{
return (p1 - p2).Length();
}
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)));
}
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 Point3D PointOntoSphere(Sphere3D sphere, Point3D point)
{
var vector3D = point - sphere.Origin;
vector3D.Normalise();
return new Point3D();
}
public Sphere3D(Point3D origin, double radius)
{
Origin = origin;
Radius = radius;
}
public Plane3D(Point3D point, Vector3D normal)
{
Point = point;
Normal = normal;
}
