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;