Unstable distance constraints

[korzen303]

Hi All,

I need a simple distance constraint to connect particles in a chain. So far, I am emulating it using Point2PointConstraint, which works very well, but my initial measures show that DistanceJoint would be much faster. I don't need pivots etc. Just spheres connected by theirs centers of mass.

I implemented it in JBullet (2.72) to try it out before moving to 2.82 C/C++ version. The code works fine for very small timestep = 0.001, it is noticeably vibrating for dt=0.002, and my model explodes using any larger timestep. The emulated version goes nicely up to 0.01, so there is clearly something wrong. I would really appreciate your help. There are few topics on Distance Constraint on this board but no working source code given.

EDIT: It turned out that the code is working fine. I will leave it here for the future reference. The instabilities were caused by a too rigid spring connecting the user-input proxy vector and the chain. Decreasing the stiffness solved the problem. The question is why it was stable using the Point2PointConstraint? The source code below was almost directly derived from p2p version.

Code: Select all

@Override
public void solveConstraint(float dt) {

	float desiredDistance = 1.0f;
	Vector3f vel = Stack.alloc(Vector3f.class);
	Vector3f normal = Stack.alloc(Vector3f.class);
	
	Vector3f posA = rbA.getCenterOfMassPosition(Stack.alloc(Vector3f.class));
	Vector3f posB = rbB.getCenterOfMassPosition(Stack.alloc(Vector3f.class));
	
	normal.sub(posA, posB);
	
	float actualDistance = normal.length();
	float depth = desiredDistance - actualDistance;
	normal.normalize();
	
	Vector3f velA = rbA.getLinearVelocity(Stack.alloc(Vector3f.class));
	Vector3f velB = rbB.getLinearVelocity(Stack.alloc(Vector3f.class));
	vel.sub(velA, velB);
	
	//tau = 0.3f, damping = 1.0f, bodies invMass = 1.0f
	float relVel = normal.dot(vel);
	float k = 1.0f / (rbA.getInvMass() + rbB.getInvMass());

	float impulse = (depth * setting.tau / dt * k - setting.damping * relVel * k);

	appliedImpulse += impulse;
	
	Vector3f impulseVec = Stack.alloc(Vector3f.class);
	impulseVec.scale(impulse, normal);
	rbA.applyCentralImpulse(impulseVec);
	impulseVec.negate();
	rbB.applyCentralImpulse(impulseVec);

}

[korzen303]

Could anyone please help me to implement the above distance constraint in Bullet 2.82 in C++? The Point2PointConstraint.cpp looks nothing like the 2.72 Java version:

Code: Select all

void	btPoint2PointConstraint::buildJacobian()
{

	///we need it for both methods
	{
		m_appliedImpulse = btScalar(0.);

		btVector3	normal(0,0,0);

		for (int i=0;i<3;i++)
		{
			normal[i] = 1;
			new (&m_jac[i]) btJacobianEntry(
			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
			normal,
			m_rbA.getInvInertiaDiagLocal(),
			m_rbA.getInvMass(),
			m_rbB.getInvInertiaDiagLocal(),
			m_rbB.getInvMass());
		normal[i] = 0;
		}
	}


}

Code: Select all

void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
{
	btAssert(!m_useSolveConstraintObsolete);

	 //retrieve matrices

	// anchor points in global coordinates with respect to body PORs.
   
    // set jacobian
    info->m_J1linearAxis[0] = 1;
	info->m_J1linearAxis[info->rowskip+1] = 1;
	info->m_J1linearAxis[2*info->rowskip+2] = 1;

	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
	{
		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
		btVector3 a1neg = -a1;
		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
    
	info->m_J2linearAxis[0] = -1;
    info->m_J2linearAxis[info->rowskip+1] = -1;
    info->m_J2linearAxis[2*info->rowskip+2] = -1;
	
	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
   
	{
	//	btVector3 a2n = -a2;
		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
    


    // set right hand side
	btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
    btScalar k = info->fps * currERP;
    int j;
	for (j=0; j<3; j++)
    {
        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
    }
	if(m_flags & BT_P2P_FLAGS_CFM)
	{
		for (j=0; j<3; j++)
		{
			info->cfm[j*info->rowskip] = m_cfm;
		}
	}

	btScalar impulseClamp = m_setting.m_impulseClamp;//
	for (j=0; j<3; j++)
    {
		if (m_setting.m_impulseClamp > 0)
		{
			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
			info->m_upperLimit[j*info->rowskip] = impulseClamp;
		}
	}
	info->m_damping = m_setting.m_damping;
	
}