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++)
185#ifdef BT_DEBUG_OSTREAM
196#ifdef BT_DEBUG_OSTREAM
198 cerr <<
"Lemke-Algorithm ended with Ray-Termination (no valid solution)." <<
endl;
204#ifdef BT_DEBUG_OSTREAM
213 for (
int i = 0; i <
basis.size(); i++)
322#ifdef BT_DEBUG_OSTREAM
323 cout <<
A << std::endl;
326 for (
int i = 0; i <
A.rows(); i++)
330 for (
int j = 0;
j <
A.cols();
j++)
342#ifdef BT_DEBUG_OSTREAM
343 cout <<
A << std::endl;
345 for (
int i = 0; i <
A.cols(); i++)
349#ifdef BT_DEBUG_OSTREAM
350 cout <<
A << std::endl;
353 for (
int i = 0; i <
A.rows(); i++)
360#ifdef BT_DEBUG_OSTREAM
361 cout <<
A << std::endl;
368 for (
int i = 0; i <
vector.size(); i++)
383 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)
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
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex, const int &z0Row, bool &isRayTermination)
bool greaterZero(const btVectorXu &vector)
int DEBUGLEVEL
define level of debug output
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)