virtual bool solveMLCP(btMatrixXu & A, btVectorXu & b, btVectorXu& x, btVectorXu & lo,btVectorXu & hi,btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
{
if (!A.rows())
return true;
//the A matrix is sparse, so compute the non-zero elements
A.rowComputeNonZeroElements();
//A is a m-n matrix, m rows, n columns
Debug.Assert(A.rows() == b.rows());
int i, j, numRows = A.rows();
float delta;
for (int k = 0; k <numIterations; k++)
{
for (i = 0; i <numRows; i++)
{
delta = 0.0f;
if (useSparsity)
{
for (int h=0;h<A.m_rowNonZeroElements1[i].Count;h++)
{
int j = A.m_rowNonZeroElements1[i][h];
if (j != i)//skip main diagonal
{
delta += A(i,j) * x[j];
}
}
} else
{
for (j = 0; j <i; j++)
delta += A(i,j) * x[j];
for (j = i+1; j<numRows; j++)
delta += A(i,j) * x[j];
}
float aDiag = A(i,i);
x [i] = (b [i] - delta) / aDiag;
float s = 1;
if (limitDependency[i]>=0)
{
s = x[limitDependency[i]];
if (s<0)
s=1;
}
if (x[i]<lo[i]*s)
x[i]=lo[i]*s;
if (x[i]>hi[i]*s)
x[i]=hi[i]*s;
}
}
return true;
}
virtual bool solveMLCP(btMatrixXu & A, btVectorXu & b, btVectorXu& x, btVectorXu & lo,btVectorXu & hi,btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;