private static PointSetNode JoinTreeToRootCell(Cell rootCell, PointSetNode a)
{
if (!rootCell.Contains(a.Cell))
{
throw new InvalidOperationException();
}
if (a.IsCenteredAtOrigin)
{
throw new InvalidOperationException();
}
if (rootCell == a.Cell)
{
return(a);
}
var subcells = new PointSetNode[8];
for (var i = 0; i < 8; i++)
{
var subcell = rootCell.GetOctant(i);
if (subcell == a.Cell)
{
subcells[i] = a; break;
}
if (subcell.Contains(a.Cell))
{
subcells[i] = JoinTreeToRootCell(subcell, a); break;
}
}
var result = new PointSetNode(rootCell, a.PointCountTree, subcells.Map(x => x?.Id), a.Storage);
#if DEBUG
if (result.PointCountTree != a.PointCountTree)
{
throw new InvalidOperationException();
}
#endif
return(result);
}
private static PointSetNode MergeTreeAndTreeWithIdenticalRootCell(PointSetNode a, PointSetNode b, long octreeSplitLimit, CancellationToken ct)
{
if (a.IsLeaf || b.IsLeaf)
{
throw new InvalidOperationException();
}
if (a.Cell != b.Cell)
{
throw new InvalidOperationException();
}
if (a.PointCount > 0)
{
throw new InvalidOperationException();
}
if (b.PointCount > 0)
{
throw new InvalidOperationException();
}
var pointCountTree = 0L;
var subcells = new PointSetNode[8];
for (var i = 0; i < 8; i++)
{
var octant = a.Cell.GetOctant(i);
var x = a.Subnodes[i]?.Value;
var y = b.Subnodes[i]?.Value;
if (x != null)
{
if (y != null)
{
subcells[i] = Merge(x, y, octreeSplitLimit, ct);
pointCountTree += x.PointCountTree + y.PointCountTree;
}
else
{
subcells[i] = x;
pointCountTree += x.PointCountTree;
if (subcells[i].PointCountTree != x.PointCountTree)
{
throw new InvalidOperationException();
}
}
}
else
{
if (y != null)
{
subcells[i] = y;
pointCountTree += y.PointCountTree;
if (subcells[i].PointCountTree != y.PointCountTree)
{
throw new InvalidOperationException();
}
}
else
{
subcells[i] = null;
}
}
}
var result = new PointSetNode(a.Cell, pointCountTree, subcells.Map(x => x?.Id), a.Storage);
return(result);
}
/// <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 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);
}