Distance constraint

Please don't post Bullet support questions here, use the above forums instead.
Post Reply
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Distance constraint

Post by c0der »

Hi,

I have coded the following distance constraint but am having problems with the bias, since the relative position can sometimes be zero, here is the code, please help !

I have tested it without the bias and it still behaves like a point-to-point constraint

Thanks

Code: Select all

AMG3DDistanceJoint::AMG3DDistanceJoint(AMG3DRigidBody *pRigidBody1, AMG3DRigidBody *pRigidBody2, AMG3DVector4 vAnchorPoint)
{
	m_pRigidBody1	= pRigidBody1;
	m_pRigidBody2	= pRigidBody2;

	AMG3DMatrix4x4 Rot1, Rot1T, Rot2, Rot2T;
	AMG3DQuaternion q1 = m_pRigidBody1->quatOrientation;
	AMG3DQuaternion q2 = m_pRigidBody2->quatOrientation;

	q1.toMatrix(&Rot1);
	Rot1T.transposeOf(Rot1);
	q2.toMatrix(&Rot2);
	Rot2T.transposeOf(Rot2);

	localAnchor1 =  Rot1T*(vAnchorPoint - m_pRigidBody1->position);
	localAnchor2 =  Rot2T*(vAnchorPoint - m_pRigidBody2->position);

	initialDistance = (localAnchor1-localAnchor2).magnitude();
}

void AMG3DDistanceJoint::preStep(AMG3DScalar dt)
{
	if(dt<=0.0f)
		return;

	AMG3DScalar k_biasFactor = (AMG3DPhysicsWorld::g_AMG3D_PositionCorrection) ? 0.2f : 0.0f;

	AMG3DMatrix4x4 Rot1, Rot2;
	AMG3DQuaternion q1 = m_pRigidBody1->quatOrientation;
	AMG3DQuaternion q2 = m_pRigidBody2->quatOrientation;

	q1.toMatrix(&Rot1);
	q2.toMatrix(&Rot2);

	AMG3DVector4 ra = Rot1*localAnchor1;
	AMG3DVector4 rb = Rot2*localAnchor2;

	AMG3DMatrix3x3 I;
	I.identity();
	
	// Compute the bias factor to prevent drift
	AMG3DVector4 dp = m_pRigidBody1->position + ra - m_pRigidBody2->position - rb; // Relative position
	AMG3DScalar C = dp.magnitude() - initialDistance;
	bias = k_biasFactor / dt * C;
	
	dp.normalize();
	n = dp; // n = dp/|dp|
	
	InvMa = I*m_pRigidBody1->invMass; // Ma^-1
	SkewRa = ra.skew(); // [~ra]
	AMG3DMatrix3x3 InvIa = m_pRigidBody1->invIWorld; // Ia^-1
	SkewRaT.transposeOf(SkewRa); // [~ra]T

	InvMb = I*m_pRigidBody2->invMass; // Mb^-1
	SkewRb = rb.skew(); // [~rb]
	AMG3DMatrix3x3 InvIb = m_pRigidBody2->invIWorld; // Ib^-1
	SkewRbT.transposeOf(SkewRb); // [~rb]T

	// a = JM^-1JT
	// a = n.(Ma^-1*n + [~ra]T*Ia^-1*n*[~ra] + Mb^-1*n + [~rb]T*Ib^-1*n*[~rb])
	a = n.dot(InvMa*n + SkewRaT*InvIa*n*SkewRa + InvMb*n + SkewRbT*InvIb*n*SkewRb);
}

void AMG3DDistanceJoint::applyImpulse()
{
	AMG3DVector4 va = m_pRigidBody1->velocity;
	AMG3DVector4 wa = m_pRigidBody1->angularVelocity;
	AMG3DVector4 vb = m_pRigidBody2->velocity;
	AMG3DVector4 wb = m_pRigidBody2->angularVelocity;

	// b = -Jvi - beta/h*C
	AMG3DScalar b = n.reverseOf().dot(va + SkewRaT*wa - vb - SkewRbT*wb) - bias;

	// x = lambda (corrective impulse)
	lambda = a>0 ? b/a : 0;

	P += lambda; // Accumulate impulse for warm starting (To Do: Add warm starting)

	// vfa = via + Ma^-1*n*1*lambda
	// wfa = wia + Ia^-1-n*[~ra]*lambda
	// vfb = vib + Mb^-1*n*-1*lambda
	// wfb = wib + Ib^-1*n*-[~rb]*lambda
	m_pRigidBody1->velocity			+= InvMa*n*lambda;
	m_pRigidBody1->angularVelocity	+= m_pRigidBody1->invIWorld*n*SkewRa*lambda;
	m_pRigidBody2->velocity			-= InvMb*n*lambda;
	m_pRigidBody2->angularVelocity	-= m_pRigidBody2->invIWorld*n*SkewRb*lambda;
}
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Re: Distance constraint

Post by c0der »

Never mind, was a stupid mistake not to use two anchors. Works perfect now with drift correction.
Post Reply