PointHashGrid2.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
// Port of geometry3Sharp PointHashGrid2
 
#pragma once
 
#include "CoreMinimal.h"
#include "Misc/ScopeLock.h"
#include "Util/GridIndexing2.h"
 
namespace UE
{
namespace Geometry
{
 
using namespace UE::Math;
 
template<typename PointDataType, typename RealType>
class TPointHashGrid2
{
private:
    TMultiMap<FVector2i, PointDataType> Hash;
    FCriticalSection CriticalSection;
    TScaleGridIndexer2<RealType> Indexer;
    PointDataType InvalidValue;
 
public:
 
    TPointHashGrid2(RealType cellSize, PointDataType InvalidValue) : Indexer(cellSize), InvalidValue(InvalidValue)
    {
    }
 
    void Reserve(int32 Num)
    {
        Hash.Reserve(Num);
    }
 
    PointDataType GetInvalidValue() const
    {
        return InvalidValue;
    }
 
    void InsertPoint(const PointDataType& Value, const TVector2<RealType>& Position)
    {
        FVector2i idx = Indexer.ToGrid(Position);
        {
            FScopeLock Lock(&CriticalSection);
            Hash.Add(idx, Value);
        }
    }
 
    void InsertPointUnsafe(const PointDataType& Value, const TVector2<RealType>& Position)
    {
        FVector2i idx = Indexer.ToGrid(Position);
        Hash.Add(idx, Value);
    }
 
 
    bool RemovePoint(const PointDataType& Value, const TVector2<RealType>& Position)
    {
        FVector2i idx = Indexer.ToGrid(Position);
        {
            FScopeLock Lock(&CriticalSection);
            return Hash.RemoveSingle(idx, Value) > 0;
        }
    }
 
    bool RemovePointUnsafe(const PointDataType& Value, const TVector2<RealType>& Position)
    {
        FVector2i idx = Indexer.ToGrid(Position);
        return Hash.RemoveSingle(idx, Value) > 0;
    }
 
 
    void UpdatePoint(const PointDataType& Value, const TVector2<RealType>& OldPosition, const TVector2<RealType>& NewPosition)
    {
        FVector2i old_idx = Indexer.ToGrid(OldPosition);
        FVector2i new_idx = Indexer.ToGrid(NewPosition);
        if (old_idx == new_idx)
        {
            return;
        }
        bool bWasAtOldPos;
        {
            FScopeLock Lock(&CriticalSection);
            bWasAtOldPos = Hash.RemoveSingle(old_idx, Value) > 0;
        }
        check(bWasAtOldPos);
        {
            FScopeLock Lock(&CriticalSection);
            Hash.Add(new_idx, Value);
        }
        return;
    }
 
 
    void UpdatePointUnsafe(const PointDataType& Value, const TVector2<RealType>& OldPosition, const TVector2<RealType>& NewPosition)
    {
        FVector2i old_idx = Indexer.ToGrid(OldPosition);
        FVector2i 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 TVector2<RealType>& QueryPoint, RealType Radius, 
        TFunctionRef<RealType(const PointDataType&)> DistanceSqFunc,
        TFunctionRef<bool(const PointDataType&)> IgnoreFunc = [](const PointDataType& data) { return false; }) const
    {
        if (!Hash.Num())
        {
            return TPair<PointDataType, RealType>(GetInvalidValue(), TNumericLimits<RealType>::Max());
        }
 
        FVector2i min_idx = Indexer.ToGrid(QueryPoint - Radius * TVector2<RealType>::One());
        FVector2i max_idx = Indexer.ToGrid(QueryPoint + Radius * TVector2<RealType>::One());
 
        RealType min_distsq = TNumericLimits<RealType>::Max();
        PointDataType nearest = GetInvalidValue();
        RealType RadiusSquared = Radius * Radius;
 
        TArray<PointDataType> Values;
        for (int yi = min_idx.Y; yi <= max_idx.Y; yi++) 
        {
            for (int xi = min_idx.X; xi <= max_idx.X; xi++) 
            {
                FVector2i idx(xi, yi);
                Values.Reset();
                Hash.MultiFind(idx, Values);
                for (PointDataType Value : Values) 
                {
                    if (IgnoreFunc(Value))
                    {
                        continue;
                    }
                    RealType distsq = DistanceSqFunc(Value);
                    if (distsq < RadiusSquared && distsq < min_distsq)
                    {
                        nearest = Value;
                        min_distsq = distsq;
                    }
                }
            }
        }
 
        return TPair<PointDataType, RealType>(nearest, min_distsq);
    }
};
 
template <typename PointDataType> using TPointHashGrid2d = TPointHashGrid2<PointDataType, double>;
template <typename PointDataType> using TPointHashGrid2f = TPointHashGrid2<PointDataType, float>;
 
} // end namespace UE::Geometry
} // end namespace UE