public static double Distance(DirLineSeg <Vec2> u, DirLineSeg <Vec2> v)
{
double d1 = VecX.Length(Distance(v.p - u.p, u.d));
double d2 = VecX.Length(Distance(v.p + v.d - u.p, u.d));
double d3 = VecX.Length(Distance(u.p - v.p, v.d));
double d4 = VecX.Length(Distance(u.p + u.d - v.p, v.d));
double d = Math.Min(Math.Min(d1, d2), Math.Min(d3, d4));
return(d);
}
public static bool CommonPerpendicular <T>(DirLineSeg <T> u, DirLineSeg <T> v, out double s, out double t) where T : IVector, new()
{
double A = VecX.Dot(u.d, v.d);
double B = VecX.SqrLength(u.d);
double C = VecX.SqrLength(v.d);
double div = A * A - B * C;
if (div == 0.0)
{
s = t = 0;
return(false);
}
T dp = VecX.Sub(v.p, u.p);
double D = VecX.Dot(dp, u.d);
double E = VecX.Dot(dp, v.d);
s = (A * E - C * D) / div;
t = (B * E - A * D) / div;
return(true);
}
public static bool CommonPerpendicular(DirLineSeg <Vec3> u, DirLineSeg <Vec3> v, out double s, out double t)
{
double A = VecX.Dot(u.d, v.d);
double B = VecX.SqrLength(u.d);
double C = VecX.SqrLength(v.d);
double div = A * A - B * C;
if (div == 0.0)
{
s = t = 0;
return(false);
}
Vec3 dp = v.p - u.p;
double D = VecX.Dot(dp, u.d);
double E = VecX.Dot(dp, v.d);
s = (C * D - A * E) / div;
t = (A * D - B * E) / div;
return(true);
}
public static LineSeg <T> Distance <T>(DirLineSeg <T> u, DirLineSeg <T> v) where T : IVector, new()
{
LineSeg <T> r;
T dn1 = Distance(v.p.Sub(u.p), u.d);
T dn2 = Distance(v.p.Add(v.d).Sub(u.p), u.d);
T dn3 = Distance(u.p.Sub(v.p), v.d);
T dn4 = Distance(u.p.Add(u.d).Sub(v.p), v.d);
double d1 = VecX.SqrLength(dn1);
double d2 = VecX.SqrLength(dn2);
double d3 = VecX.SqrLength(dn3);
double d4 = VecX.SqrLength(dn4);
if (d1 < d2 && d1 < d3 && d1 < d4)
{
r.p1 = v.p; r.p0 = r.p1.Sub(dn1);
}
else if (d2 < d3 && d2 < d4)
{
r.p1 = v.p.Add(v.d); r.p0 = r.p1.Sub(dn2);
}
else if (d3 < d4)
{
r.p0 = u.p; r.p1 = r.p0.Sub(dn3);
}
else
{
r.p0 = u.p.Add(u.d); r.p1 = r.p0.Sub(dn4);
}
double s, t;
if (CommonPerpendicular(u, v, out s, out t) &&
s > 0.0 && s < 1.0 && t > 0.0 && t < 1.0)
{
r.p0 = u.d.Mul(s).Add(u.p);
r.p1 = v.d.Mul(t).Add(v.p);
}
return(r);
}
public static LineSeg <Vec3> Distance(DirLineSeg <Vec3> u, DirLineSeg <Vec3> v)
{
LineSeg <Vec3> r;
Vec3 dn1 = Distance(v.p - u.p, u.d);
Vec3 dn2 = Distance(v.p + v.d - u.p, u.d);
Vec3 dn3 = Distance(u.p - v.p, v.d);
Vec3 dn4 = Distance(u.p + u.d - v.p, v.d);
double d1 = VecX.SqrLength(dn1);
double d2 = VecX.SqrLength(dn2);
double d3 = VecX.SqrLength(dn3);
double d4 = VecX.SqrLength(dn4);
if (d1 < d2 && d1 < d3 && d1 < d4)
{
r.p1 = v.p; r.p0 = r.p1 - dn1;
}
else if (d2 < d3 && d2 < d4)
{
r.p1 = v.p + v.d; r.p0 = r.p1 - dn2;
}
else if (d3 < d4)
{
r.p0 = u.p; r.p1 = r.p0 - dn3;
}
else
{
r.p0 = u.p + u.d; r.p1 = r.p0 - dn4;
}
double s, t;
if (CommonPerpendicular(u, v, out s, out t) &&
s > 0.0 && s < 1.0 && t > 0.0 && t < 1.0)
{
r.p0 = u.p + s * u.d;
r.p1 = v.p + t * v.d;
}
return(r);
}