My test case is a single fixed particle (mass = infinite; invmass = 0) constrained to a particle with finite mass m. The particles are under the influence of gravity and I can also apply a spring-like force with the mouse. When m < 0.25 the position correction step is not stable and the first assert fires in the SolvePositionConstraint function (see code below).
Further, if I do not under-relax the impulse in the SolvePositionConstraint function the same thing happens. By experiment any value over 0.5 causes instabilities.
The basic formulation with full explanation follows. Can anybody explain the reason for the instabilities I am seeing? Is my method perhaps incorrect?
Many thanks
Distance constraint code:
Code: Select all
void ParticleDistanceConstraint::InitVelocityConstraint()
{
// Calcluate the effective inverse mass matrix. Actually a scalar for this constraint.
m_k = m_pParticle[ 0 ]->GetInverseMass() + m_pParticle[ 1 ]->GetInverseMass();
// The Jacobian is the two component row vector [ -n^T n^T ]. We need only store the normal
// This can be calculated once here and remains constant when solving the velocity constraint.
m_normal = Normalise( m_pParticle[ 1 ]->GetPos() - m_pParticle[ 0 ]->GetPos() );
}
void ParticleDistanceConstraint::SolveVelocityConstraint( float dt )
{
// particle separation
const Vector d = m_pParticle[ 1 ]->GetPos() - m_pParticle[ 0 ]->GetPos();
// position constraint is error in length
const float c = Length( d ) - m_length;
// relative velocity
const Vector dv = m_pParticle[ 1 ]->GetVel() - m_pParticle[ 0 ]->GetVel();
// Definition of velocity constraint cDot = Jv + beta * c / dt
const float beta = 0.001f;
const float cDot = Dot( m_normal, dv ) + beta * c / dt;
// calculate the Lagrange Multiplier lambda i.e. the constraint space impulse magnitude
// J M^-1 J^T lambda = -( J v + beta * c ) / dt
const float lambdaDt = -cDot / m_k;
// calculate the corrective impulse
// F = J^T * lambda therefore J^T * lambda * dt = F * dt = impulse
const Vector impulse0 = -lambdaDt * m_normal;
const Vector impulse1 = lambdaDt * m_normal;
// apply the corrective impulses to the particles
m_pParticle[ 0 ]->ApplyImpulse( impulse0 );
m_pParticle[ 1 ]->ApplyImpulse( impulse1 );
}
// Problem: This functions is NOT stable for particle mass < 0.25, and for position
// correction relaxation factor > 0.5
bool ParticleDistanceConstraint::SolvePositionConstraint()
{
const Vector d = m_pParticle[ 1 ]->GetPos() - m_pParticle[ 0 ]->GetPos();
// position constraint is error in length
const float c = Length( d ) - m_length;
// calculate the Lagrange Multiplier lambda
const float lambda = -c * m_k;
// calculate the corrective impulse
Vector impulse = m_normal * lambda;
// under-relax the impulse
impulse *= s_positionCorrectionRelaxationFactor;
// apply instantaneous (impulse-like) change in positions to the particles
const Vector dx0 = -impulse * m_pParticle[ 0 ]->GetInverseMass();
const Vector dx1 = impulse * m_pParticle[ 1 ]->GetInverseMass();
ASSERT( LengthSqr( dx0 ) < 100.0f * 100.0f, "Very large position correction attempted on particle 0. Solution is unstable." );
ASSERT( LengthSqr( dx1 ) < 100.0f * 100.0f, "Very large position correction attempted on particle 1. Solution is unstable." );
m_pParticle[ 0 ]->SetPos( m_pParticle[ 0 ]->GetPos() + dx0 );
m_pParticle[ 1 ]->SetPos( m_pParticle[ 1 ]->GetPos() + dx1 );
// TODO: Use this constant for all linear constraints - move to suitable location as appropriate
static const float kLinearSlop = 0.005f;
return abs( c ) < kLinearSlop;
}
Code: Select all
void ParticleWorld::SequentialImpulseSolve( float dt, u32 numIterations )
{
// Integrate applied forces to find new tentative particle velocities.
// These new velocities may well violate any imposed velocity constraints.
IntegrateForces( dt );
for( u32 i = 0; i < m_numConstraints; i++ )
{
m_pConstraint[ i ]->InitVelocityConstraint();
}
// Solve velocity constraints
for( u32 it = 0; it < numIterations; it++ )
{
for( u32 i = 0; i < m_numConstraints; i++ )
{
m_pConstraint[ i ]->SolveVelocityConstraint( dt );
}
}
// Integrate the particle position using the velocities that satisfy the velocity constraints
IntegrateVelocities( dt );
// Satisfy the position constraints
for( u32 it = 0; it < numIterations; it++ )
{
bool bAllSatisfied = true;
for( u32 i = 0; i < m_numConstraints; i++ )
{
bAllSatisfied &= m_pConstraint[ i ]->SolvePositionConstraint();
}
if( bAllSatisfied )
{
DebugPrintf( "All position constraints satisfied on iteration %d\n", it );
break;
}
}
}