Example how to create the A matrix and b vector and solve the LCP using Projected Gauss Seidel for Bullet 2.x. Requires Eigen lib and Bullet 2.82. Note that one of the differences between plain projected Gauss Seidel and the Bullet iterative solver is that the friction limits are computed at each iterations, from the related normal force. (This …

gistfile1.txt

//#include "Eigen/Dense"
#include "Eigen/Sparse"

static Eigen::VectorXf Solve_GaussSeidel (Eigen::MatrixXf & A, Eigen::VectorXf & b, Eigen::VectorXf & lo,Eigen::VectorXf & hi,int kMax = 5)
{
  //A is a m-n matrix, m rows, n columns
  if (A.rows() != b.rows()) 
    return b;

  int i, j, n = A.rows();

  Eigen::VectorXf x(b.rows());
  x.setZero();

  float delta;

  // Gauss-Seidel Solver
  for (int k = 0; k <kMax; k++)
  {
    for (i = 0; i <n; i++)
    {
      delta = 0.0f;

      for (j = 0; j <i; j++) 
		  delta += A(i,j) * x[j];
      for (j = i+1; j<n; j++) 
		  delta += A(i,j) * x[j];

	  float aDiag = A(i,i);
      x [i] = (b [i] - delta) / A(i,i);
	  if (x[i]<lo[i])
		  x[i]=lo[i];
	  if (x[i]>hi[i])
		  x[i]=hi[i];
    }
  }

  return x;
}



btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numOrgBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
	bool useGaussSeidel = true;
	if (useGaussSeidel)
	{
		//convert constraints to A,b form
		//int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
		//int numJointConstraints = + m_tmpSolverNonContactConstraintPool.size();
		
		btConstraintArray allConstraintArray;
		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
			allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);

		for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
			allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
	
		int numBodies = m_tmpSolverBodyPool.size();
		int allConstraints = allConstraintArray.size();


		Eigen::VectorXf x(allConstraints );
		Eigen::VectorXf b(allConstraints );
		x.setZero();
		b.setZero();


		for (int i=0;i<allConstraints ;i++)
			b[i]=allConstraintArray[i].m_rhs;

		Eigen::MatrixXf Minv;
		Minv.resize(6*numBodies,6*numBodies);
		Minv.setZero();

		for (int i=0;i<numBodies;i++)
		{
			const btSolverBody& rb = m_tmpSolverBodyPool[i];
			const btVector3& invMass = rb.m_invMass;
			Minv(i*6+0,i*6+0) = invMass[0];
			Minv(i*6+1,i*6+1) = invMass[1];
			Minv(i*6+2,i*6+2) = invMass[2];
			btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;

			for (int r=0;r<3;r++)
				for (int c=0;c<3;c++)
					Minv(i*6+3+r,i*6+3+c)=orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0;
		}

		Eigen::MatrixXf J;
		J.resize(allConstraints,6*numBodies);
		J.setZero();

		Eigen::VectorXf lo,hi;
		lo.resize(allConstraints);
		hi.resize(allConstraints);

		for (int i=0;i<allConstraints;i++)
		{
			lo[i] = allConstraintArray[i].m_lowerLimit;
			hi[i] = allConstraintArray[i].m_upperLimit;

			int bodyIndex0 = allConstraintArray[i].m_solverBodyIdA;
			int bodyIndex1 = allConstraintArray[i].m_solverBodyIdB;
			if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
			{
				J(i,6*bodyIndex0+0) = allConstraintArray[i].m_contactNormal1[0];
				J(i,6*bodyIndex0+1) = allConstraintArray[i].m_contactNormal1[1];
				J(i,6*bodyIndex0+2) = allConstraintArray[i].m_contactNormal1[2];
				J(i,6*bodyIndex0+3) = allConstraintArray[i].m_relpos1CrossNormal[0];
				J(i,6*bodyIndex0+4) = allConstraintArray[i].m_relpos1CrossNormal[1];
				J(i,6*bodyIndex0+5) = allConstraintArray[i].m_relpos1CrossNormal[2];
			}
			if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
			{
				J(i,6*bodyIndex1+0) = allConstraintArray[i].m_contactNormal2[0];
				J(i,6*bodyIndex1+1) = allConstraintArray[i].m_contactNormal2[1];
				J(i,6*bodyIndex1+2) = allConstraintArray[i].m_contactNormal2[2];
				J(i,6*bodyIndex1+3) = allConstraintArray[i].m_relpos2CrossNormal[0];
				J(i,6*bodyIndex1+4) = allConstraintArray[i].m_relpos2CrossNormal[1];
				J(i,6*bodyIndex1+5) = allConstraintArray[i].m_relpos2CrossNormal[2];
			}
		}

		Eigen::MatrixXf J_transpose = J.transpose();
		Eigen::MatrixXf A;

		A = J*Minv*J_transpose;

		x = Solve_GaussSeidel(A,b,lo,hi,15);
	
		for (int i=0;i<allConstraintArray.size();i++)
		{
			{
				btSolverConstraint& c = allConstraintArray[i];

//				printf("x[%d]=%f\n",i,x[i]);
				int sbA = c.m_solverBodyIdA;
				int sbB = c.m_solverBodyIdB;
				btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;

				m_tmpSolverBodyPool[sbA].internalApplyImpulse(c.m_contactNormal1,c.m_angularComponentA,x[i]);
				m_tmpSolverBodyPool[sbB].internalApplyImpulse(c.m_contactNormal2,c.m_angularComponentB,x[i]);
				c.m_appliedImpulse = 0;//x[i];

			}

		}

	} else
	{
		int numBodies = numOrgBodies;
		BT_PROFILE("solveGroupCacheFriendlyIterations");

		{
			///this is a special step to resolve penetrations (just for contacts)
			solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);

			int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;

			for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
			//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
			{			
				solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
			}
		
		}
	}
	return 0.f;
}