voxel_raycaster.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2026  Kenji Koide (k.koide@aist.go.jp)
#pragma once
 
#include <Eigen/Core>
 
namespace gtsam_points {
 
class VoxelRaycaster {
public:
  class Iterator {
  public:
    using iterator_category = std::forward_iterator_tag;
    using value_type = Eigen::Vector3i;
    using difference_type = std::ptrdiff_t;
    using pointer = const Eigen::Vector3i*;
    using reference = Eigen::Vector3i&;
 
    Iterator(const VoxelRaycaster* traversal, const Eigen::Vector3i& coord);
 
    const Eigen::Vector3i& operator*() const;
 
    bool operator!=(const Iterator& other) const;
 
    Iterator& operator++();
 
  private:
    const VoxelRaycaster* traversal;
    Eigen::Vector3i coord;  
    Eigen::Vector3d t_max;  
  };
 
  VoxelRaycaster(const Eigen::Vector4d& start, const Eigen::Vector4d& end, double voxel_size);
 
  Iterator begin() const;
 
  Iterator end() const;
 
  const Eigen::Vector3i& start_voxel() const;
 
  const Eigen::Vector3i& end_voxel() const;
 
private:
  Eigen::Vector3i step;         
  Eigen::Vector3i start_coord;  
  Eigen::Vector3i end_coord;    
  Eigen::Vector3d t_delta;      
  Eigen::Vector3d t_max_init;   
};
 
}  // namespace gtsam_points