/// <summary>
/// Estimates a normal vector for each point by least-squares-fitting a plane through its k nearest neighbours.
/// </summary>
public static V3f[] EstimateNormals(this V3f[] points, int k)
{
var kd = new PointRkdTreeD <V3f[], V3f>(
3, points.Length, points,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, 0
);
return(EstimateNormals(points, kd, k));
}
public PointSetNode ToPointSetCell(Storage storage, CancellationToken ct, double kdTreeEps = 1e-6)
{
var center = new V3d(_centerX, _centerY, _centerZ);
V3f[] ps = null;
C4b[] cs = null;
V3f[] ns = null;
int[] js = null;
PointRkdTreeD <V3f[], V3f> kdTree = null;
if (_ia != null)
{
var allPs = _octree.m_ps;
var count = _ia.Count;
ps = new V3f[count];
for (var i = 0; i < count; i++)
{
ps[i] = (V3f)(allPs[_ia[i]] - center);
}
if (_octree.m_cs != null)
{
var allCs = _octree.m_cs;
cs = new C4b[count];
for (var i = 0; i < count; i++)
{
cs[i] = allCs[_ia[i]];
}
}
if (_octree.m_ns != null)
{
var allNs = _octree.m_ns;
ns = new V3f[count];
for (var i = 0; i < count; i++)
{
ns[i] = allNs[_ia[i]];
}
}
if (_octree.m_is != null)
{
var allIs = _octree.m_is;
js = new int[count];
for (var i = 0; i < count; i++)
{
js[i] = allIs[_ia[i]];
}
}
kdTree = new PointRkdTreeD <V3f[], V3f>(
3, ps.Length, ps,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, kdTreeEps
);
}
Guid?psId = ps != null ? (Guid?)Guid.NewGuid() : null;
Guid?csId = cs != null ? (Guid?)Guid.NewGuid() : null;
Guid?nsId = ns != null ? (Guid?)Guid.NewGuid() : null;
Guid?isId = js != null ? (Guid?)Guid.NewGuid() : null;
Guid?kdId = kdTree != null ? (Guid?)Guid.NewGuid() : null;
var subcells = _subnodes?.Map(x => x?.ToPointSetCell(storage, ct, kdTreeEps));
var subcellIds = subcells?.Map(x => x?.Id);
#if DEBUG
if (ps != null && _subnodes != null)
{
throw new InvalidOperationException();
}
#endif
var pointCountTree = ps != null
? ps.Length
: subcells.Sum(n => n != null ? n.PointCountTree : 0)
;
if (psId != null)
{
storage.Add(psId.ToString(), ps, ct);
}
if (csId != null)
{
storage.Add(csId.ToString(), cs, ct);
}
if (nsId != null)
{
storage.Add(nsId.ToString(), ns, ct);
}
if (isId != null)
{
storage.Add(isId.ToString(), js, ct);
}
if (kdId != null)
{
storage.Add(kdId.ToString(), kdTree.Data, ct);
}
if (subcellIds == null) // leaf
{
return(new PointSetNode(_cell, pointCountTree, psId, csId, kdId, nsId, isId, storage));
}
else
{
return(new PointSetNode(_cell, pointCountTree, subcellIds, storage));
}
}
/// <summary> /// Estimates a normal vector for each point by least-squares-fitting a plane through its k nearest neighbours. /// Requires that the supplied kdtree is built from given points. /// </summary> public static V3f[] EstimateNormals(this IList <V3d> points, PointRkdTreeD <V3d[], V3d> kdtree, int k) => points.Map((p, i) =>
