/// <summary>
/// Points within given distance of a line segment (at most 1000).
/// </summary>
public static IEnumerable <PointsNearObject <Line3d> > QueryPointsNearLineSegment(
this PointSetNode node, Line3d lineSegment, double maxDistanceToRay
)
{
if (!node.BoundingBox.Intersects(lineSegment))
{
yield break;
}
else if (node.PointCount > 0)
{
var center = node.Center;
var ia = node.KdTree.Value.GetClosestToLine((V3f)(lineSegment.P0 - center), (V3f)(lineSegment.P1 - center), (float)maxDistanceToRay, 1000);
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];
}
ds[i] = ia[i].Dist;
}
var chunk = new PointsNearObject <Line3d>(lineSegment, maxDistanceToRay, ps, cs, ns, js, ds);
yield return(chunk);
}
}
else if (node.Subnodes != null)
{
for (var i = 0; i < 8; i++)
{
var n = node.Subnodes[i];
if (n == null)
{
continue;
}
foreach (var x in QueryPointsNearLineSegment(n.Value, lineSegment, maxDistanceToRay))
{
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);
}
}
