[SFML] Ulepszenie kolizji zderzających się kółek

#include <SFML/Graphics.hpp>
#include <cmath>

sf::Vector2f getKeyCursorDir( float step );

template < typename T >
float distance( const T & v1, const T & v2 )
{
    T t;
    t.x = v1.x - v2.x;
    t.y = v1.y - v2.y;
    return std::sqrt( t.x * t.x + t.y * t.y );
}

template < typename T >
T reflect( const T & dir, const T & normal )
{
    T t;
    t.x = dir.x - 2 * normal.x;
    t.y = dir.y - 2 * normal.y;
    return t;
}

template < typename T >
T normalize( T & v )
{
    float length = v.x * v.x + v.y * v.y;
    if( length )
    {
        length = 1.f / std::sqrt( length );
        v.x = v.x * length;
        v.y = v.y * length;
    }
    return v;
}

bool collideCircleCircle( const sf::Vector2f & positionA, const float radiusA,
const sf::Vector2f & positionB, const float radiusB,
float & depth, sf::Vector2f & normal )
{
    float lp = distance( positionA, positionB );
    float lr = radiusA + radiusB;
   
    if( lp > lr )
         return false;
   
    depth = lr - lp; //std::abs(lp - lr);
    if( depth <= 0.0f )
         return false;
   
    normal = positionA - positionB;
    return true;
}

class Circle
    : public sf::CircleShape
{
public:
    Circle( float radius, const sf::Color & color )
        : sf::CircleShape( radius )
        , Direction( sf::Vector2f() )
    {
        CircleShape::setOrigin( CircleShape::getRadius(), CircleShape::getRadius() );
        CircleShape::setFillColor( color );
    }
   
    void step()
    {
        sf::CircleShape::move( Direction );
    }
    sf::Vector2f Direction;
};

int main()
{
    sf::RenderWindow window( sf::VideoMode( 800, 600 ), "[ SFML Template ]" );
    window.setFramerateLimit( 60 );
   
    Circle CircleA( 50.0f, sf::Color::Blue );
    CircleA.setPosition( 400, 300 + 150 );
   
    Circle CircleB( 50.0f, sf::Color::Green );
    CircleB.setPosition( 400, 300 );
   
    const float step = 2.f;
   
    while( window.isOpen() )
    {
        sf::Event event;
        while( window.pollEvent( event ) )
        {
            if( event.type == sf::Event::KeyPressed && event.key.code == sf::Keyboard::Escape )
                 window.close();
           
        }
       
        // ręczne sterowanie kulą A
        sf::Vector2f newDir = getKeyCursorDir( 1.0f );
        if( newDir.x != 0.0 || newDir.y != 0.0 )
        {
            normalize( newDir );
            newDir *= step;
            CircleA.Direction = newDir;
        }
       
       
        float depth;
        sf::Vector2f normal;
       
        if( collideCircleCircle( CircleA.getPosition(), CircleA.getRadius(), CircleB.getPosition(), CircleB.getRadius(), depth, normal ) )
        {
            // wycofanie kuli A o głębokość na którą wpadły w siebie
            normalize( normal );
            normal *= depth;
            CircleA.move( normal );
           
            #if 1       // 0 bez odbicia
            {
                // nowy kierunek dla kuli A
                //sf::Vector2f refl = sf::Vector2f(CircleA.Direction.x-2*-normal.x, CircleA.Direction.y-2*-normal.y);
                sf::Vector2f refl = reflect( CircleA.Direction, - normal );
                CircleA.Direction = normalize( refl ) * step;;
            }
            {
                // nowy kierunek dla kuli B
                sf::Vector2f refl = reflect( CircleB.Direction, normal );
                CircleB.Direction = normalize( refl ) * step;
            }
            #endif
        }
       
        CircleA.step();
        CircleB.step();
       
        window.clear();
        window.draw( CircleA );
        window.draw( CircleB );
        window.display();
    }
    return 0;
}

sf::Vector2f getKeyCursorDir( float step )
{
    sf::Vector2f dir( 0.0, 0.0 );
    if( sf::Keyboard::isKeyPressed( sf::Keyboard::Up ) )
         dir.y -= step;
   
    if( sf::Keyboard::isKeyPressed( sf::Keyboard::Down ) )
         dir.y += step;
   
    if( sf::Keyboard::isKeyPressed( sf::Keyboard::Left ) )
         dir.x -= step;
   
    if( sf::Keyboard::isKeyPressed( sf::Keyboard::Right ) )
         dir.x += step;
   
    return dir;
}