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); }