Cascading voxel grid.

Cascade.cc

#include <cmath>
#include <cstdint>
#include <algorithm>
#include <iostream>
#include <unordered_map>
#include <vector>


union SimpleVoxelCell {
  SimpleVoxelCell() = default;
  SimpleVoxelCell(unsigned x, unsigned y, unsigned z)
    : s(x, y, z) {}

  SimpleVoxelCell(unsigned index)
    : index(index) {}

  struct __Anon {
    __Anon(unsigned x, unsigned y, unsigned z)
      : x(x),
        y(y),
        z(z)
    {}

    signed x:12;
    signed y:12;
    signed z:8;
  } s;

  unsigned index;
};

struct Point {
  Point() = default;
  Point(double x, double y, double z)
    : x(x),
      y(y),
      z(z)
  {}

  double x;
  double y;
  double z;
};

class SimpleVoxelCellSuperSampler {
 public:
  SimpleVoxelCellSuperSampler() = default;
  explicit SimpleVoxelCellSuperSampler(std::size_t samples)
    : samples(samples)
  {}

  SimpleVoxelCell
  Map(double x, double y, double z)
  {
    return SimpleVoxelCell(x / samples,
                           y / samples,
                           z / samples);
  }

  Point
  Map(SimpleVoxelCell cell)
  {
    return Point(cell.s.x * samples,
                 cell.s.y * samples,
                 cell.s.z * samples);
  }

 private:
  std::size_t samples;
};


template <typename Type>
class CascadingVoxelGrid {
 public:
  struct Options {
    // These are set sufficiently small s.t. the example demonstrates usage.
    std::size_t hi_water_mark = 4 * 1024;
    std::size_t lo_water_mark = 1 * 1024;
    SimpleVoxelCellSuperSampler sampler;
  };

  explicit CascadingVoxelGrid(Options options)
    : options(options),
      byte_size(0)
  {}

  Type&
  Retrieve(double x, double y, double z)
  {
    // Map this to a cell index.
    auto index = options.sampler.Map(x, y, z);

    // Increment the current memory usage, this is rough and aproximate.. it
    // does not consider the memory usages of the unordered_map.
    auto iter = data.find(index.index);
    if (iter == data.end())
      byte_size += SizeOfRecord();

    // If the byte_size is beyond our defined high water mark, then it's time
    // for us to pay the amortized cost.
    if (byte_size >= options.hi_water_mark)
      Drain(index);

    return data[index.index];
  }

  std::size_t
  Size() const
  { return data.size(); }

 private:
  Options options;
  std::unordered_map<unsigned, Type> data;
  std::size_t byte_size;

  std::size_t
  SizeOfRecord(std::size_t n = 1)
  {
    return n * (sizeof(Type) + sizeof(SimpleVoxelCell));
  }

  void
  Drain(SimpleVoxelCell pose)
  {
    std::cerr << "DRAIN\n";

    // We are going to create a very simply algorithm which takes the N largest
    // elements.  Where, N is the number of items required for us.  There are
    // some tricky efficient ways to do this on-line.  We're going to put all of
    // the indices in a vector and sort to the Nth element based on XYZ norm.
    std::size_t items_to_go = (byte_size - options.lo_water_mark) / SizeOfRecord();

    struct CleanupHolder {
      double   score;
      unsigned index;
    };
    std::vector<CleanupHolder> cells;
    cells.reserve(data.size());

    for (const auto& pair: data) {
      const auto cell = SimpleVoxelCell(pair.first);

      double score = std::pow(cell.s.x - pose.s.x, 2)
                   + std::pow(cell.s.y - pose.s.y, 2)
                   + std::pow(cell.s.z - pose.s.z, 2);

      cells.push_back((CleanupHolder){.score=score, .index=cell.index});
    }

    std::nth_element(
        cells.begin(),
        cells.begin() + items_to_go,
        cells.end(),
        [&](const CleanupHolder& lhs, const CleanupHolder& rhs) {
          // ie: reverse sorted order.
          return lhs.score > rhs.score;
        });

    for (std::size_t n = 0; n < items_to_go; ++n) {
      data.erase(cells[n].index);
      byte_size -= SizeOfRecord();
    }
  }
};



int
main()
{
  using Grid = CascadingVoxelGrid<int>;

  Grid::Options opts;
  opts.sampler = SimpleVoxelCellSuperSampler(8);
  Grid cvg(opts);

  for (std::size_t x = 0; x < 1000; ++x)
  for (std::size_t y = 0; y < 1000; ++y)
  for (std::size_t z = 0; z < 100; ++z) {
    cvg.Retrieve(x, y, z) = x + y + z;
    std::cerr << cvg.Size() << "\n";
  }

  return 0;
}