<commit_before>#pragma once

#include "GridMap.hpp"
#include "DiscreteTree.hpp"

namespace maps { namespace grid
{

template<class CellT>
class VoxelGridMap : public GridMap< DiscreteTree<CellT> >
{
    typedef GridMap< DiscreteTree<CellT> > _Base;
public:
    VoxelGridMap(const Vector2ui &num_cells,
                const Eigen::Vector3d &resolution) :
                GridMap< DiscreteTree<CellT> >(num_cells,
                resolution.block(0,0,2,1), DiscreteTree<CellT>(resolution.z())) {}

    bool hasVoxelCell(const Eigen::Vector3d &position) const
    {
        Index idx;
        if(_Base::toGrid(position, idx))
        {
            return _Base::at(idx).hasCell(position.z());
        }
        return false;
    }

    bool hasVoxelCell(const Eigen::Vector3i &index) const
    {
        Index idx = index.block(0,0,2,1);
        return _Base::at(idx).hasCell(index.z());
    }

    CellT& getVoxelCell(const Eigen::Vector3i &index)
    {
        Index idx = index.block(0,0,2,1);
        return _Base::at(idx).getCellAt(index.z());
    }

    CellT& getVoxelCell(const Eigen::Vector3d &position)
    {
        return _Base::at(position)[position.z()];
    }

    bool fromVoxelGrid(const Eigen::Vector3i& idx, Eigen::Vector3d& position) const
    {
        Index idx_2d = idx.block(0,0,2,1);
        if(_Base::fromGrid(idx_2d, position))
        {
            position << position.block(0,0,2,1), _Base::at(idx_2d).getCellCenter(idx.z());
            return true;
        }
        return false;
    }

    bool toVoxelGrid(const Eigen::Vector3d& position, Eigen::Vector3i& idx) const
    {
        Index idx_2d;
        if(_Base::toGrid(position, idx_2d))
        {
            idx << idx_2d, _Base::at(idx_2d).getCellIndex(position.z());
            return true;
        }
        return false;
    }

    bool toVoxelGrid(const Eigen::Vector3d& position, Eigen::Vector3i& idx, Eigen::Vector3d &pos_diff) const
    {
        Index idx_2d;<commit_msg>use default tree to lookup the index and cell center<commit_after>#pragma once

#include "GridMap.hpp"
#include "DiscreteTree.hpp"

namespace maps { namespace grid
{

template<class CellT>
class VoxelGridMap : public GridMap< DiscreteTree<CellT> >
{
    typedef GridMap< DiscreteTree<CellT> > _Base;
public:
    VoxelGridMap(const Vector2ui &num_cells,
                const Eigen::Vector3d &resolution) :
                GridMap< DiscreteTree<CellT> >(num_cells,
                resolution.block(0,0,2,1), DiscreteTree<CellT>(resolution.z())) {}

    bool hasVoxelCell(const Eigen::Vector3d &position) const
    {
        Index idx;
        if(_Base::toGrid(position, idx))
        {
            return _Base::at(idx).hasCell(position.z());
        }
        return false;
    }

    bool hasVoxelCell(const Eigen::Vector3i &index) const
    {
        Index idx = index.block(0,0,2,1);
        return _Base::at(idx).hasCell(index.z());
    }

    CellT& getVoxelCell(const Eigen::Vector3i &index)
    {
        Index idx = index.block(0,0,2,1);
        return _Base::at(idx).getCellAt(index.z());
    }

    CellT& getVoxelCell(const Eigen::Vector3d &position)
    {
        return _Base::at(position)[position.z()];
    }

    bool fromVoxelGrid(const Eigen::Vector3i& idx, Eigen::Vector3d& position, bool checkIndex = true) const
    {
        Index idx_2d = idx.block(0,0,2,1);
        if(_Base::fromGrid(idx_2d, position, checkIndex))
        {
            position << position.block(0,0,2,1), _Base::getDefaultValue().getCellCenter(idx.z());
            return true;
        }
        return false;
    }

    bool toVoxelGrid(const Eigen::Vector3d& position, Eigen::Vector3i& idx, bool checkIndex = true) const
    {
        Index idx_2d;
        if(_Base::toGrid(position, idx_2d, checkIndex))
        {
            idx << idx_2d, _Base::getDefaultValue().getCellIndex(position.z(), checkIndex);
            return true;
        }
        return false;
    }

    bool toVoxelGrid(const Eigen::Vector3d& position, Eigen::Vector3i& idx, Eigen::Vector3d &pos_diff) const
    {
        Index idx_2d;
{'fraction_non_alphanumeric': 0.12994350282485875, 'fraction_numerical': 0.019651191353475806, 'mean_word_length': 2.678410117434508, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4505909', 'n_tokens_mistral': 1399, 'n_tokens_neox': 1347, 'n_words': 255}