private static PointSetNode MergeLeafAndTreeWithIdenticalRootCell(PointSetNode a, PointSetNode b, long octreeSplitLimit, CancellationToken ct) { if (a == null) { throw new ArgumentNullException(nameof(a)); } if (b == null) { throw new ArgumentNullException(nameof(b)); } if (a.IsNotLeaf || b.IsLeaf) { throw new InvalidOperationException(); } if (a.Cell != b.Cell) { throw new InvalidOperationException(); } var center = a.Center; var result = InjectPointsIntoTree(a.PositionsAbsolute, a.Colors?.Value, a.Normals?.Value, a.Intensities?.Value, b, a.Cell, octreeSplitLimit, a.Storage, ct); return(result); }
/// <summary> /// </summary> public static PointSetNode Delete(this PointSetNode node, Func <PointSetNode, bool> isNodeFullyInside, Func <PointSetNode, bool> isNodeFullyOutside, Func <V3d, bool> isPositionInside, CancellationToken ct ) { if (node == null) { return(null); } if (isNodeFullyInside(node)) { return(null); } if (isNodeFullyOutside(node)) { return(node); } if (node.IsLeaf) { Guid?newPsId = null; Guid?newCsId = null; Guid?newNsId = null; Guid?newIsId = null; Guid?newKdId = null; if (!node.HasPositions) { throw new InvalidOperationException(); } var ps = new List <V3f>(); var cs = node.HasColors ? new List <C4b>() : null; var ns = node.HasNormals ? new List <V3f>() : null; var js = node.HasIntensities ? new List <int>() : null; var oldPsAbsolute = node.PositionsAbsolute; var oldPs = node.Positions.Value; var oldCs = node.Colors?.Value; var oldNs = node.Normals?.Value; var oldIs = node.Intensities?.Value; for (var i = 0; i < oldPsAbsolute.Length; i++) { if (!isPositionInside(oldPsAbsolute[i])) { ps.Add(oldPs[i]); if (oldCs != null) { cs.Add(oldCs[i]); } if (oldNs != null) { ns.Add(oldNs[i]); } if (oldIs != null) { js.Add(oldIs[i]); } } } if (ps.Count > 0) { newPsId = Guid.NewGuid(); var psa = ps.ToArray(); node.Storage.Add(newPsId.Value, psa, ct); newKdId = Guid.NewGuid(); node.Storage.Add(newKdId.Value, psa.BuildKdTree().Data, ct); if (node.HasColors) { newCsId = Guid.NewGuid(); node.Storage.Add(newCsId.Value, cs.ToArray(), ct); } if (node.HasNormals) { newNsId = Guid.NewGuid(); node.Storage.Add(newNsId.Value, ns.ToArray(), ct); } if (node.HasIntensities) { newIsId = Guid.NewGuid(); node.Storage.Add(newIsId.Value, js.ToArray(), ct); } var result = new PointSetNode(node.Cell, ps.Count, newPsId, newCsId, newKdId, newNsId, newIsId, node.Storage); if (node.HasLodPositions) { result = result.WithLod(); } return(result); } else { return(null); } } else { Guid?newLodPsId = null; Guid?newLodCsId = null; Guid?newLodNsId = null; Guid?newLodIsId = null; Guid?newLodKdId = null; if (node.HasLodPositions) { var ps = node.HasLodPositions ? new List <V3f>() : null; var cs = node.HasLodColors ? new List <C4b>() : null; var ns = node.HasLodNormals ? new List <V3f>() : null; var js = node.HasLodIntensities ? new List <int>() : null; var oldLodPsAbsolute = node.LodPositionsAbsolute; var oldLodPs = node.LodPositions.Value; var oldLodCs = node.LodColors?.Value; var oldLodNs = node.LodNormals?.Value; var oldLodIs = node.LodIntensities?.Value; for (var i = 0; i < oldLodPsAbsolute.Length; i++) { if (!isPositionInside(oldLodPsAbsolute[i])) { ps.Add(oldLodPs[i]); if (oldLodCs != null) { cs.Add(oldLodCs[i]); } if (oldLodNs != null) { ns.Add(oldLodNs[i]); } if (oldLodIs != null) { js.Add(oldLodIs[i]); } } } if (ps.Count > 0) { newLodPsId = Guid.NewGuid(); var psa = ps.ToArray(); node.Storage.Add(newLodPsId.Value, psa, ct); newLodKdId = Guid.NewGuid(); node.Storage.Add(newLodKdId.Value, psa.BuildKdTree().Data, ct); if (node.HasLodColors) { newLodCsId = Guid.NewGuid(); node.Storage.Add(newLodCsId.Value, cs.ToArray(), ct); } if (node.HasLodNormals) { newLodNsId = Guid.NewGuid(); node.Storage.Add(newLodNsId.Value, ns.ToArray(), ct); } if (node.HasLodIntensities) { newLodIsId = Guid.NewGuid(); node.Storage.Add(newLodIsId.Value, js.ToArray(), ct); } } } var newSubnodes = node.Subnodes.Map(n => n?.Value.Delete(isNodeFullyInside, isNodeFullyOutside, isPositionInside, ct)); if (newSubnodes.All(n => n == null)) { return(null); } return(node.WithLod(newLodPsId, newLodCsId, newLodNsId, newLodIsId, newLodKdId, newSubnodes)); } }
/// <summary> /// All points outside convex hull (excluding boundary). /// </summary> public static IEnumerable <Chunk> QueryPointsOutsideConvexHull( this PointSetNode self, Hull3d query, int minCellExponent = int.MinValue ) => QueryPointsInsideConvexHull(self, query.Reversed(), minCellExponent);
private static PointSetNode ParseBinary(byte[] buffer, Storage storage) { var masks = BitConverter.ToUInt32(buffer, 0); var subcellmask = masks >> 24; var attributemask = masks & 0b00000000_11111111_11111111_11111111; var offset = 4; var id = ParseGuid(buffer, ref offset); var cell = new Cell( BitConverter.ToInt64(buffer, 20), BitConverter.ToInt64(buffer, 28), BitConverter.ToInt64(buffer, 36), BitConverter.ToInt32(buffer, 44) ); var pointCountTree = BitConverter.ToInt64(buffer, 48); offset = 56; Guid[] subcellIds = null; if (subcellmask != 0) { subcellIds = new Guid[8]; if ((subcellmask & 0x01) != 0) { subcellIds[0] = ParseGuid(buffer, ref offset); } if ((subcellmask & 0x02) != 0) { subcellIds[1] = ParseGuid(buffer, ref offset); } if ((subcellmask & 0x04) != 0) { subcellIds[2] = ParseGuid(buffer, ref offset); } if ((subcellmask & 0x08) != 0) { subcellIds[3] = ParseGuid(buffer, ref offset); } if ((subcellmask & 0x10) != 0) { subcellIds[4] = ParseGuid(buffer, ref offset); } if ((subcellmask & 0x20) != 0) { subcellIds[5] = ParseGuid(buffer, ref offset); } if ((subcellmask & 0x40) != 0) { subcellIds[6] = ParseGuid(buffer, ref offset); } if ((subcellmask & 0x80) != 0) { subcellIds[7] = ParseGuid(buffer, ref offset); } } var psId = (attributemask & (uint)PointSetAttributes.Positions) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var csId = (attributemask & (uint)PointSetAttributes.Colors) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var nsId = (attributemask & (uint)PointSetAttributes.Normals) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var isId = (attributemask & (uint)PointSetAttributes.Intensities) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var kdId = (attributemask & (uint)PointSetAttributes.KdTree) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; #pragma warning disable CS0612 // Type or member is obsolete var lodPsId = (attributemask & (uint)PointSetAttributes.LodPositions) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var lodCsId = (attributemask & (uint)PointSetAttributes.LodColors) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var lodNsId = (attributemask & (uint)PointSetAttributes.LodNormals) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var lodIsId = (attributemask & (uint)PointSetAttributes.LodIntensities) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var lodKdId = (attributemask & (uint)PointSetAttributes.LodKdTree) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var ksId = (attributemask & (uint)PointSetAttributes.Classifications) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; var lodKsId = (attributemask & (uint)PointSetAttributes.LodClassifications) != 0 ? ParseGuid(buffer, ref offset) : (Guid?)null; #pragma warning restore CS0612 // Type or member is obsolete #region backwards compatibility with obsolete lod entries if (lodPsId.HasValue) { psId = lodPsId; } if (lodCsId.HasValue) { csId = lodCsId; } if (lodNsId.HasValue) { nsId = lodNsId; } if (lodIsId.HasValue) { isId = lodIsId; } if (lodKdId.HasValue) { kdId = lodKdId; } if (lodKsId.HasValue) { ksId = lodKsId; } #endregion var data = ImmutableDictionary <Durable.Def, object> .Empty .Add(Durable.Octree.NodeId, id) .Add(Durable.Octree.Cell, cell) .Add(Durable.Octree.PointCountTreeLeafs, pointCountTree) ; if (psId.HasValue) { data = data.Add(Durable.Octree.PositionsLocal3fReference, psId.Value); } if (csId.HasValue) { data = data.Add(Durable.Octree.Colors4bReference, csId.Value); } if (kdId.HasValue) { data = data.Add(Durable.Octree.PointRkdTreeDDataReference, kdId.Value); } if (nsId.HasValue) { data = data.Add(Durable.Octree.Normals3fReference, nsId.Value); } if (isId.HasValue) { data = data.Add(Durable.Octree.Intensities1iReference, isId.Value); } if (ksId.HasValue) { data = data.Add(Durable.Octree.Classifications1bReference, ksId.Value); } if (subcellIds != null) { data = data.Add(Durable.Octree.SubnodesGuids, subcellIds); } var result = new PointSetNode(data, storage, false); return(result); }
/// <summary> /// Exact count. /// </summary> public static long CountPoints(this PointSetNode node, Func <PointSetNode, bool> isNodeFullyInside, Func <PointSetNode, bool> isNodeFullyOutside, Func <V3d, bool> isPositionInside, int minCellExponent = int.MinValue ) { if (node.Cell.Exponent < minCellExponent) { return(0L); } if (isNodeFullyOutside(node)) { return(0L); } if (node.IsLeaf || node.Cell.Exponent == minCellExponent) { if (isNodeFullyInside(node)) { if (node.HasPositions) { return(node.PointCount); } else if (node.HasLodPositions) { return(node.LodPointCount); } return(0L); } else // partially inside { var count = 0L; var psRaw = node.HasPositions ? node.PositionsAbsolute : node.LodPositionsAbsolute; for (var i = 0; i < psRaw.Length; i++) { var p = psRaw[i]; if (isPositionInside(p)) { count++; } } return(count); } } else { var sum = 0L; for (var i = 0; i < 8; i++) { var n = node.Subnodes[i]; if (n == null) { continue; } sum += CountPoints(n.Value, isNodeFullyInside, isNodeFullyOutside, isPositionInside, minCellExponent); } return(sum); } }
/// <summary> /// </summary> private static PointSetNode GenerateLod(this PointSetNode self, long octreeSplitLimit, Action callback, CancellationToken ct) { if (self == null) { throw new ArgumentNullException(nameof(self)); } ct.ThrowIfCancellationRequested(); callback?.Invoke(); if (self.IsLeaf) { return(self.WithLod()); } if (self.Subnodes == null || self.Subnodes.Length != 8) { throw new InvalidOperationException(); } var subcells = self.Subnodes.Map(x => x?.Value.GenerateLod(octreeSplitLimit, callback, ct)); var subcellsTotalCount = (long)subcells.Sum(x => x?.PointCountTree); var needsCs = subcells.Any(x => x != null ? (x.HasColors || x.HasLodColors) : false); var needsNs = subcells.Any(x => x != null ? (x.HasNormals || x.HasLodNormals) : false); var needsIs = subcells.Any(x => x != null ? (x.HasIntensities || x.HasLodIntensities) : false); var fractions = new double[8].SetByIndex( ci => subcells[ci] != null ? (subcells[ci].PointCountTree / (double)subcellsTotalCount) : 0.0 ); var remainder = 0.1; var counts = fractions.Map(x => { var fn = octreeSplitLimit * x + remainder; var n = (int)fn; remainder = fn - n; return(n); }); var e = octreeSplitLimit - counts.Sum(); if (e != 0) { throw new InvalidOperationException(); } // generate LoD data ... var lodPs = new V3f[octreeSplitLimit]; var lodCs = needsCs ? new C4b[octreeSplitLimit] : null; var lodNs = needsNs ? new V3f[octreeSplitLimit] : null; var lodIs = needsIs ? new int[octreeSplitLimit] : null; var i = 0; for (var ci = 0; ci < 8; ci++) { if (counts[ci] == 0) { continue; } var subcell = subcells[ci]; if (subcell == null) { continue; } var subps = subcell.IsLeaf ? subcell.Positions.Value : subcell.LodPositions.Value; var subcs = needsCs ? (subcell.IsLeaf ? subcell.Colors.Value : subcell.LodColors.Value) : null; var subns = needsNs ? (subcell.IsLeaf ? subcell.Normals.Value : subcell.LodNormals.Value) : null; var subis = needsIs ? (subcell.IsLeaf ? subcell.Intensities.Value : subcell.LodIntensities.Value) : null; var jmax = subps.Length; var dj = (jmax + 0.49) / counts[ci]; var oldI = i; for (var j = 0.0; j < jmax; j += dj) { var jj = (int)j; lodPs[i] = (V3f)(((V3d)subps[jj] + subcell.Center) - self.Center); if (needsCs) { lodCs[i] = subcs[jj]; } if (needsNs) { lodNs[i] = subns[jj]; } if (needsIs) { lodIs[i] = subis[jj]; } i++; } } var lodKd = lodPs.BuildKdTree(); // store LoD data ... var lodPsId = Guid.NewGuid(); self.Storage.Add(lodPsId, lodPs, ct); var lodKdId = Guid.NewGuid(); self.Storage.Add(lodKdId, lodKd.Data, ct); var lodCsId = needsCs ? (Guid?)Guid.NewGuid() : null; if (needsCs) { self.Storage.Add(lodCsId.Value, lodCs, ct); } var lodNsId = needsNs ? (Guid?)Guid.NewGuid() : null; if (needsNs) { self.Storage.Add(lodNsId.Value, lodNs, ct); } var lodIsId = needsIs ? (Guid?)Guid.NewGuid() : null; if (needsIs) { self.Storage.Add(lodIsId.Value, lodIs, ct); } var result = self.WithLod(lodPsId, lodCsId, lodNsId, lodIsId, lodKdId, subcells); return(result); }
/// <summary></summary> public static Selection Partial(PointSetNode node, IList <int> selectedPointIndices) => new Selection(node, selectedPointIndices, false, false);
/// <summary></summary> public static Selection All(PointSetNode node) => new Selection(node, null, true, false);
/// <summary> /// </summary> public CellQueryResult(PointSetNode cell, bool isFullyInside) { Cell = cell; IsFullyInside = isFullyInside; }
/// <summary> /// Returns true if this node is fully inside the negative halfspace defined by given plane. /// </summary> public static bool InsideNegativeHalfSpace(this PointSetNode self, Plane3d plane) { self.BoundingBox.GetMinMaxInDirection(-plane.Normal, out V3d min, out V3d max); return(plane.Height(min) < 0); }
/// <summary> /// Returns true if this node intersects the negative halfspace defined by given plane. /// </summary> public static bool IntersectsNegativeHalfSpace(this PointSetNode self, Plane3d plane) => self.Corners.Any(p => plane.Height(p) < 0);
/// <summary> /// Returns true if this node intersects the space within a given distance to a plane. /// </summary> public static bool Intersects(this PointSetNode self, Plane3d plane, double distance) => self.BoundingBox.Intersects(plane, distance);
/// <summary> /// Computes centroid of absolute (LoD) positions in this node, or null if no (LoD) positions. /// </summary> public static V3d?ComputeCentroid(this PointSetNode self) => self.HasPositions ? self.PositionsAbsolute.Average() : (self.HasLodPositions ? self.LodPositionsAbsolute.Average() : (V3d?)null) ;
/// <summary> /// Calls action for each node in this tree. /// </summary> public static IEnumerable <PointSetNode> ForEachNode( this PointSetNode self, int minCellExponent = int.MinValue ) => _ForEachNode(self, minCellExponent);
/// <summary> /// Returns new tree with regenerated normals using given estimateNormals function. /// </summary> private static PointSetNode RegenerateNormals(this PointSetNode self, Func <IList <V3d>, IList <V3f> > estimateNormals, Action callback, CancellationToken ct ) { if (self == null) { throw new ArgumentNullException(nameof(self)); } ct.ThrowIfCancellationRequested(); callback?.Invoke(); if (self.IsLeaf) { // generate and store normals var ns = estimateNormals(self.PositionsAbsolute).ToArray(); var nsId = Guid.NewGuid(); self.Storage.Add(nsId, ns, ct); // create node with new normals and LoD normals var r = self.WithNormals(nsId).WithLod(); return(r); } if (self.Subnodes == null || self.Subnodes.Length != 8) { throw new InvalidOperationException(); } var subcells = self.Subnodes.Map(x => x?.Value.RegenerateNormals(estimateNormals, callback, ct)); var subcellsTotalCount = (long)subcells.Sum(x => x?.PointCountTree); var octreeSplitLimit = self.LodPositions.Value.Length; var fractions = new double[8].SetByIndex( ci => subcells[ci] != null ? (subcells[ci].PointCountTree / (double)subcellsTotalCount) : 0.0 ); var remainder = 0.1; var counts = fractions.Map(x => { var fn = octreeSplitLimit * x + remainder; var n = (int)fn; remainder = fn - n; return(n); }); var e = octreeSplitLimit - counts.Sum(); if (e != 0) { throw new InvalidOperationException(); } // generate LodNormals ... var lodNs = new V3f[octreeSplitLimit]; var i = 0; for (var ci = 0; ci < 8; ci++) { if (counts[ci] == 0) { continue; } var subcell = subcells[ci]; if (subcell == null) { continue; } var subns = subcell.LodNormals.Value; var jmax = subns.Length; var dj = (jmax + 0.49) / counts[ci]; var oldI = i; for (var j = 0.0; j < jmax; j += dj) { var jj = (int)j; lodNs[i] = subns[jj]; i++; } } // store LoD data ... var lodNsId = Guid.NewGuid(); self.Storage.Add(lodNsId, lodNs, ct); var result = self.WithLod(self.LodPositionsId, self.LodColorsId, lodNsId, self.LodIntensitiesId, self.LodKdTreeId, subcells); return(result); }
/// <summary> /// Enumerates (chunked) all points in tree. /// </summary> public static IEnumerable <Chunk> QueryAllPoints(this PointSetNode node) => node.QueryPoints(_ => true, _ => false, _ => true);
/// <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); }
/// <summary></summary> public static Selection None(PointSetNode node) => new Selection(node, null, false, true);
/// <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); }
/// <summary></summary> public IPointCloudNode Materialize() { var newId = Guid.NewGuid(); var data = ImmutableDictionary <Def, object> .Empty .Add(Octree.NodeId, newId) .Add(Octree.Cell, Cell) .Add(Octree.BoundingBoxExactLocal, BoundingBoxExactLocal) ; if (IsLeaf) { data = data.Add(Octree.BoundingBoxExactGlobal, BoundingBoxExactGlobal); } else { var subnodes = Subnodes.Map(x => x?.Value.Materialize()); var subnodeIds = subnodes.Map(x => x?.Id ?? Guid.Empty); var bbExactGlobal = new Box3d(subnodes.Where(x => x != null).Select(x => x.BoundingBoxExactGlobal)); data = data .Add(Octree.SubnodesGuids, subnodeIds) .Add(Octree.BoundingBoxExactGlobal, bbExactGlobal) ; } if (HasPositions) { var id = Guid.NewGuid(); Storage.Add(id, Positions.Value); data = data.Add(Octree.PositionsLocal3fReference, id); } if (HasKdTree) { var id = Guid.NewGuid(); Storage.Add(id, KdTree.Value.Data); data = data.Add(Octree.PointRkdTreeFDataReference, id); } if (HasColors) { var id = Guid.NewGuid(); Storage.Add(id, Colors.Value); data = data.Add(Octree.Colors4bReference, id); } if (HasNormals) { var id = Guid.NewGuid(); Storage.Add(id, Normals.Value); data = data.Add(Octree.Normals3fReference, id); } if (HasClassifications) { var id = Guid.NewGuid(); Storage.Add(id, Classifications.Value); data = data.Add(Octree.Classifications1bReference, id); } if (HasIntensities) { var id = Guid.NewGuid(); Storage.Add(id, Intensities.Value); data = data.Add(Octree.Intensities1iReference, id); } var result = new PointSetNode(data, Storage, writeToStore: true); 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 }
public PointSetNode ToPointSetCell(Storage storage, bool isTemporaryImportNode) { var center = new V3d(_centerX, _centerY, _centerZ); V3f[] ps = null; C4b[] cs = null; V3f[] ns = null; int[] js = null; byte[] ks = null; if (_ia != null) { var allPs = _octree.m_ps; var count = _ia.Count; ps = new V3f[count]; for (var i = 0; i < count; i++) { ps[i] = (V3f)(allPs[_ia[i]] - center); } if (_octree.m_cs != null) { var allCs = _octree.m_cs; cs = new C4b[count]; for (var i = 0; i < count; i++) { cs[i] = allCs[_ia[i]]; } } if (_octree.m_ns != null) { var allNs = _octree.m_ns; ns = new V3f[count]; for (var i = 0; i < count; i++) { ns[i] = allNs[_ia[i]]; } } if (_octree.m_is != null) { var allIs = _octree.m_is; js = new int[count]; for (var i = 0; i < count; i++) { js[i] = allIs[_ia[i]]; } } if (_octree.m_ks != null) { var allKs = _octree.m_ks; ks = new byte[count]; for (var i = 0; i < count; i++) { ks[i] = allKs[_ia[i]]; } } } Guid?psId = ps != null ? (Guid?)Guid.NewGuid() : null; Guid?csId = cs != null ? (Guid?)Guid.NewGuid() : null; Guid?nsId = ns != null ? (Guid?)Guid.NewGuid() : null; Guid?isId = js != null ? (Guid?)Guid.NewGuid() : null; Guid?ksId = ks != null ? (Guid?)Guid.NewGuid() : null; var subcells = _subnodes?.Map(x => x?.ToPointSetCell(storage, isTemporaryImportNode)); var subcellIds = subcells?.Map(x => x?.Id); var isLeaf = _subnodes == null; #if DEBUG if (_subnodes != null) { if (ps != null) { throw new InvalidOperationException("Invariant d98ea55b-760c-4564-8076-ce9cf7d293a0."); } for (var i = 0; i < 8; i++) { var sn = _subnodes[i]; if (sn == null) { continue; } if (sn._cell.Exponent != this._cell.Exponent - 1) { throw new InvalidOperationException("Invariant 2c33afb4-683b-4f71-9e1f-36ec4a79fba1."); } } } #endif var pointCountTreeLeafs = subcells != null ? subcells.Sum(n => n != null?n.PointCountTree : 0) : ps.Length ; var data = ImmutableDictionary <Durable.Def, object> .Empty .Add(Durable.Octree.NodeId, Guid.NewGuid()) .Add(Durable.Octree.Cell, _cell) .Add(Durable.Octree.PointCountTreeLeafs, pointCountTreeLeafs) ; if (isTemporaryImportNode) { data = data.Add(PointSetNode.TemporaryImportNode, 0); } if (psId != null) { storage.Add(psId.ToString(), ps); var bbExactLocal = new Box3f(ps); data = data .Add(Durable.Octree.PointCountCell, ps.Length) .Add(Durable.Octree.PositionsLocal3fReference, psId.Value) .Add(Durable.Octree.BoundingBoxExactLocal, bbExactLocal) ; if (isLeaf) { var bbExactGlobal = (Box3d)bbExactLocal + center; data = data .Add(Durable.Octree.BoundingBoxExactGlobal, bbExactGlobal) ; } } else { data = data .Add(Durable.Octree.PointCountCell, 0) ; } if (csId != null) { storage.Add(csId.ToString(), cs); data = data.Add(Durable.Octree.Colors4bReference, csId.Value); } if (nsId != null) { storage.Add(nsId.ToString(), ns); data = data.Add(Durable.Octree.Normals3fReference, nsId.Value); } if (isId != null) { storage.Add(isId.ToString(), js); data = data.Add(Durable.Octree.Intensities1iReference, isId.Value); } if (ksId != null) { storage.Add(ksId.ToString(), ks); data = data.Add(Durable.Octree.Classifications1bReference, ksId.Value); } if (isLeaf) // leaf { var result = new PointSetNode(data, storage, writeToStore: true); if (storage.GetPointCloudNode(result.Id) == null) { throw new InvalidOperationException("Invariant d1022027-2dbf-4b11-9b40-4829436f5789."); } return(result); } else { for (var i = 0; i < 8; i++) { var x = subcellIds[i]; if (x.HasValue) { var id = x.Value; if (storage.GetPointCloudNode(id) == null) { throw new InvalidOperationException("Invariant 01830b8b-3c0e-4a8b-a1bd-bfd1b1be1844."); } } } var bbExactGlobal = new Box3d(subcells.Where(x => x != null).Select(x => x.BoundingBoxExactGlobal)); data = data .Add(Durable.Octree.BoundingBoxExactGlobal, bbExactGlobal) .Add(Durable.Octree.SubnodesGuids, subcellIds.Map(x => x ?? Guid.Empty)) ; var result = new PointSetNode(data, storage, writeToStore: true); if (storage.GetPointCloudNode(result.Id) == null) { throw new InvalidOperationException("Invariant 7b09eccb-b6a0-4b99-be7a-eeff53b6a98b."); } return(result); } }
private static PointSetNode MergeLeafAndLeafWithIdenticalRootCell(PointSetNode a, PointSetNode b, long octreeSplitLimit, CancellationToken ct) { if (a.IsNotLeaf || b.IsNotLeaf) { throw new InvalidOperationException(); } if (a.Cell != b.Cell) { throw new InvalidOperationException(); } if (b.PositionsAbsolute == null) { throw new InvalidOperationException(); } if (a.HasColors != b.HasColors) { throw new InvalidOperationException(); } if (a.HasNormals != b.HasNormals) { throw new InvalidOperationException(); } if (a.HasIntensities != b.HasIntensities) { throw new InvalidOperationException(); } var ps = Concat(a.PositionsAbsolute, b.PositionsAbsolute); var cs = Concat(a.Colors?.Value, b.Colors?.Value); var ns = Concat(a.Normals?.Value, b.Normals?.Value); var js = Concat(a.Intensities?.Value, b.Intensities?.Value); var result = InMemoryPointSet.Build(ps, cs, ns, js, a.Cell, octreeSplitLimit).ToPointSetCell(a.Storage, ct: ct); return(result); }
/// <summary> /// </summary> /// <param name="node"></param> /// <param name="isNodeFullyInside"></param> /// <param name="isNodeFullyOutside"></param> /// <param name="isPositionInside"></param> /// <param name="minCellExponent">Limit traversal depth to minCellExponent (inclusive).</param> /// <returns></returns> public static IEnumerable <Chunk> QueryPoints(this PointSetNode node, Func <PointSetNode, bool> isNodeFullyInside, Func <PointSetNode, bool> isNodeFullyOutside, Func <V3d, bool> isPositionInside, int minCellExponent = int.MinValue ) { if (node.Cell.Exponent < minCellExponent) { yield break; } if (isNodeFullyOutside(node)) { yield break; } if (node.IsLeaf || node.Cell.Exponent == minCellExponent) { if (isNodeFullyInside(node)) { if (node.HasPositions) { yield return(new Chunk(node.PositionsAbsolute, node.Colors?.Value, node.Normals?.Value)); } else if (node.HasLodPositions) { yield return(new Chunk(node.LodPositionsAbsolute, node.LodColors?.Value, node.LodNormals?.Value)); } yield break; } else // partially inside { var psRaw = node.HasPositions ? node.PositionsAbsolute : node.LodPositionsAbsolute; var csRaw = node.HasColors ? node.Colors?.Value : node.LodColors?.Value; var nsRaw = node.HasNormals ? node.Normals?.Value : node.LodNormals?.Value; var ps = new List <V3d>(); var cs = csRaw != null ? new List <C4b>() : null; var ns = nsRaw != null ? new List <V3f>() : null; for (var i = 0; i < psRaw.Length; i++) { var p = psRaw[i]; if (isPositionInside(p)) { ps.Add(p); if (csRaw != null) { cs.Add(csRaw[i]); } if (nsRaw != null) { ns.Add(nsRaw[i]); } } } if (ps.Count > 0) { yield return(new Chunk(ps, cs, ns)); } } } else { for (var i = 0; i < 8; i++) { var n = node.Subnodes[i]; if (n == null) { continue; } var xs = QueryPoints(n.Value, isNodeFullyInside, isNodeFullyOutside, isPositionInside, minCellExponent); foreach (var x in xs) { yield return(x); } } } }
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); }
internal static async Task <IPointCloudNode> GenerateLod(this PointSetNode self, int octreeSplitLimit, Action callback, CancellationToken ct) { if (self == null) { throw new ArgumentNullException(nameof(self)); } ct.ThrowIfCancellationRequested(); try { var originalId = self.Id; if (self.IsLeaf) { if (!self.HasKdTree) { var kd = await self.Positions.Value.BuildKdTreeAsync(); var kdKey = Guid.NewGuid(); self.Storage.Add(kdKey, kd.Data); self = self .WithUpsert(Durable.Octree.PointRkdTreeFDataReference, kdKey) ; } if (!self.HasNormals) { var ns = await self.Positions.Value.EstimateNormalsAsync(16, self.KdTree.Value); var nsId = Guid.NewGuid(); self.Storage.Add(nsId, ns); self = self .WithUpsert(Durable.Octree.Normals3fReference, nsId) ; } self = self.Without(PointSetNode.TemporaryImportNode); if (self.Id != originalId) { self = self.WriteToStore(); } return(self); } if (self.Subnodes == null || self.Subnodes.Length != 8) { throw new InvalidOperationException(); } var subcellsAsync = self.Subnodes.Map(x => (x?.Value as PointSetNode)?.GenerateLod(octreeSplitLimit, callback, ct)); await Task.WhenAll(subcellsAsync.Where(x => x != null)); var subcells = subcellsAsync.Map(x => x?.Result); var subcellsTotalCount = (long)subcells.Sum(x => x?.PointCountTree); var fractions = ComputeLodFractions(subcells); var counts = ComputeLodCounts(octreeSplitLimit, fractions); // generate LoD data ... var needsCs = subcells.Any(x => x != null ? x.HasColors : false); var needsNs = subcells.Any(x => x != null ? x.HasNormals : false); var needsIs = subcells.Any(x => x != null ? x.HasIntensities : false); var needsKs = subcells.Any(x => x != null ? x.HasClassifications : false); var subcenters = subcells.Map(x => x?.Center); var lodPs = AggregateSubPositions(counts, octreeSplitLimit, self.Center, subcenters, subcells.Map(x => x?.Positions?.Value)); var lodCs = needsCs ? AggregateSubArrays(counts, octreeSplitLimit, subcells.Map(x => x?.Colors?.Value)) : null; var lodIs = needsIs ? AggregateSubArrays(counts, octreeSplitLimit, subcells.Map(x => x?.Intensities?.Value)) : null; var lodKs = needsKs ? AggregateSubArrays(counts, octreeSplitLimit, subcells.Map(x => x?.Classifications?.Value)) : null; var lodKd = await lodPs.BuildKdTreeAsync(); var lodNs = await lodPs.EstimateNormalsAsync(16, lodKd); // Lod.AggregateSubArrays(counts, octreeSplitLimit, subcells.Map(x => x?.GetNormals3f()?.Value)) var subnodeIds = subcells.Map(x => x != null ? x.Id : Guid.Empty); self = self.WithUpsert(Durable.Octree.SubnodesGuids, subnodeIds); // store LoD data ... var lodPsKey = Guid.NewGuid(); self.Storage.Add(lodPsKey, lodPs); var lodKdKey = Guid.NewGuid(); self.Storage.Add(lodKdKey, lodKd.Data); self = self .WithUpsert(Durable.Octree.PointCountCell, lodPs.Length) .WithUpsert(Durable.Octree.PointRkdTreeFDataReference, lodKdKey) .WithUpsert(Durable.Octree.PositionsLocal3fReference, lodPsKey) ; if (needsCs) { var key = Guid.NewGuid(); self.Storage.Add(key, lodCs); self = self.WithUpsert(Durable.Octree.Colors4bReference, key); } if (needsNs) { var key = Guid.NewGuid(); self.Storage.Add(key, lodNs); self = self.WithUpsert(Durable.Octree.Normals3fReference, key); } if (needsIs) { var key = Guid.NewGuid(); self.Storage.Add(key, lodIs); self = self.WithUpsert(Durable.Octree.Intensities1iReference, key); } if (needsKs) { var key = Guid.NewGuid(); self.Storage.Add(key, lodKs); self = self.WithUpsert(Durable.Octree.Classifications1bReference, key); } self = self.Without(PointSetNode.TemporaryImportNode); if (self.Id != originalId) { self = self.WriteToStore(); } return(self); } finally { callback?.Invoke(); } }
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> /// Counts points outside convex hull. /// </summary> internal static long CountPointsOutsideConvexHull( this PointSetNode self, Hull3d query, int minCellExponent = int.MinValue ) => CountPointsInsideConvexHull(self, query.Reversed(), minCellExponent);
/// <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); } }