Position correction algorithm

[hop]

Can anyone tell me if this formulation for position correction is correct?

Code: Select all

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;

       // Calcluate the effective inverse mass matrix. Actually a scalar for this constraint.
       float k = m_pParticle[ 0 ]->GetInverseMass() + m_pParticle[ 1 ]->GetInverseMass();

	// calculate the Lagrange Multiplier lambda
	const float lambda = -c * k;

	// calculate the corrective "impulse"
	Vector normal = Normalise( d );
	Vector impulse = normal * lambda;

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

	m_pParticle[ 0 ]->SetPos( m_pParticle[ 0 ]->GetPos() + dx0 );
	m_pParticle[ 1 ]->SetPos( m_pParticle[ 1 ]->GetPos() + dx1 );

	const float kLinearSlop = 0.005f;
	return abs( c ) < kLinearSlop;
}

Edit: I think I figured it out. The corrective translation for each particle should scale inversely with mass i.e. dx_i = dx * m_i^-1 / m^-1. so the corrected line in the code is const float lambda = -c / k;

[Dirk Gregorius]

You are right. The effective mass is Me = 1 / ( 1/m1 + 1/m2 )