space_hash.h

Go to the documentation of this file.

 
#ifndef SPACE_HASH_H
#define SPACE_HASH_H
 
namespace argos {
   class CSpace;
   class CVector3;
   class CRay3;
}
 
#include <argos3/core/utility/math/ray3.h>
#include <argos3/core/utility/datatypes/set.h>
#include <argos3/core/simulator/space/positional_indices/positional_index.h>
 
namespace argos {
 
   /****************************************/
   /****************************************/
 
   template <class ENTITY>
   class CAbstractSpaceHash : public CPositionalIndex<ENTITY> {
 
   public:
 
      typedef CSet<ENTITY*> TEntityList;
 
   public:
 
      CAbstractSpaceHash() : m_unSize(0) {}
 
      virtual ~CAbstractSpaceHash() {}
 
      virtual void AddEntity(ENTITY& c_entity) {
         m_tEntities.insert(&c_entity);
      }
 
      inline TEntityList& GetEntities() {
         return m_tEntities;
      }
 
      virtual void RemoveEntity(ENTITY& c_entity) {
         typename TEntityList::iterator it = m_tEntities.find(&c_entity);
         if(it != m_tEntities.end()) {
            m_tEntities.erase(it);
         }
         else {
            THROW_ARGOSEXCEPTION("Entity not found when removing it from space hash.");
         }
      }
 
      inline size_t GetSize() {
         return m_unSize;
      }
 
      virtual void SetSize(size_t un_size) {
         m_unSize = un_size;
      }
 
      inline CVector3& GetCellSize() {
         return m_cCellSize;
      }
 
      inline CVector3& GetInvCellSize() {
         return m_cInvCellSize;
      }
 
      virtual void SetCellSize(const CVector3& c_cell_size) {
         m_cCellSize = c_cell_size;
         m_cInvCellSize.Set(1.0f / m_cCellSize.GetX(),
                            1.0f / m_cCellSize.GetY(),
                            1.0f / m_cCellSize.GetZ());
      }
 
      virtual void Update() = 0;
 
      virtual void UpdateCell(SInt32 n_x,
                              SInt32 n_y,
                              SInt32 n_z,
                              ENTITY& c_entity) = 0;
 
      virtual SInt32 SpaceToHashTable(Real f_coord,
                                             UInt32 un_axis) {
         return RoundClosestToZero(f_coord * GetInvCellSize()[un_axis]);
      }
 
      virtual Real HashTableToSpace(SInt32 n_coord,
                                           UInt32 un_axis) {
         return n_coord * m_cCellSize[un_axis];
      }
 
      virtual void SpaceToHashTable(SInt32& n_i,
                                           SInt32& n_j,
                                           SInt32& n_k,
                                           const CVector3& c_pos) {
         n_i = RoundClosestToZero(c_pos.GetX() * CAbstractSpaceHash::GetInvCellSize().GetX());
         n_j = RoundClosestToZero(c_pos.GetY() * CAbstractSpaceHash::GetInvCellSize().GetY());
         n_k = RoundClosestToZero(c_pos.GetZ() * CAbstractSpaceHash::GetInvCellSize().GetZ());
      }
 
      virtual bool CheckCell(SInt32 n_i,
                             SInt32 n_j,
                             SInt32 n_k,
                             TEntityList& t_entities) = 0;
 
      virtual void Dump(CARGoSLog& c_os) = 0;
 
   protected:
 
      inline UInt32 CoordinateHash(SInt32 n_i,
                                   SInt32 n_j,
                                   SInt32 n_k) {
         return
            ((73856093u * n_i) ^
             (19349663u * n_j) ^
             (83492791u * n_k)) %
            m_unSize;
      }
      
   private:
 
      TEntityList m_tEntities;
 
      size_t m_unSize;
 
      CVector3 m_cCellSize;
 
      CVector3 m_cInvCellSize;
 
   };
 
   /****************************************/
   /****************************************/
 
   template <class ENTITY>
   class CSpaceHashUpdater {
 
     public:
 
      virtual ~CSpaceHashUpdater() {}
 
      virtual void operator()(CAbstractSpaceHash<ENTITY>& c_space_hash,
                              ENTITY& c_entity) = 0;
 
   };
 
   /****************************************/
   /****************************************/
 
   template <class ENTITY, class UPDATER>
   class CSpaceHash : public CAbstractSpaceHash<ENTITY> {
 
   public:
 
      virtual void Update() {
         /* Go through all the entities */
         for(typename CAbstractSpaceHash<ENTITY>::TEntityList::const_iterator el = CAbstractSpaceHash<ENTITY>::GetEntities().begin();
             el != CAbstractSpaceHash<ENTITY>::GetEntities().end(); ++el) {
            m_cUpdater(*this, **el);
         }
      }
 
   private:
 
      UPDATER m_cUpdater;
 
   };
 
   /****************************************/
   /****************************************/
 
}
 
#endif