Assert Failure When Constraining MultiBody to Kinematic Body

[VladSmarandache]

In btMultiBodyFixedConstraint for example (but also in other multibody constraints), if A is a multibody and B is a kinematic body, m_rigidBodyB->getCompanionId() returns -2 here:

if (m_rigidBodyB)
{
	constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
	pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
	frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
}

Once fillMultiBodyConstraint is called with an invalid m_solverBodyIdB, it fails at btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);.

If I pass a pointer to the solver into the constraint and replace the straight call to m_rigidBodyB->getCompanionId() with something like solver->getOrInitSolverBody(*m_rigidBodyB, infoGlobal.m_timeStep), I get the correct behavior. With BT_THREADSAFE on, are kinematic bodies even supposed to have a companionId? Judging by the following block in getOrInitSolverBody, it doesn't look like it:

else if (isRigidBodyType && isKinematic)
{
	//
	// NOTE: must test for kinematic before static because some kinematic objects also
	//   identify as "static"
	//
	// Kinematic bodies can be in multiple islands at once, so it is a
	// race condition to write to them, so we use an alternate method
	// to record the solverBodyId
	int uniqueId = body.getWorldArrayIndex();
	const int INVALID_SOLVER_BODY_ID = -1;
	if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
	{
		m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
	}
	solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
	// if no table entry yet,
	if (solverBodyId == INVALID_SOLVER_BODY_ID)
	{
		// create a table entry for this body
		solverBodyId = m_tmpSolverBodyPool.size();
		btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
		initSolverBody(&solverBody, &body, timeStep);
		m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
	}
}