/// <summary>
/// Gets approximate number of points at given octree level within given bounds.
/// For cells that only partially overlap the specified bounds all points are counted anyway.
/// For performance reasons, in order to avoid per-point bounds checks.
/// </summary>
public static long CountPointsInOctreeLevel(
this IPointCloudNode node, int level, Box3d bounds
)
{
if (level < 0)
{
return(0);
}
if (!node.BoundingBoxExactGlobal.Intersects(bounds))
{
return(0);
}
if (level == 0 || node.IsLeaf())
{
return(node.Positions.Value.Length);
}
else
{
var nextLevel = level - 1;
var sum = 0L;
for (var i = 0; i < 8; i++)
{
var n = node.Subnodes[i];
if (n == null)
{
continue;
}
sum += CountPointsInOctreeLevel(n.Value, nextLevel, bounds);
}
return(sum);
}
}
/// <summary>
/// Gets total number of lod-points in all cells at given octree level.
/// </summary>
public static long CountPointsInOctreeLevel(
this IPointCloudNode node, int level
)
{
if (level < 0)
{
return(0);
}
if (level == 0 || node.IsLeaf())
{
return(node.Positions.Value.Count());
}
else
{
var nextLevel = level - 1;
var sum = 0L;
for (var i = 0; i < 8; i++)
{
var n = node.Subnodes[i];
if (n == null)
{
continue;
}
sum += CountPointsInOctreeLevel(n.Value, nextLevel);
}
return(sum);
}
}
/// <summary>
/// Returns lod points for given octree depth/front, where level 0 is the root node.
/// Front will include leafs higher up than given level.
/// </summary>
public static IEnumerable <Chunk> QueryPointsInOctreeLevel(
this IPointCloudNode node, int level
)
{
if (level < 0)
{
yield break;
}
if (level == 0 || node.IsLeaf())
{
var ps = node.PositionsAbsolute;
var cs = node?.TryGetColors4b()?.Value;
if (ps != null && cs != null && ps.Length != cs.Length)
{
cs = new C4b[ps.Length];
Report.Warn("[Chunk] inconsistent length: pos.length = {0} vs cs.length = {1}", ps.Length, cs.Length);
}
var ns = node?.TryGetNormals3f()?.Value;
var js = node?.TryGetIntensities()?.Value;
var ks = node?.TryGetClassifications()?.Value;
var chunk = new Chunk(ps, cs, ns, js, ks);
yield return(chunk);
}
else
{
if (node.Subnodes == null)
{
yield break;
}
for (var i = 0; i < 8; i++)
{
var n = node.Subnodes[i];
if (n == null)
{
continue;
}
foreach (var x in QueryPointsInOctreeLevel(n.Value, level - 1))
{
yield return(x);
}
}
}
}
/// <summary>
/// Returns lod points for given octree depth/front of cells intersecting given bounds, where level 0 is the root node.
/// Front will include leafs higher up than given level.
/// </summary>
public static IEnumerable <Chunk> QueryPointsInOctreeLevel(
this IPointCloudNode node, int level, Box3d bounds
)
{
if (level < 0)
{
yield break;
}
if (!node.BoundingBoxExactGlobal.Intersects(bounds))
{
yield break;
}
if (level == 0 || node.IsLeaf())
{
var ps = node.PositionsAbsolute;
var cs = node?.TryGetColors4b()?.Value;
var ns = node?.TryGetNormals3f()?.Value;
var js = node?.TryGetIntensities()?.Value;
var ks = node?.TryGetClassifications()?.Value;
var chunk = new Chunk(ps, cs, ns, js, ks);
yield return(chunk);
}
else
{
if (node.Subnodes == null)
{
yield break;
}
for (var i = 0; i < 8; i++)
{
var n = node.Subnodes[i];
if (n == null)
{
continue;
}
foreach (var x in QueryPointsInOctreeLevel(n.Value, level - 1, bounds))
{
yield return(x);
}
}
}
}
/// <summary>
/// </summary>
public static IEnumerable <Chunk> QueryPoints(this IPointCloudNode node,
Func <IPointCloudNode, bool> isNodeFullyInside,
Func <IPointCloudNode, 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))
{
yield return(node.ToChunk());
}
else // partially inside
{
var psRaw = node.PositionsAbsolute;
var csRaw = node.HasColors ? node.Colors.Value : null;
var nsRaw = node.HasNormals ? node.Normals.Value : null;
var jsRaw = node.HasIntensities ? node.Intensities.Value : null;
var ksRaw = node.HasClassifications ? node.Classifications.Value : null;
var ps = new List <V3d>();
var cs = csRaw != null ? new List <C4b>() : null;
var ns = nsRaw != null ? new List <V3f>() : null;
var js = jsRaw != null ? new List <int>() : null;
var ks = ksRaw != null ? new List <byte>() : 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 (jsRaw != null)
{
js.Add(jsRaw[i]);
}
if (ksRaw != null)
{
ks.Add(ksRaw[i]);
}
}
}
if (ps.Count > 0)
{
yield return(new Chunk(ps, cs, ns, js, ks));
}
}
}
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);
}
}
}
}
/// <summary>
/// Points within given distance of a point.
/// </summary>
public static PointsNearObject <V3d> QueryPointsNearPoint(
this IPointCloudNode 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.BoundingBoxExactGlobal.Distance(query);
if (eps > maxDistanceToPoint)
{
return(PointsNearObject <V3d> .Empty);
}
if (node.IsLeaf())
{
var nodePositions = node.Positions;
#if PARANOID
if (nodePositions.Value.Length <= 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 ks = node.HasClassifications ? new byte[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];
}
if (node.HasClassifications)
{
ks[i] = node.Classifications.Value[index];
}
ds[i] = ia[i].Dist;
}
var chunk = new PointsNearObject <V3d>(query, maxDistanceToPoint, ps, cs, ns, js, ks, 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);
}
}
