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>
/// Returns union of trees as new tree (immutable operation).
/// </summary>
public static PointSetNode Merge(this PointSetNode a, PointSetNode b, long octreeSplitLimit, CancellationToken ct)
{
if (a == null || a.PointCountTree == 0)
{
return(b);
}
if (b == null || b.PointCountTree == 0)
{
return(a);
}
#if DEBUG
var debugPointCountTree = a.PointCountTree + b.PointCountTree;
#endif
// if A and B have identical root cells, then merge ...
if (a.Cell == b.Cell)
{
var result = a.IsLeaf
? (b.IsLeaf ? MergeLeafAndLeafWithIdenticalRootCell(a, b, octreeSplitLimit, ct)
: MergeLeafAndTreeWithIdenticalRootCell(a, b, octreeSplitLimit, ct))
: (b.IsLeaf ? MergeLeafAndTreeWithIdenticalRootCell(b, a, octreeSplitLimit, ct)
: MergeTreeAndTreeWithIdenticalRootCell(a, b, octreeSplitLimit, ct))
;
return(result);
}
// if A and B do not intersect ...
if (!a.Cell.Intersects(b.Cell))
{
var rootCell = new Cell(new Box3d(a.BoundingBox, b.BoundingBox));
var result = JoinNonOverlappingTrees(rootCell, a, b, octreeSplitLimit, ct);
#if DEBUG
if (result.PointCountTree != debugPointCountTree)
{
throw new InvalidOperationException();
}
#endif
return(result);
}
if (a.IsCenteredAtOrigin || b.IsCenteredAtOrigin)
{
// enumerate all non-IsCenteredAtOrigin (sub)cells of A and B
var parts = new List <PointSetNode>();
if (a.IsCenteredAtOrigin)
{
if (a.IsLeaf)
{
// split A into 8 subcells to get rid of centered cell
return(Merge(a.ForceSplitLeaf(ct), b, octreeSplitLimit, ct));
}
else
{
parts.AddRange(a.Subnodes.Select(x => x?.Value));
}
}
else
{
parts.Add(a);
}
if (b.IsCenteredAtOrigin)
{
if (b.IsLeaf)
{
// split B into 8 subcells to get rid of centered cell
return(Merge(a, b.ForceSplitLeaf(ct), octreeSplitLimit, ct));
}
else
{
parts.AddRange(b.Subnodes.Select(x => x?.Value));
}
}
else
{
parts.Add(b);
}
// special case: there is only 1 part -> finished
parts = parts.Where(x => x != null).ToList();
if (parts.Count == 0)
{
throw new InvalidOperationException();
}
if (parts.Count == 1)
{
return(parts.Single());
}
// common case: multiple parts
var rootCellBounds = new Box3d(a.Cell.BoundingBox, b.Cell.BoundingBox);
var rootCell = new Cell(rootCellBounds);
var roots = new PointSetNode[8];
Func <Cell, int> octant = x =>
{
if (x.IsCenteredAtOrigin)
{
throw new InvalidOperationException();
}
return((x.X >= 0 ? 1 : 0) + (x.Y >= 0 ? 2 : 0) + (x.Z >= 0 ? 4 : 0));
};
foreach (var x in parts)
{
var oi = octant(x.Cell);
var oct = rootCell.GetOctant(oi);
if (roots[oi] == null)
{
if (x.Cell != oct)
{
if (!oct.Contains(x.Cell))
{
throw new InvalidOperationException();
}
roots[oi] = JoinTreeToRootCell(oct, x);
}
else
{
roots[oi] = x;
}
}
else
{
roots[oi] = Merge(roots[oi], x, octreeSplitLimit, ct);
}
if (oct != roots[oi].Cell)
{
throw new InvalidOperationException();
}
}
var pointCountTree = roots.Where(x => x != null).Sum(x => x.PointCountTree);
return(new PointSetNode(rootCell, pointCountTree, roots.Map(n => n?.Id), a.Storage));
}
#if DEBUG
if (a.Cell.Exponent == b.Cell.Exponent)
{
if (!a.IsCenteredAtOrigin && !b.IsCenteredAtOrigin)
{
throw new InvalidOperationException(
$"merge {a.Cell} with {b.Cell}")
;
}
}
#endif
// ... otherwise ensure that A's root cell is bigger than B's to reduce number of cases to handle ...
if (a.Cell.Exponent < b.Cell.Exponent)
{
var result = Merge(b, a, octreeSplitLimit, ct);
#if DEBUG
if (result.PointCountTree != debugPointCountTree)
{
throw new InvalidOperationException();
}
#endif
return(result);
}
// ... B must now be contained in exactly one of A's subcells
#if DEBUG
var isExactlyOne = false;
#endif
var subcells = a.Subnodes?.Map(x => x?.Value) ?? new PointSetNode[8];
for (var i = 0; i < 8; i++)
{
var subcellIndex = a.Cell.GetOctant(i);
if (subcellIndex.Contains(b.Cell))
{
#if DEBUG
if (isExactlyOne)
{
throw new InvalidOperationException();
}
isExactlyOne = true;
#endif
if (subcells[i] == null)
{
subcells[i] = JoinTreeToRootCell(subcellIndex, b);
}
else
{
subcells[i] = Merge(subcells[i], b, octreeSplitLimit, ct);
}
}
}
#if DEBUG
if (!isExactlyOne)
{
throw new InvalidOperationException();
}
#endif
PointSetNode result2 = null;
if (a.IsLeaf)
{
result2 = a.ToInnerNode(subcells);
result2 = InjectPointsIntoTree(a.PositionsAbsolute, a.Colors?.Value, a.Normals?.Value, a.Intensities?.Value, result2, result2.Cell, octreeSplitLimit, a.Storage, ct);
}
else
{
result2 = a.WithSubNodes(subcells);
}
#if DEBUG
// this no longer holds due to removal of duplicate points
//if (result2.PointCountTree != debugPointCountTree) throw new InvalidOperationException();
#endif
return(result2);
}
