private float PintToPlaneClosestPoint(geoPoint Q, geoPoint V0, ref geoPoint V1, geoPoint V2, ref geoPoint projection)
{
// Input: P = a 3D point
// PL = a plane with point V0 and normal n
// Output: ClosestPt = base point on PL of perpendicular from P
float sb = 0;
float sn = 0;
float sd = 0;
geoPoint a = null;
geoPoint b = null;
geoPoint n = null;
a = V0.subtract(V1);
b = V0.subtract(V2);
n = a.CrossProduct(b);
sn = -n.DotProduct(Q.subtract(V0));
sd = n.DotProduct(n);
sb = sn / sd;
projection = Q.Addition(n.Multiply(sb));
return(projection.distance(ref Q));
}
private float SegmentToSegmentClosestPoint(geoLine Line1, geoLine Line2, ref geoPoint ClosestPt1, ref geoPoint ClosestPt2)
{
geoPoint pt1 = null;
geoPoint pt2 = null;
geoPoint pt3 = null;
geoPoint pt4 = null;
pt1 = Line1.P1;
pt2 = Line1.P2;
pt3 = Line2.P1;
pt4 = Line2.P2;
// using arrays for saving differencess
geoPoint u = new geoPoint(pt2.X - pt1.X, pt2.Y - pt1.Y, pt2.Z - pt1.Z);
geoPoint v = new geoPoint(pt4.X - pt3.X, pt4.Y - pt3.Y, pt4.Z - pt3.Z);
geoPoint w = new geoPoint(pt1.X - pt3.X, pt1.Y - pt3.Y, pt1.Z - pt3.Z);
float a = u.X * u.X + u.Y * u.Y + u.Z * u.Z;
//U.U>=0
float b = u.X * v.X + u.Y * v.Y + u.Z * v.Z;
//U.V
float c = v.X * v.X + v.Y * v.Y + v.Z * v.Z;
//V.V>=0
float d = u.X * w.X + u.Y * w.Y + u.Z * w.Z;
//U.W
float e = v.X * w.X + v.Y * w.Y + v.Z * w.Z;
//V.W
float D_ = a * c - b * b;
// >=0
float sN = D_;
float sD = D_;
float tN = D_;
float tD = D_;
float sc = D_;
float tc = D_;
if (D_ < 1E-13)
{
//PARALLEL
sN = 0;
sD = 1;
tN = e;
tD = c;
}
else
{
//Inifinite
sN = (b * e) - (c * d);
tN = (a * e) - (b * d);
if (sN < 0)
{
sN = 0;
tN = e;
tD = c;
}
else if (sN > sD)
{
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0)
{
tN = 0;
if (-d < 0)
{
sN = 0;
}
else if (-d > a)
{
sN = sD;
}
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{
tN = tD;
if ((-d + b) < 0)
{
sN = 0;
}
else if ((-d + b) > a)
{
sN = sD;
}
else
{
sN = (-d + b);
sD = a;
}
}
sc = sN / sD;
tc = tN / tD;
geoPoint dp = new geoPoint(w.X + sc * u.X - v.X * tc, w.Y + sc * u.Y - v.Y * tc, w.Z + sc * u.Z - v.Z * tc);
//Dim dis As float = Math.Sqrt(dp.X * dp.X + dp.Y * dp.Y + dp.Z * dp.Z)
ClosestPt1 = new geoPoint(pt1.X + sc * u.X, pt1.Y + sc * u.Y, pt1.Z + sc * u.Z);
ClosestPt2 = new geoPoint(pt3.X + tc * v.X, pt3.Y + tc * v.Y, pt3.Z + tc * v.Z);
return(ClosestPt1.distance(ref ClosestPt2));
}