void CEPuckRangeAndBearingSensor::Update() { /* Clear the previous received packets */ ClearRABReceivedPackets(); /* Get robot position */ const CVector3& cRobotPosition = m_pcEmbodiedEntity->GetPosition(); /* Get robot orientation */ CRadians cTmp1, cTmp2, cOrientationZ; m_pcEmbodiedEntity->GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2); /* Buffer for calculating the message--robot distance */ CVector3 cVectorToMessage; CVector3 cVectorRobotToMessage; Real fMessageDistance; /* Buffer for the received packet */ TEPuckRangeAndBearingReceivedPacket tPacket; /* Initialize the occlusion check ray start to the position of the robot */ CRay cOcclusionCheckRay; cOcclusionCheckRay.SetStart(cRobotPosition); /* Buffer to store the intersection data */ CSpace::SEntityIntersectionItem<CEmbodiedEntity> sIntersectionData; /* Ignore the sensing robot when checking for occlusions */ TEmbodiedEntitySet tIgnoreEntities; tIgnoreEntities.insert(m_pcEmbodiedEntity); /* * 1. Go through all the CRABEquippedEntities<2> (those compatible with this sensor) * 2. For each of them * a) Check that the receiver is not out of range * b) Check if there is an occlusion * c) If there isn't, get the info and set reading for that robot */ CSpace::TAnyEntityMap& tEntityMap = m_cSpace.GetEntitiesByType("rab_equipped_entity<2>"); for(CSpace::TAnyEntityMap::iterator it = tEntityMap.begin(); it != tEntityMap.end(); ++it) { CRABEquippedEntity<2>& cRABEntity = *(any_cast<CRABEquippedEntity<2>*>(it->second)); /* Check the RAB equipped entity is not this robot (avoid self-messaging) */ if(&cRABEntity != m_pcRABEquippedEntity) { /* Get the position of the RAB equipped entity */ cVectorToMessage = cRABEntity.GetPosition(); cVectorRobotToMessage = (cVectorToMessage - cRobotPosition) * 100; // in cms /* Check that the distance is lower than the range */ fMessageDistance = cVectorRobotToMessage.Length(); if(fMessageDistance < cRABEntity.GetRange()) { /* Set the ray end */ cOcclusionCheckRay.SetEnd(cVectorToMessage); /* Check occlusion between robot and message location */ if(!m_bCheckOcclusions || (! m_cSpace.GetClosestEmbodiedEntityIntersectedByRay(sIntersectionData, cOcclusionCheckRay, tIgnoreEntities)) || sIntersectionData.IntersectedEntity->GetId() == cRABEntity.GetId()) { /* The message is not occluded */ if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(false, cOcclusionCheckRay); /* Set the reading */ tPacket.Id = m_unLatestPacketId++; CRadians cVertical = CRadians::ZERO; cVectorRobotToMessage.ToSphericalCoordsHorizontal(tPacket.Range, cVertical, tPacket.BearingHorizontal); tPacket.BearingHorizontal -= cOrientationZ; tPacket.BearingHorizontal.SignedNormalize(); cRABEntity.GetData(tPacket.Data); m_tLastReceivedPackets.push_back(tPacket); } else { /* The message is occluded */ if(m_bShowRays) { m_pcControllableEntity->AddCheckedRay(true, cOcclusionCheckRay); m_pcControllableEntity->AddIntersectionPoint(cOcclusionCheckRay, sIntersectionData.TOnRay); } } } } } }