Sztywne kulki sfml

#include <SFML/Graphics.hpp>
#include <cstdio>
#include <vector>
#include <cmath>
#include <algorithm>
#include <string>
#include <set>


 int W=800;
 int H=600;

float predkosc=100;
const int kulek=100;
const double promien=30;

using namespace std;


void sleep (float ms)
{
    sf::Clock clock; // starts the clock

    sf::Time t = clock.getElapsedTime();

    while( (clock.getElapsedTime()-t).asMilliseconds()<ms ){}

}

float mod(float a,float m)
{

    while (a>m)a-=m;
    while (a<0)a+=m;
    return a;
}

template<class T>
T dot(sf::Vector2<T> a,sf::Vector2<T> b)
{
    return a.x*b.x+a.y*b.y;
}

template<class T>
T dot(sf::Vector2<T> a)
{
    return dot(a,a);
}


template <class T>
T kw(const T a)
{
    return a*a;
}

bool kolor_predkosciowy;


class kulka
{
public:
    sf::CircleShape shape;
    sf::Vector2f pozycja;
    sf::Vector2f predkosc;



    float r;
    kulka()
    {

        r=promien;
        pozycja.x=rand()%(int)(W-r);
        pozycja.y=rand()%(int)(H-r);
        predkosc.x=((rand()%1000)-500)/500.0;
        predkosc.y=((rand()%1000)-500)/500.0;
        shape=sf::CircleShape(r);
        shape.setFillColor(sf::Color::Blue);
        shape.setPosition(pozycja.x,pozycja.y);
    }

    void evolucja(float dt)
    {
        //dt=std::min(dt,(float)1);
        pozycja+=predkosc*dt;
        if (pozycja.y<0) predkosc.y=abs(predkosc.y);
        if (pozycja.x<0) predkosc.x=abs(predkosc.x);
        if (pozycja.y>H-2*r) predkosc.y=-abs(predkosc.y);
        if (pozycja.x>W-2*r) predkosc.x=-abs(predkosc.x);

        if (kolor_predkosciowy)
        {
            double V= sqrt(dot(predkosc))/2.0+0.1;
            if (V<1)
            shape.setFillColor( sf::Color(255*V,250*V,100*(1-V) ));
            else shape.setFillColor( sf::Color(255,255,250 ));
        }

        shape.setPosition(pozycja.x,pozycja.y);
    }

    bool kolizja(kulka &druga)
    {
        bool zaszla=false;
        if (dot(pozycja-druga.pozycja)<kw(r+druga.r))
        {
            zaszla=true;

            sf::Vector2< float > normalna = pozycja-druga.pozycja;
            normalna = normalna / sqrt(dot(normalna));

            float v_zbl = dot(normalna,predkosc) - dot(normalna,druga.predkosc);
            sf::Vector2f dv =  normalna* v_zbl;
            if (v_zbl<0)
            {
                predkosc -= dv;
                druga.predkosc += dv;
            }

        }

        return zaszla;
    }
};




int main()
{
    sf::ContextSettings settings;
    settings.antialiasingLevel = 8;

    sf::RenderWindow window(sf::VideoMode(W, H), "SFML works!", sf::Style::Default, settings);
    srand(0);
    sf::CircleShape shape(50.f);
    shape.setFillColor(sf::Color::Green);

    vector<kulka> kule(kulek);

    sf::Clock clock; // starts the clock

    sf::Time t = clock.getElapsedTime();
    sf::Time tt;

    sf::Font font;


    if (!font.loadFromFile("arial.ttf"))
    {
       window.close();
    }


    int N_pomiarow = 200;

    vector<sf::Vertex> vertices(N_pomiarow);
    vector<sf::Vertex> usrednione(N_pomiarow);

    vector <long long int> zliczenia(N_pomiarow,0);
    double licznik=0;
    for (int ii=0;ii<N_pomiarow;ii++)
    {
        vertices[ii].position.x=ii*2;
        vertices[ii].position.y=H-1;
        usrednione[ii].position.x=ii*2;
        usrednione[ii].position.y=H-1;
        vertices[ii].color=sf::Color(50,50,50);
    }

    while (window.isOpen())
    {
        sf::Event event;
        while (window.pollEvent(event))
        {
            if (event.type == sf::Event::Closed)
                window.close();
            else
            if (event.type == sf::Event::Resized)
            {

                //window.setSize(   sf::Vector2u(event.size.width,event.size.height)  );
                W=event.size.width;
                H=event.size.height;
                window.setView(sf::View(sf::FloatRect(0.f, 0.f, event.size.width,event.size.height)));
            }else
            if (event.type == sf::Event::KeyPressed)
            {
                if (event.key.code==sf::Keyboard::Right)
                {
                    predkosc=1.2*predkosc;
                }else
                if (event.key.code==sf::Keyboard::Left)
                {
                    predkosc=predkosc/1.2;
                }else
                if (event.key.code==sf::Keyboard::Escape)
                {
                    window.close();
                }
                if (event.key.code==sf::Keyboard::Return)
                {
                    kolor_predkosciowy=true;
                }

            }



        }

        window.clear();


        //if ((clock.getElapsedTime()-t).asSeconds()>0.02)
        {

            sort(kule.begin(),kule.end(),[](kulka&a,kulka&b){return (a.pozycja.x<b.pozycja.x);});

            /*struct comp
            {
                bool operator ()(kulka*a,kulka*b)
                {
                    return make_pair(a->pozycja.y,a->pozycja.x) < make_pair(b->pozycja.y,b->pozycja.x);
                }
            };*/



            for (auto it=kule.begin();it!=kule.end();++it)
            {
                    it->evolucja((clock.getElapsedTime()-t).asSeconds()*predkosc);

                    for (auto itt=it+1; (itt!=kule.end()) && abs(itt->pozycja.x - it->pozycja.x)<=itt->r+it->r;++itt)
                    {
                        it->kolizja(*itt);
                    }
                    //sleep(300);
            }
            t=clock.getElapsedTime();
        }



        for (auto it=kule.begin();it!=kule.end();++it)
        {
            window.draw(it->shape);
        }




        for (auto it=kule.begin();it!=kule.end();++it)
        {
            int przedzial =sqrt(dot(it->predkosc))*50;
            przedzial = min(przedzial,N_pomiarow);
            zliczenia[przedzial]++;
        }


        for (int ii=0;ii<N_pomiarow;ii++)
        {
            usrednione[ii].position.y= H-20000.0*zliczenia[ii]/licznik/kule.size();
        }
        licznik+=1;

        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Space))
        {
            kolor_predkosciowy=false;
            for_each(kule.begin(),kule.end(), [=](kulka&k)
                {
                    if (k.pozycja.x>W/2)
                    {
                        if (k.pozycja.y>H/2)
                        {
                            k.shape.setFillColor(sf::Color::Blue);
                        }else
                        {
                            k.shape.setFillColor(sf::Color::Red);
                        };
                    }else
                    {
                        if (k.pozycja.y>H/2)
                        {
                            k.shape.setFillColor(sf::Color::Magenta);
                        }else
                        {
                            k.shape.setFillColor(sf::Color::Green);
                        };
                    }

                });

        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::B))
        {
            for_each(kule.begin(),kule.end(), [](kulka&k)
                {
                        k.shape.setFillColor(sf::Color::Blue);
                });
            kolor_predkosciowy=false;
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Y))
        {
            for_each(kule.begin(),kule.end(), [](kulka&k)
                {
                        k.shape.setFillColor(sf::Color::Yellow);
                });
            kolor_predkosciowy=false;
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::G))
        {
            for_each(kule.begin(),kule.end(), [](kulka&k)
                {
                        k.shape.setFillColor(sf::Color::Green);
                });
            kolor_predkosciowy=false;
        }
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::R))
        {
            for_each(kule.begin(),kule.end(), [](kulka&k)
                {
                        k.shape.setFillColor(sf::Color::Red);
                });
            kolor_predkosciowy=false;
        }

        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Delete))
        {
            for (int ii=0;ii<N_pomiarow;ii++)
            {
                zliczenia[ii]=0;
                licznik=0;
            }
        }

        int pomiar=0;;
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Up))
        {
            pomiar+=1;
        }

        if (sf::Keyboard::isKeyPressed(sf::Keyboard::Down))
        {
            pomiar+=2;
        }

    //  window.draw(&vertices[0],vertices.size(),sf::LinesStrip);
        if (sf::Keyboard::isKeyPressed(sf::Keyboard::RControl) || sf::Keyboard::isKeyPressed(sf::Keyboard::LControl))
        {
            window.draw(&usrednione[0],vertices.size(),sf::LinesStrip);
        }
        //tekst.setColor(sf::Color::White);

        wstring s = std::to_wstring(  (long double)(1./(clock.getElapsedTime()-tt).asSeconds() ));
        s=  wstring(L" pr�dko�� ")+to_wstring((long double) predkosc)+ L" pomiar guzik�w " + to_wstring((long double)pomiar ) +wstring(L"  fps ")+s;
        tt=clock.getElapsedTime();
        sf::Text tekst;
        tekst.setFont(font);
        tekst.setCharacterSize(18);
        tekst.setString(s.c_str());
        sf::LinesStrip ;


        tekst.setPosition(1,1);
        window.draw(tekst);
        window.display();
        //sleep(1000);



    }
    window.close();
    return 0;
}