ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/entities/rab_equipped_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "rab_equipped_entity.h"
00008 #include <argos3/core/utility/string_utilities.h>
00009 #include <argos3/core/simulator/simulator.h>
00010 #include <argos3/core/simulator/space/space.h>
00011 
00012 namespace argos {
00013 
00014    /****************************************/
00015    /****************************************/
00016 
00017    CRABEquippedEntity::CRABEquippedEntity(CComposableEntity* pc_parent) :
00018       CPositionalEntity(pc_parent),
00019       m_pcReference(NULL),
00020       m_fRange(0.0f) {
00021       Disable();
00022       SetCanBeEnabledIfDisabled(false);
00023    }
00024 
00025    /****************************************/
00026    /****************************************/
00027 
00028    CRABEquippedEntity::CRABEquippedEntity(CComposableEntity* pc_parent,
00029                                           const std::string& str_id,
00030                                           size_t un_msg_size,
00031                                           Real f_range,
00032                                           CEmbodiedEntity& c_reference,
00033                                           const CVector3& c_pos_offset,
00034                                           const CQuaternion& c_rot_offset) :
00035       CPositionalEntity(pc_parent,
00036                         str_id,
00037                         c_reference.GetInitPosition() + c_pos_offset,
00038                         c_reference.GetInitOrientation() * c_rot_offset),
00039       m_pcReference(&c_reference),
00040       m_cPosOffset(c_pos_offset),
00041       m_cRotOffset(c_rot_offset),
00042       m_cData(un_msg_size),
00043       m_fRange(f_range) {
00044       Disable();
00045       SetCanBeEnabledIfDisabled(false);
00046    }
00047 
00048    /****************************************/
00049    /****************************************/
00050 
00051    void CRABEquippedEntity::Init(TConfigurationNode& t_tree) {
00052       try {
00053          /*
00054           * Init entity.
00055           * Here we explicitly avoid to call CPositionalEntity::Init() because that
00056           * would also initialize position and orientation, which, instead, must
00057           * be calculated from reference entity and offsets.
00058           */
00059          CEntity::Init(t_tree);
00060          /* Get reference entity */
00061          std::string strReference;
00062          GetNodeAttribute(t_tree, "reference", strReference);
00063          m_pcReference = dynamic_cast<CEmbodiedEntity*>(&CSimulator::GetInstance().GetSpace().GetEntity(strReference));
00064          if(m_pcReference == NULL) {
00065             THROW_ARGOSEXCEPTION("Entity \"" << strReference << "\" can't be used as a reference for range and bearing entity \"" << GetId() << "\"");
00066          }
00067          /* Get offsets */
00068          GetNodeAttributeOrDefault(t_tree, "pos_offset", m_cPosOffset, m_cPosOffset);
00069          std::string strRotOffset;
00070          GetNodeAttributeOrDefault(t_tree, "rot_offset", strRotOffset, strRotOffset);
00071          if(strRotOffset != "") {
00072             CDegrees cRotOffsetEuler[3];
00073             ParseValues(strRotOffset, 3, cRotOffsetEuler, ',');
00074             m_cRotOffset.FromEulerAngles(ToRadians(cRotOffsetEuler[0]),
00075                                          ToRadians(cRotOffsetEuler[1]),
00076                                          ToRadians(cRotOffsetEuler[2]));
00077          }
00078          /* Set init position and orientation */
00079          SetInitPosition(m_pcReference->GetInitPosition() + m_cPosOffset);
00080          SetInitOrientation(m_pcReference->GetInitOrientation() * m_cRotOffset);
00081          SetPosition(GetInitPosition());
00082          SetOrientation(GetInitOrientation());
00083          /* Get message size */
00084          size_t unMsgSize;
00085          GetNodeAttribute(t_tree, "msg_size", unMsgSize);
00086          m_cData.Resize(unMsgSize);
00087          /* Get transmission range */
00088          GetNodeAttribute(t_tree, "range", m_fRange);
00089       }
00090       catch(CARGoSException& ex) {
00091          THROW_ARGOSEXCEPTION_NESTED("Error initializing a range and bearing entity \"" << GetId() << "\"", ex);
00092       }
00093    }
00094 
00095    /****************************************/
00096    /****************************************/
00097 
00098    void CRABEquippedEntity::Update() {
00099       SetPosition(m_pcReference->GetPosition() + m_cPosOffset);
00100       SetOrientation(m_pcReference->GetOrientation() * m_cRotOffset);
00101    }
00102 
00103    /****************************************/
00104    /****************************************/
00105 
00106    void CRABEquippedEntity::Reset() {
00107       m_cData.Zero();
00108    }
00109 
00110    /****************************************/
00111    /****************************************/
00112 
00113    void CRABEquippedEntity::SetData(const CByteArray& c_data) {
00114       if(m_cData.Size() == c_data.Size()) {
00115          m_cData = c_data;
00116       }
00117       else {
00118          THROW_ARGOSEXCEPTION("CRABEquippedEntity::SetData() : data size does not match, expected " << m_cData.Size() << ", got " << c_data.Size());
00119       }
00120    }
00121 
00122    /****************************************/
00123    /****************************************/
00124 
00125    void CRABEquippedEntity::ClearData() {
00126       m_cData.Zero();
00127    }
00128 
00129    /****************************************/
00130    /****************************************/
00131 
00132    void CRABEquippedEntitySpaceHashUpdater::operator()(CAbstractSpaceHash<CRABEquippedEntity>& c_space_hash,
00133                                                        CRABEquippedEntity& c_element) {
00134       /* Calculate the position of the center of the RAB equipped entity in the space hash */
00135       c_space_hash.SpaceToHashTable(m_nCenterI,
00136                                     m_nCenterJ,
00137                                     m_nCenterK,
00138                                     c_element.GetPosition());
00139       /* Update the cells in a sphere around it */
00140       SInt32 nRangeI = c_space_hash.SpaceToHashTable(c_element.GetRange(), 0);
00141       SInt32 nRangeJ;
00142       SInt32 nRangeK;
00143       for(SInt32 i = 0; i <= nRangeI; ++i) {
00144          nRangeJ =
00145             c_space_hash.SpaceToHashTable(
00146                ::sqrt(
00147                   Square(c_element.GetRange()) -
00148                   Square(c_space_hash.HashTableToSpace(i, 0))
00149                   ),
00150                1);
00151          for(SInt32 j = 0; j <= nRangeJ; ++j) {
00152             nRangeK =
00153                c_space_hash.SpaceToHashTable(
00154                   ::sqrt(
00155                      Square(c_element.GetRange()) -
00156                      Square(c_space_hash.HashTableToSpace(j, 1))
00157                      ),
00158                   2);
00159             for(SInt32 k = 0; k <= nRangeK; ++k) {
00160                if(i > 0) {
00161                   /*
00162                    * i > 0
00163                    */
00164                   if(j > 0) {
00165                      /*
00166                       * i > 0
00167                       * j > 0
00168                       */
00169                      if(k > 0) {
00170                         /*
00171                          * i > 0
00172                          * j > 0
00173                          * k > 0
00174                          */
00175                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK + k, c_element);
00176                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK - k, c_element);
00177                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK + k, c_element);
00178                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK - k, c_element);
00179                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK + k, c_element);
00180                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK - k, c_element);
00181                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK + k, c_element);
00182                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK - k, c_element);
00183                      }
00184                      else {
00185                         /*
00186                          * i > 0
00187                          * j > 0
00188                          * k == 0
00189                          */
00190                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK, c_element);
00191                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK, c_element);
00192                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK, c_element);
00193                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK, c_element);
00194                      }
00195                   }
00196                   else {
00197                      /*
00198                       * i > 0
00199                       * j == 0
00200                       */
00201                      if(k > 0) {
00202                         /*
00203                          * i > 0
00204                          * j == 0
00205                          * k > 0
00206                          */
00207                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK + k, c_element);
00208                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK - k, c_element);
00209                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK + k, c_element);
00210                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK - k, c_element);
00211                      }
00212                      else {
00213                         /*
00214                          * i > 0
00215                          * j == 0
00216                          * k == 0
00217                          */
00218                         c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK, c_element);
00219                         c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK, c_element);
00220                      }
00221                   }
00222                }
00223                else {
00224                   /*
00225                    * i == 0
00226                    */
00227                   if(j > 0) {
00228                      /*
00229                       * i == 0
00230                       * j > 0
00231                       */
00232                      if(k > 0) {
00233                         /*
00234                          * i == 0
00235                          * j > 0
00236                          * k > 0
00237                          */
00238                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK + k, c_element);
00239                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK - k, c_element);
00240                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK + k, c_element);
00241                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK - k, c_element);
00242                      }
00243                      else {
00244                         /*
00245                          * i == 0
00246                          * j > 0
00247                          * k == 0
00248                          */
00249                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK, c_element);
00250                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK, c_element);
00251                      }
00252                   }
00253                   else {                     
00254                      /*
00255                       * i == 0
00256                       * j == 0
00257                       */
00258                      if(k > 0) {
00259                         /*
00260                          * i == 0
00261                          * j == 0
00262                          * k > 0
00263                          */
00264                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK + k, c_element);
00265                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK - k, c_element);
00266                      }
00267                      else {
00268                         /*
00269                          * i == 0
00270                          * j == 0
00271                          * k == 0
00272                          */
00273                         c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK, c_element);
00274                      }
00275                   }
00276                }
00277             }
00278          }
00279       }
00280    }
00281 
00282    /****************************************/
00283    /****************************************/
00284 
00285    REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CRABEquippedEntity);
00286 
00287    /****************************************/
00288    /****************************************/
00289 
00290    CRABEquippedEntityGridCellUpdater::CRABEquippedEntityGridCellUpdater(CGrid<CRABEquippedEntity>& c_grid) :
00291       m_cGrid(c_grid) {}
00292    
00293    bool CRABEquippedEntityGridCellUpdater::operator()(SInt32 n_i,
00294                                                       SInt32 n_j,
00295                                                       SInt32 n_k,
00296                                                       CGrid<CRABEquippedEntity>::SCell& s_cell) {
00297       /* Update cell */
00298       m_cGrid.UpdateCell(n_i, n_j, n_k, *m_pcEntity);
00299       /* Continue with other cells */
00300       return true;
00301    }
00302    
00303    void CRABEquippedEntityGridCellUpdater::SetEntity(CRABEquippedEntity& c_entity) {
00304       m_pcEntity = &c_entity;
00305    }
00306 
00307    CRABEquippedEntityGridEntityUpdater::CRABEquippedEntityGridEntityUpdater(CGrid<CRABEquippedEntity>& c_grid) :
00308       m_cGrid(c_grid),
00309       m_cCellUpdater(c_grid) {}
00310 
00311    bool CRABEquippedEntityGridEntityUpdater::operator()(CRABEquippedEntity& c_entity) {
00312       try {
00313          m_cCellUpdater.SetEntity(c_entity);
00314          m_cGrid.ForCellsInBoxRange(c_entity.GetPosition(),
00315                                     CVector3(c_entity.GetRange(),
00316                                              c_entity.GetRange(),
00317                                              c_entity.GetRange()),
00318                                     m_cCellUpdater);
00319          /* Continue with the other entities */
00320          return true;
00321       }
00322       catch(CARGoSException& ex) {
00323          THROW_ARGOSEXCEPTION_NESTED("While updating the RAB entity grid for RAB entity \"" << c_entity.GetContext() << c_entity.GetId() << "\"", ex);
00324       }
00325    }
00326 
00327    /****************************************/
00328    /****************************************/
00329 
00330 }