void CFootBotBaseGroundRotZOnlySensor::Update() {
/*
* We make the assumption that the robot is rotated only wrt to Z
*/
/* Get robot position and orientation */
const CVector3& cEntityPos = m_pcEmbodiedEntity->GetPosition();
const CQuaternion& cEntityRot = m_pcEmbodiedEntity->GetOrientation();
CRadians cRotZ, cRotY, cRotX;
cEntityRot.ToEulerAngles(cRotZ, cRotY, cRotX);
/* Set robot center */
CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
/* Position of sensor on the ground after rototranslation */
CVector2 cSensorPos;
/* Go through the sensors */
for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
/* Calculate sensor position on the ground */
cSensorPos = m_pcGroundSensorEntity->GetSensor(i+4).Offset;
cSensorPos.Rotate(cRotZ);
cSensorPos += cCenterPos;
/* Get the color */
const CColor& cColor = m_pcFloorEntity->GetColorAtPoint(cSensorPos.GetX(),
cSensorPos.GetY());
/* Set the reading */
m_tReadings[i].Value = cColor.ToGrayScale() / 255.0f;
/* Apply noise to the sensor */
if(m_bAddNoise) {
m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange);
}
/* Set the final reading */
m_tReadings[i].Value = m_tReadings[i].Value < 0.5f ? 0.0f : 1.0f;
}
}
void CFootBotMotorGroundSensor::Update() {
/* We make the assumption that the foot-bot is rotated only wrt to Z */
CFloorEntity& cFloorEntity = m_cSpace.GetFloorEntity();
const CVector3& cEntityPos = GetEntity().GetEmbodiedEntity().GetPosition();
const CQuaternion& cEntityRot = GetEntity().GetEmbodiedEntity().GetOrientation();
CRadians cRotZ, cRotY, cRotX;
cEntityRot.ToEulerAngles( cRotZ, cRotY, cRotX );
CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
CVector2 cSensorPos;
for(UInt32 i = 0; i < CCI_FootBotMotorGroundSensor::NUM_READINGS; ++i) {
cSensorPos = m_tReadings[i].Offset;
cSensorPos.Rotate(cRotZ);
cSensorPos *= 0.01;
cSensorPos += cCenterPos;
const CColor& cColor = cFloorEntity.GetColorAtPoint(cSensorPos.GetX(),cSensorPos.GetY());
m_tReadings[i].Value = cColor.ToGrayScale()/255*FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.GetSpan();
if( m_fNoiseLevel > 0.0f ) {
AddNoise(i);
}
/* Normalize reading between 0 and 1, only if calibration has been performed */
if( m_bCalibrated ) {
m_tReadings[i].Value = FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.NormalizeValue(m_tReadings[i].Value);
}
}
}
