Почему мой двойной маятник теряет энергию? Написан на С++

Я попытался написать физически точный двойной маятник. И я нашел метод под названием XPBD. https://matthias-research.github.io/pages/publications/XPBD.pdf вы можете прочитать это отсюда.

https://thewikihow.com/video_EzuNFaqsfEY

Как видно на видео, система со временем теряет энергию. Я думаю, что это связано с ограничением длины в классе Spring, но я не смог найти свою ошибку. Как заставить систему работать правильно без потери энергии?

void Spring::solveForConstraints(float dt) {

    Vector2Custom diffVector = end_p->pos - start_p->pos;
    double currentLen = diffVector.Length();

    double a_hat = inverseStiffnes / (dt * dt);
    double w1 = start_p->invMass;
    double w2 = end_p->invMass;


    if (currentLen != originalLen) {


        double lambda = 0;
        double lambda_delta;

        for (int k = 0; k < 8; k++)
        {

            double C = (end_p->pos - start_p->pos).Length() - originalLen;

            Vector2Custom delta_C1 = (start_p->pos - end_p->pos).Normalize();
            Vector2Custom delta_C2 = (end_p->pos - start_p->pos).Normalize(); // -delta_C1

            // formula from paper   delta_x = inverseMass * delta_C * delta_lambda;
            // formula from paper   delta lambda = (-C - â * lambda) / (delta_C * inverseMass  + a_hat);



            if ((!start_p->isFixed) && (!end_p->isFixed)) { // w1 / (w1+ w2) * (l-l0 ) * diffvectorNormalized


                lambda_delta = (-C - a_hat * lambda) / (delta_C1.DotProduct(delta_C1 * w1) + delta_C2.DotProduct(delta_C2 * w2) + a_hat);
                lambda += lambda_delta;

                Vector2Custom delta_x1 = delta_C1 * lambda_delta * w1;
                Vector2Custom delta_x2 = delta_C2 * lambda_delta * w2;

                start_p->pos = start_p->pos + delta_x1;
                end_p->pos = end_p->pos + delta_x2;


            }
            else if (!start_p->isFixed) {

                lambda_delta = (-C - a_hat * lambda) / (delta_C1.DotProduct(delta_C1 * w1) + a_hat);
                lambda += lambda_delta;

                Vector2Custom delta_x1 = delta_C1 * lambda_delta * w1;

                start_p->pos = start_p->pos + delta_x1;

            }
            else if (!end_p->isFixed) {

                lambda_delta = (-C - a_hat * lambda) / (delta_C2.DotProduct(delta_C2 * w2) + a_hat);
                lambda += lambda_delta;

                Vector2Custom delta_x2 = delta_C2 * lambda_delta * w2;

                end_p->pos = end_p->pos + delta_x2;

            }
        }
    }
}

часть, где я взял формулы

формулы 17 и 18 — это то, что я пытался сделать.

для лучшего понимания. есть алгоритм ниже

алгоритм

Ниже я вызываю функцию.

        // Update 
        int substeps = 10;
        float dt_sub = dt / substeps;

        for (int j = 0; j < substeps; j++)
        {

            //Verlet Integration
            // First, guess where is particles is going to be.

            for (int i = 1; i < numOfParticles; i++)
            {
                Particle* currentBall = &ParticleList[i];

                Vector2Custom accVector = gravVec * currentBall->invMass;               //acceleration vector  a = F/m 
                currentBall->pos = currentBall->pos + accVector * (dt_sub * dt_sub);    //add acc * dt * dt 
                currentBall->pos = currentBall->pos + currentBall->speed * dt_sub;      //add speed * dt

            }

            // Solve lenght Constraints
      
            for (int j = 0; j < springList.size(); j++)
            {
                springList[j].solveForConstraints(dt_sub);
            }

            //Update velocity
            for (int i = 1; i < numOfParticles; i++)
            {
                Particle* currentBall = &ParticleList[i];
                currentBall->speed = (currentBall->pos - currentBall->old_pos) / dt_sub;
                currentBall->old_pos = currentBall->pos;

            }

        }

Как исправить формулу, если я использую ее неправильно?