2 #include <argos3/core/simulator/entity/embodied_entity.h>
3 #include <argos3/core/simulator/simulator.h>
4 #include <argos3/core/simulator/space/space.h>
5 #include <argos3/core/simulator/space/positional_indices/grid.h>
6 #include <argos3/core/utility/configuration/argos_exception.h>
7 #include <argos3/core/utility/logging/argos_log.h>
30 std::string strPosIndexMethod(
"grid");
39 if(strPosIndexMethod ==
"grid") {
40 size_t punGridSize[3];
42 punGridSize[0] = cArenaSize.
GetX();
43 punGridSize[1] = cArenaSize.
GetY();
44 punGridSize[2] = cArenaSize.
GetZ();
47 std::string strPosGridSize;
49 ParseValues<size_t>(strPosGridSize, 3, punGridSize,
',');
52 cArenaCenter - cArenaSize * 0.5f, cArenaCenter + cArenaSize * 0.5f,
53 punGridSize[0], punGridSize[1], punGridSize[2]);
56 m_pcRABEquippedEntityIndex = pcGrid;
59 THROW_ARGOSEXCEPTION(
"Unknown method \"" << strPosIndexMethod <<
"\" for the positional index.");
79 m_pcRABEquippedEntityIndex->Reset();
81 for(TRoutingTable::iterator it = m_tRoutingTable.begin();
82 it != m_tRoutingTable.end();
92 delete m_pcRABEquippedEntityIndex;
93 if(m_pcRABEquippedEntityGridUpdateOperation != NULL) {
94 delete m_pcRABEquippedEntityGridUpdateOperation;
101 static UInt64 HashRABPair(
const std::pair<CRABEquippedEntity*, CRABEquippedEntity*>& c_pair) {
102 UInt64 unA = *
reinterpret_cast<unsigned long long*
>(c_pair.first) & 0xFFFFFFFF;
103 UInt64 unB = *
reinterpret_cast<unsigned long long*
>(c_pair.second) & 0xFFFFFFFF;
104 return (unA << 32) | unB;
109 m_pcRABEquippedEntityIndex->Update();
111 for(TRoutingTable::iterator it = m_tRoutingTable.begin();
112 it != m_tRoutingTable.end();
117 std::map<UInt64, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> > mapPairsAlreadyChecked;
119 std::map<UInt64, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> >::iterator itPair;
121 std::pair<CRABEquippedEntity*, CRABEquippedEntity*> cTestKey;
125 CRay3 cOcclusionCheckRay;
133 for(TRoutingTable::iterator it = m_tRoutingTable.begin();
134 it != m_tRoutingTable.end();
142 m_pcRABEquippedEntityIndex->GetEntitiesAt(cOtherRABs, cRAB.
GetPosition());
145 it2 != cOtherRABs.
end();
150 if(&cRAB != &cOtherRAB) {
152 if(&cRAB < &cOtherRAB) {
153 cTestKey.first = &cRAB;
154 cTestKey.second = &cOtherRAB;
157 cTestKey.first = &cOtherRAB;
158 cTestKey.second = &cRAB;
160 unTestHash = HashRABPair(cTestKey);
161 itPair = mapPairsAlreadyChecked.find(unTestHash);
162 if(itPair == mapPairsAlreadyChecked.end() ||
163 itPair->second.first != cTestKey.first ||
164 itPair->second.second != cTestKey.second) {
166 mapPairsAlreadyChecked[unTestHash] = cTestKey;
178 fDistance = cOcclusionCheckRay.
GetLength();
179 if(fDistance < cOtherRAB.
GetRange()) {
181 it->second.insert(&cOtherRAB);
185 m_tRoutingTable[&cOtherRAB].insert(&cRAB);
199 m_tRoutingTable.insert(
202 m_pcRABEquippedEntityIndex->AddEntity(c_entity);
209 TRoutingTable::iterator it = m_tRoutingTable.find(&c_entity);
210 if(it != m_tRoutingTable.end()) {
211 m_pcRABEquippedEntityIndex->RemoveEntity(c_entity);
212 m_tRoutingTable.erase(it);
223 TRoutingTable::const_iterator it = m_tRoutingTable.find(&c_entity);
224 if(it != m_tRoutingTable.end()) {
237 "Carlo Pinciroli [ilpincy@gmail.com]",
239 "It simulates the communication across range-and-bearing-equipped robots.",
240 "This medium is required to simulate communication across range-and-bearing-\n"
241 "equipped robots. You need to add it to the <media> section every time you add\n"
242 "a range-and-bearing-equipped entity whose controller has a range-and-bearing\n"
243 "device activated.\n\n"
244 "REQUIRED XML CONFIGURATION\n\n"
245 "<range_and_bearing id=\"rab\" />\n\n"
246 "OPTIONAL XML CONFIGURATION\n\n"
247 "None for the time being\n",