I tried to write a physicaly accurate double pendulum. And I found a method called XPBD.
https://matthias-research.github.io/pages/publications/XPBD.pdf
you can read it from here.

As you can see in the video, the system is losing energy over time. I think this is due to the length constraint in the Spring class, but I couldn’t find my mistake. How can I make the system work properly without losing energy?

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;

            }
        }
    }
}

the part where I got the formulas

formulas 17 and 18 is what i tried to do.

for better understanding. there is a algorithm below

algorithm

Below is where i call the function.

        // 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 < stickList.size(); j++)
            {
                stickList[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;

            }

        }

How can I fix the formula if I am using it incorrectly?