/// <summary>
/// Constructs rkd-tree from points and kd-tree data.
/// </summary>
public static PointRkdTreeF <V3f[], V3f> ToKdTree(this V3f[] points, PointRkdTreeFData data)
=> new PointRkdTreeF <V3f[], V3f>(
3, points.Length, points,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, 1e-6f,
data
);
/// <summary>
/// Creates point rkd-tree.
/// </summary>
public static PointRkdTreeD <V3f[], V3f> BuildKdTree(this V3f[] self, double kdTreeEps = 1e-6)
{
return(new PointRkdTreeD <V3f[], V3f>(
3, self.Length, self,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, kdTreeEps
));
}
/// <summary>
/// Estimates a normal vector for each point by least-squares-fitting a plane through its k nearest neighbours.
/// </summary>
public static V3f[] EstimateNormals(this V3f[] points, int k)
{
var kd = new PointRkdTreeD <V3f[], V3f>(
3, points.Length, points,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, 0
);
return(EstimateNormals(points, kd, k));
}
/// <summary>
/// Computes rkd-tree from given points.
/// </summary>
public static PointRkdTreeF <V3f[], V3f> BuildKdTree(this V3f[] points, float kdTreeEps = 1e-6f)
{
if (points == null)
{
throw new ArgumentNullException(nameof(points));
}
if (points.Length == 0)
{
return(points.ToKdTree(new PointRkdTreeFData()));
}
return(new PointRkdTreeF <V3f[], V3f>(
3, points.Length, points,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, kdTreeEps
));
}
public PointSetNode ToPointSetCell(Storage storage, CancellationToken ct, double kdTreeEps = 1e-6)
{
var center = new V3d(_centerX, _centerY, _centerZ);
V3f[] ps = null;
C4b[] cs = null;
V3f[] ns = null;
int[] js = null;
PointRkdTreeD <V3f[], V3f> kdTree = 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]];
}
}
kdTree = new PointRkdTreeD <V3f[], V3f>(
3, ps.Length, ps,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, kdTreeEps
);
}
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?kdId = kdTree != null ? (Guid?)Guid.NewGuid() : null;
var subcells = _subnodes?.Map(x => x?.ToPointSetCell(storage, ct, kdTreeEps));
var subcellIds = subcells?.Map(x => x?.Id);
#if DEBUG
if (ps != null && _subnodes != null)
{
throw new InvalidOperationException();
}
#endif
var pointCountTree = ps != null
? ps.Length
: subcells.Sum(n => n != null ? n.PointCountTree : 0)
;
if (psId != null)
{
storage.Add(psId.ToString(), ps, ct);
}
if (csId != null)
{
storage.Add(csId.ToString(), cs, ct);
}
if (nsId != null)
{
storage.Add(nsId.ToString(), ns, ct);
}
if (isId != null)
{
storage.Add(isId.ToString(), js, ct);
}
if (kdId != null)
{
storage.Add(kdId.ToString(), kdTree.Data, ct);
}
if (subcellIds == null) // leaf
{
return(new PointSetNode(_cell, pointCountTree, psId, csId, kdId, nsId, isId, storage));
}
else
{
return(new PointSetNode(_cell, pointCountTree, subcellIds, storage));
}
}
private PointSetNode(Guid id,
Cell cell, long pointCountTree,
Guid?psId, Guid?csId, Guid?kdId, Guid?nsId, Guid?isId,
Guid?lodPsId, Guid?lodCsId, Guid?lodKdId, Guid?lodNsId, Guid?lodIsId,
Guid?[] subnodeIds, Storage storage, bool writeToStore
)
{
if (subnodeIds != null && subnodeIds.Count(x => x.HasValue) > 0 && pointCountTree == 0)
{
throw new ArgumentException(nameof(pointCountTree), "Must not be 0 for inner nodes.");
}
Storage = storage;
Id = id;
Cell = cell;
PointCountTree = pointCountTree;
SubnodeIds = subnodeIds;
if (psId.HasValue)
{
Attributes[C_POSITIONS] = psId.Value;
}
if (csId.HasValue)
{
Attributes[C_COLORS] = csId.Value;
}
if (kdId.HasValue)
{
Attributes[C_KDTREE] = kdId.Value;
}
if (nsId.HasValue)
{
Attributes[C_NORMALS] = nsId.Value;
}
if (isId.HasValue)
{
Attributes[C_INTENSITIES] = isId.Value;
}
if (lodPsId.HasValue)
{
Attributes[C_LOD_POSITIONS] = lodPsId.Value;
}
if (lodCsId.HasValue)
{
Attributes[C_LOD_COLORS] = lodCsId.Value;
}
if (lodKdId.HasValue)
{
Attributes[C_LOD_KDTREE] = lodKdId.Value;
}
if (lodNsId.HasValue)
{
Attributes[C_LOD_NORMALS] = lodNsId.Value;
}
if (lodIsId.HasValue)
{
Attributes[C_LOD_INTENSITIES] = lodIsId.Value;
}
if (IsLeaf && PointCount != PointCountTree)
{
throw new InvalidOperationException();
}
if (psId != null)
{
Positions = new PersistentRef <V3f[]>(psId.ToString(), storage.GetV3fArray);
}
if (csId != null)
{
Colors = new PersistentRef <C4b[]>(csId.ToString(), storage.GetC4bArray);
}
if (kdId != null)
{
KdTree = new PersistentRef <PointRkdTreeD <V3f[], V3f> >(kdId.ToString(), LoadKdTree);
}
if (nsId != null)
{
Normals = new PersistentRef <V3f[]>(nsId.ToString(), storage.GetV3fArray);
}
if (isId != null)
{
Intensities = new PersistentRef <int[]>(isId.ToString(), storage.GetIntArray);
}
if (lodPsId != null)
{
LodPositions = new PersistentRef <V3f[]>(lodPsId.ToString(), storage.GetV3fArray);
}
if (lodCsId != null)
{
LodColors = new PersistentRef <C4b[]>(lodCsId.ToString(), storage.GetC4bArray);
}
if (lodKdId != null)
{
LodKdTree = new PersistentRef <PointRkdTreeD <V3f[], V3f> >(lodKdId.ToString(), LoadKdTree);
}
if (lodNsId != null)
{
LodNormals = new PersistentRef <V3f[]>(lodNsId.ToString(), storage.GetV3fArray);
}
if (lodIsId != null)
{
LodIntensities = new PersistentRef <int[]>(lodIsId.ToString(), storage.GetIntArray);
}
if (subnodeIds != null)
{
Subnodes = new PersistentRef <PointSetNode> [8];
for (var i = 0; i < 8; i++)
{
if (subnodeIds[i] == null)
{
continue;
}
var pRef = new PersistentRef <PointSetNode>(subnodeIds[i].ToString(), storage.GetPointSetNode);
Subnodes[i] = pRef;
#if DEBUG && PEDANTIC
var subNodeIndex = pRef.Value.Cell;
if (Cell.GetOctant(i) != subNodeIndex)
{
throw new InvalidOperationException();
}
if (!Cell.Contains(subNodeIndex))
{
throw new InvalidOperationException();
}
if (Cell.Exponent != subNodeIndex.Exponent + 1)
{
throw new InvalidOperationException();
}
#endif
}
#if DEBUG && PEDANTIC
if (PointCountTree != PointCount + Subnodes.Map(x => x?.Value != null ? x.Value.PointCountTree : 0).Sum())
{
throw new InvalidOperationException();
}
#endif
}
BoundingBox = Cell.BoundingBox;
Center = BoundingBox.Center;
Corners = BoundingBox.ComputeCorners();
if (writeToStore)
{
storage.Add(Id.ToString(), this, CancellationToken.None);
}
#if DEBUG
if (PositionsId == null && PointCount != 0)
{
throw new InvalidOperationException();
}
#if PEDANTIC
if (PositionsId != null && Positions.Value.Length != PointCount)
{
throw new InvalidOperationException();
}
#endif
if (IsLeaf)
{
if (PositionsId == null)
{
throw new InvalidOperationException();
}
if (KdTreeId == null)
{
throw new InvalidOperationException();
}
}
else
{
if (PositionsId != null)
{
throw new InvalidOperationException();
}
if (ColorsId != null)
{
throw new InvalidOperationException();
}
if (KdTreeId != null)
{
throw new InvalidOperationException();
}
if (NormalsId != null)
{
throw new InvalidOperationException();
}
if (IntensitiesId != null)
{
throw new InvalidOperationException();
}
}
#endif
PointRkdTreeD <V3f[], V3f> LoadKdTree(string key, CancellationToken ct)
{
var data = Storage.GetPointRkdTreeDData(key, ct);
var ps = Positions.Value;
return(new PointRkdTreeD <V3f[], V3f>(
3, ps.Length, ps,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, 1e-9,
data
));
}
}
