Help with ball and socket joint separating.

[MarkZ]

Hello,

I've been trying to make a ball and socket joint based on constraint impulses. I do this by calculating the relative velocities at the joint ends on the two connected objects and using this to find an impulse that will make this relative velocity zero. I also add a drift velocity to the relative velocity so that the impulse also corrects drift. The problem is the joint is separating like in the screen shot below.


I was hoping someone could have a look at my code and offer some suggestions about why this would be happening as I have no idea where to look.

Code: Select all

void cBallSocketJoint::PreApply(float dt)
{
	// Calculate the deviation between the end points and the joint anchor
	cVector deviation = m_a->WorldPos(m_ra) - m_b->WorldPos(m_rb);
	float deviationMag = deviation.Mag();
	if (deviationMag > MAX_DEVIATION)
	{
		cVector deviationDir = deviation.Unit();
		m_vrExtra = deviationDir * (deviationMag / dt);
	}
	else
	{
		m_vrExtra.Zero();
	}
}

void cBallSocketJoint::Apply(float dt)
{
	// Align joint ends based on current rotation
	cVector ra = m_a->Q() * m_ra;
	cVector rb = m_b->Q() * m_rb;

	// Get velocity of joint ends
	cVector va = m_a->LocalPointVel(ra);
	cVector vb = m_b->LocalPointVel(rb);

	cVector vr = m_vrExtra + va - vb;

	if (vr.Mag() < 0.0001f)
		return;

	cVector normal = vr.Unit();

	float num = -Dot(normal, vr);
	float term1 = 1.0f / m_a->Mass();
	float term2 = 1.0f / m_b->Mass();
	float term3 = Dot(normal, Cross(m_a->IInv() * Cross(ra, normal), ra));
	float term4 = Dot(normal, Cross(m_b->IInv() * Cross(rb, normal), rb));
	float den = term1 + term2 + term3 + term4;

	if (absf(den) < 0.0001f)
		return;

	cVector impulse = normal * (num / den);

	// Apply constraint impulse
	m_a->ApplyLocalImpulse(impulse, ra);
	m_b->ApplyLocalImpulse(-impulse, rb);
}

Cheers,
Mark.

[Dirk Gregorius]

I haven't checked, but is the sign of the extra velocity correct? It must be:

K * impulse = -0.1 * deviation / dt - v_rel

Where deviations is x1 + r1 - x2 - r2
You should also apply the extra velocity always and not only when some MAY_DEVIATION is exceeded...

-Dirk

[MarkZ]

Thanks for the reply.

I was wondering what K is and where the -0.1 come from in the equation?

Cheers,
Mark

[Dirk Gregorius]

A Ball-Socket Joint removes 3DOF you have formulated a distance constraint. which is a 1D constraint. Look at Erin Catto's slides from GDC 2006 or Bullet.

K is the 3x3 system matrix
0.1 is the Baumgarte factor (tau in Bullet and erp in the ODE)

Let me know if you have problems to formulate this.