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