void draw(){
_window.Clear();
{
sf::Rect<float> frect = sf::Rect<float>(0,0,window_width,window_height);
_camera.SetFromRect(frect);
}
_window.Draw(_backgroundSprite);
// draw the range shade
_rangeSprite.SetPosition(current_pose.x + window_width/2, window_height/2 - current_pose.y);
_window.Draw(_rangeSprite);
// draw the landmarks
for(unsigned int i=0; i<landmarks.size(); i++){
_landmarkSprite.SetPosition(landmarks[i].x + window_width/2, window_height/2 - landmarks[i].y);
_window.Draw(_landmarkSprite);
}
// draw the robot
_robotSprite.SetPosition(current_pose.x + window_width/2, window_height/2 - current_pose.y);
_robotSprite.SetRotation(current_pose.theta * 180 / M_PI);
_window.Draw(_robotSprite);
// draw the "other" robot
_fakeRobotSprite.SetPosition(fake_current_pose.x + window_width/2, window_height/2 - fake_current_pose.y);
_fakeRobotSprite.SetRotation(fake_current_pose.theta * 180 / M_PI);
_window.Draw(_fakeRobotSprite);
// display everything
_window.Display();
}
float Functions::calculateRotation(sf::Sprite &sprite, float posX, float posY, float radius, float speed, float angle, float angleOffset)
{
float rad = (float)(angle * (3.1415 / -180));
sprite.SetPosition(posX + radius * sin(rad), posY + radius * cos(rad));
angle -= speed;
angle = fmod(angle, 360);
if (angle == 0)
{
angle = 360;
}
sprite.SetRotation(angle + angleOffset);
return angle;
}