VoxelSpaceImpl.hpp

/* $Id$ */

#define _VOXEL_SPACE_ VoxelSpace<_dimension_, _TObject_, _BoundingBoxGetter_>

namespace jafar {
  namespace geom {
    
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    _VOXEL_SPACE_::VoxelSpace(double voxelSize_)
      : m_voxelSize(voxelSize_)
    {
      
    }
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    _VOXEL_SPACE_::~VoxelSpace()
    {
      // Delete voxels
    }
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    void _VOXEL_SPACE_::insertObject(_TObject_* object)
    {
      InsertFunctor insertFunctor;
      insertFunctor.object = object;
      BoundingBoxD bb = _BoundingBoxGetter_::boundingBox(object);
      m_voxels.iterateCreateVoxels( indexFor( bb.corner1()), indexFor( bb.corner2() ), &insertFunctor );
    }
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    void _VOXEL_SPACE_::removeObject(_TObject_* object)
    {
      RemoveFunctor removeFunctor;
      removeFunctor.object = object;
      BoundingBoxD bb = _BoundingBoxGetter_::boundingBox(object);
      m_voxels.iterateAllVoxels( &removeFunctor );
//       m_voxels.iterateCreateVoxels( indexFor( bb.corner1()), indexFor( bb.corner2() ), &removeFunctor ); // Maybe it should iterates over the voxel space just to be safe... but that could be costly, or keep track of the bounding box of objects used for insertion
    }
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    std::list<const typename _VOXEL_SPACE_::Voxel*> _VOXEL_SPACE_::voxelsCountaining( const _TObject_* object) const
    {
      FindVoxelsFunctor findFunctor;
      BoundingBoxD bb = _BoundingBoxGetter_::boundingBox(object);
      m_voxels.iterateVoxels( indexFor( bb.corner1()), indexFor( bb.corner2() ), &findFunctor );
      return findFunctor.voxels;
    }
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    std::list<_TObject_*> _VOXEL_SPACE_::objectsIn( const BoundingBoxD& bb) const
    {
      FindObjectsFunctor findFunctor;
      findFunctor.boundingBox = bb;
      m_voxels.iterateVoxels( indexFor( bb.corner1()), indexFor( bb.corner2() ), &findFunctor );
      return findFunctor.objects;
    }
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    std::ostream& operator<<(std::ostream& s, const VoxelSpace<_dimension_, _TObject_, _BoundingBoxGetter_>& _vs)
    {
      typename VoxelSpace<_dimension_, _TObject_, _BoundingBoxGetter_>::StreamFunctor streamFunctor( s );
      _vs.m_voxels.iterateAllVoxels( &streamFunctor );
      return s;
    }
    // Index For
    template<int _dimension_, class _TObject_, class _BoundingBoxGetter_>
    typename _VOXEL_SPACE_::IndexVec _VOXEL_SPACE_::indexFor(const HomogenousVecD& v ) const
    {
      // Normalized as the index is easily computed when size of a voxel is equal to 1
      IndexVec iv;
      for(int i = 0; i < _dimension_; i++)
      {
        iv(i) = (int) nearbyint(v(i) / m_voxelSize);
      }
      return iv;
    }
  }
}

#undef _VOXEL_SPACE_