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

bool kolizja( vec pozycjaA, float promienA, vec pozycjaB, float promienB, float & glebokosc, vec & normalna )
{
    float lp = dlugosc( pozycjaA, pozycjaB );
    float lr = promienA + promienB;
    if( lp > lr )
         return false;
   
    glebokosc = fabs( lp - lr );
    if( glebokosc <= 0 )
         return false;
   
    normalna = pozycjaA - pozycjaB;
    return true;
}
///
if( kolizja( pozycjaA, promienA, pozycjaB, promienB, glebokosc, normalna ) )
{
    // cofniecie kuli
    normalize( normalna );
    normalna *= glebokosc;
    pozycjaA += normalna;
    // kierunek po odbiciu
    vec nowyKierunek( kierunekA.x - 2 *- normalna.x, kierunekA.y - 2 *- normalna.y );
    normalize( nowyKierunek );
    nowyKierunek *= szybkosc;
    kierunekA = nowyKierunek;
}

normalize( vec & v )
{
    float l = dlugosc( v );
    if( l )
         v /= l;
   
}