HashGrid.h

#pragma once
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <span>
#include <vector>
 
namespace environment::field::aggregation
{
 
struct Box
{
   float xMin = 0.0F;
   float xMax = 0.0F;
   float yMin = 0.0F;
   float yMax = 0.0F;
   float zMin = 0.0F;
   float zMax = 0.0F;
};
 
[[nodiscard]] inline float RoundDown(double v)
{
   const float f = static_cast<float>(v);
   return static_cast<double>(f) > v ? std::nextafter(f, -std::numeric_limits<float>::infinity()) : f;
}
 
[[nodiscard]] inline float RoundUp(double v)
{
   const float f = static_cast<float>(v);
   return static_cast<double>(f) < v ? std::nextafter(f, std::numeric_limits<float>::infinity()) : f;
}
 
[[nodiscard]] inline bool Overlaps(const Box& a, const Box& b)
{
   return a.xMax >= b.xMin && a.xMin <= b.xMax && a.yMax >= b.yMin && a.yMin <= b.yMax && a.zMax >= b.zMin && a.zMin <= b.zMax;
}
 
class HashGrid
{
public:
   static constexpr std::size_t kMaxCells = 4'000'000;
 
   HashGrid() = default;
 
   HashGrid(std::span<const Box> boxes, double cellSize);
 
   [[nodiscard]] double CellSize() const { return m_cellSize; }
 
   [[nodiscard]] std::size_t CellCount() const { return static_cast<std::size_t>(m_nx) * static_cast<std::size_t>(m_ny); }
 
   template <class Visitor>
   void ForEachCandidate(const Box& query, Visitor&& visit) const
   {
      if (m_entries.empty()) {
         return;
      }
      const int ix0 = CellX(query.xMin);
      const int ix1 = CellX(query.xMax);
      const int iy0 = CellY(query.yMin);
      const int iy1 = CellY(query.yMax);
      for (int iy = iy0; iy <= iy1; ++iy) {
         for (int ix = ix0; ix <= ix1; ++ix) {
            const std::size_t cell = static_cast<std::size_t>(iy) * static_cast<std::size_t>(m_nx) + static_cast<std::size_t>(ix);
            for (std::uint32_t k = m_cellStart[cell]; k < m_cellStart[cell + 1]; ++k) {
               const Entry& e = m_entries[k];
               // Report the box in the first cell that the box and the query share.
               if (std::max(e.ix0, ix0) != ix || std::max(e.iy0, iy0) != iy) {
                  continue;
               }
               if (Overlaps(e.box, query)) {
                  visit(e.id);
               }
            }
         }
      }
   }
 
   void Candidates(const Box& query, std::vector<std::uint32_t>& out) const;
 
private:
   struct Entry
   {
      std::uint32_t id  = 0;
      std::int32_t  ix0 = 0; 
      std::int32_t  iy0 = 0; 
      Box           box;
   };
 
   [[nodiscard]] int CellX(double x) const { return CellIndex(x, m_originX, m_nx); }
   [[nodiscard]] int CellY(double y) const { return CellIndex(y, m_originY, m_ny); }
   [[nodiscard]] int CellIndex(double v, double origin, int count) const
   {
      const double cell = std::floor((v - origin) * m_inverseCell);
      return static_cast<int>(std::clamp(cell, 0.0, static_cast<double>(count - 1)));
   }
 
   double                     m_cellSize    = 1.0;
   double                     m_inverseCell = 1.0;
   double                     m_originX     = 0.0;
   double                     m_originY     = 0.0;
   int                        m_nx          = 0;
   int                        m_ny          = 0;
   std::vector<std::uint32_t> m_cellStart;  
   std::vector<Entry>         m_entries;
};
 
} // namespace environment::field::aggregation