Sphere Collisions

void OpenGLWidget::collisions()
{
    // sprawdzenie i odpowiedź zderzenia cząstki ze ścianami pojemnika:
    for (auto p: particle)
    {
        if (p->getPosition().x() - p->getRadius() <= -1.0)
        {
            p->setPosition(QVector3D(p->getPosition().x() + p->getRadius(), p->getPosition().y(), p->getPosition().z()));
            p->setVelocity(QVector3D(-1.0*p->getVelocity().x(), p->getVelocity().y(), p->getVelocity().z()));
        }
        if (p->getPosition().x() + p->getRadius() >= 1.0)
        {
            p->setPosition(QVector3D(p->getPosition().x() - p->getRadius(), p->getPosition().y(), p->getPosition().z()));
            p->setVelocity(QVector3D(-1.0*p->getVelocity().x(), p->getVelocity().y(), p->getVelocity().z()));
        }
        if (p->getPosition().y() - p->getRadius() <= -1.0)
        {
            p->setPosition(QVector3D(p->getPosition().x(), p->getPosition().y() + p->getRadius(), p->getPosition().z()));
            p->setVelocity(QVector3D(p->getVelocity().x(), -1.0*p->getVelocity().y(), p->getVelocity().z()));
        }
        if (p->getPosition().y() + p->getRadius() >= 1.0)
        {
            p->setPosition(QVector3D(p->getPosition().x(), p->getPosition().y() - p->getRadius(), p->getPosition().z()));
            p->setVelocity(QVector3D(p->getVelocity().x(), -1.0*p->getVelocity().y(), p->getVelocity().z()));
        }
        if (p->getPosition().z() - p->getRadius() <= -1.0)
        {
            p->setPosition(QVector3D(p->getPosition().x(), p->getPosition().y(), p->getPosition().z() + p->getRadius()));
            p->setVelocity(QVector3D(p->getVelocity().x(), p->getVelocity().y(), -1.0*p->getVelocity().z()));
        }
        if (p->getPosition().z() + p->getRadius() >= 1.0)
        {
            p->setPosition(QVector3D(p->getPosition().x(), p->getPosition().y(), p->getPosition().z() - p->getRadius()));
            p->setVelocity(QVector3D(p->getVelocity().x(), p->getVelocity().y(), -1.0*p->getVelocity().z()));
        }
    }

    // sprawdzenie i odpowiedź zderzenia cząstki z cząstką:
    float ctol = 0.0001;
    float fCr = 1.0;
    for (int i = 0; i < particle.size()-1; ++i)
    {
        for (int j = i+1; j < particle.size(); ++j)
        {
            float r = particle[i]->getRadius() + particle[j]->getRadius();
            QVector3D d = particle[i]->getPosition() - particle[j]->getPosition();
            float s = d.length() - 2*r;
            d.normalize();
            QVector3D collisionNormal = d;
            QVector3D relativeVelocity = particle[i]->getVelocity() - particle[j]->getVelocity();

            float Vrn = QVector3D::dotProduct(relativeVelocity, collisionNormal);

            if ((qFabs(s) <= ctol) && (Vrn < 0.0) || s < -ctol)
            {
                particle[i]->setPosition(particle[i]->getPosition() - (s)*(QVector3D::dotProduct(collisionNormal, particle[i]->getVelocity()))*particle[i]->getVelocity());
                particle[j]->setPosition(particle[j]->getPosition() - (s)*(QVector3D::dotProduct(collisionNormal, particle[j]->getVelocity()))*particle[j]->getVelocity());

                float jk = (-(1.0 + fCr)*(QVector3D::dotProduct(relativeVelocity, collisionNormal)))/((QVector3D::dotProduct(collisionNormal, collisionNormal)*(1.0/particle[i]->getMass() + 1.0/particle[j]->getMass())));
                particle[i]->setVelocity(particle[i]->getVelocity() + (jk*collisionNormal)/particle[i]->getMass());
                particle[j]->setVelocity(particle[j]->getVelocity() - (jk*collisionNormal)/particle[j]->getMass());
            }
        }
    }
}