/// <summary>
/// If cell is a leaf, it will be split once (non-recursive, without taking into account any split limit).
/// If cell is not a leaf, this is an invalid operation.
/// </summary>
public static PointSetNode ForceSplitLeaf(this PointSetNode cell, CancellationToken ct)
{
if (cell == null)
{
throw new ArgumentNullException(nameof(cell));
}
if (cell.IsNotLeaf)
{
throw new InvalidOperationException();
}
if (cell.PointCount == 0)
{
throw new InvalidOperationException();
}
if (cell.PointCountTree != cell.PointCount)
{
throw new InvalidOperationException();
}
var subnodesPoints = new List <V3d> [8];
var subnodesColors = cell.HasColors ? new List <C4b> [8] : null;
var subnodesNormals = cell.HasNormals ? new List <V3f> [8] : null;
var subnodesIntensities = cell.HasIntensities ? new List <int> [8] : null;
var pa = cell.PositionsAbsolute;
var ca = cell.Colors?.Value;
var na = cell.Normals?.Value;
var ia = cell.Intensities?.Value;
var imax = cell.PointCount;
if (pa.Length != imax)
{
throw new InvalidOperationException();
}
for (var i = 0; i < imax; i++)
{
var si = cell.GetSubIndex(pa[i]);
if (subnodesPoints[si] == null)
{
subnodesPoints[si] = new List <V3d>();
if (subnodesColors != null)
{
subnodesColors[si] = new List <C4b>();
}
if (subnodesNormals != null)
{
subnodesNormals[si] = new List <V3f>();
}
if (subnodesIntensities != null)
{
subnodesIntensities[si] = new List <int>();
}
}
subnodesPoints[si].Add(pa[i]);
if (subnodesColors != null)
{
subnodesColors[si].Add(ca[i]);
}
if (subnodesNormals != null)
{
subnodesNormals[si].Add(na[i]);
}
if (subnodesIntensities != null)
{
subnodesIntensities[si].Add(ia[i]);
}
}
var subnodes = new PointSetNode[8];
for (var i = 0; i < 8; i++)
{
if (subnodesPoints[i] == null)
{
continue;
}
var subCellIndex = cell.Cell.GetOctant(i);
if (!cell.Cell.Contains(subCellIndex))
{
throw new InvalidOperationException();
}
if (cell.Cell.Exponent != subCellIndex.Exponent + 1)
{
throw new InvalidOperationException();
}
var builder = InMemoryPointSet.Build(subnodesPoints[i], subnodesColors?[i], subnodesNormals?[i], subnodesIntensities?[i], subCellIndex, int.MaxValue);
var subnode = builder.ToPointSetCell(cell.Storage, ct: ct);
if (subnode.PointCountTree > subnodesPoints[i].Count)
{
throw new InvalidOperationException();
}
if (!cell.Cell.Contains(subnode.Cell))
{
throw new InvalidOperationException();
}
if (cell.Cell.Exponent != subnode.Cell.Exponent + 1)
{
throw new InvalidOperationException();
}
subnodes[i] = subnode;
}
var result = new PointSetNode(cell.Cell, imax, subnodes.Map(x => x?.Id), cell.Storage);
// POST
if (result.IsLeaf)
{
throw new InvalidOperationException();
}
if (result.PointCountTree != cell.PointCountTree)
{
throw new InvalidOperationException();
}
if (result.PointCount != 0)
{
throw new InvalidOperationException();
}
if (result.Subnodes.Sum(x => x?.Value?.PointCountTree) > cell.PointCountTree)
{
throw new InvalidOperationException();
}
return(result);
}
private static PointSetNode InjectPointsIntoTree(IList <V3d> psAbsolute, IList <C4b> cs, IList <V3f> ns, IList <int> js, PointSetNode a, Cell cell, long octreeSplitLimit, Storage storage, CancellationToken ct)
{
if (a == null)
{
var result0 = InMemoryPointSet.Build(psAbsolute, cs, ns, js, cell, octreeSplitLimit).ToPointSetCell(storage, ct: ct);
if (result0.PointCountTree > psAbsolute.Count)
{
throw new InvalidOperationException();
}
return(result0);
}
if (a.Cell != cell)
{
throw new InvalidOperationException();
}
if (a.IsLeaf)
{
if (cs != null && !a.HasColors)
{
throw new InvalidOperationException();
}
if (cs == null && a.HasColors)
{
throw new InvalidOperationException();
}
if (ns != null && !a.HasNormals)
{
throw new InvalidOperationException();
}
if (ns == null && a.HasNormals)
{
throw new InvalidOperationException();
}
var newPs = new List <V3d>(psAbsolute); newPs.AddRange(a.PositionsAbsolute);
var newCs = cs != null ? new List <C4b>(cs) : null; newCs?.AddRange(a.Colors.Value);
var newNs = ns != null ? new List <V3f>(ns) : null; newNs?.AddRange(a.Normals.Value);
var newIs = js != null ? new List <int>(js) : null; newIs?.AddRange(a.Intensities.Value);
var result0 = InMemoryPointSet.Build(newPs, newCs, newNs, newIs, cell, octreeSplitLimit).ToPointSetCell(a.Storage, ct: ct);
return(result0);
}
var pss = new List <V3d> [8];
var css = cs != null ? new List <C4b> [8] : null;
var nss = ns != null ? new List <V3f> [8] : null;
var iss = js != null ? new List <int> [8] : null;
for (var i = 0; i < psAbsolute.Count; i++)
{
var j = a.GetSubIndex(psAbsolute[i]);
if (pss[j] == null)
{
pss[j] = new List <V3d>();
if (cs != null)
{
css[j] = new List <C4b>();
}
if (ns != null)
{
nss[j] = new List <V3f>();
}
if (js != null)
{
iss[j] = new List <int>();
}
}
pss[j].Add(psAbsolute[i]);
if (cs != null)
{
css[j].Add(cs[i]);
}
if (ns != null)
{
nss[j].Add(ns[i]);
}
if (js != null)
{
iss[j].Add(js[i]);
}
}
if (pss.Sum(x => x?.Count) != psAbsolute.Count)
{
throw new InvalidOperationException();
}
var subcells = new PointSetNode[8];
for (var j = 0; j < 8; j++)
{
var x = a.Subnodes[j]?.Value;
if (pss[j] != null)
{
subcells[j] = InjectPointsIntoTree(pss[j], css?[j], nss?[j], iss?[j], x, cell.GetOctant(j), octreeSplitLimit, storage, ct);
}
else
{
subcells[j] = x;
}
}
return(a.WithSubNodes(subcells));
}
/// <summary>
/// Points within given distance of a point.
/// </summary>
public static PointsNearObject <V3d> QueryPointsNearPoint(
this PointSetNode node, V3d query, double maxDistanceToPoint, int maxCount
)
{
if (node == null)
{
return(PointsNearObject <V3d> .Empty);
}
// if query point is farther from bounding box than maxDistanceToPoint,
// then there cannot be a result and we are done
var eps = node.BoundingBox.Distance(query);
if (eps > maxDistanceToPoint)
{
return(PointsNearObject <V3d> .Empty);
}
if (node.IsLeaf)
{
#if PARANOID
if (node.PointCount <= 0)
{
throw new InvalidOperationException();
}
#endif
var center = node.Center;
var ia = node.KdTree.Value.GetClosest((V3f)(query - center), (float)maxDistanceToPoint, maxCount);
if (ia.Count > 0)
{
var ps = new V3d[ia.Count];
var cs = node.HasColors ? new C4b[ia.Count] : null;
var ns = node.HasNormals ? new V3f[ia.Count] : null;
var js = node.HasIntensities ? new int[ia.Count] : null;
var ds = new double[ia.Count];
for (var i = 0; i < ia.Count; i++)
{
var index = (int)ia[i].Index;
ps[i] = center + (V3d)node.Positions.Value[index];
if (node.HasColors)
{
cs[i] = node.Colors.Value[index];
}
if (node.HasNormals)
{
ns[i] = node.Normals.Value[index];
}
if (node.HasIntensities)
{
js[i] = node.Intensities.Value[index];
}
ds[i] = ia[i].Dist;
}
var chunk = new PointsNearObject <V3d>(query, maxDistanceToPoint, ps, cs, ns, js, ds);
return(chunk);
}
else
{
return(PointsNearObject <V3d> .Empty);
}
}
else
{
// first traverse octant containing query point
var index = node.GetSubIndex(query);
var n = node.Subnodes[index];
var result = n != null?n.Value.QueryPointsNearPoint(query, maxDistanceToPoint, maxCount) : PointsNearObject <V3d> .Empty;
if (!result.IsEmpty && result.MaxDistance < maxDistanceToPoint)
{
maxDistanceToPoint = result.MaxDistance;
}
// now traverse other octants
for (var i = 0; i < 8; i++)
{
if (i == index)
{
continue;
}
n = node.Subnodes[i];
if (n == null)
{
continue;
}
var x = n.Value.QueryPointsNearPoint(query, maxDistanceToPoint, maxCount);
result = result.Merge(x, maxCount);
if (!result.IsEmpty && result.MaxDistance < maxDistanceToPoint)
{
maxDistanceToPoint = result.MaxDistance;
}
}
return(result);
}
}
