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();
}
Esempio n. 2
0
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;
}