/// <summary>
/// Compute the line that best fit a set of input points (least squared orthogonal distance)
/// </summary>
/// <param name="points">The input points</param>
/// <param name="lineOrigin">Origin of the best fit line</param>
/// <param name="lineDirection">Direction of the best fit line</param>
/// <param name="tolerance">The tolerance that is used the determined if the input points are coincidental, colinear, or non-colinear</param>
/// <returns>0 if the input points are coincidental; 1 if they are colinear; 2 otherwise</returns>
public static int ComputeBestFitLine(List <Triple> points, out Triple lineOrigin, out Triple lineDirection, float tolerance = 1E-10f)
{
Triple centroid = Triple.Zero;
for (int i = 0; i < points.Count; i++)
{
centroid += points[i];
}
centroid /= points.Count;
float[,] P = new float[points.Count, 3];
for (int i = 0; i < points.Count; i++)
{
P[i, 0] = points[i].X - centroid.X;
P[i, 1] = points[i].Y - centroid.Y;
P[i, 2] = points[i].Z - centroid.Z;
}
float c00 = 0f, c01 = 0f, c02 = 0f;
float c10 = 0f, c11 = 0f, c12 = 0f;
float c20 = 0f, c21 = 0f, c22 = 0f;
for (int k = 0; k < points.Count; k++)
{
c00 += P[k, 0] * P[k, 0];
c01 += P[k, 0] * P[k, 1];
c02 += P[k, 0] * P[k, 2];
c10 += P[k, 1] * P[k, 0];
c11 += P[k, 1] * P[k, 1];
c12 += P[k, 1] * P[k, 2];
c20 += P[k, 2] * P[k, 0];
c21 += P[k, 2] * P[k, 1];
c22 += P[k, 2] * P[k, 2];
}
FastSvd3x3.Compute(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
out float u00, out float u01, out float u02,
out float u10, out float u11, out float u12,
out float u20, out float u21, out float u22,
out float s00, out float s11, out float s22,
out float v00, out float v01, out float v02,
out float v10, out float v11, out float v12,
out float v20, out float v21, out float v22);
lineOrigin = centroid;
// Case 0: The input points are coincidental, so we just need to pick an arbitrary line direction
if (s00 < tolerance)
{
lineDirection = Triple.BasisX;
return(0);
}
// Case 1: The input points are not coincidental, therefore we pick the dominant eigen vector as the line direction
lineDirection = new Triple(u00, u10, u20);
return(s11 < tolerance
? 1 // The input points are colinear
: 2); // The input points are NOT colinear
}
/// <summary>
/// Compute the plane that best fit a set of input points (least squared orthogonal distance)
/// </summary>
/// <param name="points">The input points</param>
/// <param name="planeOrigin">Origin of the best fit plane</param>
/// <param name="planeNormal">Normal of the best fit plane</param>
/// <param name="tolerance">The tolerance that is used the determined if the input points are coincidental, colinear, coplanar, or non-coplanar</param>
/// <returns>0 if the input points are coincidental; 1 if they are colinear; 2 if they are coplanar; 3 otherwise</returns>
public static int ComputeBestFitPlane(List <Triple> points, out Triple planeOrigin, out Triple planeNormal, float tolerance = 1E-10f)
{
Triple centroid = Triple.Zero;
for (int i = 0; i < points.Count; i++)
{
centroid += points[i];
}
centroid /= points.Count;
float[,] P = new float[points.Count, 3];
for (int i = 0; i < points.Count; i++)
{
P[i, 0] = points[i].X - centroid.X;
P[i, 1] = points[i].Y - centroid.Y;
P[i, 2] = points[i].Z - centroid.Z;
}
float c00 = 0f, c01 = 0f, c02 = 0f;
float c10 = 0f, c11 = 0f, c12 = 0f;
float c20 = 0f, c21 = 0f, c22 = 0f;
for (int k = 0; k < points.Count; k++)
{
c00 += P[k, 0] * P[k, 0];
c01 += P[k, 0] * P[k, 1];
c02 += P[k, 0] * P[k, 2];
c10 += P[k, 1] * P[k, 0];
c11 += P[k, 1] * P[k, 1];
c12 += P[k, 1] * P[k, 2];
c20 += P[k, 2] * P[k, 0];
c21 += P[k, 2] * P[k, 1];
c22 += P[k, 2] * P[k, 2];
}
FastSvd3x3.Compute(
c00, c01, c02,
c10, c11, c12,
c20, c21, c22,
out float u00, out float u01, out float u02,
out float u10, out float u11, out float u12,
out float u20, out float u21, out float u22,
out float s00, out float s11, out float s22,
out float v00, out float v01, out float v02,
out float v10, out float v11, out float v12,
out float v20, out float v21, out float v22);
planeOrigin = centroid;
// Case 0: The input points are coincidental, so we just need to pick an arbitrary normal vector
if (s00 < tolerance)
{
planeNormal = Triple.BasisZ;
return(0);
}
// Case 1: The input points are colinear, so we just pick an arbitrary vector perpendicular to the dominant eigenvector
if (s11 < tolerance)
{
planeNormal = new Triple(u00, u10, u20).GeneratePerpendicular();
return(1);
}
// Case 2: The input points are neigher coincidental nor colinear, therefore the best fit plane is determined by the two dominant eigenvectors
planeNormal = new Triple(u00, u10, u20).Cross(new Triple(u01, u11, u21)).Normalise();
return(s22 < tolerance
? 2 // The input points are coplanar
: 3); // The input points are NOT coplanar
}
