/*
* There are some not obvious calculations.
* With them we can find the closest points of
* two straits. Function returns mean point of
* them.
* p11, p21 - some points of straits.
* p12, p22 - some other points of straits.
* a1, a2 - vectors of straits.
*/
public static Line3D GetPerpendicular(Line3D line1, Line3D line2)
{
Point3D p11 = line1.Begin;
Point3D p21 = line2.Begin;
Point3D a1 = line1.Direction.Normalize();
Point3D a2 = line2.Direction.Normalize();
if (Math.Abs(a1.X - a2.X) < Epsilon && Math.Abs(a1.Y - a2.Y) < Epsilon &&
Math.Abs(a1.Z - a2.Z) < Epsilon)
throw new Exception("straits are parallel");
var p12 = new Point3D(p11.X + a1.X, p11.Y + a1.Y, p11.Z + a1.Z);
var p22 = new Point3D(p21.X + a2.X, p21.Y + a2.Y, p21.Z + a2.Z);
double p1 = (p12.X - p11.X)*(p12.X - p11.X) + (p12.Y - p11.Y)*(p12.Y - p11.Y) +
(p12.Z - p11.Z)*(p12.Z - p11.Z);
double p2 = (p12.X - p11.X)*(p22.X - p21.X) + (p12.Y - p11.Y)*(p22.Y - p21.Y) +
(p12.Z - p11.Z)*(p22.Z - p21.Z);
double q1 =
-((p22.X - p21.X)*(p12.X - p11.X) + (p22.Y - p21.Y)*(p12.Y - p11.Y) + (p22.Z - p21.Z)*(p12.Z - p11.Z));
double q2 =
-((p22.X - p21.X)*(p22.X - p21.X) + (p22.Y - p21.Y)*(p22.Y - p21.Y) + (p22.Z - p21.Z)*(p22.Z - p21.Z));
double r1 = (p21.X - p11.X)*(p12.X - p11.X) + (p21.Y - p11.Y)*(p12.Y - p11.Y) +
(p21.Z - p11.Z)*(p12.Z - p11.Z);
double r2 = (p21.X - p11.X)*(p22.X - p21.X) + (p21.Y - p11.Y)*(p22.Y - p21.Y) +
(p21.Z - p11.Z)*(p22.Z - p21.Z);
double m = (q2*r1 - q1*r2)/(p1*q2 - p2*q1);
double n = (p1*r2 - p2*r1)/(p1*q2 - p2*q1);
double x1 = p11.X + m*(p12.X - p11.X);
double y1 = p11.Y + m*(p12.Y - p11.Y);
double z1 = p11.Z + m*(p12.Z - p11.Z);
double x2 = p21.X + n*(p22.X - p21.X);
double y2 = p21.Y + n*(p22.Y - p21.Y);
double z2 = p21.Z + n*(p22.Z - p21.Z);
return new Line3D(new Point3D(x1, y1, z1), new Point3D(x2, y2, z2));
}
public Line3D Apply(Line3D arg)
{
return(new Line3D(Apply(arg.Begin), Apply(arg.End)));
}
public static double Distance(Point3D point, Line3D line)
{
Point3D vector = point - line.Begin;
return vector.MultiplyVector(line.Direction).Norm()/line.Direction.Norm();
}
public static bool IsParallel(Line3D line1, Line3D line2)
{
return AreCollinear(line1.Direction, line2.Direction);
}
public static double Distance(Point3D point, Line3D line)
{
Point3D vector = point - line.Begin;
return(vector.MultiplyVector(line.Direction).Norm() / line.Direction.Norm());
}
public static bool IsParallel(Line3D line1, Line3D line2)
{
return(AreCollinear(line1.Direction, line2.Direction));
}
public Line3D Apply(Line3D arg)
{
return new Line3D(Apply(arg.Begin), Apply(arg.End));
}