Real-Time Physics Simulation Forum

Code: Select all

// before the Step :
for (int = 0; i < nb_joint; i++)
{
   joint[i]->motorImpulseInThatStep = 0;
}

// modification of btHingeConstraint::solveConstraintObsolete
void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
{
        ....
    if (m_enableAngularMotor)
    {
        clippedMotorImpulse = ... ;
        motorImpulseInThatStep += clippedMotorImpulse;
    }
}

// after the Step
for (int = 0; i < nb_joint; i++)
{
    float imp = joint[i]->motorImpulseInThatStep;
    totalEnergy += imp*imp;
}

Code: Select all

btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(...)
{
    float totalEnergy = 0;
    for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
    {
         ....
         ///solve all joint constraints, using SIMD, if available
         for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
         {
             btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
             resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
         }
         ...
    }
    
    // at the end :
    // sum (squared) the appliedimpulse field of the 6th "row" (btSolverConstraint struct for limit and motor calculation) of each hinge joint 
    for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    {
        if (j is the 6th row of a hingeJoint)
        {
            float imp = m_tmpSolverNonContactConstraintPool[i].m_appliedImpulse;
            totalEnergyOfTheStep += imp*imp;
        }
    }
}