I have a physics simulation that I wrote using the xpbd method. I can’t understand where I made a mistake. I am trying to simulate a double pendulum, but the system is constantly losing energy and the pendulum stops over time.I think the problem is in the length constraint.

void solveForConstraints(float dt) { // For every lenght constarint
    Vector2Custom diffVector = end_p->pos - start_p->pos ;
    double currentLen = diffVector.Length();
    //double epsilon = 1e-12;
    double weird_a = inverseStiffnes / (dt * dt);

    
    if (currentLen != originalLen) {


        double lambda = 0;
        double lambda_delta;

        //cout << " --------------" << endl;
        for (int k = 0; k < 8; k++)
        {

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

            Vector2Custom C_dist = diffVector.Normalize() * (currentLen - originalLen);

            double C = currentLen - originalLen;
            double w1 = start_p->invMass;
            double w2 = end_p->invMass;

            

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

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

                
                lambda_delta = (-C - weird_a * lambda) / (w1 + w2 + weird_a);
                lambda += lambda_delta;
                //cout << "l:" << lambda << endl;
                //cout << "l_delta:" << lambda_delta << endl;
                
                //lambda = (-C) / (w1 + w2 + weird_a + epsilon);

                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) {

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

                lambda_delta = (-C - weird_a * lambda) / (w1 + weird_a );
                lambda += lambda_delta;



                Vector2Custom delta_x1 = delta_C1 * lambda_delta * w1;

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


            }
            else if (!end_p->isFixed) {

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



                lambda_delta = (-C - weird_a * lambda) / (w2 + weird_a );
                lambda += lambda_delta;

                Vector2Custom delta_x2 = delta_C2 * lambda_delta * w2;

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


            }
        }
    }
}

I’m trying to simulate a double pendulum where I can move the center point . The simulation works fine and behaves normaly, but its speed decreases over time. Help me.