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.
https://www.youtube.com/watch?v=EzuNFaqsfEY
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
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 < 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;
}
}
How can I fix the formula if I am using it incorrectly?
It actually states that energy loss will be a limitation of the XPBD method in the paper:
5 Damping
Our method is derived from an implicit time stepping scheme that naturally includes some dissipation of mechanical energy. Nevertheless, it can be useful to model additional constraint damping[.......]
Energy loss is expected.