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);
}
}
}
void CGroundSensorEquippedEntity::AddSensorRing(const CVector2& c_center,
Real f_radius,
const CRadians& c_start_angle,
ESensorType e_type,
UInt32 un_num_sensors,
const SAnchor& s_anchor) {
CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
CRadians cAngle;
CVector2 cOffset;
for(UInt32 i = 0; i < un_num_sensors; ++i) {
cAngle = c_start_angle + i * cSensorSpacing;
cAngle.SignedNormalize();
cOffset.Set(f_radius, 0.0f);
cOffset.Rotate(cAngle);
cOffset += c_center;
AddSensor(cOffset, e_type, s_anchor);
}
}
bool CKinematics2DEngine::CheckCollisions( const CKinematics2DCollisionCircle* pc_circle, const CKinematics2DCollisionRectangle* pc_rectangle ) {
/* Rototranslate the plane so that the rectangle is axis aligned and centered in O */
CVector2 center = pc_circle->GetPosition() - pc_rectangle->GetPosition();
center.Rotate(pc_rectangle->GetOrientation() );
center.Absolute();
/* Find the Voronoi Region that the circle is in, exploiting the symmetries */
CVector2 c_half_size = pc_rectangle->GetHalfSize();
Real f_radius = pc_circle->GetRadius();
if( center.GetX() <= c_half_size.GetX() ) {
/* The circle is in the top or bottom region */
return (center.GetY() <= c_half_size.GetY() + f_radius);
}
if( center.GetY() <= c_half_size.GetY() ) {
/* The circle is in the left or right region */
return (center.GetX() <= c_half_size.GetX() + f_radius);
}
/* The circle is in one of the four corner regions */
return (SquareDistance( c_half_size, center ) <= f_radius*f_radius);
}
