Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp

Show All 13 Lines
*/ */
//The original version is here //The original version is here
//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc //https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
//This file is re-distributed under the ZLib license, with permission of the original author //This file is re-distributed under the ZLib license, with permission of the original author
//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
//STL/std::vector replaced by btAlignedObjectArray //STL/std::vector replaced by btAlignedObjectArray
#include "btLemkeAlgorithm.h" #include "btLemkeAlgorithm.h"
#undef BT_DEBUG_OSTREAM #undef BT_DEBUG_OSTREAM
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
using namespace std; using namespace std;
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
btScalar btMachEps() btScalar btMachEps()
{ {
static bool calculated=false; static bool calculated = false;
static btScalar machEps = btScalar(1.); static btScalar machEps = btScalar(1.);
if (!calculated) if (!calculated)
{ {
do { do
{
machEps /= btScalar(2.0); machEps /= btScalar(2.0);
// If next epsilon yields 1, then break, because current // If next epsilon yields 1, then break, because current
// epsilon is the machine epsilon. // epsilon is the machine epsilon.
} } while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0));
// printf( "\nCalculated Machine epsilon: %G\n", machEps ); // printf( "\nCalculated Machine epsilon: %G\n", machEps );
calculated=true; calculated = true;
} }
return machEps; return machEps;
} }
btScalar btEpsRoot() { btScalar btEpsRoot()
{
static btScalar epsroot = 0.; static btScalar epsroot = 0.;
static bool alreadyCalculated = false; static bool alreadyCalculated = false;
if (!alreadyCalculated) { if (!alreadyCalculated)
{
epsroot = btSqrt(btMachEps()); epsroot = btSqrt(btMachEps());
alreadyCalculated = true; alreadyCalculated = true;
} }
return epsroot; return epsroot;
} }
btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/) btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
{ {
steps = 0; steps = 0;
int dim = m_q.size(); int dim = m_q.size();
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) { if (DEBUGLEVEL >= 1)
{
cout << "Dimension = " << dim << endl; cout << "Dimension = " << dim << endl;
} }
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
btVectorXu solutionVector(2 * dim); btVectorXu solutionVector(2 * dim);
solutionVector.setZero(); solutionVector.setZero();
//, INIT, 0.); //, INIT, 0.);
btMatrixXu ident(dim, dim); btMatrixXu ident(dim, dim);
ident.setIdentity(); ident.setIdentity();
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
cout << m_M << std::endl; cout << m_M << std::endl;
#endif #endif
btMatrixXu mNeg = m_M.negative(); btMatrixXu mNeg = m_M.negative();
btMatrixXu A(dim, 2 * dim + 2); btMatrixXu A(dim, 2 * dim + 2);
// //
A.setSubMatrix(0, 0, dim - 1, dim - 1,ident); A.setSubMatrix(0, 0, dim - 1, dim - 1, ident);
A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg); A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1, mNeg);
A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f); A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q); A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1, m_q);
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
cout << A << std::endl; cout << A << std::endl;
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
// btVectorXu q_; // btVectorXu q_;
// q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
btAlignedObjectArray<int> basis; btAlignedObjectArray<int> basis;
//At first, all w-values are in the basis //At first, all w-values are in the basis
for (int i = 0; i < dim; i++) for (int i = 0; i < dim; i++)
basis.push_back(i); basis.push_back(i);
int pivotRowIndex = -1; int pivotRowIndex = -1;
btScalar minValue = 1e30f; btScalar minValue = 1e30f;
bool greaterZero = true; bool greaterZero = true;
for (int i=0;i<dim;i++) for (int i = 0; i < dim; i++)
{ {
btScalar v =A(i,2*dim+1); btScalar v = A(i, 2 * dim + 1);
if (v<minValue) if (v < minValue)
{ {
minValue=v; minValue = v;
pivotRowIndex = i; pivotRowIndex = i;
} }
if (v<0) if (v < 0)
greaterZero = false; greaterZero = false;
} }
// int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value // int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value
int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards
int pivotColIndex = 2 * dim; // first col is that of z0 int pivotColIndex = 2 * dim; // first col is that of z0
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 3) if (DEBUGLEVEL >= 3)
{ {
// cout << "A: " << A << endl; // cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl; cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl; cout << "pivotColIndex " << pivotColIndex << endl;
cout << "Basis: "; cout << "Basis: ";
for (int i = 0; i < basis.size(); i++) for (int i = 0; i < basis.size(); i++)
cout << basis[i] << " "; cout << basis[i] << " ";
cout << endl; cout << endl;
} }
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
if (!greaterZero) if (!greaterZero)
{ {
if (maxloops == 0)
if (maxloops == 0) { {
maxloops = 100; maxloops = 100;
// maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now... // maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
} }
/*start looping*/ /*start looping*/
for(steps = 0; steps < maxloops; steps++) { for (steps = 0; steps < maxloops; steps++)
{
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 3) { if (DEBUGLEVEL >= 3)
{
// cout << "A: " << A << endl; // cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl; cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl; cout << "pivotColIndex " << pivotColIndex << endl;
cout << "Basis: "; cout << "Basis: ";
for (int i = 0; i < basis.size(); i++) for (int i = 0; i < basis.size(); i++)
cout << basis[i] << " "; cout << basis[i] << " ";
cout << endl; cout << endl;
} }
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
int pivotColIndexOld = pivotColIndex; int pivotColIndexOld = pivotColIndex;
/*find new column index */ /*find new column index */
if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
pivotColIndex = basis[pivotRowIndex] + dim; pivotColIndex = basis[pivotRowIndex] + dim;
else else
//else do it the other way round and get in the corresponding w-value //else do it the other way round and get in the corresponding w-value
pivotColIndex = basis[pivotRowIndex] - dim; pivotColIndex = basis[pivotRowIndex] - dim;
/*the column becomes part of the basis*/ /*the column becomes part of the basis*/
basis[pivotRowIndex] = pivotColIndexOld; basis[pivotRowIndex] = pivotColIndexOld;
pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary if (z0Row == pivotRowIndex)
{ //if z0 leaves the basis the solution is found --> one last elimination step is necessary
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
basis[pivotRowIndex] = pivotColIndex; //update basis basis[pivotRowIndex] = pivotColIndex; //update basis
break; break;
} }
} }
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) { if (DEBUGLEVEL >= 1)
{
cout << "Number of loops: " << steps << endl; cout << "Number of loops: " << steps << endl;
cout << "Number of maximal loops: " << maxloops << endl; cout << "Number of maximal loops: " << maxloops << endl;
} }
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
if(!validBasis(basis)) { if (!validBasis(basis))
{
info = -1; info = -1;
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) if (DEBUGLEVEL >= 1)
cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
return solutionVector; return solutionVector;
} }
} }
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 2) { if (DEBUGLEVEL >= 2)
{
// cout << "A: " << A << endl; // cout << "A: " << A << endl;
cout << "pivotRowIndex " << pivotRowIndex << endl; cout << "pivotRowIndex " << pivotRowIndex << endl;
cout << "pivotColIndex " << pivotColIndex << endl; cout << "pivotColIndex " << pivotColIndex << endl;
} }
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
for (int i = 0; i < basis.size(); i++) for (int i = 0; i < basis.size(); i++)
{ {
solutionVector[basis[i]] = A(i,2*dim+1);//q_[i]; solutionVector[basis[i]] = A(i, 2 * dim + 1); //q_[i];
} }
info = 0; info = 0;
return solutionVector; return solutionVector;
} }
int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) { int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex)
{
int RowIndex = 0; int RowIndex = 0;
int dim = A.rows(); int dim = A.rows();
btAlignedObjectArray<btVectorXu> Rows; btAlignedObjectArray<btVectorXu> Rows;
for (int row = 0; row < dim; row++) for (int row = 0; row < dim; row++)
{ {
btVectorXu vec(dim + 1); btVectorXu vec(dim + 1);
vec.setZero();//, INIT, 0.) vec.setZero(); //, INIT, 0.)
Rows.push_back(vec); Rows.push_back(vec);
btScalar a = A(row, pivotColIndex); btScalar a = A(row, pivotColIndex);
if (a > 0) { if (a > 0)
{
Rows[row][0] = A(row, 2 * dim + 1) / a; Rows[row][0] = A(row, 2 * dim + 1) / a;
Rows[row][1] = A(row, 2 * dim) / a; Rows[row][1] = A(row, 2 * dim) / a;
for (int j = 2; j < dim + 1; j++) for (int j = 2; j < dim + 1; j++)
Rows[row][j] = A(row, j - 1) / a; Rows[row][j] = A(row, j - 1) / a;
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
// if (DEBUGLEVEL) { // if (DEBUGLEVEL) {
// cout << "Rows(" << row << ") = " << Rows[row] << endl; // cout << "Rows(" << row << ") = " << Rows[row] << endl;
// } // }
#endif #endif
} }
} }
for (int i = 0; i < Rows.size(); i++) for (int i = 0; i < Rows.size(); i++)
{ {
if (Rows[i].nrm2() > 0.) { if (Rows[i].nrm2() > 0.)
{
int j = 0; int j = 0;
for (; j < Rows.size(); j++) for (; j < Rows.size(); j++)
{ {
if(i != j) if (i != j)
{ {
if(Rows[j].nrm2() > 0.) if (Rows[j].nrm2() > 0.)
{ {
btVectorXu test(dim + 1); btVectorXu test(dim + 1);
for (int ii=0;ii<dim+1;ii++) for (int ii = 0; ii < dim + 1; ii++)
{ {
test[ii] = Rows[j][ii] - Rows[i][ii]; test[ii] = Rows[j][ii] - Rows[i][ii];
} }
//=Rows[j] - Rows[i] //=Rows[j] - Rows[i]
if (! LexicographicPositive(test)) if (!LexicographicPositive(test))
break; break;
} }
} }
} }
if (j == Rows.size()) if (j == Rows.size())
{ {
RowIndex += i; RowIndex += i;
break; break;
} }
} }
} }
return RowIndex; return RowIndex;
} }
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v) bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
{ {
int i = 0; int i = 0;
// if (DEBUGLEVEL) // if (DEBUGLEVEL)
// cout << "v " << v << endl; // cout << "v " << v << endl;
while(i < v.size()-1 && fabs(v[i]) < btMachEps()) while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
i++; i++;
if (v[i] > 0) if (v[i] > 0)
return true; return true;
return false; return false;
} }
void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
{ {
btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex); btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
cout << A << std::endl; cout << A << std::endl;
#endif #endif
for (int i = 0; i < A.rows(); i++) for (int i = 0; i < A.rows(); i++)
{ {
if (i != pivotRowIndex) if (i != pivotRowIndex)
{ {
for (int j = 0; j < A.cols(); j++) for (int j = 0; j < A.cols(); j++)
{ {
if (j != pivotColumnIndex) if (j != pivotColumnIndex)
{ {
btScalar v = A(i, j); btScalar v = A(i, j);
v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
A.setElem(i, j, v); A.setElem(i, j, v);
} }
} }
} }
} }
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
cout << A << std::endl; cout << A << std::endl;
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
for (int i = 0; i < A.cols(); i++) for (int i = 0; i < A.cols(); i++)
{ {
A.mulElem(pivotRowIndex, i,-a); A.mulElem(pivotRowIndex, i, -a);
} }
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
cout << A << std::endl; cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM #endif //#ifdef BT_DEBUG_OSTREAM
for (int i = 0; i < A.rows(); i++) for (int i = 0; i < A.rows(); i++)
{ {
if (i != pivotRowIndex) if (i != pivotRowIndex)
{ {
A.setElem(i, pivotColumnIndex,0); A.setElem(i, pivotColumnIndex, 0);
} }
} }
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
cout << A << std::endl; cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM #endif //#ifdef BT_DEBUG_OSTREAM
} }
bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector) bool btLemkeAlgorithm::greaterZero(const btVectorXu& vector)
{ {
bool isGreater = true; bool isGreater = true;
for (int i = 0; i < vector.size(); i++) { for (int i = 0; i < vector.size(); i++)
if (vector[i] < 0) { {
if (vector[i] < 0)
{
isGreater = false; isGreater = false;
break; break;
} }
} }
return isGreater; return isGreater;
} }
bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
{ {
bool isValid = true; bool isValid = true;
for (int i = 0; i < basis.size(); i++) { for (int i = 0; i < basis.size(); i++)
if (basis[i] >= basis.size() * 2) { //then z0 is in the base {
if (basis[i] >= basis.size() * 2)
{ //then z0 is in the base
isValid = false; isValid = false;
break; break;
} }
} }
return isValid; return isValid;
} }