/// <summary>
/// Counts points outside convex hull (approximately).
/// Result is always equal or greater than exact number.
/// </summary>
internal static long CountPointsApproximatelyOutsideConvexHull(
this IPointCloudNode self, Hull3d query, int minCellExponent = int.MinValue
)
=> CountPointsApproximately(self,
n => !query.Intersects(n.BoundingBoxExactGlobal),
n => query.Contains(n.BoundingBoxExactGlobal),
minCellExponent);
/// <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>
/// Counts points approximately inside axis-aligned box (cell granularity).
/// Result is always equal or greater than exact number.
/// Faster than CountPointsInsideBox.
/// </summary>
public static long CountPointsApproximatelyInsideBox(
this IPointCloudNode self, Box3d query, int minCellExponent = int.MinValue
)
=> CountPointsApproximately(self,
n => query.Contains(n.BoundingBoxExactGlobal),
n => !query.Intersects(n.BoundingBoxExactGlobal),
minCellExponent);
/// <summary>
/// </summary>
public static IEnumerable <IPointCloudNode> ForEachNode(
this IPointCloudNode self, int minCellExponent = int.MinValue
)
{
if (self == null)
{
yield break;
}
if (self.Cell.Exponent < minCellExponent)
{
yield break;
}
yield return(self);
if (self.Subnodes != null)
{
for (var i = 0; i < 8; i++)
{
var n = self.Subnodes[i];
if (n == null)
{
continue;
}
foreach (var x in ForEachNode(n.Value, minCellExponent))
{
yield return(x);
}
}
}
}
/// <summary>
/// Calls action for each node in this tree.
/// </summary>
public static void ForEachNode(
this IPointCloudNode self, bool outOfCore, Action <IPointCloudNode> action)
{
action(self);
if (self.Subnodes == null)
{
return;
}
if (outOfCore)
{
for (var i = 0; i < 8; i++)
{
self.Subnodes[i]?.Value.ForEachNode(outOfCore, action);
}
}
else
{
for (var i = 0; i < 8; i++)
{
var n = self.Subnodes[i];
if (n != null)
{
if (n.TryGetValue(out IPointCloudNode node))
{
node.ForEachNode(outOfCore, action);
}
}
}
}
}
/// <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);
}
}
public static void ValidateTree(this IPointCloudNode node, int splitLimit, bool hasLod = true)
{
if (node != null)
{
Assert.IsTrue(node.PointCountTree > 0);
Assert.IsTrue(node.HasBoundingBoxExactGlobal || node.HasBoundingBoxExactLocal);
if (node.IsLeaf || hasLod)
{
Assert.IsTrue(node.PointCountCell > 0);
Assert.IsTrue(node.HasPositions);
var ps = node.PositionsAbsolute;
var realBb = node.BoundingBoxExactGlobal;
var bb = realBb.EnlargedByRelativeEps(0.01);
Assert.IsTrue(ps.All(p => bb.Contains(p)));
Assert.IsTrue(ps.Length <= 1 || node.HasKdTree);
if (node.HasNormals)
{
Assert.IsTrue(node.Normals.Value.Length == ps.Length);
}
if (node.HasColors)
{
Assert.IsTrue(node.Colors.Value.Length == ps.Length);
}
if (node.HasIntensities)
{
Assert.IsTrue(node.Intensities.Value.Length == ps.Length);
}
if (node.HasClassifications)
{
Assert.IsTrue(node.Classifications.Value.Length == ps.Length);
}
}
if (node.IsLeaf)
{
var ps = node.PositionsAbsolute;
Assert.IsTrue(node.PointCountCell == node.PointCountTree);
Assert.IsTrue(ps.Length == node.PointCountTree);
Assert.IsTrue(ps.Length <= splitLimit);
}
else
{
var realBb = node.BoundingBoxExactGlobal;
var bb = realBb.EnlargedByRelativeEps(0.01);
Assert.IsTrue(node.PointCountTree > splitLimit);
var nodes = node.Subnodes.Map(n => n?.Value);
var nodeBB = new Box3d(nodes.Select(n => n != null ? n.BoundingBoxExactGlobal : Box3d.Invalid));
Assert.IsTrue(realBb == nodeBB);
Assert.IsTrue(node.PointCountTree == nodes.Sum(n => n != null ? n.PointCountTree : 0));
foreach (var n in nodes)
{
ValidateTree(n, splitLimit, hasLod);
}
}
}
}
/// <summary>
/// Calls action for each (node, fullyInside) in this tree that is intersecting the given hull.
/// </summary>
public static IEnumerable <CellQueryResult> ForEachNodeIntersecting(
this IPointCloudNode self,
Hull3d hull, bool doNotTraverseSubnodesWhenFullyInside, int minCellExponent = int.MinValue
)
{
if (self == null)
{
yield break;
}
if (self.Cell.Exponent < minCellExponent)
{
yield break;
}
for (var i = 0; i < hull.PlaneCount; i++)
{
if (!self.IntersectsNegativeHalfSpace(hull.PlaneArray[i]))
{
yield break;
}
}
bool fullyInside = true;
for (var i = 0; i < hull.PlaneCount; i++)
{
if (!self.InsideNegativeHalfSpace(hull.PlaneArray[i]))
{
fullyInside = false;
break;
}
}
yield return(new CellQueryResult(self, fullyInside));
if (fullyInside && doNotTraverseSubnodesWhenFullyInside)
{
yield break;
}
if (self.Subnodes == null)
{
yield break;
}
for (var i = 0; i < 8; i++)
{
var n = self.Subnodes[i];
if (n == null)
{
continue;
}
var xs = ForEachNodeIntersecting(n.Value, hull, doNotTraverseSubnodesWhenFullyInside, minCellExponent);
foreach (var x in xs)
{
yield return(x);
}
}
}
/// <summary>
/// Count points approximately NOT within maxDistance of ALL the given planes.
/// Result is always equal or greater than exact number.
/// Faster than CountPointsNotNearPlanes.
/// </summary>
public static long CountPointsApproximatelyNotNearPlanes(
this IPointCloudNode node, Plane3d[] planes, double maxDistance, int minCellExponent = int.MinValue
)
=> CountPointsApproximately(node,
n => !planes.Any(plane => node.BoundingBoxExactGlobal.Intersects(plane, maxDistance)),
n => planes.Any(plane => plane.Contains(maxDistance, node.BoundingBoxExactGlobal)),
minCellExponent
);
/// <summary>
/// All points inside convex hull (including boundary).
/// </summary>
public static IEnumerable <Chunk> QueryPointsInsideConvexHull(
this IPointCloudNode self, Hull3d query, int minCellExponent = int.MinValue
)
=> QueryPoints(self,
n => query.Contains(n.BoundingBoxExactGlobal),
n => !query.Intersects(n.BoundingBoxExactGlobal),
p => query.Contains(p),
minCellExponent);
/// <summary>
/// Count points within maxDistance of ANY of the given planes.
/// </summary>
public static long CountPointsNearPlanes(
this IPointCloudNode node, Plane3d[] planes, double maxDistance, int minCellExponent = int.MinValue
)
=> CountPoints(node,
n => planes.Any(plane => plane.Contains(maxDistance, node.BoundingBoxExactGlobal)),
n => !planes.Any(plane => node.BoundingBoxExactGlobal.Intersects(plane, maxDistance)),
p => planes.Any(plane => Math.Abs(plane.Height(p)) <= maxDistance),
minCellExponent
);
/// <summary>
/// All points NOT within maxDistance of given plane.
/// </summary>
public static IEnumerable <Chunk> QueryPointsNotNearPlane(
this IPointCloudNode node, Plane3d plane, double maxDistance, int minCellExponent = int.MinValue
)
=> QueryPoints(node,
n => !node.BoundingBoxExactGlobal.Intersects(plane, maxDistance),
n => plane.Contains(maxDistance, node.BoundingBoxExactGlobal),
p => Math.Abs(plane.Height(p)) > maxDistance,
minCellExponent
);
/// <summary>
/// </summary>
public static IPointCloudNode Create(Guid id, IPointCloudNode node, IFilter filter)
{
if (node.IsTemporaryImportNode)
{
throw new InvalidOperationException(
"FilteredNode cannot be created from temporary import node. Invariant b9c2dca3-1510-4ea7-959f-6a0737c707fa."
);
}
return(new FilteredNode(id, true, node, filter));
}
/// <summary></summary>
public HashSet <int> FilterPoints(IPointCloudNode node, HashSet <int> selected = null)
{
var a = Left.FilterPoints(node, selected);
var b = Right.FilterPoints(node, selected);
if (selected != null && a.Count == selected.Count && b.Count == selected.Count)
{
return(selected);
}
a.IntersectWith(b);
return(a);
}
/// <summary>
/// Max tree depth.
/// </summary>
public static int CountOctreeLevels(this IPointCloudNode root)
{
if (root == null)
{
return(0);
}
if (root.Subnodes == null)
{
return(1);
}
return(root.Subnodes.Select(n => CountOctreeLevels(n?.Value)).Max() + 1);
}
/// <summary>
/// Returns points in given cell,
/// or null if octree does not cover given cell.
/// Result chunk contains 0 points, if cell is covered by octree, but no points are inside given cell.
/// </summary>
public static CellQueryResult QueryCell(this IPointCloudNode root, Cell cell)
{
if (root == null)
{
throw new ArgumentNullException(nameof(root));
}
if (!root.Cell.Contains(cell))
{
throw new InvalidOperationException(
$"Octree ({root.Cell}) must contain requested cell ({cell}). " +
"Invariant 4d67081e-37de-48d4-87df-5f5022f9051f."
);
}
return(QueryCellRecursive(root));
CellQueryResult QueryCellRecursive(IPointCloudNode n)
{
if (n.Cell == cell)
{
// found!
return(new CellQueryResult(root, cell, n));
}
else
{
// continue search in subnode ...
var octant = n.Cell.GetOctant(cell);
if (octant.HasValue)
{
var subNodeRef = n.Subnodes[octant.Value];
if (subNodeRef != null)
{
return(QueryCellRecursive(subNodeRef.Value));
}
else
{
// we can't go deeper
return(new CellQueryResult(root, cell, n));
}
}
else
{
throw new InvalidOperationException(
$"Node ({n.Cell}) must contain requested cell ({cell}). " +
"Invariant fbde10dd-1c07-41c8-9fc0-8a98b74d3948."
);
}
}
}
}
/// <summary>
/// Count points approximately NOT within maxDistance of ALL the given polygons.
/// Result is always equal or greater than exact number.
/// Faster than CountPointsNotNearPolygons.
/// </summary>
public static long CountPointsApproximatelyNotNearPolygons(
this IPointCloudNode node, Polygon3d[] polygons, double maxDistance, int minCellExponent = int.MinValue
)
{
var bounds = polygons.Map(x => x.BoundingBox3d(maxDistance));
var planes = polygons.Map(x => x.GetPlane3d());
var w2p = planes.Map(x => x.GetWorldToPlane());
var poly2d = polygons.Map((x, i) => new Polygon2d(x.GetPointArray().Map(p => w2p[i].TransformPos(p).XY)));
return(CountPointsApproximately(node,
n => !bounds.Any(b => n.BoundingBoxExactGlobal.Intersects(b)),
n => false,
minCellExponent
));
}
/// <summary>
/// Count points approximately NOT within maxDistance of given polygon.
/// Result is always equal or greater than exact number.
/// Faster than CountPointsNotNearPolygon.
/// </summary>
public static long CountPointsApproximatelyNotNearPolygon(
this IPointCloudNode node, Polygon3d polygon, double maxDistance, int minCellExponent = int.MinValue
)
{
var bounds = polygon.BoundingBox3d(maxDistance);
var plane = polygon.GetPlane3d();
var w2p = plane.GetWorldToPlane();
var poly2d = new Polygon2d(polygon.GetPointArray().Map(p => w2p.TransformPos(p).XY));
return(CountPointsApproximately(node,
n => !n.BoundingBoxExactGlobal.Intersects(bounds),
n => false,
minCellExponent
));
}
/// <summary>
/// Creates pointset from given root cell.
/// </summary>
public PointSet(Storage storage, string key, IPointCloudNode root, int splitLimit)
{
if (root == null)
{
throw new ArgumentNullException(nameof(root));
}
Storage = storage;
Id = key ?? throw new ArgumentNullException(nameof(key));
SplitLimit = splitLimit;
if (key != null)
{
Root = new PersistentRef <IPointCloudNode>(root.Id.ToString(), storage.GetPointCloudNode, storage.TryGetPointCloudNode);
}
}
/// <summary>
/// All points NOT within maxDistance of given polygon.
/// </summary>
public static IEnumerable <Chunk> QueryPointsNotNearPolygon(
this IPointCloudNode node, Polygon3d polygon, double maxDistance, int minCellExponent = int.MinValue
)
{
var bounds = polygon.BoundingBox3d(maxDistance);
var plane = polygon.GetPlane3d();
var w2p = plane.GetWorldToPlane();
var poly2d = new Polygon2d(polygon.GetPointArray().Map(p => w2p.TransformPos(p).XY));
return(QueryPoints(node,
n => !n.BoundingBoxExactGlobal.Intersects(bounds),
n => false,
p => !polygon.Contains(plane, w2p, poly2d, maxDistance, p, out double d),
minCellExponent
));
}
/// <summary>
/// All points within maxDistance of ANY of the given polygons.
/// </summary>
public static IEnumerable <Chunk> QueryPointsNearPolygons(
this IPointCloudNode node, Polygon3d[] polygons, double maxDistance, int minCellExponent = int.MinValue
)
{
var bounds = polygons.Map(x => x.BoundingBox3d(maxDistance));
var planes = polygons.Map(x => x.GetPlane3d());
var w2p = planes.Map(x => x.GetWorldToPlane());
var poly2d = polygons.Map((x, i) => new Polygon2d(x.GetPointArray().Map(p => w2p[i].TransformPos(p).XY)));
return(QueryPoints(node,
n => false,
n => !bounds.Any(b => n.BoundingBoxExactGlobal.Intersects(b)),
p => planes.Any((plane, i) => polygons[i].Contains(plane, w2p[i], poly2d[i], maxDistance, p, out double d)),
minCellExponent
));
}
/// <summary>
/// Finds deepest octree level which still contains less than given number of points within given bounds.
/// </summary>
public static int GetMaxOctreeLevelWithLessThanGivenPointCount(
this IPointCloudNode node, long maxPointCount, Box3d bounds
)
{
var imax = node.CountOctreeLevels();
for (var i = 0; i < imax; i++)
{
var count = node.CountPointsInOctreeLevel(i, bounds);
if (count >= maxPointCount)
{
return(i - 1);
}
}
return(imax - 1);
}
/// <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 bool IsFullyOutside(IPointCloudNode node)
{
var xs = GetValues(node);
if (xs == null)
{
return(false);
}
for (var i = 0; i < xs.Length; i++)
{
if (Filter.Contains(xs[i]))
{
return(false);
}
}
return(true);
}
/// <summary>
/// Represents a cell 'resultCell' inside an octree ('root'),
/// where 'resultNode' is root's smallest subnode (incl. root) containing 'resultCell'.
/// </summary>
public CellQueryResult(IPointCloudNode root, Cell resultCell, IPointCloudNode resultNode)
{
Root = root ?? throw new ArgumentNullException(nameof(root));
Cell = resultCell;
m_result = resultNode;
if (!root.Cell.Contains(resultNode.Cell))
{
throw new Exception(
$"Root node {root.Cell} must contain resultNode {resultNode.Cell}. Invariant fb8dc278-fa35-4022-8aa8-281855dd41af."
);
}
if (resultNode != null && !resultNode.Cell.Contains(resultCell))
{
throw new Exception(
$"Result node {resultNode.Cell} must contain resultCell {resultCell}. Invariant 62bff5cc-61b1-4cec-a9f8-b2e1136c19d1."
);
}
}
/// <summary></summary>
public HashSet <int> FilterPoints(IPointCloudNode node, HashSet <int> selected = null)
{
var xs = GetValues(node);
if (selected != null)
{
return(new HashSet <int>(selected.Where(i => V3f.Cross(Direction, xs[i].Normalized).LengthSquared <= m_eps)));
}
else
{
var result = new HashSet <int>();
for (var i = 0; i < xs.Length; i++)
{
if (V3f.Cross(Direction, xs[i].Normalized).LengthSquared <= m_eps)
{
result.Add(i);
}
}
return(result);
}
}
/// <summary></summary>
public HashSet <int> FilterPoints(IPointCloudNode node, HashSet <int> selected = null)
{
var xs = GetValues(node);
if (selected != null)
{
return(new HashSet <int>(selected.Where(i => Range.Contains(xs[i]))));
}
else
{
var result = new HashSet <int>();
for (var i = 0; i < xs.Length; i++)
{
if (Range.Contains(xs[i]))
{
result.Add(i);
}
}
return(result);
}
}
/// <summary></summary>
public HashSet <int> FilterPoints(IPointCloudNode node, HashSet <int> selected = null)
{
var c = node.Center;
var xs = node.Positions.Value;
if (selected != null)
{
return(new HashSet <int>(selected.Where(i => Box.Contains(c + (V3d)xs[i]))));
}
else
{
var result = new HashSet <int>();
for (var i = 0; i < xs.Length; i++)
{
if (Box.Contains(c + (V3d)xs[i]))
{
result.Add(i);
}
}
return(result);
}
}
/// <summary></summary>
private FilteredNode(Guid id, bool writeToStore, IPointCloudNode node, IFilter filter)
{
Id = id;
Node = node ?? throw new ArgumentNullException(nameof(node));
Filter = filter ?? throw new ArgumentNullException(nameof(filter));
if (filter.IsFullyInside(node))
{
m_activePoints = null;
}
else if (filter.IsFullyOutside(node))
{
m_activePoints = new HashSet <int>();
}
else
{
m_activePoints = Filter.FilterPoints(node, m_activePoints);
}
if (writeToStore)
{
WriteToStore();
//Console.WriteLine("Written to store: {0}", id);
}
}
