/// <summary>
/// Estimates normals from k-closest neighbours.
/// </summary>
public static V3f[] EstimateNormals(this V3f[] points, int k, PointRkdTreeF <V3f[], V3f> kdtree)
{
if (points == null)
{
throw new ArgumentNullException(nameof(points));
}
if (k < 3)
{
throw new ArgumentOutOfRangeException($"Expected k >= 3, but k is {k}.");
}
if (kdtree == null)
{
throw new ArgumentNullException(nameof(kdtree));
}
if (points.Length == 0)
{
return(Array.Empty <V3f>());
}
return(points.Map((p, i) =>
{
if (k > points.Length)
{
k = points.Length;
}
// find k closest points
var closest = kdtree.GetClosest(p, float.MaxValue, k);
if (closest.Count == 0)
{
return V3f.Zero;
}
// compute centroid of k closest points
var c = points[closest[0].Index];
for (var j = 1; j < k; j++)
{
c += points[closest[j].Index];
}
c /= k;
// compute covariance matrix of k closest points relative to centroid
var cvm = M33f.Zero;
for (var j = 0; j < k; j++)
{
cvm.AddOuterProduct(points[closest[j].Index] - c);
}
cvm /= k;
// solve eigensystem -> eigenvector for smallest eigenvalue gives normal
Eigensystems.Dsyevh3((M33d)cvm, out M33d q, out V3d w);
return (V3f)((w.X < w.Y) ? ((w.X < w.Z) ? q.C0 : q.C2) : ((w.Y < w.Z) ? q.C1 : q.C2));
}));
}
/// <summary>
/// Estimates normals from k-closest neighbours.
/// </summary>
public static async Task <V3f[]> EstimateNormalsAsync(this V3f[] points, int k, PointRkdTreeF <V3f[], V3f> kdtree)
{
if (points == null)
{
throw new ArgumentNullException(nameof(points));
}
if (k < 3)
{
throw new ArgumentOutOfRangeException($"Expected k >= 3, but k is {k}.");
}
if (kdtree == null)
{
throw new ArgumentNullException(nameof(kdtree));
}
if (points.Length == 0)
{
return(Array.Empty <V3f>());
}
return(await Task.Run(() => EstimateNormals(points, k, kdtree)));
}