3108 lines
85 KiB
C++
3108 lines
85 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "LidarPointCloudOctree.h"
|
|
#include "LidarPointCloudOctreeMacros.h"
|
|
#include "LidarPointCloud.h"
|
|
#include "Meshing/LidarPointCloudMeshing.h"
|
|
#include "Misc/ScopeTryLock.h"
|
|
#include "Containers/Queue.h"
|
|
#include "Async/Async.h"
|
|
#include "Misc/FileHelper.h"
|
|
#include "Rendering/LidarPointCloudRenderBuffers.h"
|
|
#include "UObject/Package.h"
|
|
|
|
int32 FLidarPointCloudOctree::MaxNodeDepth = (1 << (sizeof(FLidarPointCloudOctreeNode::Depth) * 8)) - 1;
|
|
int32 FLidarPointCloudOctree::MaxBucketSize = 200;
|
|
int32 FLidarPointCloudOctree::NodeGridResolution = 96;
|
|
|
|
FArchive& operator<<(FArchive& Ar, FLidarPointCloudPoint_Legacy& P)
|
|
{
|
|
Ar << P.Location << P.Color;
|
|
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) > 8)
|
|
{
|
|
uint8 bVisible = P.bVisible;
|
|
Ar << bVisible;
|
|
P.bVisible = bVisible;
|
|
}
|
|
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) > 12)
|
|
{
|
|
uint8 ClassificationID = P.ClassificationID;
|
|
Ar << ClassificationID;
|
|
P.ClassificationID = ClassificationID;
|
|
}
|
|
|
|
return Ar;
|
|
}
|
|
|
|
struct FLidarPointCloudBulkData_Legacy : public FBulkData
|
|
{
|
|
private:
|
|
int32 ElementSize;
|
|
|
|
public:
|
|
FLidarPointCloudBulkData_Legacy(int32 ElementSize) : ElementSize(ElementSize) {}
|
|
void Serialize(FArchive& Ar, UObject* Owner);
|
|
int32 GetElementCount() const;
|
|
};
|
|
|
|
void FLidarPointCloudBulkData_Legacy::Serialize(FArchive& Ar, UObject* Owner)
|
|
{
|
|
const bool bAttemptFileMapping = false;
|
|
FBulkData::Serialize(Ar, Owner, bAttemptFileMapping, ElementSize, EFileRegionType::None);
|
|
}
|
|
|
|
int32 FLidarPointCloudBulkData_Legacy::GetElementCount() const
|
|
{
|
|
return GetBulkDataSize() / ElementSize;
|
|
}
|
|
|
|
/** Used for grid allocation calculations */
|
|
struct FGridAllocation
|
|
{
|
|
/** Index to the point inside of the AllocatedPoints */
|
|
int32 Index;
|
|
|
|
/** Index of the child node this point would be placed in */
|
|
int32 ChildNodeLocation;
|
|
|
|
/** The calculated distance squared from the center of the given point */
|
|
float DistanceFromCenter;
|
|
|
|
FGridAllocation() {}
|
|
FGridAllocation(const int32& Index, const FGridAllocation& GridAllocation)
|
|
: Index(Index)
|
|
, ChildNodeLocation(GridAllocation.ChildNodeLocation)
|
|
, DistanceFromCenter(GridAllocation.DistanceFromCenter)
|
|
{
|
|
}
|
|
};
|
|
|
|
FGridAllocation CalculateGridCellData(const FVector3f& Location, const FVector3f& Center, const FLidarPointCloudOctree::FSharedLODData& LODData)
|
|
{
|
|
const FVector3f CenterRelativeLocation = Location - Center;
|
|
const FVector3f OffsetLocation = CenterRelativeLocation + LODData.Extent;
|
|
const FVector3f NormalizedGridLocation = OffsetLocation * LODData.NormalizationMultiplier;
|
|
|
|
// Calculate the location on this node's Grid
|
|
const int32 GridX = FMath::Min(FLidarPointCloudOctree::NodeGridResolution - 1, (int32)NormalizedGridLocation.X);
|
|
const int32 GridY = FMath::Min(FLidarPointCloudOctree::NodeGridResolution - 1, (int32)NormalizedGridLocation.Y);
|
|
const int32 GridZ = FMath::Min(FLidarPointCloudOctree::NodeGridResolution - 1, (int32)NormalizedGridLocation.Z);
|
|
|
|
FGridAllocation Allocation;
|
|
Allocation.Index = GridX * FLidarPointCloudOctree::NodeGridResolution * FLidarPointCloudOctree::NodeGridResolution + GridY * FLidarPointCloudOctree::NodeGridResolution + GridZ;
|
|
Allocation.DistanceFromCenter = (FVector3f(GridX + 0.5f, GridY + 0.5f, GridZ + 0.5f) * LODData.GridSize3D - OffsetLocation).SizeSquared();
|
|
Allocation.ChildNodeLocation = (CenterRelativeLocation.X > 0 ? 4 : 0) + (CenterRelativeLocation.Y > 0 ? 2 : 0) + (CenterRelativeLocation.Z > 0);
|
|
|
|
return Allocation;
|
|
}
|
|
|
|
FORCEINLINE float BrightnessFromColor(const FColor& Color) { return 0.2126 * Color.R + 0.7152 * Color.G + 0.0722 * Color.B; }
|
|
|
|
bool IsOnBoundsEdge(const FBox& Bounds, const FVector3f& Location)
|
|
{
|
|
return (Location.X == Bounds.Min.X) || (Location.X == Bounds.Max.X) || (Location.Y == Bounds.Min.Y) || (Location.Y == Bounds.Max.Y) || (Location.Z == Bounds.Min.Z) || (Location.Z == Bounds.Max.Z);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////// FSharedLODData
|
|
|
|
FLidarPointCloudOctree::FSharedLODData::FSharedLODData(const FVector3f& InExtent)
|
|
{
|
|
const float UniformExtent = InExtent.GetMax();
|
|
|
|
Extent = FVector3f(UniformExtent);
|
|
Radius = UniformExtent * 1.73205081f; // sqrt(3)
|
|
RadiusSq = Radius * Radius;
|
|
Size = UniformExtent * 2;
|
|
GridSize = Size / FLidarPointCloudOctree::NodeGridResolution;
|
|
GridSize3D = FVector3f(GridSize);
|
|
NormalizationMultiplier = FLidarPointCloudOctree::NodeGridResolution / Size;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////// FLidarPointCloudOctreeNode
|
|
|
|
FLidarPointCloudOctreeNode::FLidarPointCloudOctreeNode(FLidarPointCloudOctree* Tree, const uint8& Depth, const uint8& LocationInParent, const FVector3f& Center)
|
|
: BulkDataLifetime(0)
|
|
, Depth(Depth)
|
|
, LocationInParent(LocationInParent)
|
|
, Center(Center)
|
|
, Tree(Tree)
|
|
, bVisibilityDirty(false)
|
|
, bInUse(false)
|
|
, bHasSelection(false)
|
|
, NumVisiblePoints(0)
|
|
, NumPoints(0)
|
|
, bHasData(false)
|
|
, DataCache(nullptr)
|
|
, VertexFactory(nullptr)
|
|
, RayTracingGeometry(nullptr)
|
|
, bRenderDataDirty(true)
|
|
, bHasDataPending(false)
|
|
, bCanReleaseData(true)
|
|
{
|
|
if (Tree)
|
|
{
|
|
Tree->NodeCount[Depth].Increment();
|
|
}
|
|
}
|
|
|
|
FLidarPointCloudOctreeNode::~FLidarPointCloudOctreeNode()
|
|
{
|
|
ReleaseDataCache();
|
|
|
|
for (int32 i = 0; i < Children.Num(); i++)
|
|
{
|
|
delete Children[i];
|
|
Children[i] = nullptr;
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::UpdateNumVisiblePoints()
|
|
{
|
|
if (bVisibilityDirty)
|
|
{
|
|
// Sort points to speed up rendering
|
|
SortVisiblePoints();
|
|
|
|
// Recalculate visibility
|
|
NumVisiblePoints = 0;
|
|
FOR_RO(Point, this)
|
|
{
|
|
if (!Point->bVisible)
|
|
{
|
|
break;
|
|
}
|
|
|
|
NumVisiblePoints++;
|
|
}
|
|
|
|
bVisibilityDirty = false;
|
|
}
|
|
}
|
|
|
|
FLidarPointCloudPoint* FLidarPointCloudOctreeNode::GetData() const
|
|
{
|
|
FLidarPointCloudOctreeNode* mutable_this = const_cast<FLidarPointCloudOctreeNode*>(this);
|
|
|
|
if (!bHasData && NumPoints > 0)
|
|
{
|
|
Tree->StreamNodeData(mutable_this);
|
|
}
|
|
|
|
return mutable_this->Data.GetData();
|
|
}
|
|
|
|
FLidarPointCloudPoint* FLidarPointCloudOctreeNode::GetPersistentData() const
|
|
{
|
|
FLidarPointCloudOctreeNode* mutable_this = const_cast<FLidarPointCloudOctreeNode*>(this);
|
|
mutable_this->bCanReleaseData = false;
|
|
return GetData();
|
|
}
|
|
|
|
bool FLidarPointCloudOctreeNode::BuildDataCache(bool bUseStaticBuffers, bool bUseRayTracing)
|
|
{
|
|
// Only include nodes with available data
|
|
if (HasData() && GetNumVisiblePoints())
|
|
{
|
|
// Make sure to release the unnecessary buffer
|
|
if (bUseStaticBuffers)
|
|
{
|
|
if (DataCache.IsValid())
|
|
{
|
|
DataCache->ReleaseResource();
|
|
DataCache.Reset();
|
|
bRenderDataDirty = true;
|
|
}
|
|
|
|
if (!VertexFactory.IsValid())
|
|
{
|
|
VertexFactory = MakeShareable(new FLidarPointCloudVertexFactory());
|
|
bRenderDataDirty = true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (VertexFactory.IsValid())
|
|
{
|
|
VertexFactory->ReleaseResource();
|
|
VertexFactory.Reset();
|
|
bRenderDataDirty = true;
|
|
}
|
|
|
|
if (!DataCache.IsValid())
|
|
{
|
|
DataCache = MakeShareable(new FLidarPointCloudRenderBuffer());
|
|
bRenderDataDirty = true;
|
|
}
|
|
}
|
|
|
|
if(bUseRayTracing)
|
|
{
|
|
if(!RayTracingGeometry.IsValid())
|
|
{
|
|
RayTracingGeometry = MakeShareable(new FLidarPointCloudRayTracingGeometry());
|
|
bRenderDataDirty = true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(RayTracingGeometry.IsValid())
|
|
{
|
|
RayTracingGeometry->ReleaseResource();
|
|
RayTracingGeometry.Reset();
|
|
bRenderDataDirty = true;
|
|
}
|
|
}
|
|
|
|
if (bRenderDataDirty)
|
|
{
|
|
if (DataCache.IsValid())
|
|
{
|
|
DataCache->Initialize(GetData(), GetNumVisiblePoints());
|
|
}
|
|
|
|
if (VertexFactory.IsValid())
|
|
{
|
|
VertexFactory->Initialize(GetData(), GetNumVisiblePoints());
|
|
}
|
|
|
|
if(RayTracingGeometry.IsValid())
|
|
{
|
|
RayTracingGeometry->Initialize(GetNumVisiblePoints());
|
|
}
|
|
|
|
bRenderDataDirty = false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
FORCEINLINE FBox FLidarPointCloudOctreeNode::GetBounds() const
|
|
{
|
|
return FBox(Center - Tree->SharedData[Depth].Extent, Center + Tree->SharedData[Depth].Extent);
|
|
}
|
|
|
|
FORCEINLINE FSphere FLidarPointCloudOctreeNode::GetSphereBounds() const
|
|
{
|
|
return FSphere((FVector)Center, Tree->SharedData[Depth].Radius);
|
|
}
|
|
|
|
FLidarPointCloudOctreeNode* FLidarPointCloudOctreeNode::GetChildNodeAtLocation(const uint8& Location) const
|
|
{
|
|
for (FLidarPointCloudOctreeNode* Child : Children)
|
|
{
|
|
if (Child->LocationInParent == Location)
|
|
{
|
|
return Child;
|
|
}
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
uint8 FLidarPointCloudOctreeNode::GetChildrenBitmask() const
|
|
{
|
|
uint8 Bitmask = 0;
|
|
|
|
for (FLidarPointCloudOctreeNode* Child : Children)
|
|
{
|
|
Bitmask |= 1 << Child->LocationInParent;
|
|
}
|
|
|
|
return Bitmask;
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::InsertPoints(const FLidarPointCloudPoint* Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, const FVector3f& Translation)
|
|
{
|
|
InsertPoints_Internal(Points, Count, DuplicateHandling, Translation);
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::InsertPoints_Dynamic(const FLidarPointCloudPoint* Points, const int64& Count, const FVector3f& Translation)
|
|
{
|
|
if (Translation.IsNearlyZero())
|
|
{
|
|
Data.Append(Points, Count);
|
|
}
|
|
else
|
|
{
|
|
for (const FLidarPointCloudPoint* PointsPtr = Points, *DataEnd = PointsPtr + Count; PointsPtr != DataEnd; ++PointsPtr)
|
|
{
|
|
Data.Emplace(PointsPtr->Location + Translation, PointsPtr->Color, !!PointsPtr->bVisible, PointsPtr->ClassificationID, PointsPtr->Normal);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::InsertPoints_Static(const FLidarPointCloudPoint* Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, const FVector3f& Translation)
|
|
{
|
|
const FLidarPointCloudOctree::FSharedLODData& LODData = Tree->SharedData[Depth];
|
|
|
|
// Local
|
|
TArray<FLidarPointCloudPoint> PointBuckets[8];
|
|
TMultiMap<int32, FGridAllocation> NewGridAllocationMap, CurrentGridAllocationMap;
|
|
|
|
int32 NumPointsAdded = 0;
|
|
|
|
const float MaxDistanceForDuplicate = GetDefault<ULidarPointCloudSettings>()->MaxDistanceForDuplicate;
|
|
|
|
// Filter the local set of incoming data
|
|
for (int32 Index = 0; Index < Count; ++Index)
|
|
{
|
|
const FVector3f AdjustedLocation = Points[Index].Location + Translation;
|
|
FGridAllocation InGridData = CalculateGridCellData(AdjustedLocation, Center, LODData);
|
|
|
|
// Attempt to allocate the point to this node
|
|
if (FGridAllocation* GridCell = NewGridAllocationMap.Find(InGridData.Index))
|
|
{
|
|
bool bStoreInBucket = true;
|
|
|
|
if (DuplicateHandling != ELidarPointCloudDuplicateHandling::Ignore && Points[GridCell->Index].Location.Equals(Points[Index].Location, MaxDistanceForDuplicate))
|
|
{
|
|
if (DuplicateHandling == ELidarPointCloudDuplicateHandling::SelectFirst || BrightnessFromColor(Points[Index].Color) <= BrightnessFromColor(Points[GridCell->Index].Color))
|
|
{
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
bStoreInBucket = false;
|
|
}
|
|
}
|
|
|
|
if (InGridData.DistanceFromCenter < GridCell->DistanceFromCenter)
|
|
{
|
|
if (bStoreInBucket)
|
|
{
|
|
const FLidarPointCloudPoint& Other = Points[GridCell->Index];
|
|
PointBuckets[GridCell->ChildNodeLocation].Emplace(Other.Location + Translation, Other.Color, !!Other.bVisible, Other.ClassificationID, Other.Normal);
|
|
}
|
|
|
|
GridCell->Index = Index;
|
|
GridCell->DistanceFromCenter = InGridData.DistanceFromCenter;
|
|
}
|
|
else if(bStoreInBucket)
|
|
{
|
|
const FLidarPointCloudPoint& Other = Points[Index];
|
|
PointBuckets[InGridData.ChildNodeLocation].Emplace(AdjustedLocation, Other.Color, !!Other.bVisible, Other.ClassificationID, Other.Normal);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
NewGridAllocationMap.Add(InGridData.Index, FGridAllocation(Index, InGridData));
|
|
}
|
|
}
|
|
|
|
GetPersistentData();
|
|
|
|
// Process incoming points
|
|
{
|
|
FScopeLock Lock(&MapLock);
|
|
|
|
// Rebuild Current Grid Mapping
|
|
for (int32 i = 0; i < Data.Num(); ++i)
|
|
{
|
|
FGridAllocation InGridData = CalculateGridCellData(Data[i].Location, Center, LODData);
|
|
|
|
// Attempt to allocate the point to this node
|
|
if (FGridAllocation* GridCell = CurrentGridAllocationMap.Find(InGridData.Index))
|
|
{
|
|
if (InGridData.DistanceFromCenter < GridCell->DistanceFromCenter)
|
|
{
|
|
PointBuckets[GridCell->ChildNodeLocation].Add(Data[GridCell->Index]);
|
|
Data[GridCell->Index] = Data[i];
|
|
GridCell->DistanceFromCenter = InGridData.DistanceFromCenter;
|
|
}
|
|
else
|
|
{
|
|
PointBuckets[InGridData.ChildNodeLocation].Add(Data[i]);
|
|
}
|
|
|
|
Data.RemoveAtSwap(i--, EAllowShrinking::No);
|
|
--NumPointsAdded;
|
|
}
|
|
else
|
|
{
|
|
CurrentGridAllocationMap.Add(InGridData.Index, FGridAllocation(i, InGridData));
|
|
}
|
|
}
|
|
|
|
// Compare the incoming data to the currently held set, and replace if necessary
|
|
for (TPair<int32, FGridAllocation>& Element : NewGridAllocationMap)
|
|
{
|
|
const int32& GridIndex = Element.Key;
|
|
const FLidarPointCloudPoint& Point = Points[Element.Value.Index];
|
|
const FVector3f AdjustedLocation = Point.Location + Translation;
|
|
|
|
// Attempt to allocate the point to this node
|
|
if (FGridAllocation* GridCell = CurrentGridAllocationMap.Find(GridIndex))
|
|
{
|
|
FLidarPointCloudPoint& AllocatedPoint = Data[GridCell->Index];
|
|
bool bStoreInBucket = true;
|
|
|
|
if (DuplicateHandling != ELidarPointCloudDuplicateHandling::Ignore && AllocatedPoint.Location.Equals((FVector3f)AdjustedLocation, MaxDistanceForDuplicate))
|
|
{
|
|
if (DuplicateHandling == ELidarPointCloudDuplicateHandling::SelectFirst || BrightnessFromColor(Point.Color) <= BrightnessFromColor(AllocatedPoint.Color))
|
|
{
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
bStoreInBucket = false;
|
|
}
|
|
}
|
|
|
|
// If the new point's distance from center of node is shorter than the existing point's, replace the point
|
|
if (Element.Value.DistanceFromCenter < GridCell->DistanceFromCenter)
|
|
{
|
|
if (bStoreInBucket)
|
|
{
|
|
PointBuckets[GridCell->ChildNodeLocation].Add(AllocatedPoint);
|
|
}
|
|
|
|
AllocatedPoint.Location = (FVector3f)AdjustedLocation;
|
|
AllocatedPoint.Color = Point.Color;
|
|
AllocatedPoint.bVisible = Point.bVisible;
|
|
AllocatedPoint.ClassificationID = Point.ClassificationID;
|
|
AllocatedPoint.Normal = Point.Normal;
|
|
GridCell->DistanceFromCenter = Element.Value.DistanceFromCenter;
|
|
}
|
|
// ... otherwise add it straight to the bucket
|
|
else if (bStoreInBucket)
|
|
{
|
|
PointBuckets[Element.Value.ChildNodeLocation].Emplace(AdjustedLocation, Point.Color, !!Point.bVisible, Point.ClassificationID, Point.Normal);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentGridAllocationMap.Add(GridIndex, FGridAllocation(Data.Emplace(AdjustedLocation, Point.Color, !!Point.bVisible, Point.ClassificationID, Point.Normal), Element.Value));
|
|
NumPointsAdded++;
|
|
}
|
|
}
|
|
|
|
for (uint8 i = 0; i < 8; ++i)
|
|
{
|
|
if (!GetChildNodeAtLocation(i))
|
|
{
|
|
// While the threads are locked, check if any child nodes need creating
|
|
if (Depth < FLidarPointCloudOctree::MaxNodeDepth && PointBuckets[i].Num() > FLidarPointCloudOctree::MaxBucketSize)
|
|
{
|
|
const FVector3f ChildNodeCenter = Center + LODData.Extent * (FVector3f(-0.5f) + FVector3f((i & 4) == 4, (i & 2) == 2, (i & 1) == 1));
|
|
Children.Add(new FLidarPointCloudOctreeNode(Tree, Depth + 1, i, ChildNodeCenter));
|
|
|
|
// The recursive InserPoints call will happen later, after the Lock is released
|
|
}
|
|
// ... otherwise, points can be re-added back as padding
|
|
else
|
|
{
|
|
Data.Append(PointBuckets[i]);
|
|
NumPointsAdded += PointBuckets[i].Num();
|
|
PointBuckets[i].Reset();
|
|
}
|
|
}
|
|
}
|
|
|
|
// Shrink the data usage
|
|
Data.Shrink();
|
|
|
|
NumPoints = Data.Num();
|
|
bHasData = true;
|
|
bRenderDataDirty = true;
|
|
}
|
|
|
|
AddPointCount(NumPointsAdded);
|
|
|
|
// Pass surplus points
|
|
for (uint8 i = 0; i < 8; ++i)
|
|
{
|
|
if (PointBuckets[i].Num() > 0)
|
|
{
|
|
GetChildNodeAtLocation(i)->InsertPoints_Static(PointBuckets[i].GetData(), PointBuckets[i].Num(), DuplicateHandling, FVector3f::ZeroVector);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::InsertPoints(FLidarPointCloudPoint** Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, const FVector3f& Translation)
|
|
{
|
|
InsertPoints_Internal(Points, Count, DuplicateHandling, Translation);
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::InsertPoints_Dynamic(FLidarPointCloudPoint** Points, const int64& Count, const FVector3f& Translation)
|
|
{
|
|
if (Translation.IsNearlyZero())
|
|
{
|
|
for (FLidarPointCloudPoint** PointsPtr = Points, **DataEnd = PointsPtr + Count; PointsPtr != DataEnd; ++PointsPtr)
|
|
{
|
|
Data.Add(**PointsPtr);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (FLidarPointCloudPoint** PointsPtr = Points, **DataEnd = PointsPtr + Count; PointsPtr != DataEnd; ++PointsPtr)
|
|
{
|
|
const FLidarPointCloudPoint* PointData = *PointsPtr;
|
|
Data.Emplace(PointData->Location + Translation, PointData->Color, !!PointData->bVisible, PointData->ClassificationID, PointData->Normal);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::InsertPoints_Static(FLidarPointCloudPoint** Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, const FVector3f& Translation)
|
|
{
|
|
const FLidarPointCloudOctree::FSharedLODData& LODData = Tree->SharedData[Depth];
|
|
|
|
// Local
|
|
TArray<FLidarPointCloudPoint> PointBuckets[8];
|
|
TMultiMap<int32, FGridAllocation> NewGridAllocationMap, CurrentGridAllocationMap;
|
|
|
|
int32 NumPointsAdded = 0;
|
|
|
|
const float MaxDistanceForDuplicate = GetDefault<ULidarPointCloudSettings>()->MaxDistanceForDuplicate;
|
|
|
|
// Filter the local set of incoming data
|
|
for (int32 Index = 0; Index < Count; Index++)
|
|
{
|
|
const FVector3f AdjustedLocation = Points[Index]->Location + Translation;
|
|
FGridAllocation InGridData = CalculateGridCellData(AdjustedLocation, Center, LODData);
|
|
|
|
// Attempt to allocate the point to this node
|
|
if (FGridAllocation* GridCell = NewGridAllocationMap.Find(InGridData.Index))
|
|
{
|
|
bool bStoreInBucket = true;
|
|
|
|
if (DuplicateHandling != ELidarPointCloudDuplicateHandling::Ignore && Points[GridCell->Index]->Location.Equals(Points[Index]->Location, MaxDistanceForDuplicate))
|
|
{
|
|
if (DuplicateHandling == ELidarPointCloudDuplicateHandling::SelectFirst || BrightnessFromColor(Points[Index]->Color) <= BrightnessFromColor(Points[GridCell->Index]->Color))
|
|
{
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
bStoreInBucket = false;
|
|
}
|
|
}
|
|
|
|
if (InGridData.DistanceFromCenter < GridCell->DistanceFromCenter)
|
|
{
|
|
if (bStoreInBucket)
|
|
{
|
|
const FLidarPointCloudPoint& Other = *Points[GridCell->Index];
|
|
PointBuckets[GridCell->ChildNodeLocation].Emplace(Other.Location + Translation, Other.Color, !!Other.bVisible, Other.ClassificationID, Other.Normal);
|
|
}
|
|
|
|
GridCell->Index = Index;
|
|
GridCell->DistanceFromCenter = InGridData.DistanceFromCenter;
|
|
}
|
|
else if (bStoreInBucket)
|
|
{
|
|
const FLidarPointCloudPoint& Other = *Points[Index];
|
|
PointBuckets[InGridData.ChildNodeLocation].Emplace(AdjustedLocation, Other.Color, !!Other.bVisible, Other.ClassificationID, Other.Normal);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
NewGridAllocationMap.Add(InGridData.Index, FGridAllocation(Index, InGridData));
|
|
}
|
|
}
|
|
|
|
GetPersistentData();
|
|
|
|
// Process incoming points
|
|
{
|
|
FScopeLock Lock(&MapLock);
|
|
|
|
// Rebuild Current Grid Mapping
|
|
for (int32 i = 0; i < Data.Num(); ++i)
|
|
{
|
|
FGridAllocation InGridData = CalculateGridCellData(Data[i].Location, Center, LODData);
|
|
|
|
// Attempt to allocate the point to this node
|
|
if (FGridAllocation* GridCell = CurrentGridAllocationMap.Find(InGridData.Index))
|
|
{
|
|
if (InGridData.DistanceFromCenter < GridCell->DistanceFromCenter)
|
|
{
|
|
PointBuckets[GridCell->ChildNodeLocation].Add(Data[GridCell->Index]);
|
|
Data[GridCell->Index] = Data[i];
|
|
GridCell->DistanceFromCenter = InGridData.DistanceFromCenter;
|
|
}
|
|
else
|
|
{
|
|
PointBuckets[InGridData.ChildNodeLocation].Add(Data[i]);
|
|
}
|
|
|
|
Data.RemoveAtSwap(i--, EAllowShrinking::No);
|
|
--NumPointsAdded;
|
|
}
|
|
else
|
|
{
|
|
CurrentGridAllocationMap.Add(InGridData.Index, FGridAllocation(i, InGridData));
|
|
}
|
|
}
|
|
|
|
// Compare the incoming data to the currently held set, and replace if necessary
|
|
for (TPair<int32, FGridAllocation>& Element : NewGridAllocationMap)
|
|
{
|
|
const int32& GridIndex = Element.Key;
|
|
const FLidarPointCloudPoint& Point = *Points[Element.Value.Index];
|
|
FVector3f AdjustedLocation = Point.Location + Translation;
|
|
|
|
// Attempt to allocate the point to this node
|
|
if (FGridAllocation* GridCell = CurrentGridAllocationMap.Find(GridIndex))
|
|
{
|
|
FLidarPointCloudPoint& AllocatedPoint = Data[GridCell->Index];
|
|
bool bStoreInBucket = true;
|
|
|
|
if (DuplicateHandling != ELidarPointCloudDuplicateHandling::Ignore && AllocatedPoint.Location.Equals((FVector3f)AdjustedLocation, MaxDistanceForDuplicate))
|
|
{
|
|
if (DuplicateHandling == ELidarPointCloudDuplicateHandling::SelectFirst || BrightnessFromColor(Point.Color) <= BrightnessFromColor(AllocatedPoint.Color))
|
|
{
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
bStoreInBucket = false;
|
|
}
|
|
}
|
|
|
|
// If the new point's distance from center of node is shorter than the existing point's, replace the point
|
|
if (Element.Value.DistanceFromCenter < GridCell->DistanceFromCenter)
|
|
{
|
|
if (bStoreInBucket)
|
|
{
|
|
PointBuckets[GridCell->ChildNodeLocation].Add(AllocatedPoint);
|
|
}
|
|
|
|
AllocatedPoint.Location = (FVector3f)AdjustedLocation;
|
|
AllocatedPoint.Color = Point.Color;
|
|
AllocatedPoint.bVisible = Point.bVisible;
|
|
AllocatedPoint.ClassificationID = Point.ClassificationID;
|
|
AllocatedPoint.Normal = Point.Normal;
|
|
GridCell->DistanceFromCenter = Element.Value.DistanceFromCenter;
|
|
}
|
|
// ... otherwise add it straight to the bucket
|
|
else if (bStoreInBucket)
|
|
{
|
|
PointBuckets[Element.Value.ChildNodeLocation].Emplace(AdjustedLocation, Point.Color, !!Point.bVisible, Point.ClassificationID, Point.Normal);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentGridAllocationMap.Add(GridIndex, FGridAllocation(Data.Emplace(AdjustedLocation, Point.Color, !!Point.bVisible, Point.ClassificationID, Point.Normal), Element.Value));
|
|
NumPointsAdded++;
|
|
}
|
|
}
|
|
|
|
for (uint8 i = 0; i < 8; i++)
|
|
{
|
|
if (!GetChildNodeAtLocation(i))
|
|
{
|
|
// While the threads are locked, check if any child nodes need creating
|
|
if (Depth < FLidarPointCloudOctree::MaxNodeDepth && PointBuckets[i].Num() > FLidarPointCloudOctree::MaxBucketSize)
|
|
{
|
|
const FVector3f ChildNodeCenter = Center + LODData.Extent * (FVector3f(-0.5f) + FVector3f((i & 4) == 4, (i & 2) == 2, (i & 1) == 1));
|
|
Children.Add(new FLidarPointCloudOctreeNode(Tree, Depth + 1, i, ChildNodeCenter));
|
|
|
|
// The recursive InserPoints call will happen later, after the Lock is released
|
|
}
|
|
// ... otherwise, points can be re-added back as padding
|
|
else
|
|
{
|
|
Data.Append(PointBuckets[i]);
|
|
NumPointsAdded += PointBuckets[i].Num();
|
|
PointBuckets[i].Reset();
|
|
}
|
|
}
|
|
}
|
|
|
|
// Shrink the data usage
|
|
Data.Shrink();
|
|
|
|
NumPoints = Data.Num();
|
|
bHasData = true;
|
|
bRenderDataDirty = true;
|
|
}
|
|
|
|
AddPointCount(NumPointsAdded);
|
|
|
|
// Pass surplus points
|
|
for (uint8 i = 0; i < 8; i++)
|
|
{
|
|
if (PointBuckets[i].Num() > 0)
|
|
{
|
|
GetChildNodeAtLocation(i)->InsertPoints_Static(PointBuckets[i].GetData(), PointBuckets[i].Num(), DuplicateHandling, FVector3f::ZeroVector);
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctreeNode::InsertPoints_Internal(T Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, const FVector3f& Translation)
|
|
{
|
|
if (Tree->IsOptimizedForDynamicData())
|
|
{
|
|
Data.Reserve(NumPoints + Count);
|
|
|
|
InsertPoints_Dynamic(Points, Count, Translation);
|
|
|
|
NumPoints = Data.Num();
|
|
bCanReleaseData = false;
|
|
bRenderDataDirty = true;
|
|
bHasData = true;
|
|
|
|
AddPointCount(Count);
|
|
}
|
|
else
|
|
{
|
|
InsertPoints_Static(Points, Count, DuplicateHandling, Translation);
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::Empty(bool bRecursive)
|
|
{
|
|
if (bHasData)
|
|
{
|
|
bHasData = false;
|
|
bInUse = false;
|
|
NumPoints = 0;
|
|
NumVisiblePoints = 0;
|
|
Data.Empty();
|
|
}
|
|
|
|
bRenderDataDirty = true;
|
|
|
|
if (bRecursive)
|
|
{
|
|
for (int32 i = 0; i < Children.Num(); i++)
|
|
{
|
|
Children[i]->Empty(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
uint32 FLidarPointCloudOctreeNode::GetMaxDepth() const
|
|
{
|
|
uint32 MaxDepth = Depth;
|
|
|
|
for (FLidarPointCloudOctreeNode* Child : Children)
|
|
{
|
|
MaxDepth = FMath::Max(MaxDepth, Child->GetMaxDepth());
|
|
}
|
|
|
|
return MaxDepth;
|
|
}
|
|
|
|
int64 FLidarPointCloudOctreeNode::GetAllocatedSize(bool bRecursive, bool bIncludeBulkData) const
|
|
{
|
|
int64 Size = sizeof(FLidarPointCloudOctreeNode);
|
|
|
|
Size += Children.GetAllocatedSize();
|
|
|
|
if (bIncludeBulkData)
|
|
{
|
|
Size += sizeof(FLidarPointCloudPoint) * GetNumPoints();
|
|
}
|
|
|
|
if (bRecursive)
|
|
{
|
|
for (FLidarPointCloudOctreeNode* Child : Children)
|
|
{
|
|
Size += Child->GetAllocatedSize(true, bIncludeBulkData);
|
|
}
|
|
}
|
|
|
|
return Size;
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::ReleaseData(bool bForce)
|
|
{
|
|
// Check if the data can be released
|
|
if ((bCanReleaseData
|
|
#if WITH_EDITOR
|
|
&& !bHasSelection
|
|
#endif
|
|
) || bForce)
|
|
{
|
|
bHasDataPending = false;
|
|
bCanReleaseData = true;
|
|
|
|
if (bHasData)
|
|
{
|
|
bHasData = false;
|
|
bInUse = false;
|
|
Data.Empty();
|
|
}
|
|
}
|
|
|
|
ReleaseDataCache();
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::ReleaseDataCache()
|
|
{
|
|
if (VertexFactory.IsValid() || DataCache.IsValid())
|
|
{
|
|
ENQUEUE_RENDER_COMMAND(LidarPointCloudOctreeNode_ReleaseDataCache)([
|
|
DataCache = DataCache, VertexFactory = VertexFactory, RayTracingGeometry = RayTracingGeometry](FRHICommandListImmediate& RHICmdList)
|
|
{
|
|
if (DataCache.IsValid())
|
|
{
|
|
DataCache->ReleaseResource();
|
|
}
|
|
|
|
if (VertexFactory.IsValid())
|
|
{
|
|
VertexFactory->ReleaseResource();
|
|
}
|
|
|
|
if(RayTracingGeometry.IsValid())
|
|
{
|
|
RayTracingGeometry->ReleaseResource();
|
|
}
|
|
});
|
|
|
|
DataCache.Reset();
|
|
VertexFactory.Reset();
|
|
RayTracingGeometry.Reset();
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::AddPointCount(int32 PointCount)
|
|
{
|
|
Tree->PointCount[Depth].Add(PointCount);
|
|
NumVisiblePoints += PointCount;
|
|
}
|
|
|
|
void FLidarPointCloudOctreeNode::SortVisiblePoints()
|
|
{
|
|
TArrayView<FLidarPointCloudPoint> Points(GetData(), Data.Num());
|
|
Algo::Sort(Points, [](const FLidarPointCloudPoint& A, const FLidarPointCloudPoint& B)
|
|
{
|
|
return A.bVisible > B.bVisible;
|
|
});
|
|
bCanReleaseData = false;
|
|
bRenderDataDirty = true;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////// FLidarPointCloudOctree
|
|
|
|
FLidarPointCloudOctree::FLidarPointCloudOctree(ULidarPointCloud* Owner)
|
|
: Root(new FLidarPointCloudOctreeNode(nullptr, 0))
|
|
, Owner(Owner)
|
|
#if WITH_EDITOR
|
|
, SavingBulkData(this)
|
|
#endif
|
|
, bStreamingBusy(false)
|
|
, bIsFullyLoaded(false)
|
|
{
|
|
PointCount.AddDefaulted(MaxNodeDepth + 1);
|
|
NodeCount.AddDefaulted(MaxNodeDepth + 1);
|
|
SharedData.AddDefaulted(MaxNodeDepth + 1);
|
|
|
|
// Account for the Root
|
|
NodeCount[0].Increment();
|
|
|
|
Root->Tree = this;
|
|
}
|
|
|
|
FLidarPointCloudOctree::~FLidarPointCloudOctree()
|
|
{
|
|
Empty(true);
|
|
}
|
|
|
|
int32 FLidarPointCloudOctree::GetNumLODs() const
|
|
{
|
|
int32 LODCount = 0;
|
|
|
|
for (; LODCount < NodeCount.Num(); LODCount++)
|
|
{
|
|
if (NodeCount[LODCount].GetValue() == 0)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return LODCount;
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RefreshBounds()
|
|
{
|
|
FBox Bounds(EForceInit::ForceInit);
|
|
|
|
// Calculate the current bounds
|
|
ITERATE_NODES_CONST({ FOR_RO(Point, CurrentNode) { Bounds += (FVector)Point->Location; } }, true);
|
|
|
|
Extent = (FVector3f)Bounds.GetExtent();
|
|
const FVector3f Offset = (FVector3f)Bounds.GetCenter();
|
|
|
|
if (!Offset.IsNearlyZero(0.1f))
|
|
{
|
|
Owner->LocationOffset += (FVector)Offset;
|
|
Owner->OriginalCoordinates += (FVector)Offset;
|
|
|
|
// Shift the points back to the relative position
|
|
ITERATE_NODES(
|
|
{
|
|
CurrentNode->Center -= Offset;
|
|
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
Point->Location -= Offset;
|
|
}
|
|
}, true);
|
|
}
|
|
}
|
|
|
|
int64 FLidarPointCloudOctree::GetNumPoints() const
|
|
{
|
|
int64 TotalPointCount = 0;
|
|
|
|
for (int32 i = 0; i < PointCount.Num(); i++)
|
|
{
|
|
int64 NumPoints = PointCount[i].GetValue();
|
|
|
|
if (NumPoints > 0)
|
|
{
|
|
TotalPointCount += NumPoints;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return TotalPointCount;
|
|
}
|
|
|
|
int64 FLidarPointCloudOctree::GetNumVisiblePoints() const
|
|
{
|
|
int64 NumVisiblePoints = 0;
|
|
|
|
ITERATE_NODES_CONST({ NumVisiblePoints += CurrentNode->GetNumVisiblePoints(); }, true);
|
|
|
|
return NumVisiblePoints;
|
|
}
|
|
|
|
int32 FLidarPointCloudOctree::GetNumNodes() const
|
|
{
|
|
int32 TotalNodeCount = 0;
|
|
|
|
for (int32 i = 0; i < NodeCount.Num(); i++)
|
|
{
|
|
int32 NumNodes = NodeCount[i].GetValue();
|
|
if (NumNodes > 0)
|
|
{
|
|
TotalNodeCount += NumNodes;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return TotalNodeCount;
|
|
}
|
|
|
|
int64 FLidarPointCloudOctree::GetAllocatedSize() const
|
|
{
|
|
if (PreviousPointCount != GetNumPoints() || PreviousNodeCount != GetNumNodes())
|
|
{
|
|
FLidarPointCloudOctree* mutable_this = const_cast<FLidarPointCloudOctree*>(this);
|
|
mutable_this->RefreshAllocatedSize();
|
|
}
|
|
|
|
return PreviousAllocatedSize;
|
|
}
|
|
|
|
int64 FLidarPointCloudOctree::GetAllocatedStructureSize() const
|
|
{
|
|
if (PreviousPointCount != GetNumPoints() || PreviousNodeCount != GetNumNodes())
|
|
{
|
|
FLidarPointCloudOctree* mutable_this = const_cast<FLidarPointCloudOctree*>(this);
|
|
mutable_this->RefreshAllocatedSize();
|
|
}
|
|
|
|
return PreviousAllocatedStructureSize;
|
|
}
|
|
|
|
float FLidarPointCloudOctree::GetEstimatedPointSpacing() const
|
|
{
|
|
float Spacing = 0;
|
|
const int64 TotalPointCount = GetNumPoints();
|
|
|
|
for (int32 i = 0; i < PointCount.Num(); ++i)
|
|
{
|
|
Spacing += SharedData[i].GridSize * PointCount[i].GetValue() / TotalPointCount;
|
|
}
|
|
|
|
return Spacing;
|
|
}
|
|
|
|
void FLidarPointCloudOctree::BuildCollision(const float& Accuracy, const bool& bVisibleOnly)
|
|
{
|
|
LidarPointCloudMeshing::BuildCollisionMesh(this, Accuracy, &CollisionMesh);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemoveCollision()
|
|
{
|
|
FScopeLock Lock(&DataLock);
|
|
|
|
CollisionMesh.~FTriMeshCollisionData();
|
|
new (&CollisionMesh) FTriMeshCollisionData();
|
|
}
|
|
|
|
void FLidarPointCloudOctree::BuildStaticMeshBuffers(float CellSize, LidarPointCloudMeshing::FMeshBuffers* OutMeshBuffers, const FTransform& Transform)
|
|
{
|
|
if(CellSize == 0)
|
|
{
|
|
CellSize = GetEstimatedPointSpacing() * 1.1f;
|
|
}
|
|
|
|
LidarPointCloudMeshing::BuildStaticMeshBuffers(this, CellSize, false, OutMeshBuffers, Transform);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPoints(TArray<FLidarPointCloudPoint*>& SelectedPoints, int64 StartIndex /*= 0*/, int64 Count /*= -1*/)
|
|
{
|
|
GetPoints_Internal(SelectedPoints, StartIndex, Count);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPoints(TArray64<FLidarPointCloudPoint*>& SelectedPoints, int64 StartIndex /*= 0*/, int64 Count /*= -1*/)
|
|
{
|
|
GetPoints_Internal(SelectedPoints, StartIndex, Count);
|
|
}
|
|
|
|
template<typename T>
|
|
void FLidarPointCloudOctree::GetPoints_Internal(TArray<FLidarPointCloudPoint*, T>& Points, int64 StartIndex, int64 Count)
|
|
{
|
|
check(StartIndex >= 0 && StartIndex < GetNumPoints());
|
|
|
|
if (Count < 0)
|
|
{
|
|
Count = GetNumPoints();
|
|
}
|
|
|
|
Count = FMath::Min(Count, GetNumPoints() - StartIndex);
|
|
|
|
Points.Empty(Count);
|
|
|
|
FScopeLock Lock(&DataLock);
|
|
|
|
ITERATE_NODES(
|
|
{
|
|
// If no data is required, quit
|
|
if (Count == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Should this node's points be injected?
|
|
if (StartIndex < CurrentNode->GetNumPoints())
|
|
{
|
|
// If the index is at 0 and the requested count is at least the size of this node, append whole arrays
|
|
if (StartIndex == 0 && Count >= CurrentNode->GetNumPoints())
|
|
{
|
|
// Expand the array
|
|
int64 Offset = Points.Num();
|
|
Points.AddUninitialized(CurrentNode->GetNumPoints());
|
|
FLidarPointCloudPoint** Dest = Points.GetData() + Offset;
|
|
|
|
// Assign pointers to the data
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
*Dest++ = Point;
|
|
}
|
|
|
|
Count -= CurrentNode->GetNumPoints();
|
|
}
|
|
// ... otherwise iterate over needed data
|
|
else
|
|
{
|
|
int64 NumPointsToCopy = FMath::Max(0LL, FMath::Min((int64)CurrentNode->GetNumPoints(), Count + StartIndex) - StartIndex);
|
|
if (NumPointsToCopy > 0)
|
|
{
|
|
// Expand the array
|
|
int64 Offset = Points.Num();
|
|
Points.AddUninitialized(NumPointsToCopy);
|
|
|
|
// Setup pointers
|
|
FLidarPointCloudPoint* Src = CurrentNode->GetData() + StartIndex;
|
|
FLidarPointCloudPoint** Dest = Points.GetData() + Offset;
|
|
|
|
// Assign source pointers to the destination
|
|
for (int64 i = 0; i < NumPointsToCopy; i++)
|
|
{
|
|
*Dest++ = Src++;
|
|
}
|
|
|
|
StartIndex = FMath::Max(0LL, StartIndex - NumPointsToCopy);
|
|
Count -= NumPointsToCopy;
|
|
}
|
|
}
|
|
}
|
|
// ... or skipped completely?
|
|
else
|
|
{
|
|
StartIndex -= CurrentNode->GetNumPoints();
|
|
}
|
|
}, true);
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetPointsInSphere_Internal(TArray<FLidarPointCloudPoint*, T>& SelectedPoints, const FSphere& Sphere, const bool& bVisibleOnly)
|
|
{
|
|
SelectedPoints.Reset();
|
|
PROCESS_IN_SPHERE({ SelectedPoints.Add(Point); });
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetPointsInBox_Internal(TArray<FLidarPointCloudPoint*, T>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly)
|
|
{
|
|
SelectedPoints.Reset();
|
|
PROCESS_IN_BOX({ SelectedPoints.Add(Point); });
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetPointsInBox_Internal(TArray<const FLidarPointCloudPoint*, T>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly) const
|
|
{
|
|
SelectedPoints.Reset();
|
|
PROCESS_IN_BOX_CONST({ SelectedPoints.Add(Point); });
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetPointsInConvexVolume_Internal(TArray<FLidarPointCloudPoint*, T>& SelectedPoints, const FConvexVolume& ConvexVolume, const bool& bVisibleOnly)
|
|
{
|
|
SelectedPoints.Reset();
|
|
PROCESS_IN_CONVEX_VOLUME({ SelectedPoints.Add(Point); });
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetPointsAsCopies_Internal(TArray<FLidarPointCloudPoint, T>& Points, const FTransform* LocalToWorld, int64 StartIndex, int64 Count) const
|
|
{
|
|
// If empty, abort
|
|
if (GetNumPoints() == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Make sure to operate on correct range
|
|
check(StartIndex >= 0 && StartIndex < GetNumPoints());
|
|
|
|
if (Count < 0)
|
|
{
|
|
Count = GetNumPoints();
|
|
}
|
|
|
|
Count = FMath::Min(Count, GetNumPoints() - StartIndex);
|
|
|
|
// TArray only supports int32 ax max number of elements!
|
|
check(Count <= INT32_MAX);
|
|
|
|
Points.Empty(Count);
|
|
|
|
FScopeLock Lock(&DataLock);
|
|
|
|
if (LocalToWorld)
|
|
{
|
|
const FTransform3f Transform = (FTransform3f)*LocalToWorld;
|
|
ITERATE_NODES_CONST({
|
|
// If no data is required, quit
|
|
if (Count == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Should this node's points be injected?
|
|
if (StartIndex < CurrentNode->GetNumPoints())
|
|
{
|
|
const int32 NumPointsToCopy = FMath::Max(0LL, FMath::Min((int64)CurrentNode->GetNumPoints(), Count + StartIndex) - StartIndex);
|
|
if (NumPointsToCopy > 0)
|
|
{
|
|
for (FLidarPointCloudPoint* Point = CurrentNode->GetData() + StartIndex, *DataEnd = Point + StartIndex + NumPointsToCopy; Point != DataEnd; ++Point)
|
|
{
|
|
Points.Add(Point->Transform(Transform));
|
|
}
|
|
|
|
StartIndex = FMath::Max(0LL, StartIndex - NumPointsToCopy);
|
|
Count -= NumPointsToCopy;
|
|
}
|
|
}
|
|
// ... or skipped completely?
|
|
else
|
|
{
|
|
StartIndex -= CurrentNode->GetNumPoints();
|
|
}
|
|
}, true);
|
|
}
|
|
else
|
|
{
|
|
ITERATE_NODES_CONST({
|
|
// If no data is required, quit
|
|
if (Count == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Should this node's points be injected?
|
|
if (StartIndex < CurrentNode->GetNumPoints())
|
|
{
|
|
// If the index is at 0 and the requested count is at least the size of this node, append whole arrays
|
|
if (StartIndex == 0 && Count >= CurrentNode->GetNumPoints())
|
|
{
|
|
Points.Append(CurrentNode->GetData(), CurrentNode->GetNumPoints());
|
|
Count -= CurrentNode->GetNumPoints();
|
|
}
|
|
// ... otherwise iterate over needed data
|
|
else
|
|
{
|
|
int32 NumPointsToCopy = FMath::Max(0LL, FMath::Min((int64)CurrentNode->GetNumPoints(), Count + StartIndex) - StartIndex);
|
|
if (NumPointsToCopy > 0)
|
|
{
|
|
int32 SrcOffset = StartIndex;
|
|
int32 DestOffset = Points.Num();
|
|
|
|
// Expand the array
|
|
Points.AddUninitialized(NumPointsToCopy);
|
|
|
|
// Copy contents
|
|
FMemory::Memcpy(Points.GetData() + DestOffset, CurrentNode->GetData() + SrcOffset, NumPointsToCopy * sizeof(FLidarPointCloudPoint));
|
|
|
|
StartIndex = FMath::Max(0LL, StartIndex - NumPointsToCopy);
|
|
Count -= NumPointsToCopy;
|
|
}
|
|
}
|
|
}
|
|
// ... or skipped completely?
|
|
else
|
|
{
|
|
StartIndex -= CurrentNode->GetNumPoints();
|
|
}
|
|
}, true);
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInSphere(TArray64<FLidarPointCloudPoint*>& SelectedPoints, const FSphere& Sphere, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInSphere_Internal(SelectedPoints, Sphere, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInSphere(TArray<FLidarPointCloudPoint*>& SelectedPoints, const FSphere& Sphere, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInSphere_Internal(SelectedPoints, Sphere, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInBox(TArray64<FLidarPointCloudPoint*>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInBox_Internal(SelectedPoints, Box, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInBox(TArray<FLidarPointCloudPoint*>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInBox_Internal(SelectedPoints, Box, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInBox(TArray64<const FLidarPointCloudPoint*>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly) const
|
|
{
|
|
GetPointsInBox_Internal(SelectedPoints, Box, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInBox(TArray<const FLidarPointCloudPoint*>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly) const
|
|
{
|
|
GetPointsInBox_Internal(SelectedPoints, Box, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInConvexVolume(TArray<FLidarPointCloudPoint*>& SelectedPoints, const FConvexVolume& ConvexVolume, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInConvexVolume_Internal(SelectedPoints, ConvexVolume, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInConvexVolume(TArray64<FLidarPointCloudPoint*>& SelectedPoints, const FConvexVolume& ConvexVolume, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInConvexVolume_Internal(SelectedPoints, ConvexVolume, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInFrustum(TArray<FLidarPointCloudPoint*>& SelectedPoints, const FConvexVolume& Frustum, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInConvexVolume_Internal(SelectedPoints, Frustum, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInFrustum(TArray64<FLidarPointCloudPoint*>& SelectedPoints, const FConvexVolume& Frustum, const bool& bVisibleOnly)
|
|
{
|
|
GetPointsInConvexVolume_Internal(SelectedPoints, Frustum, bVisibleOnly);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsAsCopies(TArray64<FLidarPointCloudPoint>& SelectedPoints, const FTransform* LocalToWorld, int64 StartIndex /*= 0*/, int64 Count /*= -1*/) const
|
|
{
|
|
GetPointsAsCopies_Internal(SelectedPoints, LocalToWorld, StartIndex, Count);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsAsCopies(TArray<FLidarPointCloudPoint>& SelectedPoints, const FTransform* LocalToWorld, int64 StartIndex /*= 0*/, int64 Count /*= -1*/) const
|
|
{
|
|
GetPointsAsCopies_Internal(SelectedPoints, LocalToWorld, StartIndex, Count);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsAsCopiesInBatches(TFunction<void(TSharedPtr<TArray64<FLidarPointCloudPoint>>)> Action, const int64& BatchSize, const bool& bVisibleOnly)
|
|
{
|
|
TSharedPtr<TArray64<FLidarPointCloudPoint>> Points(new TArray64<FLidarPointCloudPoint>());
|
|
Points->Reserve(BatchSize);
|
|
|
|
TQueue<FLidarPointCloudOctreeNode*> Nodes;
|
|
FLidarPointCloudOctreeNode* CurrentNode = nullptr;
|
|
Nodes.Enqueue(Root);
|
|
while (Nodes.Dequeue(CurrentNode))
|
|
{
|
|
FOR_RO(Point, CurrentNode)
|
|
{
|
|
if (Points->Add(*Point) == BatchSize - 1)
|
|
{
|
|
Action(Points);
|
|
Points = TSharedPtr<TArray64<FLidarPointCloudPoint>>(new TArray64<FLidarPointCloudPoint>());
|
|
Points->Reserve(BatchSize);
|
|
}
|
|
}
|
|
|
|
for (FLidarPointCloudOctreeNode* Child : CurrentNode->Children)
|
|
{
|
|
Nodes.Enqueue(Child);
|
|
}
|
|
|
|
CurrentNode->ReleaseData(false);
|
|
}
|
|
|
|
if (Points->Num() > 0)
|
|
{
|
|
Action(Points);
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetPointsInSphereAsCopies_Internal(TArray<FLidarPointCloudPoint, T>& SelectedPoints, const FSphere& Sphere, const bool& bVisibleOnly, const FTransform* LocalToWorld) const
|
|
{
|
|
SelectedPoints.Reset();
|
|
if (LocalToWorld)
|
|
{
|
|
const FTransform3f Transform = (FTransform3f)*LocalToWorld;
|
|
PROCESS_IN_SPHERE_CONST({ SelectedPoints.Add(Point->Transform(Transform)); });
|
|
}
|
|
else
|
|
{
|
|
PROCESS_IN_SPHERE_CONST({ SelectedPoints.Add(*Point); });
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetPointsInBoxAsCopies_Internal(TArray<FLidarPointCloudPoint, T>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly, const FTransform* LocalToWorld) const
|
|
{
|
|
SelectedPoints.Reset();
|
|
if (LocalToWorld)
|
|
{
|
|
const FTransform3f Transform = (FTransform3f)*LocalToWorld;
|
|
PROCESS_IN_BOX_CONST({ SelectedPoints.Add(Point->Transform(Transform)); });
|
|
}
|
|
else
|
|
{
|
|
PROCESS_IN_BOX_CONST({ SelectedPoints.Add(*Point); });
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInSphereAsCopies(TArray64<FLidarPointCloudPoint>& SelectedPoints, const FSphere& Sphere, const bool& bVisibleOnly, const FTransform* LocalToWorld) const
|
|
{
|
|
GetPointsInSphereAsCopies_Internal(SelectedPoints, Sphere, bVisibleOnly, LocalToWorld);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInSphereAsCopies(TArray<FLidarPointCloudPoint>& SelectedPoints, const FSphere& Sphere, const bool& bVisibleOnly, const FTransform* LocalToWorld) const
|
|
{
|
|
GetPointsInSphereAsCopies_Internal(SelectedPoints, Sphere, bVisibleOnly, LocalToWorld);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInBoxAsCopies(TArray64<FLidarPointCloudPoint>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly, const FTransform* LocalToWorld) const
|
|
{
|
|
GetPointsInBoxAsCopies_Internal(SelectedPoints, Box, bVisibleOnly, LocalToWorld);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetPointsInBoxAsCopies(TArray<FLidarPointCloudPoint>& SelectedPoints, const FBox& Box, const bool& bVisibleOnly, const FTransform* LocalToWorld) const
|
|
{
|
|
GetPointsInBoxAsCopies_Internal(SelectedPoints, Box, bVisibleOnly, LocalToWorld);
|
|
}
|
|
|
|
FLidarPointCloudPoint* FLidarPointCloudOctree::RaycastSingle(const FLidarPointCloudRay& Ray, const float& Radius, const bool& bVisibleOnly)
|
|
{
|
|
PROCESS_BY_RAY({ return Point; });
|
|
return nullptr;
|
|
}
|
|
|
|
bool FLidarPointCloudOctree::RaycastMulti(const FLidarPointCloudRay& Ray, const float& Radius, const bool& bVisibleOnly, TArray<FLidarPointCloudPoint*>& OutHits)
|
|
{
|
|
OutHits.Reset();
|
|
PROCESS_BY_RAY({ OutHits.Add(Point); });
|
|
return OutHits.Num() > 0;
|
|
}
|
|
|
|
bool FLidarPointCloudOctree::RaycastMulti(const FLidarPointCloudRay& Ray, const float& Radius, const bool& bVisibleOnly, const FTransform* LocalToWorld, TArray<FLidarPointCloudPoint>& OutHits)
|
|
{
|
|
OutHits.Reset();
|
|
if (LocalToWorld)
|
|
{
|
|
const FTransform3f Transform = (FTransform3f)*LocalToWorld;
|
|
PROCESS_BY_RAY_CONST({ OutHits.Add(Point->Transform(Transform)); });
|
|
}
|
|
else
|
|
{
|
|
PROCESS_BY_RAY_CONST({ OutHits.Add(*Point); });
|
|
}
|
|
return OutHits.Num() > 0;
|
|
}
|
|
|
|
bool FLidarPointCloudOctree::HasPointsInSphere(const FSphere& Sphere, const bool& bVisibleOnly) const
|
|
{
|
|
PROCESS_IN_SPHERE_CONST({ return true; });
|
|
return false;
|
|
}
|
|
|
|
bool FLidarPointCloudOctree::HasPointsInBox(const FBox& Box, const bool& bVisibleOnly) const
|
|
{
|
|
PROCESS_IN_BOX_CONST({ return true; });
|
|
return false;
|
|
}
|
|
|
|
bool FLidarPointCloudOctree::HasPointsByRay(const FLidarPointCloudRay& Ray, const float& Radius, const bool& bVisibleOnly) const
|
|
{
|
|
PROCESS_BY_RAY_CONST({ return true; });
|
|
return false;
|
|
}
|
|
|
|
void FLidarPointCloudOctree::SetVisibilityOfPointsInSphere(const bool& bNewVisibility, const FSphere& Sphere)
|
|
{
|
|
// Build a box to quickly filter out the points - (IsInsideOrOn vs comparing DistSquared)
|
|
const FBox Box(Sphere.Center - FVector(Sphere.W), Sphere.Center + FVector(Sphere.W));
|
|
const float RadiusSq = Sphere.W * Sphere.W;
|
|
|
|
ITERATE_NODES({
|
|
// Skip node if it already has all points set to the required visibility state
|
|
bool bSkipNode = (CurrentNode->NumVisiblePoints == CurrentNode->GetNumPoints() && bNewVisibility) || (CurrentNode->NumVisiblePoints == 0 && !bNewVisibility);
|
|
if (!bSkipNode)
|
|
{
|
|
CurrentNode->NumVisiblePoints = 0;
|
|
|
|
// If node fully inside the radius - do not check individual points
|
|
if (CurrentNode->GetSphereBounds().IsInside(Sphere))
|
|
{
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
Point->bVisible = bNewVisibility;
|
|
}
|
|
|
|
if (bNewVisibility)
|
|
{
|
|
CurrentNode->NumVisiblePoints = CurrentNode->GetNumPoints();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
if (Point->bVisible != bNewVisibility && POINT_IN_SPHERE)
|
|
{
|
|
Point->bVisible = bNewVisibility;
|
|
}
|
|
|
|
if (Point->bVisible)
|
|
{
|
|
++CurrentNode->NumVisiblePoints;
|
|
}
|
|
}
|
|
}
|
|
|
|
CurrentNode->bVisibilityDirty = false;
|
|
|
|
// Sort points to speed up rendering
|
|
CurrentNode->SortVisiblePoints();
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
}, NODE_IN_BOX);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::SetVisibilityOfPointsInBox(const bool& bNewVisibility, const FBox& Box)
|
|
{
|
|
ITERATE_NODES({
|
|
// Skip node if it already has all points set to the required visibility state
|
|
bool bSkipNode = (CurrentNode->NumVisiblePoints == CurrentNode->GetNumPoints() && bNewVisibility) || (CurrentNode->NumVisiblePoints == 0 && !bNewVisibility);
|
|
if (!bSkipNode)
|
|
{
|
|
CurrentNode->NumVisiblePoints = 0;
|
|
|
|
// If node fully inside the radius - do not check individual points
|
|
if (Box.IsInsideOrOn((FVector)(CurrentNode->Center - SharedData[CurrentNode->Depth].Extent)) && Box.IsInsideOrOn((FVector)(CurrentNode->Center + SharedData[CurrentNode->Depth].Extent)))
|
|
{
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
Point->bVisible = bNewVisibility;
|
|
}
|
|
|
|
if (bNewVisibility)
|
|
{
|
|
CurrentNode->NumVisiblePoints = CurrentNode->GetNumPoints();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
if (Point->bVisible != bNewVisibility && POINT_IN_BOX)
|
|
{
|
|
Point->bVisible = bNewVisibility;
|
|
}
|
|
|
|
if (Point->bVisible)
|
|
{
|
|
++CurrentNode->NumVisiblePoints;
|
|
}
|
|
}
|
|
}
|
|
|
|
CurrentNode->bVisibilityDirty = false;
|
|
|
|
// Sort points to speed up rendering
|
|
CurrentNode->SortVisiblePoints();
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
}, NODE_IN_BOX);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::SetVisibilityOfFirstPointByRay(const bool& bNewVisibility, const FLidarPointCloudRay& Ray, const float& Radius)
|
|
{
|
|
bool bPointProcessed = false;
|
|
const float RadiusSq = Radius * Radius;
|
|
ITERATE_NODES({
|
|
// Skip node if it already has all points set to the required visibility state
|
|
bool bSkipNode = (CurrentNode->NumVisiblePoints == CurrentNode->GetNumPoints() && bNewVisibility) || (CurrentNode->NumVisiblePoints == 0 && !bNewVisibility);
|
|
if (!bSkipNode && Ray.Intersects(CurrentNode->GetBounds()))
|
|
{
|
|
CurrentNode->NumVisiblePoints = 0;
|
|
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
if (Point->bVisible != bNewVisibility && POINT_BY_RAY)
|
|
{
|
|
Point->bVisible = bNewVisibility;
|
|
bPointProcessed = true;
|
|
}
|
|
|
|
if (Point->bVisible)
|
|
{
|
|
++CurrentNode->NumVisiblePoints;
|
|
}
|
|
|
|
if (bPointProcessed)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
CurrentNode->bVisibilityDirty = false;
|
|
|
|
// Sort points to speed up rendering
|
|
CurrentNode->SortVisiblePoints();
|
|
CurrentNode->bRenderDataDirty = true;
|
|
|
|
if (bPointProcessed)
|
|
{
|
|
return;
|
|
}
|
|
|
|
for (FLidarPointCloudOctreeNode*& Child : CurrentNode->Children)
|
|
{
|
|
Nodes.Enqueue(Child);
|
|
}
|
|
}
|
|
}, false);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::SetVisibilityOfPointsByRay(const bool& bNewVisibility, const FLidarPointCloudRay& Ray, const float& Radius)
|
|
{
|
|
const float RadiusSq = Radius * Radius;
|
|
ITERATE_NODES({
|
|
// Skip node if it already has all points set to the required visibility state
|
|
bool bSkipNode = (CurrentNode->NumVisiblePoints == CurrentNode->GetNumPoints() && bNewVisibility) || (CurrentNode->NumVisiblePoints == 0 && !bNewVisibility);
|
|
if (!bSkipNode && Ray.Intersects(CurrentNode->GetBounds()))
|
|
{
|
|
CurrentNode->NumVisiblePoints = 0;
|
|
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
if (Point->bVisible != bNewVisibility && POINT_BY_RAY)
|
|
{
|
|
Point->bVisible = bNewVisibility;
|
|
}
|
|
|
|
if (Point->bVisible)
|
|
{
|
|
++CurrentNode->NumVisiblePoints;
|
|
}
|
|
}
|
|
|
|
CurrentNode->bVisibilityDirty = false;
|
|
|
|
// Sort points to speed up rendering
|
|
CurrentNode->SortVisiblePoints();
|
|
CurrentNode->bRenderDataDirty = true;
|
|
|
|
for (FLidarPointCloudOctreeNode*& Child : CurrentNode->Children)
|
|
{
|
|
Nodes.Enqueue(Child);
|
|
}
|
|
}
|
|
}, false);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::HideAll()
|
|
{
|
|
ITERATE_NODES({
|
|
if (CurrentNode->NumVisiblePoints > 0)
|
|
{
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
Point->bVisible = false;
|
|
}
|
|
|
|
CurrentNode->NumVisiblePoints = 0;
|
|
CurrentNode->bVisibilityDirty = false;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
}, true);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::UnhideAll()
|
|
{
|
|
ITERATE_NODES({
|
|
if (CurrentNode->NumVisiblePoints != CurrentNode->GetNumPoints())
|
|
{
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
Point->bVisible = true;
|
|
}
|
|
|
|
CurrentNode->NumVisiblePoints = CurrentNode->GetNumPoints();
|
|
CurrentNode->bVisibilityDirty = false;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
}, true);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ExecuteActionOnAllPoints(TFunction<void(FLidarPointCloudPoint*)> Action, const bool& bVisibleOnly)
|
|
{
|
|
PROCESS_ALL_EX({ Action(Point); }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ExecuteActionOnPointsInSphere(TFunction<void(FLidarPointCloudPoint*)> Action, const FSphere& Sphere, const bool& bVisibleOnly)
|
|
{
|
|
PROCESS_IN_SPHERE_EX({ Action(Point); }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ExecuteActionOnPointsInBox(TFunction<void(FLidarPointCloudPoint*)> Action, const FBox& Box, const bool& bVisibleOnly)
|
|
{
|
|
PROCESS_IN_BOX_EX({ Action(Point); }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ExecuteActionOnFirstPointByRay(TFunction<void(FLidarPointCloudPoint*)> Action, const FLidarPointCloudRay& Ray, const float& Radius, bool bVisibleOnly)
|
|
{
|
|
PROCESS_BY_RAY({
|
|
Action(Point);
|
|
CurrentNode->bRenderDataDirty = true;
|
|
return;
|
|
});
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ExecuteActionOnPointsByRay(TFunction<void(FLidarPointCloudPoint*)> Action, const FLidarPointCloudRay& Ray, const float& Radius, bool bVisibleOnly)
|
|
{
|
|
PROCESS_BY_RAY_EX({ Action(Point); }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ApplyColorToAllPoints(const FColor& NewColor, const bool& bVisibleOnly)
|
|
{
|
|
PROCESS_ALL_EX({ Point->Color = NewColor; }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ApplyColorToPointsInSphere(const FColor& NewColor, const FSphere& Sphere, const bool& bVisibleOnly)
|
|
{
|
|
PROCESS_IN_SPHERE_EX({ Point->Color = NewColor; }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ApplyColorToPointsInBox(const FColor& NewColor, const FBox& Box, const bool& bVisibleOnly)
|
|
{
|
|
PROCESS_IN_BOX_EX({ Point->Color = NewColor; }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ApplyColorToFirstPointByRay(const FColor& NewColor, const FLidarPointCloudRay& Ray, const float& Radius, bool bVisibleOnly)
|
|
{
|
|
PROCESS_BY_RAY({
|
|
Point->Color = NewColor;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
return;
|
|
});
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ApplyColorToPointsByRay(const FColor& NewColor, const FLidarPointCloudRay& Ray, const float& Radius, bool bVisibleOnly)
|
|
{
|
|
PROCESS_BY_RAY_EX({ Point->Color = NewColor; }, { CurrentNode->bRenderDataDirty = true; });
|
|
}
|
|
|
|
void FLidarPointCloudOctree::MarkPointVisibilityDirty()
|
|
{
|
|
ITERATE_NODES(
|
|
{
|
|
CurrentNode->bVisibilityDirty = true;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}, true);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::MarkRenderDataDirty()
|
|
{
|
|
ITERATE_NODES({ CurrentNode->bRenderDataDirty = true; }, true);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::MarkRenderDataInSphereDirty(const FSphere& Sphere)
|
|
{
|
|
const FBox Box(Sphere.Center - FVector(Sphere.W), Sphere.Center + FVector(Sphere.W));
|
|
ITERATE_NODES({ CurrentNode->bRenderDataDirty = true; }, NODE_IN_BOX);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::MarkRenderDataInConvexVolumeDirty(const FConvexVolume& ConvexVolume)
|
|
{
|
|
ITERATE_NODES({ CurrentNode->bRenderDataDirty = true; }, NODE_IN_CONVEX_VOLUME);
|
|
}
|
|
|
|
#if WITH_EDITOR
|
|
void FLidarPointCloudOctree::SelectByConvexVolume(const FConvexVolume& ConvexVolume, bool bAdditive, bool bVisibleOnly)
|
|
{
|
|
ITERATE_NODES(
|
|
{
|
|
// This will be used to determine if the payload should be released once convex check is complete
|
|
const bool bHadData = CurrentNode->bHasData;
|
|
bool bModified = false;
|
|
|
|
// Proactively flag as selected to avoid async release of the node's data
|
|
const bool bHadSelection = CurrentNode->bHasSelection;
|
|
CurrentNode->bHasSelection = true;
|
|
|
|
PROCESS_IN_CONVEX_VOLUME_BODY(
|
|
{
|
|
bModified = true;
|
|
Point->bSelected = bAdditive;
|
|
}, _RO)
|
|
|
|
if(bModified)
|
|
{
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
else
|
|
{
|
|
CurrentNode->bHasSelection = bHadSelection;
|
|
|
|
// Only attempt to release payload if the node didn't have any data loaded before
|
|
if(!bHadData)
|
|
{
|
|
CurrentNode->ReleaseData();
|
|
}
|
|
}
|
|
}, NODE_IN_CONVEX_VOLUME);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::SelectBySphere(const FSphere& Sphere, bool bAdditive, bool bVisibleOnly)
|
|
{
|
|
const FBox Box(Sphere.Center - FVector(Sphere.W), Sphere.Center + FVector(Sphere.W));
|
|
const float RadiusSq = Sphere.W * Sphere.W;
|
|
|
|
ITERATE_NODES(
|
|
{
|
|
// This will be used to determine if the payload should be released once convex check is complete
|
|
const bool bHadData = CurrentNode->bHasData;
|
|
bool bModified = false;
|
|
|
|
// Proactively flag as selected to avoid async release of the node's data
|
|
const bool bHadSelection = CurrentNode->bHasSelection;
|
|
CurrentNode->bHasSelection = true;
|
|
|
|
PROCESS_IN_SPHERE_BODY(
|
|
{
|
|
bModified = true;
|
|
Point->bSelected = bAdditive;
|
|
}, _RO)
|
|
|
|
if(bModified)
|
|
{
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
else
|
|
{
|
|
CurrentNode->bHasSelection = bHadSelection;
|
|
|
|
// Only attempt to release payload if the node didn't have any data loaded before
|
|
if(!bHadData)
|
|
{
|
|
CurrentNode->ReleaseData();
|
|
}
|
|
}
|
|
}, NODE_IN_BOX);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::HideSelected()
|
|
{
|
|
ITERATE_SELECTED({
|
|
Point->bVisible = false;
|
|
Point->bSelected = false;
|
|
}, {
|
|
CurrentNode->bHasSelection = false;
|
|
CurrentNode->bCanReleaseData = false;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
CurrentNode->bVisibilityDirty = true;
|
|
});
|
|
}
|
|
|
|
void FLidarPointCloudOctree::DeleteSelected()
|
|
{
|
|
ITERATE_SELECTED_NODES({
|
|
const int32 NumElements = CurrentNode->GetNumPoints();
|
|
FLidarPointCloudPoint* Start = CurrentNode->GetData();
|
|
|
|
int64 NumRemoved = 0;
|
|
|
|
for (FLidarPointCloudPoint* P = Start, *DataEnd = Start + NumElements; P != DataEnd; ++P)
|
|
{
|
|
if (P->bSelected)
|
|
{
|
|
CurrentNode->Data.RemoveAtSwap(P - Start, EAllowShrinking::No);
|
|
++NumRemoved;
|
|
--DataEnd;
|
|
--P;
|
|
}
|
|
}
|
|
|
|
CurrentNode->AddPointCount(-NumRemoved);
|
|
|
|
// Reduce space usage
|
|
CurrentNode->Data.Shrink();
|
|
|
|
// Copy the updated array back to the BulkData
|
|
CurrentNode->NumPoints = CurrentNode->Data.Num();
|
|
|
|
CurrentNode->bHasSelection = false;
|
|
CurrentNode->bCanReleaseData = false;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
CurrentNode->bVisibilityDirty = true;
|
|
});
|
|
}
|
|
|
|
void FLidarPointCloudOctree::InvertSelection()
|
|
{
|
|
ITERATE_NODES(
|
|
{
|
|
// This will be used to determine if the payload should be released once invert is complete
|
|
bool bNewHasSelection = false;
|
|
|
|
FOR_RO(Point, CurrentNode)
|
|
{
|
|
Point->bSelected = !Point->bSelected;
|
|
if(Point->bSelected)
|
|
{
|
|
bNewHasSelection = true;
|
|
}
|
|
}
|
|
|
|
CurrentNode->bHasSelection = bNewHasSelection;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
|
|
// Only attempt to release payload if the node isn't also used by rendering
|
|
if(!bNewHasSelection && !CurrentNode->bInUse)
|
|
{
|
|
CurrentNode->ReleaseData();
|
|
}
|
|
}, true);
|
|
}
|
|
|
|
int64 FLidarPointCloudOctree::NumSelectedPoints() const
|
|
{
|
|
int64 Count = 0;
|
|
ITERATE_SELECTED({ ++Count; }, {});
|
|
return Count;
|
|
}
|
|
|
|
bool FLidarPointCloudOctree::HasSelectedPoints() const
|
|
{
|
|
ITERATE_SELECTED({ return true; }, {});
|
|
return false;
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetSelectedPoints(TArray64<FLidarPointCloudPoint*>& SelectedPoints) const
|
|
{
|
|
ITERATE_SELECTED({ SelectedPoints.Add(Point); }, {});
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::GetSelectedPointsAsCopies_Internal(TArray<FLidarPointCloudPoint, T>& SelectedPoints, const FTransform& Transform) const
|
|
{
|
|
const FTransform3f Transform3F = (FTransform3f)Transform;
|
|
|
|
ITERATE_SELECTED(
|
|
{
|
|
SelectedPoints.Add(Point->Transform(Transform3F));
|
|
},{});
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetSelectedPointsAsCopies(TArray<FLidarPointCloudPoint>& SelectedPoints, const FTransform& Transform) const
|
|
{
|
|
GetSelectedPointsAsCopies_Internal(SelectedPoints, Transform);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetSelectedPointsAsCopies(TArray64<FLidarPointCloudPoint>& SelectedPoints, const FTransform& Transform) const
|
|
{
|
|
GetSelectedPointsAsCopies_Internal(SelectedPoints, Transform);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::GetSelectedPointsInBox(TArray64<const FLidarPointCloudPoint*>& SelectedPoints, const FBox& Box) const
|
|
{
|
|
constexpr bool bVisibleOnly = false;
|
|
|
|
SelectedPoints.Reset();
|
|
PROCESS_IN_BOX({
|
|
if(Point->bSelected)
|
|
{
|
|
SelectedPoints.Add(Point);
|
|
}
|
|
});
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ClearSelection()
|
|
{
|
|
ITERATE_SELECTED({
|
|
Point->bSelected = false;
|
|
}, {
|
|
CurrentNode->bRenderDataDirty = true;
|
|
CurrentNode->bHasSelection = false;
|
|
|
|
// Only attempt to release payload if the node isn't also used by rendering
|
|
if(!CurrentNode->bInUse)
|
|
{
|
|
CurrentNode->ReleaseData();
|
|
}
|
|
});
|
|
}
|
|
|
|
void FLidarPointCloudOctree::BuildStaticMeshBuffersForSelection(float CellSize, LidarPointCloudMeshing::FMeshBuffers* OutMeshBuffers, const FTransform& Transform)
|
|
{
|
|
if(CellSize == 0)
|
|
{
|
|
CellSize = GetEstimatedPointSpacing() * 1.1f;
|
|
}
|
|
|
|
LidarPointCloudMeshing::BuildStaticMeshBuffers(this, CellSize, true, OutMeshBuffers, Transform);
|
|
}
|
|
#endif // WITH_EDITOR
|
|
|
|
void FLidarPointCloudOctree::MarkRenderDataInFrustumDirty(const FConvexVolume& Frustum)
|
|
{
|
|
MarkRenderDataInConvexVolumeDirty(Frustum);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::Initialize(const FVector3f& InExtent)
|
|
{
|
|
const bool bValidExtent = InExtent.X > 0 && InExtent.Y > 0 && InExtent.Z > 0;
|
|
if (!bValidExtent)
|
|
{
|
|
PC_ERROR("Provided bounds are incorrect: %s", *InExtent.ToString());
|
|
return;
|
|
}
|
|
|
|
Extent = InExtent;
|
|
const FVector3f UniformExtent = FVector3f(InExtent.GetMax());
|
|
|
|
MaxBucketSize = GetDefault<ULidarPointCloudSettings>()->MaxBucketSize;
|
|
NodeGridResolution = GetDefault<ULidarPointCloudSettings>()->NodeGridResolution;
|
|
|
|
// Pre-calculate the shared per LOD data
|
|
for (int32 i = 0; i < SharedData.Num(); i++)
|
|
{
|
|
SharedData[i] = FSharedLODData(UniformExtent / FMath::Pow(2.f, i));
|
|
NodeCount[i].Reset();
|
|
PointCount[i].Reset();
|
|
}
|
|
|
|
Empty(true);
|
|
|
|
bIsFullyLoaded = false;
|
|
}
|
|
|
|
void FLidarPointCloudOctree::InsertPoint(const FLidarPointCloudPoint* Point, ELidarPointCloudDuplicateHandling DuplicateHandling, bool bRefreshPointsBounds, const FVector3f& Translation)
|
|
{
|
|
InsertPoints_Internal(Point, 1, DuplicateHandling, bRefreshPointsBounds, Translation);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::InsertPoints(FLidarPointCloudPoint** Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, bool bRefreshPointsBounds, const FVector3f& Translation)
|
|
{
|
|
InsertPoints_Internal(Points, Count, DuplicateHandling, bRefreshPointsBounds, Translation);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::InsertPoints(const FLidarPointCloudPoint* Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, bool bRefreshPointsBounds, const FVector3f& Translation)
|
|
{
|
|
InsertPoints_Internal(Points, Count, DuplicateHandling, bRefreshPointsBounds, Translation);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::InsertPoints(FLidarPointCloudPoint* Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, bool bRefreshPointsBounds, const FVector3f& Translation)
|
|
{
|
|
InsertPoints_Internal(Points, Count, DuplicateHandling, bRefreshPointsBounds, Translation);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePoint(const FLidarPointCloudPoint* Point)
|
|
{
|
|
if (!Point)
|
|
{
|
|
return;
|
|
}
|
|
|
|
int32 Index = -1;
|
|
FLidarPointCloudOctreeNode* CurrentNode = Root;
|
|
while (CurrentNode)
|
|
{
|
|
const FLidarPointCloudPoint* RESTRICT Start = CurrentNode->GetData();
|
|
const int32 NumElements = CurrentNode->GetNumPoints();
|
|
for (const FLidarPointCloudPoint* RESTRICT P = Start, *RESTRICT DataEnd = Start + NumElements; P != DataEnd; ++P)
|
|
{
|
|
if (P == Point)
|
|
{
|
|
Index = P - Start;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (Index > INDEX_NONE)
|
|
{
|
|
RemovePoint_Internal(CurrentNode, Index);
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
const FVector3f CenterRelativeLocation = Point->Location - CurrentNode->Center;
|
|
CurrentNode = CurrentNode->GetChildNodeAtLocation((CenterRelativeLocation.X > 0 ? 4 : 0) + (CenterRelativeLocation.Y > 0 ? 2 : 0) + (CenterRelativeLocation.Z > 0));
|
|
}
|
|
}
|
|
|
|
RefreshBounds();
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePoint(FLidarPointCloudPoint Point)
|
|
{
|
|
int32 Index = -1;
|
|
FLidarPointCloudOctreeNode* CurrentNode = Root;
|
|
while (CurrentNode)
|
|
{
|
|
const FLidarPointCloudPoint* RESTRICT Start = CurrentNode->GetData();
|
|
const int32 NumElements = CurrentNode->GetNumPoints();
|
|
for (const FLidarPointCloudPoint* RESTRICT P = Start, *RESTRICT DataEnd = Start + NumElements; P != DataEnd; ++P)
|
|
{
|
|
if (*P == Point)
|
|
{
|
|
Index = P - Start;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (Index != INDEX_NONE)
|
|
{
|
|
RemovePoint_Internal(CurrentNode, Index);
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
const FVector3f CenterRelativeLocation = Point.Location - CurrentNode->Center;
|
|
CurrentNode = CurrentNode->GetChildNodeAtLocation((CenterRelativeLocation.X > 0 ? 4 : 0) + (CenterRelativeLocation.Y > 0 ? 2 : 0) + (CenterRelativeLocation.Z > 0));
|
|
}
|
|
}
|
|
|
|
RefreshBounds();
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePoints(TArray<FLidarPointCloudPoint*>& Points)
|
|
{
|
|
RemovePoints_Internal(Points);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePoints(TArray64<FLidarPointCloudPoint*>& Points)
|
|
{
|
|
RemovePoints_Internal(Points);
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::RemovePoints_Internal(TArray<FLidarPointCloudPoint*, T>& Points)
|
|
{
|
|
if (Points.Num() == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
for (FLidarPointCloudPoint** Point = Points.GetData(), ** DataEnd = Point + Points.Num(); Point != DataEnd; ++Point)
|
|
{
|
|
(*Point)->bMarkedForDeletion = true;
|
|
}
|
|
|
|
ITERATE_NODES(
|
|
{
|
|
FLidarPointCloudPoint* Start = CurrentNode->GetData();
|
|
const int32 NumElements = CurrentNode->GetNumPoints();
|
|
|
|
bool bHasPointsToRemove = false;
|
|
for (FLidarPointCloudPoint* P = Start, *DataEnd = Start + NumElements; P != DataEnd; ++P)
|
|
{
|
|
if (P->bMarkedForDeletion)
|
|
{
|
|
bHasPointsToRemove = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (bHasPointsToRemove)
|
|
{
|
|
int64 NumRemoved = 0;
|
|
|
|
for (FLidarPointCloudPoint* P = Start, *DataEnd = Start + NumElements; P != DataEnd; ++P)
|
|
{
|
|
if (P->bMarkedForDeletion)
|
|
{
|
|
CurrentNode->Data.RemoveAtSwap(P - Start, EAllowShrinking::No);
|
|
++NumRemoved;
|
|
--DataEnd;
|
|
--P;
|
|
}
|
|
}
|
|
|
|
// #todo: Fetch points from child nodes / padding points to fill the gap
|
|
|
|
CurrentNode->AddPointCount(-NumRemoved);
|
|
|
|
// Reduce space usage
|
|
CurrentNode->Data.Shrink();
|
|
|
|
// Copy the updated array back to the BulkData
|
|
CurrentNode->NumPoints = CurrentNode->Data.Num();
|
|
CurrentNode->bCanReleaseData = false;
|
|
|
|
// Sort points to speed up rendering
|
|
CurrentNode->SortVisiblePoints();
|
|
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
}, true);
|
|
|
|
RefreshBounds();
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePointsInSphere(const FSphere& Sphere, const bool& bVisibleOnly)
|
|
{
|
|
// #todo: This can be optimized by removing points inline
|
|
TArray<FLidarPointCloudPoint*> SelectedPoints;
|
|
GetPointsInSphere(SelectedPoints, Sphere, bVisibleOnly);
|
|
RemovePoints(SelectedPoints);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePointsInBox(const FBox& Box, const bool& bVisibleOnly)
|
|
{
|
|
// #todo: This can be optimized by removing points inline
|
|
TArray<FLidarPointCloudPoint*> SelectedPoints;
|
|
GetPointsInBox(SelectedPoints, Box, bVisibleOnly);
|
|
RemovePoints(SelectedPoints);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemoveFirstPointByRay(const FLidarPointCloudRay& Ray, const float& Radius, const bool& bVisibleOnly)
|
|
{
|
|
RemovePoint(RaycastSingle(Ray, Radius, bVisibleOnly));
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePointsByRay(const FLidarPointCloudRay& Ray, const float& Radius, const bool& bVisibleOnly)
|
|
{
|
|
// #todo: This can be optimized by removing points inline
|
|
TArray<FLidarPointCloudPoint*> SelectedPoints;
|
|
RaycastMulti(Ray, Radius, bVisibleOnly, SelectedPoints);
|
|
RemovePoints(SelectedPoints);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemoveHiddenPoints()
|
|
{
|
|
ITERATE_NODES(
|
|
{
|
|
FLidarPointCloudPoint* Start = CurrentNode->GetData();
|
|
const int32 NumElements = CurrentNode->GetNumPoints();
|
|
|
|
bool bHasPointsToRemove = false;
|
|
for (FLidarPointCloudPoint* P = Start, *DataEnd = Start + NumElements; P != DataEnd; ++P)
|
|
{
|
|
if (!P->bVisible)
|
|
{
|
|
bHasPointsToRemove = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (bHasPointsToRemove)
|
|
{
|
|
int64 NumRemoved = 0;
|
|
|
|
for (FLidarPointCloudPoint* P = Start, *DataEnd = Start + NumElements; P != DataEnd; ++P)
|
|
{
|
|
if (!P->bVisible)
|
|
{
|
|
CurrentNode->Data.RemoveAtSwap(P - Start, EAllowShrinking::No);
|
|
++NumRemoved;
|
|
--DataEnd;
|
|
--P;
|
|
}
|
|
}
|
|
|
|
// #todo: Fetch points from child nodes / padding points to fill the gap
|
|
|
|
CurrentNode->AddPointCount(-NumRemoved);
|
|
|
|
// Reduce space usage
|
|
CurrentNode->Data.Shrink();
|
|
|
|
// Copy the updated array back to the BulkData
|
|
CurrentNode->NumPoints = CurrentNode->Data.Num();
|
|
CurrentNode->bCanReleaseData = false;
|
|
|
|
// Set visibility data
|
|
CurrentNode->NumVisiblePoints = CurrentNode->GetNumPoints();
|
|
CurrentNode->bVisibilityDirty = false;
|
|
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}
|
|
}, true);
|
|
|
|
RefreshBounds();
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ResetNormals()
|
|
{
|
|
ITERATE_NODES({
|
|
FOR(Point, CurrentNode)
|
|
{
|
|
Point->Normal.Reset();
|
|
}
|
|
|
|
CurrentNode->bRenderDataDirty = true;
|
|
}, true);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::CalculateNormals(FThreadSafeBool* bCancelled, int32 Quality, float Tolerance, TArray64<FLidarPointCloudPoint*>* InPointSelection)
|
|
{
|
|
FScopeBenchmarkTimer Timer("Calculate Normals");
|
|
|
|
TArray64<FLidarPointCloudPoint*> EmptyArray;
|
|
|
|
if (InPointSelection)
|
|
{
|
|
for (FLidarPointCloudPoint** Point = InPointSelection->GetData(), **DataEnd = Point + InPointSelection->Num(); Point != DataEnd; ++Point)
|
|
{
|
|
(*Point)->Normal.Reset();
|
|
}
|
|
|
|
MarkRenderDataDirty();
|
|
}
|
|
else
|
|
{
|
|
ResetNormals();
|
|
}
|
|
|
|
LidarPointCloudMeshing::CalculateNormals(this, bCancelled, Quality, Tolerance, InPointSelection ? *InPointSelection : EmptyArray);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::Empty(bool bDestroyNodes)
|
|
{
|
|
FScopeLock OctreeLock(&DataLock);
|
|
MarkTraversalOctreesForInvalidation();
|
|
|
|
if (bDestroyNodes)
|
|
{
|
|
// Reset node counters
|
|
for (FThreadSafeCounter& Count : NodeCount)
|
|
{
|
|
Count.Reset();
|
|
}
|
|
|
|
// Perform the actual deletion on the RT, so there is no concurrent access issues
|
|
ENQUEUE_RENDER_COMMAND(LidarPointCloudOctreeNode_DeleteOldData)([OldRoot = Root](FRHICommandListImmediate& RHICmdList)
|
|
{
|
|
delete OldRoot;
|
|
});
|
|
|
|
Root = new FLidarPointCloudOctreeNode(this, 0);
|
|
|
|
QueuedNodes.Empty();
|
|
NodesInUse.Reset();
|
|
}
|
|
else
|
|
{
|
|
Root->Empty(true);
|
|
}
|
|
|
|
// Reset point counters
|
|
for (FThreadSafeCounter64& Count : PointCount)
|
|
{
|
|
Count.Reset();
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::UnregisterTraversalOctree(FLidarPointCloudTraversalOctree* TraversalOctree)
|
|
{
|
|
if (TraversalOctree)
|
|
{
|
|
bool bRemoved = false;
|
|
|
|
for (int32 i = 0; i < LinkedTraversalOctrees.Num(); ++i)
|
|
{
|
|
if (TSharedPtr<FLidarPointCloudTraversalOctree, ESPMode::ThreadSafe> TO = LinkedTraversalOctrees[i].Pin())
|
|
{
|
|
if (TO.Get() == TraversalOctree)
|
|
{
|
|
LinkedTraversalOctrees.RemoveAtSwap(i--);
|
|
bRemoved = true;
|
|
}
|
|
}
|
|
// Remove null
|
|
else
|
|
{
|
|
LinkedTraversalOctrees.RemoveAtSwap(i--);
|
|
}
|
|
}
|
|
|
|
// If nothing is using this Octree, release all non-persistent nodes
|
|
if (bRemoved && LinkedTraversalOctrees.Num() == 0)
|
|
{
|
|
FScopeLock Lock(&DataLock);
|
|
ReleaseAllNodes(false);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::StreamNodes(TArray<FLidarPointCloudOctreeNode*>& Nodes, const float& CurrentTime)
|
|
{
|
|
const float BulkDataLifetime = CurrentTime + GetDefault<ULidarPointCloudSettings>()->CachedNodeLifetime;
|
|
|
|
for (FLidarPointCloudOctreeNode* Node : Nodes)
|
|
{
|
|
// Refresh lifetime of the BulkData
|
|
Node->BulkDataLifetime = BulkDataLifetime;
|
|
|
|
// Enqueue for processing
|
|
QueuedNodes.Enqueue(Node);
|
|
}
|
|
|
|
// Only one process at a time
|
|
if (!bStreamingBusy)
|
|
{
|
|
bStreamingBusy = true;
|
|
|
|
// Add previously queued nodes
|
|
FLidarPointCloudOctreeNode* QueuedNode;
|
|
while (QueuedNodes.Dequeue(QueuedNode))
|
|
{
|
|
if (!QueuedNode->bInUse)
|
|
{
|
|
NodesInUse.Add(QueuedNode);
|
|
}
|
|
}
|
|
|
|
// Process nodes' streaming requests
|
|
// If the release lock has been acquired, release unused nodes in one go
|
|
// Otherwise, just load the ones requested
|
|
FScopeTryLock ReleaseLock(&DataReleaseLock);
|
|
if(ReleaseLock.IsLocked())
|
|
{
|
|
for (int32 i = 0; i < NodesInUse.Num(); ++i)
|
|
{
|
|
FLidarPointCloudOctreeNode* Node = NodesInUse[i];
|
|
|
|
// Check if the node should still be in use
|
|
Node->bInUse = Node->BulkDataLifetime >= CurrentTime;
|
|
|
|
// If node is alive, make sure it has data loaded
|
|
if (Node->bInUse)
|
|
{
|
|
Node->GetData();
|
|
}
|
|
// ... otherwise, release it
|
|
else
|
|
{
|
|
Node->ReleaseData();
|
|
NodesInUse.RemoveAtSwap(i--, EAllowShrinking::No);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (FLidarPointCloudOctreeNode* Node : NodesInUse)
|
|
{
|
|
if (Node->BulkDataLifetime >= CurrentTime)
|
|
{
|
|
Node->GetData();
|
|
}
|
|
}
|
|
}
|
|
|
|
bStreamingBusy = false;
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::LoadAllNodes(bool bLoadPersistently)
|
|
{
|
|
if (bLoadPersistently)
|
|
{
|
|
ITERATE_NODES({ CurrentNode->GetPersistentData(); }, true);
|
|
bIsFullyLoaded = true;
|
|
}
|
|
else
|
|
{
|
|
ITERATE_NODES({ CurrentNode->GetData(); }, true);
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::ReleaseAllNodes(bool bIncludePersistent)
|
|
{
|
|
ITERATE_NODES({ CurrentNode->ReleaseData(bIncludePersistent); }, true);
|
|
|
|
if (bIncludePersistent)
|
|
{
|
|
bIsFullyLoaded = false;
|
|
}
|
|
}
|
|
|
|
bool FLidarPointCloudOctree::IsOptimizedForDynamicData() const
|
|
{
|
|
return Owner && Owner->IsOptimizedForDynamicData();
|
|
}
|
|
|
|
void FLidarPointCloudOctree::OptimizeForDynamicData()
|
|
{
|
|
if (!IsOptimizedForDynamicData())
|
|
{
|
|
// Move all data from the nodes
|
|
TArray<FLidarPointCloudPoint> SourceData;
|
|
SourceData.Reserve(GetNumPoints());
|
|
|
|
ITERATE_NODES(
|
|
{
|
|
SourceData.Append(CurrentNode->Data);
|
|
CurrentNode->Data.Empty();
|
|
}, true);
|
|
|
|
// Destroy current tree structure
|
|
Empty(true);
|
|
|
|
bIsFullyLoaded = true;
|
|
|
|
// Insert data back into the tree
|
|
InsertPoints_Internal(SourceData.GetData(), SourceData.Num(), ELidarPointCloudDuplicateHandling::Ignore, false, FVector3f::ZeroVector);
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::OptimizeForStaticData()
|
|
{
|
|
if (IsOptimizedForDynamicData())
|
|
{
|
|
// Move data out of the root node
|
|
TArray<FLidarPointCloudPoint> SourceData = MoveTemp(Root->Data);
|
|
|
|
// Destroy current tree structure
|
|
Empty(true);
|
|
|
|
bIsFullyLoaded = true;
|
|
|
|
// Insert data back into the tree
|
|
InsertPoints_Internal(SourceData.GetData(), SourceData.Num(), ELidarPointCloudDuplicateHandling::Ignore, false, FVector3f::ZeroVector);
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RefreshAllocatedSize()
|
|
{
|
|
FScopeTryLock Lock(&DataLock);
|
|
if (!Lock.IsLocked())
|
|
{
|
|
return;
|
|
}
|
|
|
|
PreviousPointCount = GetNumPoints();
|
|
PreviousNodeCount = GetNumNodes();
|
|
|
|
PreviousAllocatedStructureSize = sizeof(FLidarPointCloudOctree);
|
|
|
|
PreviousAllocatedStructureSize += SharedData.GetAllocatedSize();
|
|
PreviousAllocatedStructureSize += PointCount.GetAllocatedSize();
|
|
|
|
PreviousAllocatedSize = PreviousAllocatedStructureSize + Root->GetAllocatedSize(true, true);
|
|
PreviousAllocatedStructureSize += Root->GetAllocatedSize(true, false);
|
|
}
|
|
|
|
template <typename T>
|
|
void FLidarPointCloudOctree::InsertPoints_Internal(T Points, const int64& Count, ELidarPointCloudDuplicateHandling DuplicateHandling, bool bRefreshPointsBounds, const FVector3f& Translation)
|
|
{
|
|
Root->InsertPoints(Points, Count, DuplicateHandling, Translation);
|
|
MarkTraversalOctreesForInvalidation();
|
|
if (bRefreshPointsBounds)
|
|
{
|
|
RefreshBounds();
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::RemovePoint_Internal(FLidarPointCloudOctreeNode* Node, int32 Index)
|
|
{
|
|
Node->GetPersistentData();
|
|
Node->AddPointCount(-1);
|
|
Node->bRenderDataDirty = true;
|
|
Node->Data.RemoveAt(Index);
|
|
Node->NumPoints = Node->Data.Num();
|
|
|
|
// #todo: Fetch points from child nodes / padding points to fill the gap
|
|
}
|
|
|
|
void FLidarPointCloudOctree::MarkTraversalOctreesForInvalidation()
|
|
{
|
|
for (int32 i = 0; i < LinkedTraversalOctrees.Num(); ++i)
|
|
{
|
|
if (TSharedPtr<FLidarPointCloudTraversalOctree, ESPMode::ThreadSafe> TraversalOctree = LinkedTraversalOctrees[i].Pin())
|
|
{
|
|
TraversalOctree->bValid = false;
|
|
}
|
|
// Remove null
|
|
else
|
|
{
|
|
LinkedTraversalOctrees.RemoveAtSwap(i--);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::Serialize(FArchive& Ar)
|
|
{
|
|
// Extent
|
|
{
|
|
FVector3f NodesExtent = SharedData[0].Extent;
|
|
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) > 16)
|
|
{
|
|
Ar << NodesExtent;
|
|
}
|
|
else
|
|
{
|
|
FBox Bounds;
|
|
Ar << Bounds;
|
|
NodesExtent = (FVector3f)Bounds.GetExtent();
|
|
}
|
|
|
|
if (Ar.IsLoading())
|
|
{
|
|
Initialize(NodesExtent);
|
|
}
|
|
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) >= 20)
|
|
{
|
|
Ar << Extent;
|
|
}
|
|
}
|
|
|
|
// Collision Mesh data
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) > 13)
|
|
{
|
|
FTriMeshCollisionData Dummy;
|
|
FTriMeshCollisionData* CollisionMeshPtr = Ar.IsCooking() ? &Dummy : &CollisionMesh;
|
|
|
|
Ar << CollisionMeshPtr->Vertices;
|
|
|
|
int32 NumIndices = CollisionMeshPtr->Indices.Num();
|
|
Ar << NumIndices;
|
|
|
|
if (Ar.IsLoading())
|
|
{
|
|
CollisionMeshPtr->Indices.AddUninitialized(NumIndices);
|
|
}
|
|
|
|
Ar.Serialize(CollisionMeshPtr->Indices.GetData(), NumIndices * sizeof(FTriIndices));
|
|
}
|
|
|
|
// Used for backwards compatibility with pre-streaming formats
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) < 16)
|
|
{
|
|
TArray<FLidarPointCloudPoint_Legacy> DataArray;
|
|
|
|
TArray<FLidarPointCloudOctreeNode*> Nodes;
|
|
Nodes.Add(Root);
|
|
while (Nodes.Num())
|
|
{
|
|
FLidarPointCloudOctreeNode* CurrentNode = Nodes.Pop(EAllowShrinking::No);
|
|
|
|
Ar << CurrentNode->LocationInParent << CurrentNode->Center;
|
|
|
|
CurrentNode->Data.Empty();
|
|
Ar << DataArray; // AllocatedPoints
|
|
CurrentNode->Data.Append(DataArray);
|
|
Ar << DataArray; // PaddingPoints
|
|
CurrentNode->Data.Append(DataArray);
|
|
|
|
CurrentNode->bHasData = true;
|
|
CurrentNode->bCanReleaseData = false;
|
|
|
|
int32 NumChildren = CurrentNode->Children.Num();
|
|
Ar << NumChildren;
|
|
|
|
PointCount[CurrentNode->Depth].Add(CurrentNode->NumPoints);
|
|
CurrentNode->NumVisiblePoints = CurrentNode->NumPoints;
|
|
|
|
CurrentNode->Children.AddUninitialized(NumChildren);
|
|
for (int32 i = 0; i < NumChildren; ++i)
|
|
{
|
|
CurrentNode->Children[i] = new FLidarPointCloudOctreeNode(this, CurrentNode->Depth + 1);
|
|
}
|
|
|
|
for (int32 i = NumChildren - 1; i >= 0; --i)
|
|
{
|
|
Nodes.Add(CurrentNode->Children[i]);
|
|
}
|
|
}
|
|
}
|
|
// Used for backwards compatibility with BulkData
|
|
else if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) < 20)
|
|
{
|
|
ITERATE_NODES({
|
|
// Pre-normals
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) < 19)
|
|
{
|
|
FLidarPointCloudBulkData_Legacy LegacyBulkData(sizeof(FLidarPointCloudPoint_Legacy));
|
|
|
|
// Get the legacy data
|
|
void* TempData = nullptr;
|
|
LegacyBulkData.Serialize(Ar, Owner);
|
|
LegacyBulkData.GetCopy(&TempData);
|
|
|
|
// Allocate the data buffer
|
|
CurrentNode->NumPoints = LegacyBulkData.GetElementCount();
|
|
CurrentNode->Data.SetNumUninitialized(CurrentNode->NumPoints);
|
|
FLidarPointCloudPoint* DataPtr = CurrentNode->Data.GetData();
|
|
|
|
// Copy the legacy data
|
|
for (FLidarPointCloudPoint_Legacy* Data = (FLidarPointCloudPoint_Legacy*)TempData, *DataEnd = Data + CurrentNode->NumPoints; Data != DataEnd; ++Data, ++DataPtr)
|
|
{
|
|
*DataPtr = *Data;
|
|
}
|
|
|
|
// Release the legacy data
|
|
FMemory::Free(TempData);
|
|
}
|
|
else
|
|
{
|
|
FLidarPointCloudBulkData_Legacy LegacyBulkData(sizeof(FLidarPointCloudPoint));
|
|
LegacyBulkData.Serialize(Ar, Owner);
|
|
|
|
CurrentNode->NumPoints = LegacyBulkData.GetElementCount();
|
|
CurrentNode->Data.SetNumUninitialized(CurrentNode->NumPoints);
|
|
void* DataPtr = CurrentNode->Data.GetData();
|
|
LegacyBulkData.GetCopy(&DataPtr);
|
|
}
|
|
|
|
CurrentNode->bHasData = true;
|
|
CurrentNode->bCanReleaseData = false;
|
|
|
|
int32 NumChildren = CurrentNode->Children.Num();
|
|
Ar << CurrentNode->LocationInParent << CurrentNode->Center << NumChildren;
|
|
|
|
PointCount[CurrentNode->Depth].Add(CurrentNode->NumPoints);
|
|
CurrentNode->NumVisiblePoints = CurrentNode->NumPoints;
|
|
|
|
CurrentNode->Children.AddUninitialized(NumChildren);
|
|
for (int32 i = 0; i < NumChildren; i++)
|
|
{
|
|
CurrentNode->Children[i] = new FLidarPointCloudOctreeNode(this, CurrentNode->Depth + 1);
|
|
}
|
|
}, true);
|
|
}
|
|
else
|
|
{
|
|
// Make sure to load all data persistently before saving, as the linker will detach
|
|
if (Ar.IsSaving())
|
|
{
|
|
LoadAllNodes(true);
|
|
CloseReadHandle();
|
|
}
|
|
|
|
int64 BulkDataOffset = 0;
|
|
|
|
ITERATE_NODES({
|
|
int32 NumChildren = CurrentNode->Children.Num();
|
|
Ar << CurrentNode->LocationInParent << CurrentNode->Center << NumChildren << CurrentNode->NumPoints;
|
|
|
|
CurrentNode->BulkDataSize = CurrentNode->NumPoints * sizeof(FLidarPointCloudPoint);
|
|
CurrentNode->BulkDataOffset = BulkDataOffset;
|
|
BulkDataOffset += CurrentNode->BulkDataSize;
|
|
Ar << CurrentNode->BulkDataSize << CurrentNode->BulkDataOffset;
|
|
|
|
if (Ar.IsSaving())
|
|
{
|
|
// Make sure the points are in optimized order before saving
|
|
CurrentNode->SortVisiblePoints();
|
|
}
|
|
else
|
|
{
|
|
PointCount[CurrentNode->Depth].Add(CurrentNode->NumPoints);
|
|
CurrentNode->NumVisiblePoints = CurrentNode->NumPoints;
|
|
|
|
// Build sub-nodes
|
|
CurrentNode->Children.AddUninitialized(NumChildren);
|
|
for (int32 i = 0; i < NumChildren; i++)
|
|
{
|
|
CurrentNode->Children[i] = new FLidarPointCloudOctreeNode(this, CurrentNode->Depth + 1);
|
|
}
|
|
}
|
|
}, true);
|
|
|
|
#if WITH_EDITOR
|
|
if(Ar.HasAnyPortFlags(PPF_Duplicate))
|
|
{
|
|
if(Ar.IsLoading())
|
|
{
|
|
ITERATE_NODES({
|
|
CurrentNode->Data.SetNumUninitialized(CurrentNode->NumPoints);
|
|
Ar.Serialize(CurrentNode->Data.GetData(), CurrentNode->BulkDataSize);
|
|
CurrentNode->bCanReleaseData = false;
|
|
CurrentNode->bRenderDataDirty = true;
|
|
CurrentNode->bHasData = true;
|
|
}, true);
|
|
}
|
|
else
|
|
{
|
|
ITERATE_NODES({
|
|
Ar.Serialize(CurrentNode->Data.GetData(), CurrentNode->BulkDataSize);
|
|
}, true);
|
|
}
|
|
}
|
|
else if(Ar.IsSaving())
|
|
{
|
|
SavingBulkData.Serialize(Ar, Owner);
|
|
}
|
|
else
|
|
#endif
|
|
{
|
|
BulkData.Serialize(Ar, Owner);
|
|
}
|
|
}
|
|
|
|
// Legacy Points Extent
|
|
{
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) < 17)
|
|
{
|
|
FBox PointsBounds;
|
|
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) > 14)
|
|
{
|
|
Ar << PointsBounds;
|
|
}
|
|
|
|
if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) > 15)
|
|
{
|
|
Extent = (FVector3f)PointsBounds.GetExtent();
|
|
Owner->LocationOffset = PointsBounds.GetCenter();
|
|
}
|
|
else
|
|
{
|
|
RefreshBounds();
|
|
}
|
|
}
|
|
else if (Ar.CustomVer(ULidarPointCloud::PointCloudFileGUID) < 20)
|
|
{
|
|
Ar << Extent;
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::SerializeBulkData(FArchive& Ar)
|
|
{
|
|
if (Ar.IsSaving())
|
|
{
|
|
const bool bReleaseData = !!Owner->GetPackage()->GetLinker();
|
|
|
|
ITERATE_NODES({
|
|
Ar.Serialize(CurrentNode->Data.GetData(), CurrentNode->BulkDataSize);
|
|
if(bReleaseData)
|
|
{
|
|
CurrentNode->ReleaseData(true);
|
|
}
|
|
}, true);
|
|
}
|
|
}
|
|
|
|
IAsyncReadFileHandle* FLidarPointCloudOctree::GetReadHandle()
|
|
{
|
|
if(!ReadHandle)
|
|
{
|
|
ReadHandle = BulkData.OpenAsyncReadHandle();
|
|
}
|
|
|
|
return ReadHandle;
|
|
}
|
|
|
|
void FLidarPointCloudOctree::CloseReadHandle()
|
|
{
|
|
if (ReadHandle)
|
|
{
|
|
delete ReadHandle;
|
|
ReadHandle = nullptr;
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudOctree::StreamNodeData(FLidarPointCloudOctreeNode* Node)
|
|
{
|
|
if (GetReadHandle())
|
|
{
|
|
Node->Data.SetNumUninitialized(Node->NumPoints);
|
|
IAsyncReadRequest* ReadRequest = ReadHandle->ReadRequest(
|
|
BulkData.GetBulkDataOffsetInFile() + Node->BulkDataOffset,
|
|
Node->BulkDataSize,
|
|
AIOP_Normal,
|
|
nullptr,
|
|
(uint8*)Node->Data.GetData());
|
|
ReadRequest->WaitCompletion();
|
|
delete ReadRequest;
|
|
Node->bHasData = true;
|
|
}
|
|
else
|
|
{
|
|
Node->bHasData = false;
|
|
Node->Data.Empty();
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////// FLidarPointCloudBulkData
|
|
|
|
#if WITH_EDITOR
|
|
FLidarPointCloudOctree::FLidarPointCloudBulkData::FLidarPointCloudBulkData(FLidarPointCloudOctree* Octree)
|
|
{
|
|
SerializeElementsCallback = [Octree](FArchive& Ar, void*, int64, EBulkDataFlags)
|
|
{
|
|
Octree->SerializeBulkData(Ar);
|
|
};
|
|
#if !USE_RUNTIME_BULKDATA
|
|
SerializeBulkDataElements = &SerializeElementsCallback;
|
|
#endif // !USE_RUNTIME_BULKDATA
|
|
|
|
SetBulkDataFlags(BULKDATA_Force_NOT_InlinePayload | BULKDATA_Size64Bit);
|
|
}
|
|
|
|
void FLidarPointCloudOctree::FLidarPointCloudBulkData::Serialize(FArchive& Ar, UObject* InOwner)
|
|
{
|
|
Lock(LOCK_READ_WRITE);
|
|
Realloc(1,1);
|
|
Unlock();
|
|
SetBulkDataFlags(BULKDATA_Force_NOT_InlinePayload | BULKDATA_Size64Bit);
|
|
|
|
FBulkData::Serialize(Ar, InOwner, false, 1, EFileRegionType::None);
|
|
}
|
|
#endif
|
|
//////////////////////////////////////////////////////////// FLidarPointCloudTraversalOctreeNode
|
|
|
|
FLidarPointCloudTraversalOctreeNode::FLidarPointCloudTraversalOctreeNode()
|
|
: DataNode(nullptr)
|
|
, Parent(nullptr)
|
|
, bFullyContained(false)
|
|
{
|
|
|
|
}
|
|
|
|
void FLidarPointCloudTraversalOctreeNode::Build(FLidarPointCloudTraversalOctree* TraversalOctree, FLidarPointCloudOctreeNode* Node, const FTransform& LocalToWorld, const FVector3f& LocationOffset)
|
|
{
|
|
Octree = TraversalOctree;
|
|
DataNode = Node;
|
|
Center = (FVector3f)LocalToWorld.TransformPosition((FVector)(Node->Center + LocationOffset));
|
|
Depth = Node->Depth;
|
|
|
|
Children.AddZeroed(Node->Children.Num());
|
|
for (int32 i = 0; i < Children.Num(); i++)
|
|
{
|
|
if (Node->Children[i])
|
|
{
|
|
Children[i].Build(Octree, Node->Children[i], LocalToWorld, LocationOffset);
|
|
Children[i].Parent = this;
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudTraversalOctreeNode::CalculateVirtualDepth(const TArray<float>& LevelWeights, const float& PointSizeBias)
|
|
{
|
|
if (!IsAvailable())
|
|
{
|
|
VirtualDepth = 255;
|
|
return;
|
|
}
|
|
|
|
const float& VDMultiplier = Octree->VirtualDepthMultiplier;
|
|
|
|
TQueue<const FLidarPointCloudTraversalOctreeNode*> Nodes;
|
|
const FLidarPointCloudTraversalOctreeNode* CurrentNode = nullptr;
|
|
|
|
// Calculate virtual depth factor
|
|
float VDFactor = 0;
|
|
Nodes.Enqueue(this);
|
|
while (Nodes.Dequeue(CurrentNode))
|
|
{
|
|
for (const FLidarPointCloudTraversalOctreeNode& Child : CurrentNode->Children)
|
|
{
|
|
if (Child.IsAvailable())
|
|
{
|
|
Nodes.Enqueue(&Child);
|
|
}
|
|
}
|
|
|
|
float LocalVDFactor = CurrentNode->Depth * CurrentNode->DataNode->GetNumVisiblePoints() * LevelWeights[CurrentNode->Depth];
|
|
|
|
if (CurrentNode != this && PointSizeBias > 0)
|
|
{
|
|
LocalVDFactor /= (CurrentNode->Parent->Children.Num() - 1) * PointSizeBias + 1;
|
|
}
|
|
|
|
VDFactor += LocalVDFactor;
|
|
}
|
|
|
|
// Calculate weighted number of visible points
|
|
float NumPoints = 0;
|
|
Nodes.Enqueue(this);
|
|
while (Nodes.Dequeue(CurrentNode))
|
|
{
|
|
for (const FLidarPointCloudTraversalOctreeNode& Child : CurrentNode->Children)
|
|
{
|
|
if (Child.IsAvailable())
|
|
{
|
|
Nodes.Enqueue(&Child);
|
|
}
|
|
}
|
|
|
|
NumPoints += CurrentNode->DataNode->GetNumVisiblePoints() * LevelWeights[CurrentNode->Depth];
|
|
}
|
|
|
|
// Calculate the Virtual Depth
|
|
VirtualDepth = VDFactor / NumPoints * VDMultiplier;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////// FLidarPointCloudTraversalOctreeNodeSizeData
|
|
|
|
FLidarPointCloudTraversalOctreeNodeSizeData::FLidarPointCloudTraversalOctreeNodeSizeData(FLidarPointCloudTraversalOctreeNode* Node, const float& Size, const int32& ProxyIndex)
|
|
: Node(Node)
|
|
, Size(Size)
|
|
, ProxyIndex(ProxyIndex)
|
|
{
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////// FLidarPointCloudTraversalOctree
|
|
|
|
FLidarPointCloudTraversalOctree::FLidarPointCloudTraversalOctree(FLidarPointCloudOctree* Octree, const FTransform& LocalToWorld)
|
|
: Octree(Octree)
|
|
, bValid(false)
|
|
{
|
|
// Reset the tree
|
|
Extents.Empty();
|
|
RadiiSq.Empty();
|
|
Root.~FLidarPointCloudTraversalOctreeNode();
|
|
new (&Root) FLidarPointCloudTraversalOctreeNode();
|
|
|
|
// Calculate properties
|
|
NumLODs = Octree->GetNumLODs();
|
|
|
|
VirtualDepthMultiplier = 255.0f / NumLODs;
|
|
ReversedVirtualDepthMultiplier = NumLODs / 255.0f;
|
|
|
|
const FVector3f Extent = Octree->SharedData[0].Extent;
|
|
|
|
const FBox WorldBounds = FBox(-Extent, Extent).TransformBy(LocalToWorld);
|
|
for (int32 i = 0; i < NumLODs; i++)
|
|
{
|
|
Extents.Emplace(i == 0 ? (FVector3f)WorldBounds.GetExtent() : Extents.Last() * 0.5f);
|
|
RadiiSq.Emplace(FMath::Square(Extents.Last().Size()));
|
|
}
|
|
|
|
int64 NumPoints = 0;
|
|
TArray<int64> PointCount;
|
|
for (FThreadSafeCounter64& Count : Octree->PointCount)
|
|
{
|
|
PointCount.Add(Count.GetValue());
|
|
NumPoints += Count.GetValue();
|
|
}
|
|
|
|
LevelWeights.AddZeroed(NumLODs);
|
|
for (int32 i = 0; i < LevelWeights.Num(); i++)
|
|
{
|
|
LevelWeights[i] = (float)PointCount[i] / NumPoints;
|
|
}
|
|
|
|
// Star cloning the node data
|
|
Root.Build(this, Octree->Root, LocalToWorld, (FVector3f)Octree->Owner->LocationOffset);
|
|
|
|
bValid = NumLODs > 0;
|
|
}
|
|
|
|
void FLidarPointCloudTraversalOctree::CalculateVisibilityStructure(TArray<uint32>& OutData)
|
|
{
|
|
FLidarPointCloudTraversalOctreeNode* CurrentNode = nullptr;
|
|
TQueue<FLidarPointCloudTraversalOctreeNode*> Nodes;
|
|
Nodes.Enqueue(&Root);
|
|
|
|
uint16 NumNodesInQueue = 1;
|
|
|
|
while (Nodes.Dequeue(CurrentNode))
|
|
{
|
|
uint32& Data = OutData[OutData.AddZeroed()];
|
|
Data |= (0x00FFFF00 & (NumNodesInQueue-- << 8));
|
|
Data |= 0xFF000000 & (CurrentNode->VirtualDepth << 24);
|
|
|
|
for (uint8 i = 0; i < 8; ++i)
|
|
{
|
|
for (FLidarPointCloudTraversalOctreeNode& Child : CurrentNode->Children)
|
|
{
|
|
if (Child.bSelected && Child.DataNode->LocationInParent == i)
|
|
{
|
|
Nodes.Enqueue(&Child);
|
|
Data |= 1 << i;
|
|
++NumNodesInQueue;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void FLidarPointCloudTraversalOctree::CalculateLevelWeightsForSelectedNodes(TArray<float>& OutLevelWeights)
|
|
{
|
|
FLidarPointCloudTraversalOctreeNode* CurrentNode = nullptr;
|
|
TQueue<FLidarPointCloudTraversalOctreeNode*> Nodes;
|
|
Nodes.Enqueue(&Root);
|
|
|
|
OutLevelWeights.Empty();
|
|
OutLevelWeights.AddZeroed(NumLODs);
|
|
TArray<int64> PointCount;
|
|
PointCount.AddZeroed(NumLODs);
|
|
int64 NumPoints = 0;
|
|
|
|
while (Nodes.Dequeue(CurrentNode))
|
|
{
|
|
for (FLidarPointCloudTraversalOctreeNode& Child : CurrentNode->Children)
|
|
{
|
|
if (Child.bSelected)
|
|
{
|
|
Nodes.Enqueue(&Child);
|
|
}
|
|
}
|
|
|
|
const int32 NumPointsInNode = CurrentNode->DataNode->GetNumVisiblePoints();
|
|
PointCount[CurrentNode->Depth] += NumPointsInNode;
|
|
NumPoints += NumPointsInNode;
|
|
}
|
|
|
|
for (int32 i = 0; i < OutLevelWeights.Num(); i++)
|
|
{
|
|
OutLevelWeights[i] = NumPoints > 0 ? (float)PointCount[i] / NumPoints : 0;
|
|
}
|
|
}
|
|
|
|
FLidarPointCloudTraversalOctree::~FLidarPointCloudTraversalOctree()
|
|
{
|
|
if (bValid)
|
|
{
|
|
Octree->UnregisterTraversalOctree(this);
|
|
}
|
|
}
|