void CEPuckLightSensor::Update() { /* Here we assume that the e-puck is rotated only wrt to the Z axis */ /* Erase readings */ for(size_t i = 0; i < m_tReadings.size(); ++i) { m_tReadings[i].Value = 0.0f; } /* Get e-puck position */ const CVector3& cEPuckPosition = GetEntity().GetEmbodiedEntity().GetPosition(); /* Get e-puck orientation */ CRadians cTmp1, cTmp2, cOrientationZ; GetEntity().GetEmbodiedEntity().GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2); /* Buffer for calculating the light--e-puck distance */ CVector3 cLightDistance; /* Buffer for the angle of the sensor wrt to the e-puck */ CRadians cLightAngle; /* Initialize the occlusion check ray start to the baseline of the e-puck */ CRay cOcclusionCheckRay; cOcclusionCheckRay.SetStart(cEPuckPosition); /* Buffer to store the intersection data */ CSpace::SEntityIntersectionItem<CEmbodiedEntity> sIntersectionData; /* Ignore the sensing ropuck when checking for occlusions */ TEmbodiedEntitySet tIgnoreEntities; tIgnoreEntities.insert(&GetEntity().GetEmbodiedEntity()); /* * 1. go through the list of light entities in the scene * 2. check if a light is occluded * 3. if it isn't, distribute the reading across the sensors * NOTE: the readings are additive * 4. go through the sensors and clamp their values */ try{ CSpace::TAnyEntityMap& tEntityMap = m_cSpace.GetEntitiesByType("light_entity"); for(CSpace::TAnyEntityMap::iterator it = tEntityMap.begin(); it != tEntityMap.end(); ++it) { /* Get a reference to the light */ CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second)); /* Consider the light only if it has non zero intensity */ if(cLight.GetIntensity() > 0.0f) { /* Get the light position */ const CVector3& cLightPosition = cLight.GetPosition(); /* Set the ray end */ cOcclusionCheckRay.SetEnd(cLightPosition); /* Check occlusion between the e-puck and the light */ if(! m_cSpace.GetClosestEmbodiedEntityIntersectedByRay(sIntersectionData, cOcclusionCheckRay, tIgnoreEntities)) { /* The light is not occluded */ if(m_bShowRays) GetEntity().GetControllableEntity().AddCheckedRay(false, cOcclusionCheckRay); /* Get the distance between the light and the e-puck */ cOcclusionCheckRay.ToVector(cLightDistance); /* Linearly scale the distance with the light intensity The greater the intensity, the smaller the distance */ cLightDistance /= cLight.GetIntensity(); /* Get the angle wrt to e-puck rotation */ cLightAngle = cLightDistance.GetZAngle(); cLightAngle -= cOrientationZ; /* Transform it into counter-clockwise rotation */ cLightAngle.Negate().UnsignedNormalize(); /* Find reading corresponding to the sensor */ SInt16 nMin = 0; for(SInt16 i = 1; i < NUM_READINGS; ++i){ if((cLightAngle - m_tReadings[i].Angle).GetAbsoluteValue() < (cLightAngle - m_tReadings[nMin].Angle).GetAbsoluteValue()) nMin = i; } /* Set the actual readings */ Real fReading = cLightDistance.Length(); m_tReadings[Modulo((SInt16)(nMin-1), NUM_READINGS)].Value += ComputeReading(fReading * Cos(cLightAngle - m_tReadings[Modulo(nMin-1, NUM_READINGS)].Angle)); m_tReadings[ nMin ].Value += ComputeReading(fReading); m_tReadings[Modulo((SInt16)(nMin+1), NUM_READINGS)].Value += ComputeReading(fReading * Cos(cLightAngle - m_tReadings[Modulo(nMin+1, NUM_READINGS)].Angle)); } else { /* The ray is occluded */ if(m_bShowRays) { GetEntity().GetControllableEntity().AddCheckedRay(true, cOcclusionCheckRay); GetEntity().GetControllableEntity().AddIntersectionPoint(cOcclusionCheckRay, sIntersectionData.TOnRay); } } } } } catch(argos::CARGoSException& e){ } /* Now go through the sensors, add noise and clamp their values if above 1024 or under 1024 */ for(size_t i = 0; i < m_tReadings.size(); ++i) { if(m_fNoiseLevel>0.0f) AddNoise(i); if(m_tReadings[i].Value > 1024.0f) m_tReadings[i].Value = 1024.0f; if(m_tReadings[i].Value < 0.0f) m_tReadings[i].Value = 0.0f; } }