public static IMathPoint MoveAlongVector(this IMathPoint pt, IMathVector dir, double dist)
{
dir = dir.Normalise().Scale(dist) as IMathVector;
var centerPt = pt.AddVector(dir) as IMathPoint;
return(centerPt);
}
/// <summary>
/// http://geomalgorithms.com/a07-_distance.html
/// </summary>
/// <param name="pa"></param>
/// <param name="va"></param>
/// <param name="pb"></param>
/// <param name="vb"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
private static bool ClosestPointBetweenLines(IMathPoint pOrigin, IMathVector pVector, IMathPoint qOrigin, IMathVector qVector, out double pT, out double qT)
{
var w0 = (IMathVector)(pOrigin.Subtract(qOrigin));
var u = pVector.Normalise();
var v = qVector.Normalise();
var a = u.Dot(u);
var b = u.Dot(v);
var c = v.Dot(v);
var d = u.Dot(w0);
var e = v.Dot(w0);
var den = (a * c - b * b);
if (System.Math.Abs(den) < 1e-12)
{
pT = 0;
qT = 0;
return(false);
}
pT = (b * e - c * d) / den;
qT = (a * a - b * d) / den;
return(true);
}
/// <summary>
/// http://geomalgorithms.com/a07-_distance.html
/// </summary>
/// <param name="pa"></param>
/// <param name="va"></param>
/// <param name="pb"></param>
/// <param name="vb"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
private static bool ClosestPointBetweenLines(IMathPoint pOrigin, IMathVector pVector, IMathPoint qOrigin, IMathVector qVector, out double pT, out double qT)
{
var w0 = (IMathVector) (pOrigin.Subtract(qOrigin));
var u = pVector.Normalise();
var v = qVector.Normalise();
var a = u.Dot(u);
var b = u.Dot(v);
var c = v.Dot(v);
var d = u.Dot(w0);
var e = v.Dot(w0);
var den = (a*c - b*b);
if (Math.Abs(den) < 1e-12)
{
pT = 0;
qT = 0;
return false;
}
pT = (b*e - c*d)/den;
qT = (a*a - b*d)/den;
return true;
}