private static PointSetNode JoinNonOverlappingTrees(Cell rootCell, PointSetNode a, PointSetNode b, long octreeSplitLimit, CancellationToken ct)
{
#region Preconditions
// PRE: ensure that trees 'a' and 'b' do not intersect,
// because we are joining non-overlapping trees here
if (a.Cell == b.Cell || a.Cell.Intersects(b.Cell))
{
throw new InvalidOperationException();
}
// PRE: we further assume, that both trees are non-empty
if (a.PointCountTree == 0 && b.PointCountTree == 0)
{
throw new InvalidOperationException();
}
#endregion
#region Case reduction
// REDUCE CASES:
// if one tree ('a' or 'b') is centered at origin, then ensure that 'a' is centered
// (by swapping 'a' and 'b' if necessary)
if (b.IsCenteredAtOrigin)
{
#if DEBUG
// PRE: if 'b' is centered, than 'a' cannot be centered
// (because then 'a' and 'b' would overlap, and we join non-overlapping trees here)
if (a.IsCenteredAtOrigin)
{
throw new InvalidOperationException();
}
#endif
Fun.Swap(ref a, ref b);
#if DEBUG
// POST: 'a' is centered, 'b' is not centered
if (!a.IsCenteredAtOrigin)
{
throw new InvalidOperationException();
}
if (b.IsCenteredAtOrigin)
{
throw new InvalidOperationException();
}
#endif
}
#endregion
#region CASE 1 of 2: one tree is centered (must be 'a', since if it originally was 'b' we would have swapped)
if (rootCell.IsCenteredAtOrigin && a.IsCenteredAtOrigin)
{
#region special case: split 'a' into subcells to get rid of centered cell containing points
if (a.IsLeaf)
{
return(JoinNonOverlappingTrees(rootCell, a.ForceSplitLeaf(ct), b, octreeSplitLimit, ct));
}
#endregion
#if DEBUG
if (a.PointCount != 0)
{
throw new InvalidOperationException();
}
#endif
var subcells = new PointSetNode[8];
for (var i = 0; i < 8; i++)
{
var rootCellOctant = rootCell.GetOctant(i);
var aSub = a.Subnodes[i]?.Value;
var bIsContained = rootCellOctant.Contains(b.Cell);
#if DEBUG
if (!bIsContained && rootCellOctant.Intersects(b.Cell))
{
throw new InvalidOperationException();
}
#endif
if (aSub != null)
{
if (bIsContained)
{
// CASE: both contained
var merged = Merge(aSub, b, octreeSplitLimit, ct);
subcells[i] = JoinTreeToRootCell(rootCellOctant, merged);
}
else
{
// CASE: aSub contained
subcells[i] = JoinTreeToRootCell(rootCellOctant, aSub);
}
}
else
{
if (bIsContained)
{
// CASE: b contained
subcells[i] = JoinTreeToRootCell(rootCellOctant, b);
}
else
{
// CASE: none contained -> empty subcell
subcells[i] = null;
}
}
}
var result = new PointSetNode(rootCell, a.PointCountTree + b.PointCountTree, subcells.Map(x => x?.Id), a.Storage);
#if DEBUG
if (result.PointCountTree != a.PointCountTree + b.PointCountTree)
{
throw new InvalidOperationException();
}
if (result.PointCountTree != result.Subnodes.Sum(x => x?.Value?.PointCountTree))
{
throw new InvalidOperationException();
}
#endif
return(result);
}
#endregion
#region CASE 2 of 2: no tree is centered
else
{
#if DEBUG
// PRE: no tree is centered
if (a.IsCenteredAtOrigin)
{
throw new InvalidOperationException();
}
if (b.IsCenteredAtOrigin)
{
throw new InvalidOperationException();
}
#endif
var subcells = new PointSetNode[8];
var doneA = false;
var doneB = false;
for (var i = 0; i < 8; i++)
{
var subcell = rootCell.GetOctant(i);
if (subcell.Contains(a.Cell))
{
#if DEBUG
if (subcell.Intersects(b.Cell))
{
throw new InvalidOperationException();
}
#endif
subcells[i] = JoinTreeToRootCell(subcell, a);
if (doneB)
{
break;
}
doneA = true;
}
if (subcell.Intersects(b.Cell))
{
#if DEBUG
if (subcell.Intersects(a.Cell))
{
throw new InvalidOperationException();
}
#endif
subcells[i] = JoinTreeToRootCell(subcell, b);
if (doneA == true)
{
break;
}
doneB = true;
}
}
var result = new PointSetNode(rootCell, a.PointCountTree + b.PointCountTree, subcells.Map(x => x?.Id), a.Storage);
#if DEBUG
if (result.PointCountTree != a.PointCountTree + b.PointCountTree)
{
throw new InvalidOperationException();
}
if (result.PointCountTree != result.Subnodes.Sum(x => x?.Value?.PointCountTree))
{
throw new InvalidOperationException();
}
#endif
return(result);
}
#endregion
}
/// <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);
}