24#undef BT_DEBUG_OSTREAM
25#ifdef BT_DEBUG_OSTREAM
65#ifdef BT_DEBUG_OSTREAM
68 cout <<
"Dimension = " << dim <<
endl;
79#ifdef BT_DEBUG_OSTREAM
87 A.setSubMatrix(0, 0, dim - 1, dim - 1,
ident);
88 A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,
mNeg);
89 A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
90 A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,
m_q);
92#ifdef BT_DEBUG_OSTREAM
93 cout <<
A << std::endl;
101 for (
int i = 0; i < dim; i++)
107 for (
int i = 0; i < dim; i++)
123#ifdef BT_DEBUG_OSTREAM
130 for (
int i = 0; i <
basis.size(); i++)
148#ifdef BT_DEBUG_OSTREAM
155 for (
int i = 0; i <
basis.size(); i++)
182#ifdef BT_DEBUG_OSTREAM
193#ifdef BT_DEBUG_OSTREAM
195 cerr <<
"Lemke-Algorithm ended with Ray-Termination (no valid solution)." <<
endl;
201#ifdef BT_DEBUG_OSTREAM
210 for (
int i = 0; i <
basis.size(); i++)
235 for (
int j = 2;
j < dim + 1;
j++)
238#ifdef BT_DEBUG_OSTREAM
246 for (
int i = 0; i <
Rows.size(); i++)
248 if (
Rows[i].nrm2() > 0.)
251 for (;
j <
Rows.size();
j++)
255 if (
Rows[
j].nrm2() > 0.)
258 for (
int ii = 0;
ii < dim + 1;
ii++)
270 if (
j ==
Rows.size())
298#ifdef BT_DEBUG_OSTREAM
299 cout <<
A << std::endl;
302 for (
int i = 0; i <
A.rows(); i++)
306 for (
int j = 0;
j <
A.cols();
j++)
318#ifdef BT_DEBUG_OSTREAM
319 cout <<
A << std::endl;
321 for (
int i = 0; i <
A.cols(); i++)
325#ifdef BT_DEBUG_OSTREAM
326 cout <<
A << std::endl;
329 for (
int i = 0; i <
A.rows(); i++)
336#ifdef BT_DEBUG_OSTREAM
337 cout <<
A << std::endl;
344 for (
int i = 0; i <
vector.size(); i++)
359 for (
int i = 0; i <
basis.size(); i++)
const T & btMax(const T &a, const T &b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSqrt(btScalar y)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
void push_back(const T &_Val)
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex)
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemkeās Algorithm for Rigid Body Contact Simula...
bool validBasis(const btAlignedObjectArray< int > &basis)
int info
did the algorithm find a solution
bool greaterZero(const btVectorXu &vector)
int DEBUGLEVEL
define level of debug output
bool LexicographicPositive(const btVectorXu &v)
unsigned int steps
number of steps until the Lemke algorithm found a solution
void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray< int > &basis)