Cleaned up queen movement function.