PointHashGrid3.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
// Port of geometry3Sharp PointHashGrid3
 
#pragma once
 
#include "CoreMinimal.h"
#include "Async/TransactionallySafeMutex.h"
#include "Misc/ScopeLock.h"
#include "Util/GridIndexing3.h"
 
namespace UE
{
namespace Geometry
{
 
using namespace UE::Math;
 
template<typename PointDataType, typename RealType>
class TPointHashGrid3
{
private:
    TMultiMap<FVector3i, PointDataType> Hash;
    FTransactionallySafeMutex Mutex;
    TWrapAroundGridIndexer3<RealType> Indexer;
    PointDataType InvalidValue;
 
public:
 
    TPointHashGrid3(RealType CellSize, PointDataType InvalidValue) : Indexer(FMath::Max(TMathUtil<RealType>::ZeroTolerance, CellSize)), InvalidValue(InvalidValue)
    {
    }
 
    void Reserve(int32 Num)
    {
        Hash.Reserve(Num);
    }
 
    void Reset(RealType NewCellSize)
    {
        Hash.Reset();
        Indexer.CellSize = FMath::Max(TMathUtil<RealType>::ZeroTolerance, NewCellSize);
    }
 
    PointDataType GetInvalidValue() const
    {
        return InvalidValue;
    }
 
    void InsertPoint(const PointDataType& Value, const TVector<RealType>& Position)
    {
        FVector3i idx = Indexer.ToGrid(Position);
        {
            UE::TScopeLock Lock(Mutex);
            Hash.Add(idx, Value);
        }
    }
 
    void InsertPointUnsafe(const PointDataType& Value, const TVector<RealType>& Position)
    {
        FVector3i idx = Indexer.ToGrid(Position);
        Hash.Add(idx, Value);
    }
 
 
    bool RemovePoint(const PointDataType& Value, const TVector<RealType>& Position)
    {
        FVector3i idx = Indexer.ToGrid(Position);
        {
            UE::TScopeLock Lock(Mutex);
            return Hash.RemoveSingle(idx, Value) > 0;
        }
    }
 
    bool RemovePointUnsafe(const PointDataType& Value, const TVector<RealType>& Position)
    {
        FVector3i idx = Indexer.ToGrid(Position);
        return Hash.RemoveSingle(idx, Value) > 0;
    }
 
 
    bool IsCellEmpty(const TVector<RealType>& Position)
    {
        FVector3i Idx = Indexer.ToGrid(Position);
        {
            UE::TScopeLock Lock(Mutex);
            return !Hash.Contains(Idx);
        }
    }
 
 
    bool IsCellEmptyUnsafe(const TVector<RealType>& Position)
    {
        FVector3i Idx = Indexer.ToGrid(Position);
        return !Hash.Contains(Idx);
    }
 
 
    void UpdatePoint(const PointDataType& Value, const TVector<RealType>& OldPosition, const TVector<RealType>& NewPosition)
    {
        FVector3i old_idx = Indexer.ToGrid(OldPosition);
        FVector3i new_idx = Indexer.ToGrid(NewPosition);
        if (old_idx == new_idx)
        {
            return;
        }
        bool bWasAtOldPos;
        {
            UE::TScopeLock Lock(Mutex);
            bWasAtOldPos = Hash.RemoveSingle(old_idx, Value) > 0;
        }
        check(bWasAtOldPos);
        {
            UE::TScopeLock Lock(Mutex);
            Hash.Add(new_idx, Value);
        }
        return;
    }
 
 
    void UpdatePointUnsafe(const PointDataType& Value, const TVector<RealType>& OldPosition, const TVector<RealType>& NewPosition)
    {
        FVector3i old_idx = Indexer.ToGrid(OldPosition);
        FVector3i new_idx = Indexer.ToGrid(NewPosition);
        if (old_idx == new_idx)
        {
            return;
        }
        bool bWasAtOldPos = Hash.RemoveSingle(old_idx, Value) > 0;
        check(bWasAtOldPos);
        Hash.Add(new_idx, Value);
        return;
    }
 
    TPair<PointDataType, RealType> FindNearestInRadius(
        const TVector<RealType>& QueryPoint, RealType Radius,
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc,
        TFunctionRef<bool(const PointDataType&)> IgnoreFunc) const
    {
        return FindInRadiusHelper<false>(QueryPoint, Radius, DistanceSqFunc, IgnoreFunc);
    }
 
    TPair<PointDataType, RealType> FindNearestInRadius(
        const TVector<RealType>& QueryPoint, RealType Radius,
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc) const
    {
        return FindInRadiusHelper<false>(QueryPoint, Radius, DistanceSqFunc, [](const PointDataType& data) { return false; });
    }
 
    TPair<PointDataType, RealType> FindAnyInRadius(
        const TVector<RealType>& QueryPoint, RealType Radius,
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc,
        TFunctionRef<bool(const PointDataType&)> IgnoreFunc) const
    {
        return FindInRadiusHelper<true>(QueryPoint, Radius, DistanceSqFunc, IgnoreFunc);
    }
 
    TPair<PointDataType, RealType> FindAnyInRadius(
        const TVector<RealType>& QueryPoint, RealType Radius,
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc) const
    {
        return FindInRadiusHelper<true>(QueryPoint, Radius, DistanceSqFunc, [](const PointDataType& data) { return false; });
    }
 
 
    int FindPointsInBall(
        const TVector<RealType>& QueryPoint, RealType Radius,
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc,
        TArray<PointDataType>& ResultsOut,
        TFunctionRef<bool(const PointDataType&)> IgnoreFunc = [](const PointDataType& data) { return false; }) const
    {
        if (!Hash.Num())
        {
            return 0;
        }
        int32 InitialNum = ResultsOut.Num();
 
        FVector3i min_idx = Indexer.ToGrid(QueryPoint - Radius * TVector<RealType>::One());
        FVector3i max_idx = Indexer.ToGrid(QueryPoint + Radius * TVector<RealType>::One());
 
        RealType RadiusSquared = Radius * Radius;
 
        for (int zi = min_idx.Z; zi <= max_idx.Z; zi++)
        {
            for (int yi = min_idx.Y; yi <= max_idx.Y; yi++)
            {
                for (int xi = min_idx.X; xi <= max_idx.X; xi++)
                {
                    FVector3i idx(xi, yi, zi);
                    for (typename TMultiMap<FVector3i, PointDataType>::TConstKeyIterator It = Hash.CreateConstKeyIterator(idx); It; ++It)
                    {
                        const PointDataType& Value = It.Value();
                        if (IgnoreFunc(Value))
                        {
                            continue;
                        }
                        RealType distsq = DistanceSqFunc(Value);
                        if (distsq < RadiusSquared)
                        {
                            ResultsOut.Add(Value);
                        }
                    }
                }
            }
        }
 
        return ResultsOut.Num() - InitialNum;
    }
 
    void EnumeratePointsInBall(
        const TVector<RealType>& QueryPoint, RealType Radius,
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc,
        TFunctionRef<bool(PointDataType, double)> ProcessPointFunc,
        TFunctionRef<bool(const PointDataType&)> IgnoreFunc = [](const PointDataType& data) { return false; }) const
    {
        if (!Hash.Num())
        {
            return;
        }
 
        FVector3i min_idx = Indexer.ToGrid(QueryPoint - Radius * TVector<RealType>::One());
        FVector3i max_idx = Indexer.ToGrid(QueryPoint + Radius * TVector<RealType>::One());
 
        RealType RadiusSquared = Radius * Radius;
 
        for (int zi = min_idx.Z; zi <= max_idx.Z; zi++)
        {
            for (int yi = min_idx.Y; yi <= max_idx.Y; yi++)
            {
                for (int xi = min_idx.X; xi <= max_idx.X; xi++)
                {
                    FVector3i idx(xi, yi, zi);
                    for (typename TMultiMap<FVector3i, PointDataType>::TConstKeyIterator It = Hash.CreateConstKeyIterator(idx); It; ++It)
                    {
                        const PointDataType& Value = It.Value();
                        if (IgnoreFunc(Value))
                        {
                            continue;
                        }
                        RealType DistSq = DistanceSqFunc(Value);
                        if (DistSq <= RadiusSquared)
                        {
                            if (!ProcessPointFunc(Value, DistSq))
                            {
                                return;
                            }
                        }
                    }
                }
            }
        }
    }
 
private:
    template<bool bEarlyOut = false>
    TPair<PointDataType, RealType> FindInRadiusHelper(
        const TVector<RealType>& QueryPoint, RealType Radius,
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc,
        TFunctionRef<bool(const PointDataType&)> IgnoreFunc) const
    {
        if (!Hash.Num())
        {
            return TPair<PointDataType, RealType>(GetInvalidValue(), TNumericLimits<RealType>::Max());
        }
 
        RealType MinDistSq = Radius * Radius;
        PointDataType Nearest = GetInvalidValue();
 
        auto SearchCell = [this, &Nearest, &MinDistSq, &DistanceSqFunc, &IgnoreFunc](FVector3i CellIdx)
        {
            bool bFound = false;
            for (typename TMultiMap<FVector3i, PointDataType>::TConstKeyIterator It = Hash.CreateConstKeyIterator(CellIdx); It; ++It)
            {
                const PointDataType& Value = It.Value();
                if (IgnoreFunc(Value))
                {
                    continue;
                }
                RealType DistSq = DistanceSqFunc(Value);
                if (DistSq < MinDistSq)
                {
                    Nearest = Value;
                    MinDistSq = DistSq;
                    if (bEarlyOut) {
                        return true;
                    }
                    bFound = true;
                }
            }
            return bFound;
        };
 
        FVector3i CenterIdx = Indexer.ToGrid(QueryPoint);
        RealType SearchRadius = Radius;
 
        if (SearchCell(CenterIdx))
        {
            if (bEarlyOut)
            {
                return TPair<PointDataType, RealType>(Nearest, MinDistSq);
            }
            SearchRadius = FMath::Sqrt(MinDistSq);
        }
 
        Indexer.IterateAcrossBounds(
            QueryPoint - SearchRadius * TVector<RealType>::One(),
            QueryPoint + SearchRadius * TVector<RealType>::One(), [&CenterIdx, &SearchCell](const FVector3i& Idx)
        {
            if (Idx == CenterIdx)
            {
                return true; // continue
            }
            bool bFound = SearchCell(Idx);
            return !(bEarlyOut && bFound);
        });
 
        if (Nearest == GetInvalidValue())
        {
            MinDistSq = TNumericLimits<RealType>::Max();
        }
        return TPair<PointDataType, RealType>(Nearest, MinDistSq);
    }
 
};
 
template <typename PointDataType> using TPointHashGrid3d = TPointHashGrid3<PointDataType, double>;
template <typename PointDataType> using TPointHashGrid3f = TPointHashGrid3<PointDataType, float>;
 
} // end namespace UE::Geometry
} // end namespace UE