HierarchicalHashGrid2D.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#if UE_ENABLE_INCLUDE_ORDER_DEPRECATED_IN_5_6
#include "CoreMinimal.h"
#endif // UE_ENABLE_INCLUDE_ORDER_DEPRECATED_IN_5_6
#include "Containers/StaticArray.h"
#include "Math/Box.h"
 
// LWC_TODO_AI Note we are using int32 here for X and Y which does mean that we could overflow the limit of an int for LWCoords
// unless fairly large grid cell sizes are used.As WORLD_MAX is currently in flux until we have a better idea of what we are
// going to be able to support its probably not worth investing time in this potential issue right now.
template <int32 InNumLevels = 3, int32 InLevelRatio = 4, typename InItemIDType = uint32>
class THierarchicalHashGrid2D
{
public:
 
    typedef InItemIDType ItemIDType;
    static const int32 NumLevels = InNumLevels;     
    static const int32 LevelRatio = InLevelRatio;   
    struct FCell
    {
        FCell() : X(0), Y(0), Level(0) {}
        FCell(const int32 InX, const int32 InY, const int32 InLevel) : X(InX), Y(InY), Level(InLevel) {}
 
        int32 X, Y, Level;          
        int32 First = INDEX_NONE;   
        int32 ChildItemCount = 0;   
        friend uint32 GetTypeHash(const FCell& Cell)
        {
            constexpr uint32 H1 = 0x8da6b343;   // Arbitrary big primes.
            constexpr uint32 H2 = 0xd8163841;
            constexpr uint32 H3 = 0xcb1ab31f;
            return (H1 * uint32(Cell.X) + H2 * uint32(Cell.Y) + H3 * uint32(Cell.Level));
        }
 
        // Need for TSet<>
        bool operator==(const FCell& RHS) const
        {
            return X == RHS.X && Y == RHS.Y && Level == RHS.Level;
        }
    };
 
    struct FCellLocation
    {
        FCellLocation() = default;
        FCellLocation(const int32 InX, const int32 InY, const int32 InLevel) : X(InX), Y(InY), Level(InLevel) {}
 
        bool operator==(const FCellLocation& RHS) const
        {
            return X == RHS.X && Y == RHS.Y && Level == RHS.Level;
        }
 
        bool operator!=(const FCellLocation& RHS) const
        {
            return !(*this == RHS);
        }
 
        int32 X = 0;
        int32 Y = 0;
        int32 Level = INDEX_NONE;
    };
 
    struct FItem
    {
        ItemIDType ID = 0;          
        int32 Next = INDEX_NONE;    
    };
 
    struct FCellRect
    {
        FCellRect() = default;
        FCellRect(const int32 InMinX, const int32 InMinY, const int32 InMaxX, const int32 InMaxY) : MinX(InMinX), MinY(InMinY), MaxX(InMaxX), MaxY(InMaxY) {}
 
        int32 MinX = 0;
        int32 MinY = 0;
        int32 MaxX = 0;
        int32 MaxY = 0;
    };
 
    struct FCellRectIterator
    {
        // Use int64 to prevent overflow when FCellRect::MaxX or FCellRect::MaxY are equal to the maximum value of int32.
        int64 X = 0;        
        int64 Y = 0;        
        int32 Level = 0;    
        FCellRect Rect;     
    };
 
 
    THierarchicalHashGrid2D(const float InCellSize = 500.f)
        : SpillList(INDEX_NONE)
    {
        check(InCellSize > 0.0f);
 
        float CurrCellSize = InCellSize;
        for (int32 i = 0; i < NumLevels; i++)
        {
            CellSize[i] = CurrCellSize;
            InvCellSize[i] = 1.0f / CurrCellSize;
            CurrCellSize *= (float)(LevelRatio);
        }
 
        for (int32 i = 0; i < NumLevels; i++)
        {
            LevelItemCount[i] = 0;
        }
    }
 
    void Initialize(const float InCellSize)
    {
        check(InCellSize > 0.0f);
 
        Reset();
 
        float CurrCellSize = InCellSize;
        for (int32 i = 0; i < NumLevels; i++)
        {
            CellSize[i] = CurrCellSize;
            InvCellSize[i] = 1.0f / CurrCellSize;
            CurrCellSize *= (float)(LevelRatio);
        }
    }
 
    void Reset()
    {
        SpillList = INDEX_NONE;
        Cells.Reset();
        Items.Reset();
 
        for (int32 i = 0; i < NumLevels; i++)
        {
            LevelItemCount[i] = 0;
        }
    }
 
    FCellLocation Add(const ItemIDType ID, const FBox& Bounds)
    {
        const FCellLocation Location = CalcCellLocation(Bounds);
        Add(ID, Location);
        return Location;
    }
 
    void Add(const ItemIDType ID, const FCellLocation& Location)
    {
        const int32 Idx = Items.AddUninitialized().Index;
        FItem& Item = Items[Idx];
        Item.ID = ID;
 
        if (Location.Level == INDEX_NONE)
        {
            // Could not fit into any of the grids, add to spill list.
            Item.Next = SpillList;
            SpillList = Idx;
        }
        else
        {
            // Add to cell at specific level
            FCell& Cell = FindOrAddCell(Location.X, Location.Y, Location.Level);
            Item.Next = Cell.First;
            Cell.First = Idx;
 
            // Update per level counts
            LevelItemCount[Location.Level]++;
 
            // Update child counts
            FCellLocation ParentLocation = Location;
            while (ParentLocation.Level < NumLevels - 1)
            {
                ParentLocation.X = LevelDown(ParentLocation.X);
                ParentLocation.Y = LevelDown(ParentLocation.Y);
                ParentLocation.Level++;
                FCell& ParentCell = FindOrAddCell(ParentLocation.X, ParentLocation.Y, ParentLocation.Level);
                ParentCell.ChildItemCount++;
            }
        }
    }
 
    void Remove(const ItemIDType ID, const FBox& OldBounds)
    {
        const FCellLocation OldLocation = CalcCellLocation(OldBounds);
        Remove(ID, OldLocation);
    }
 
    void Remove(const ItemIDType ID, const FCellLocation& OldLocation)
    {
        if (OldLocation.Level == INDEX_NONE)
        {
            // Remove from spill list.
            int32 PrevIdx = INDEX_NONE;
            for (int32 Idx = SpillList; Idx != INDEX_NONE; PrevIdx = Idx, Idx = Items[Idx].Next)
            {
                if (Items[Idx].ID == ID)
                {
                    if (PrevIdx == INDEX_NONE)
                    {
                        SpillList = Items[Idx].Next;
                    }
                    else
                    {
                        Items[PrevIdx].Next = Items[Idx].Next;
                    }
                    Items.RemoveAtUninitialized(Idx);
                    break;
                }
            }
        }
        else
        {
            // Remove from cell
            if (FCell* Cell = FindCellMutable(OldLocation.X, OldLocation.Y, OldLocation.Level))
            {
                int32 PrevIdx = INDEX_NONE;
                for (int32 Idx = Cell->First; Idx != INDEX_NONE; PrevIdx = Idx, Idx = Items[Idx].Next)
                {
                    if (Items[Idx].ID == ID)
                    {
                        if (PrevIdx == INDEX_NONE)
                        {
                            Cell->First = Items[Idx].Next;
                        }
                        else
                        {
                            Items[PrevIdx].Next = Items[Idx].Next;
                        }
                        Items.RemoveAtUninitialized(Idx);
                        break;
                    }
                }
 
                // Update per level counts
                LevelItemCount[OldLocation.Level]--;
 
                // Update child counts
                FCellLocation ParentLocation = OldLocation;
                while (ParentLocation.Level < NumLevels - 1)
                {
                    ParentLocation.X = LevelDown(ParentLocation.X);
                    ParentLocation.Y = LevelDown(ParentLocation.Y);
                    ParentLocation.Level++;
                    FCell* ParentCell = FindCellMutable(ParentLocation.X, ParentLocation.Y, ParentLocation.Level);
                    if (ParentCell)
                    {
                        ParentCell->ChildItemCount--;
                    }
                }
            }
        }
    }
 
    FCellLocation Move(const ItemIDType ID, const FBox& OldBounds, const FBox& NewBounds)
    {
        const FCellLocation OldLocation = CalcCellLocation(OldBounds);
        return Move(ID, OldLocation, NewBounds);
    }
 
    FCellLocation Move(const ItemIDType ID, const FCellLocation& OldLocation, const FBox& NewBounds)
    {
        const FCellLocation NewLocation = CalcCellLocation(NewBounds);
        if (NewLocation != OldLocation)
        {
            Remove(ID, OldLocation);
            Add(ID, NewLocation);
        }
        return NewLocation;
    }
 
    template <typename OutT>
    void QuerySmall(const FBox& Bounds, OutT& OutResults) const
    {
        // Return items from buckets on all levels
        for (int32 Level = 0; Level < NumLevels; Level++)
        {
            // Update per level counts
            if (LevelItemCount[Level] == 0)
            {
                continue;
            }
 
            // Finest level query rect
            FCellRect Rect = CalcQueryBounds(Bounds, Level);
 
            // Use int64 to prevent overflow when FCellRect::MaxX or FCellRect::MaxY are equal to the maximum value of int32.
            for (int64 Y = Rect.MinY; Y <= Rect.MaxY; Y++)
            {
                for (int64 X = Rect.MinX; X <= Rect.MaxX; X++)
                {
                    if (const FCell* Cell = FindCell(IntCastChecked<int32>(X), IntCastChecked<int32>(Y), Level))
                    {
                        for (int32 Idx = Cell->First; Idx != INDEX_NONE; Idx = Items[Idx].Next)
                        {
                            OutResults.Add(Items[Idx].ID);
                        }
                    }
                }
            }
        }
 
        // Everything from spill list
        for (int32 Idx = SpillList; Idx != INDEX_NONE; Idx = Items[Idx].Next)
        {
            OutResults.Add(Items[Idx].ID);
        }
    }
 
    template <typename OutT>
    void Query(const FBox& Bounds, OutT& OutResults) const
    {
        FCellRect Rects[NumLevels];
        FCellRectIterator Iters[NumLevels];
        int32 IterIdx = 0;
 
        // Calculate cell bounds for each level, keep track of the coarsest level that has any items, we'll start from that.
        for (int32 Level = 0; Level < NumLevels; Level++)
        {
            Rects[Level] = CalcQueryBounds(Bounds, Level);
        }
 
        // The idea of the iterator below is that it iterates over rectangle cells recursively towards finer levels, depth first.
        // The previous level's iterator is kept in the Iters stack, and we can pop and continue that once the finer level is completed.
        // Finer iterator rectangles is clamped against that levels tight bounds so that unnecessary cells are not visited.
        // Coarser levels of the grid also store data if the finer levels under them has any items. This is used to skip iterating
        // lower levels at certain locations completely. This can be big advantage in larger query boxes, compared to iterating all cells as in QuerySmall().
 
        // Init coarsest iterator
        const int32 StartLevel = NumLevels - 1;
        Iters[IterIdx].Level = StartLevel;
        Iters[IterIdx].Rect = Rects[StartLevel];
        Iters[IterIdx].X = Iters[IterIdx].Rect.MinX;
        Iters[IterIdx].Y = Iters[IterIdx].Rect.MinY;
        IterIdx++;
 
        while (IterIdx > 0)
        {
            FCellRectIterator& Iter = Iters[IterIdx - 1];
            // Check if the iterator has finished
            if (Iter.X > Iter.Rect.MaxX)
            {
                Iter.X = Iter.Rect.MinX;
                Iter.Y++;
                if (Iter.Y > Iter.Rect.MaxY)
                {
                    IterIdx--;
                    continue;
                }
            }
 
            if (const FCell* Cell = FindCell( IntCastChecked<int32>(Iter.X) , IntCastChecked<int32>(Iter.Y), Iter.Level))
            {
                // Collect items from this cell.
                for (int32 Idx = Cell->First; Idx != INDEX_NONE; Idx = Items[Idx].Next)
                {
                    OutResults.Add(Items[Idx].ID);
                }
 
                // Advance to region at finer level if it has any items.
                if (Cell->ChildItemCount > 0)
                {
                    check(Iter.Level > 0);
                    const int FinerLevel = Iter.Level - 1;
                    // The iteration rect is intersection between current coarse cell and finer level bounds (which is more accurate).
                    const FCellRect& FinerLevelRect = Rects[FinerLevel];
                    const int32 MinX = ClampInt32(Iter.X * LevelRatio);
                    const int32 MinY = ClampInt32(Iter.Y * LevelRatio);
                    const int32 MaxX = ClampInt32(Iter.X * LevelRatio + LevelRatio - 1);
                    const int32 MaxY = ClampInt32(Iter.Y * LevelRatio + LevelRatio - 1);
                    FCellRect CurrentRect(MinX, MinY, MaxX, MaxY);
                    FCellRect NewIterRect = IntersectRect(CurrentRect, FinerLevelRect);
                    // Advance if rect is not empty.
                    if (NewIterRect.MaxX >= NewIterRect.MinX && NewIterRect.MaxY >= NewIterRect.MinY)
                    {
                        FCellRectIterator& FinerIter = Iters[IterIdx];
                        FinerIter.Rect = NewIterRect;
                        FinerIter.X = NewIterRect.MinX;
                        FinerIter.Y = NewIterRect.MinY;
                        FinerIter.Level = FinerLevel;
                        IterIdx++;
                    }
                }
            }
 
            // Advance iteration
            Iter.X++;
        }
 
        // Everything from spill list
        for (int32 Idx = SpillList; Idx != INDEX_NONE; Idx = Items[Idx].Next)
        {
            OutResults.Add(Items[Idx].ID);
        }
    }
 
    FCellRect CalcQueryBounds(const FBox& Bounds, const int32 Level) const
    {
        FCellRect Result;
        Result.MinX = ClampInt32(FMath::FloorToInt(Bounds.Min.X * InvCellSize[Level] - 0.5f));
        Result.MinY = ClampInt32(FMath::FloorToInt(Bounds.Min.Y * InvCellSize[Level] - 0.5f));
        Result.MaxX = ClampInt32(FMath::FloorToInt(Bounds.Max.X * InvCellSize[Level] + 0.5f));
        Result.MaxY = ClampInt32(FMath::FloorToInt(Bounds.Max.Y * InvCellSize[Level] + 0.5f));
        return Result;
    }
 
    FCellRect IntersectRect(const FCellRect& Left, const FCellRect& Right) const
    {
        FCellRect Result;
        Result.MinX = FMath::Max(Left.MinX, Right.MinX);
        Result.MinY = FMath::Max(Left.MinY, Right.MinY);
        Result.MaxX = FMath::Min(Left.MaxX, Right.MaxX);
        Result.MaxY = FMath::Min(Left.MaxY, Right.MaxY);
        return Result;
    }
 
    static int32 LevelDown(int32 X)
    {
        X -= X < 0 ? (LevelRatio - 1) : 0;
        return X / LevelRatio;
    }
 
    FCellLocation CalcCellLocation(const FBox& Bounds) const
    {
        FCellLocation Location(0, 0, 0);
 
        const FVector Center = Bounds.GetCenter();
        const FVector::FReal Diameter = FMath::Max(Bounds.Max.X - Bounds.Min.X, Bounds.Max.Y - Bounds.Min.Y);
        for (Location.Level = 0; Location.Level < NumLevels; Location.Level++)
        {
            const int32 DiameterCells = ClampInt32(FMath::CeilToInt(Diameter * InvCellSize[Location.Level]));
            // note that it's fine for DiameterCells to equal 0 - that would happen for 0-sized items (valid location, no extent).
            if (DiameterCells <= 1)
            {
                Location.X = ClampInt32(FMath::FloorToInt(Center.X * InvCellSize[Location.Level]));
                Location.Y = ClampInt32(FMath::FloorToInt(Center.Y * InvCellSize[Location.Level]));
                break;
            }
        }
 
        if (Location.Level == NumLevels)
        {
            // Could not fit into any of the levels.
            Location.X = 0;
            Location.Y = 0;
            Location.Level = INDEX_NONE;
        }
 
        return Location;
    }
 
    FBox CalcCellBounds(const FCellLocation& CellLocation) const
    {
        const FVector::FReal Size = CellSize[CellLocation.Level];
        const FVector::FReal X = static_cast<FVector::FReal>(CellLocation.X) * Size;
        const FVector::FReal Y = static_cast<FVector::FReal>(CellLocation.Y) * Size;
        return FBox(FVector(X, Y, 0.0), FVector(X + Size, Y + Size, 0.0));
    }
 
    FCell& FindOrAddCell(const int X, const int Y, const int Level)
    {
        FCell CellToFind(X, Y, Level);
        const uint32 Hash = GetTypeHash(CellToFind);
        FCell* Cell = Cells.FindByHash(Hash, CellToFind);
        if (Cell != nullptr)
        {
            return *Cell;
        }
        FSetElementId Index = Cells.AddByHash(Hash, FCell(X, Y, Level));
        return Cells[Index];
    }
 
    FCell* FindCellMutable(const int X, const int Y, const int Level)
    {
        FCell CellToFind(X, Y, Level);
        const uint32 Hash = GetTypeHash(CellToFind);
        return Cells.FindByHash(Hash, CellToFind);
    }
 
    const FCell* FindCell(const int X, const int Y, const int Level) const
    {
        FCell CellToFind(X, Y, Level);
        const uint32 Hash = GetTypeHash(CellToFind);
        return Cells.FindByHash(Hash, CellToFind);
    }
 
    float GetCellSize(const int32 Level) const { return CellSize[Level]; }
 
    float GetInvCellSize(const int32 Level) const { return InvCellSize[Level]; }
 
    int32 GetLevelItemCount(const int32 Level) const { return LevelItemCount[Level]; }
 
    const TSet<FCell>& GetCells() const { return Cells; }
 
    const TSparseArray<FItem>& GetItems() const { return Items; }
 
    int32 GetFirstSpillListItem() const { return SpillList; } 
 
protected:
 
    static constexpr int32 ClampInt32(const int64 Value)
    {
        return static_cast<int32>(FMath::Clamp(
            Value,
            static_cast<int64>(std::numeric_limits<int32>::lowest()),
            static_cast<int64>(std::numeric_limits<int32>::max())));
    }
    
    TStaticArray<float, NumLevels> CellSize;        
    TStaticArray<float, NumLevels> InvCellSize;     
    TStaticArray<int32, NumLevels> LevelItemCount;  
    TSet<FCell> Cells;                              
    TSparseArray<FItem> Items;                      
    int32 SpillList = INDEX_NONE;                   
};