/// <summary> Returns a linear combination of the vectors into result.</summary>
public static void LinComb(
double fA, ref Vector3d vA, double fB, ref Vector3d vB, double fC, ref Vector3d vC, out Vector3d result)
{
result.X = fA * vA.X + fB * vB.X + fC * vC.X;
result.Y = fA * vA.Y + fB * vB.Y + fC * vC.Y;
result.Z = fA * vA.Z + fB * vB.Z + fC * vC.Z;
}
/// <summary>
/// Squared distance from point to (filled) triangle
/// Implementation based on the paper, Distance Between Point and Triangle in 3D.
/// David Eberly, Geometric Tools, LLC
/// </summary>
/// <param name="pt">Point to measure distance from</param>
/// <param name="fs">ptClosest = (1-fs-ft)*ptTri0 + fs*ptTri1 + ft*ptTri2</param>
/// <param name="ft">ptClosest = (1-fs-ft)*ptTri0 + fs*ptTri1 + ft*ptTri2</param>
/// <returns>Squared distance</returns>
public double SqrDist(ref Vector3d pt, out double fs, out double ft)
{
var vD = new Vector3d
{
X = Tri.Pt0.X - pt.X,
Y = Tri.Pt0.Y - pt.Y,
Z = Tri.Pt0.Z - pt.Z
};
double fd = v01.X * vD.X + v01.Y * vD.Y + v01.Z * vD.Z; // Dot(vE0, vD);
double fe = v02.X * vD.X + v02.Y * vD.Y + v02.Z * vD.Z; // Dot(vE1, vD);
double ff = vD.X * vD.X + vD.Y * vD.Y + vD.Z * vD.Z; // Dot(vD, vD);
fs = fb * fe - fc * fd;
ft = fb * fd - fa * fe;
double result;
if (fs + ft <= fDet)
{
if (fs < 0)
{
if (ft < 0)
{
// Region 4
if (fe < 0) // minimum on edge s=0
{
fs = 0;
if (fe >= 0)
{
ft = 0;
result = ff;
}
else if (-fe >= fc)
{
ft = 1;
result = fc + 2 * fe + ff;
}
else
{
ft = -fe / fc;
// Calculate the square distance
result = (fc * ft + 2 * fe) * ft + ff;
}
}
else // minimum on edge t=0
{
ft = 0;
if (fd >= 0)
{
fs = 0;
result = ff;
}
else if (-fd >= fa)
{
fs = 1;
result = fa + 2 * fd + ff;
}
else
{
fs = -fd / fa;
result = (fa * fs + 2 * fd) * fs + ff;
}
}
}
else
{
// Region 3
fs = 0; // minimum on edge s=0
if (fe >= 0)
{
ft = 0;
result = ff;
}
else if (-fe >= fc)
{
ft = 1;
result = fc + 2 * fe + ff;
}
else
{
ft = -fe / fc;
// Calculate the square distance
result = (fc * ft + 2 * fe) * ft + ff;
};
}
}
else if (ft < 0)
{
// Region 5
ft = 0;
if (fd >= 0)
{
fs = 0;
// Calculate the square distance
result = ff;
}
else if (-fd >= fa)
{
fs = 1;
// Calculate the square distance
result = fa + 2 * fd + ff;
}
else
{
fs = -fd / fa;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + ff;
};
}
else
{
// Region 0, Inside the triangle
// First check that the triangle is not degenerated
if (fDet != 0)
{
double finvDet = 1 / fDet;
fs = fs * finvDet;
ft = ft * finvDet;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
}
else
{
// Degenerated, calculate distance to line
if ((fa == 0) && (fc == 0)) // Triangle is a point
{
fs = 0;
ft = 0;
result = ff;
}
else if ((fa == 0)) // Edge s = 0
{
fs = 0;
if (fe >= 0)
{
ft = 0;
result = ff;
}
else if (-fe >= fc)
{
ft = 1;
result = fc + 2 * fe + ff;
}
else
{
ft = -fe / fc;
// Calculate the square distance
result = (fc * ft + 2 * fe) * ft + ff;
}
}
else if ((fc == 0)) // Edge t = 0
{
ft = 0;
if (fd >= 0)
{
fs = 0;
result = ff;
}
else if (-fd >= fa)
{
fs = 1;
result = fa + 2 * fd + ff;
}
else
{
fs = -fd / fa;
result = (fa * fs + 2 * fd) * fs + ff;
}
}
else // All edges are in one line
{
double fTmp0 = fa + fd;
double fTmp1 = fb + fe;
if (fTmp1 < fTmp0) // minimum on edge s+t=1
{
double fnum = fTmp0 - fTmp1;
double fden = fa - 2 * fb + fc;
if (fnum >= fden)
{
ft = 1;
fs = 0;
// Calculate the square distance
result = fc + 2 * fe + ff;
}
else
{
ft = fnum / fden;
fs = 1 - ft;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
};
}
else
{ // minimum on edge t=0
if (fTmp0 <= 0)
{
fs = 1;
ft = 0;
// Calculate the square distance
result = fa + 2 * fd + ff;
}
else if (fd >= 0)
{
fs = 0;
ft = 0;
// Calculate the square distance
result = ff;
}
else
{
fs = -fd / fa;
ft = 0;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + ff;
}
}
}
}
}
}
else
{
if (fs < 0)
{
// Region 2
double fTmp0 = fb + fd;
double fTmp1 = fc + fe;
if (fTmp1 > fTmp0) // minimum on edge s+t=1
{
double fnum = fTmp1 - fTmp0;
double fden = fa - 2 * fb + fc;
if (fnum >= fden)
{
fs = 1;
ft = 0;
// Calculate the square distance
result = fa + 2 * fd + ff;
}
else
{
fs = fnum / fden;
ft = 1 - fs;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
};
}
else
{ // minimum on edge s=0
if (fTmp1 <= 0)
{
ft = 1;
fs = 0;
// Calculate the square distance
result = fc + 2 * fe + ff;
}
else if (fe >= 0)
{
ft = 0;
fs = 0;
// Calculate the square distance
result = ff;
}
else
{
ft = -fe / fc;
fs = 0;
// Calculate the square distance
result = (fc * ft + 2 * fe) * ft + ff;
};
};
}
else if (ft < 0)
{
// Region 6
double fTmp0 = fa + fd;
double fTmp1 = fb + fe;
if (fTmp1 < fTmp0) // minimum on edge s+t=1
{
double fnum = fTmp0 - fTmp1;
double fden = fa - 2 * fb + fc;
if (fnum >= fden)
{
ft = 1;
fs = 0;
// Calculate the square distance
result = fc + 2 * fe + ff;
}
else
{
ft = fnum / fden;
fs = 1 - ft;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
};
}
else
{ // minimum on edge t=0
if (fTmp0 <= 0)
{
fs = 1;
ft = 0;
// Calculate the square distance
result = fa + 2 * fd + ff;
}
else if (fd >= 0)
{
fs = 0;
ft = 0;
// Calculate the square distance
result = ff;
}
else
{
fs = -fd / fa;
ft = 0;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + ff;
};
};
}
else
{
// Region 1
double fnum = fc + fe - fb - fd;
if (fnum <= 0)
{
fs = 0;
ft = 1;
// Calculate the square distance
result = fc + 2 * fe + ff;
}
else
{
double fden = fa - 2 * fb + fc;
if (fnum >= fden)
{
fs = 1;
ft = 0;
// Calculate the square distance
result = fa + 2 * fd + ff;
}
else
{
fs = fnum / fden;
ft = 1 - fs;
// Calculate the square distance
result = (fa * fs + 2 * fd) * fs + (2 * (fb * fs + fe) + fc * ft) * ft + ff;
}
}
}
}
// Ensure that Result is non-negative (it may be < 0 due to numerical noise)
if (result < 0)
result = 0;
return result;
}
/// <summary>
/// Squared distance from point to (filled) triangle
/// </summary>
/// <param name="pt">Point to measure distance from</param>
/// <param name="ptTri0">Triangle corner</param>
/// <param name="ptTri1">Triangle corner</param>
/// <param name="ptTri2">Triangle corner</param>
/// <param name="ptClosest">Closest point on the triangle</param>
/// <returns>Squared distance</returns>
public double SqrDist(ref Vector3d pt, out Vector3d ptClosest)
{
double fs, ft;
var sqrDist = SqrDist(ref pt, out fs, out ft);
ptClosest.X = Tri.Pt0.X + fs * v01.X + ft * v02.X;
ptClosest.Y = Tri.Pt0.Y + fs * v01.Y + ft * v02.Y;
ptClosest.Z = Tri.Pt0.Z + fs * v01.Z + ft * v02.Z;
return sqrDist;
}
