基于XPBD的C++双摆系统能量损耗问题排查与修复咨询

我用C++实现了基于XPBD方法的物理精确双摆模拟，参考XPBD核心论文实现，但模拟中系统随时间出现明显能量损耗，推测问题出在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;

            }
        }
    }
}

原主循环代码 #

// 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;

    }

}

错误分析与修正方案 #

1. 核心问题：Verlet积分步骤错误 #

原代码的"Verlet积分"实际是半隐式欧拉方法，破坏了Verlet积分的能量守恒特性，这是能量损耗的主要原因。

2. 约束求解的冗余与精度问题 #

• 不必要的currentLen != originalLen判断可能因浮点数精度问题跳过约束修正
• 梯度计算冗余，可简化为单位向量运算，减少计算误差

修正后的约束求解代码 #

void Spring::solveForConstraints(float dt) {
    Vector2Custom diffVector = end_p->pos - start_p->pos;
    double currentLen = diffVector.Length();

    // 处理极端情况避免除以0
    if (currentLen < 1e-8) {
        return;
    }

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

    double lambda = 0;
    double lambda_delta;

    for (int k = 0; k < 8; k++) {
        // 每次迭代重新计算当前状态的约束值与梯度
        diffVector = end_p->pos - start_p->pos;
        currentLen = diffVector.Length();
        double C = currentLen - originalLen;
        Vector2Custom n = diffVector.Normalize();
        Vector2Custom gradC1 = -n;
        Vector2Custom gradC2 = n;

        if ((!start_p->isFixed) && (!end_p->isFixed)) {
            // 单位向量点乘自身为1，简化分母计算
            double denominator = w1 + w2 + a_hat;
            lambda_delta = (-C - a_hat * lambda) / denominator;
            lambda += lambda_delta;

            start_p->pos += gradC1 * lambda_delta * w1;
            end_p->pos += gradC2 * lambda_delta * w2;
        } else if (!start_p->isFixed) {
            double denominator = w1 + a_hat;
            lambda_delta = (-C - a_hat * lambda) / denominator;
            lambda += lambda_delta;

            start_p->pos += gradC1 * lambda_delta * w1;
        } else if (!end_p->isFixed) {
            double denominator = w2 + a_hat;
            lambda_delta = (-C - a_hat * lambda) / denominator;
            lambda += lambda_delta;

            end_p->pos += gradC2 * lambda_delta * w2;
        }
    }
}

修正后的主循环代码 #

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

for (int j = 0; j < substeps; j++) {
    // 标准Verlet积分预测位置
    for (int i = 1; i < numOfParticles; i++) {
        Particle* currentBall = &ParticleList[i];
        if (currentBall->isFixed) continue;

        Vector2Custom accVector = gravVec * currentBall->invMass;
        // Verlet核心公式：x_new = 2*x_current - x_old + a*dt²
        Vector2Custom newPos = currentBall->pos * 2 - currentBall->old_pos + accVector * (dt_sub * dt_sub);
        currentBall->old_pos = currentBall->pos;
        currentBall->pos = newPos;
    }

    // 求解长度约束
    for (auto& spring : springList) {
        spring.solveForConstraints(dt_sub);
    }

    // 更新速度
    for (int i = 1; i < numOfParticles; i++) {
        Particle* currentBall = &ParticleList[i];
        if (currentBall->isFixed) continue;

        currentBall->speed = (currentBall->pos - currentBall->old_pos) / dt_sub;
    }
}

内容的提问来源于stack exchange，提问作者Hero--