Implementation of voxel traversal algorithm from "A Fast Voxel Traversal Algorithm for Ray Tracing" (Amanatides and Woo)

woo.cpp

/*
 * Implementation of voxel traversal algorithm from "A Fast Voxel Traversal
 * Algorithm for Ray Tracing" (Amanatides and Woo).
 */

#include <iostream>
#include <iterator>
#include <tuple>
#include <limits>
#include <algorithm>
#include <cmath>

template <class T>
struct vector3D {
  vector3D() : x(0), y(0), z(0) {}
  vector3D(const T & x, const T & y, const T & z) : x(x), y(y), z(z) {}
  T x, y, z;
};

template <class T>
vector3D<T> operator+(const vector3D<T> & v, const vector3D<T> & w) {
  return vector3D<T>(v.x + w.x, v.y + w.y, v.z + w.z);
}

template <class T>
vector3D<T> operator/(const vector3D<T> & v, const vector3D<T> & w) {
  return vector3D<T>(v.x / w.x, v.y / w.y, v.z / w.z);
}

template <class T>
vector3D<T> & operator+=(vector3D<T> & v, const vector3D<T> & w) {
  v.x += w.x;
  v.y += w.y;
  v.z += w.z;
  return v;
}

template <class T>
vector3D<T> operator*(const T & t, const vector3D<T> & v) {
  return vector3D<T>(t * v.x, t * v.y, t * v.z);
}

template <class T>
std::ostream & operator<<(std::ostream & os, const vector3D<T> & v) {
  return os << "(" << v.x << ", " << v.y << ", " << v.z << ")";
}

class step_iterator : public std::iterator<std::forward_iterator_tag,
                                           std::tuple<int, double>> {
  public:

    step_iterator(const vector3D<double> & u, const vector3D<double> & v,
                  int ldy, int ldz,
                  const vector3D<double> & grid) {
      const vector3D<double> origin(u / grid);
      index = std::floor(origin.x) * ldy * ldz +
              std::floor(origin.y) * ldz +
              std::floor(origin.z);
      stepX = ((0 < v.x) - (v.x < 0)) * ldy * ldz;
      stepY = ((0 < v.y) - (v.y < 0)) * ldz;
      stepZ = (0 < v.z) - (v.z < 0);

      tMaxX = std::numeric_limits<double>::infinity();
      tDeltaX = std::numeric_limits<double>::infinity();
      if (v.x != 0) {
        const double t1 = (std::floor(origin.x) - origin.x) / v.x;
        const double t2 = t1 + (grid.x / v.x);
        tMaxX = std::max(t1, t2);
        tDeltaX = grid.x / v.x;
      }

      tMaxY = std::numeric_limits<double>::infinity();
      tDeltaY = std::numeric_limits<double>::infinity();
      if (v.y != 0) {
        const double t1 = (std::floor(origin.y) - origin.y) / v.y;
        const double t2 = t1 + (grid.y / v.y);
        tMaxY = std::max(t1, t2);
        tDeltaY = grid.y / v.y;
      }

      tMaxZ = std::numeric_limits<double>::infinity();
      tDeltaZ = std::numeric_limits<double>::infinity();
      if (v.z != 0) {
        const double t1 = (std::floor(origin.z) - origin.z) / v.z;
        const double t2 = t1 + (grid.z / v.z);
        tMaxZ = std::max(t1, t2);
        tDeltaZ = grid.z / v.z;
      }

      tMax = 0;
      tDelta = std::min(std::min(tMaxX, tMaxY), tMaxZ);
    }

    step_iterator & operator++() {
      if (tMaxX <= tMaxY && tMaxX <= tMaxZ) {
        index += stepX;
        tMaxX += tDeltaX;
      }
      if (tMaxY <= tMaxZ && tMaxY <= tMaxZ) {
        index += stepY;
        tMaxY += tDeltaY;
      }
      if (tMaxZ <= tMaxX && tMaxZ <= tMaxY) {
        index += stepZ;
        tMaxZ += tDeltaZ;
      }

      tMax += tDelta;
      tDelta = std::min(std::min(tMaxX, tMaxY), tMaxZ) - tMax;

      return *this;
    }

    step_iterator operator++(int) {
      step_iterator tmp(*this);
      operator++();
      return tmp;
    }

    std::tuple<int, double> operator*() const {
      return std::make_tuple(index, tDelta);
    }

    bool operator==(const step_iterator & that) const {
      return this->index == that.index &&
             this->stepX == that.stepX && this->stepY == that.stepY &&
             this->stepZ == that.stepZ &&
             this->tMaxX == that.tMaxX && this->tMaxY == that.tMaxY &&
             this->tMaxZ == that.tMaxZ &&
             this->tDeltaX == that.tDeltaX && this->tDeltaY == that.tDeltaY &&
             this->tDeltaZ == that.tDeltaZ;
    }

    bool operator!=(const step_iterator & that) const {
      return !(*this == that);
    }

  private:
    int index, stepX, stepY, stepZ;
    double tMaxX, tMaxY, tMaxZ, tDeltaX, tDeltaY, tDeltaZ, tMax, tDelta;
};

int main() {
  // Define a line starting at (0,0) travelling down/right at 45 degrees for
  // the full length of the diagonal of the grid
  {
    const vector3D<double> origin(0.0, 0.0, 0.0);
    const vector3D<double> direction(std::sqrt(1.0/3.0), std::sqrt(1.0/3.0),
                                     std::sqrt(1.0/3.0));
    const vector3D<double> separation(0.1, 0.1, 0.1);
    std::cout << "Line: origin " << origin << ", direction " << direction <<
      ", separation " << separation << std::endl;

    step_iterator it(origin, direction, 10, 10, separation);
    int index;
    double t;
    vector3D<double> position(origin);
    for (int i = 0; i < 10; ++i, ++it) {
      std::tie(index, t) = *it;
      position += t * direction;
      std::cout << "Travel " << t << " through " << index << " to " <<
        position << std::endl;
    }
  }

  return 0;
}