diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp | 482 |
1 files changed, 240 insertions, 242 deletions
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp index 1f4015c7c72..954ffaed759 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp @@ -19,64 +19,60 @@ subject to the following restrictions: //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h //STL/std::vector replaced by btAlignedObjectArray - - #include "btLemkeAlgorithm.h" #undef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM using namespace std; -#endif //BT_DEBUG_OSTREAM +#endif //BT_DEBUG_OSTREAM btScalar btMachEps() { - static bool calculated=false; + static bool calculated = false; static btScalar machEps = btScalar(1.); if (!calculated) { - do { + do + { machEps /= btScalar(2.0); // If next epsilon yields 1, then break, because current // epsilon is the machine epsilon. - } - while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0)); -// printf( "\nCalculated Machine epsilon: %G\n", machEps ); - calculated=true; + } while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0)); + // printf( "\nCalculated Machine epsilon: %G\n", machEps ); + calculated = true; } return machEps; } -btScalar btEpsRoot() { - +btScalar btEpsRoot() +{ static btScalar epsroot = 0.; static bool alreadyCalculated = false; - - if (!alreadyCalculated) { + + if (!alreadyCalculated) + { epsroot = btSqrt(btMachEps()); alreadyCalculated = true; } 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 - if(DEBUGLEVEL >= 1) { - cout << "Dimension = " << dim << endl; - } -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + { + cout << "Dimension = " << dim << endl; + } +#endif //BT_DEBUG_OSTREAM btVectorXu solutionVector(2 * dim); solutionVector.setZero(); - - //, INIT, 0.); + + //, INIT, 0.); btMatrixXu ident(dim, dim); ident.setIdentity(); @@ -85,287 +81,289 @@ btScalar btEpsRoot() { #endif 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, dim, dim - 1, 2 * dim - 1,mNeg); + A.setSubMatrix(0, 0, dim - 1, dim - 1, ident); + 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 + 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 cout << A << std::endl; -#endif //BT_DEBUG_OSTREAM +#endif //BT_DEBUG_OSTREAM + // btVectorXu q_; + // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); - // btVectorXu q_; - // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); - - btAlignedObjectArray<int> basis; - //At first, all w-values are in the basis - for (int i = 0; i < dim; i++) - basis.push_back(i); + btAlignedObjectArray<int> basis; + //At first, all w-values are in the basis + for (int i = 0; i < dim; i++) + basis.push_back(i); int pivotRowIndex = -1; btScalar minValue = 1e30f; bool greaterZero = true; - for (int i=0;i<dim;i++) + for (int i = 0; i < dim; i++) { - btScalar v =A(i,2*dim+1); - if (v<minValue) + btScalar v = A(i, 2 * dim + 1); + if (v < minValue) { - minValue=v; + minValue = v; pivotRowIndex = i; } - if (v<0) + if (v < 0) greaterZero = false; } - - - // 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 pivotColIndex = 2 * dim; // first col is that of z0 + // 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 pivotColIndex = 2 * dim; // first col is that of z0 #ifdef BT_DEBUG_OSTREAM - if (DEBUGLEVEL >= 3) + if (DEBUGLEVEL >= 3) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - cout << "Basis: "; - for (int i = 0; i < basis.size(); i++) - cout << basis[i] << " "; - cout << endl; - } -#endif //BT_DEBUG_OSTREAM + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM if (!greaterZero) { + if (maxloops == 0) + { + 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... + } - if (maxloops == 0) { - 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... - } - - /*start looping*/ - for(steps = 0; steps < maxloops; steps++) { - - GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); -#ifdef BT_DEBUG_OSTREAM - if (DEBUGLEVEL >= 3) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - cout << "Basis: "; - for (int i = 0; i < basis.size(); i++) - cout << basis[i] << " "; - cout << endl; - } -#endif //BT_DEBUG_OSTREAM - - int pivotColIndexOld = pivotColIndex; - - /*find new column index */ - if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value - pivotColIndex = basis[pivotRowIndex] + dim; - else - //else do it the other way round and get in the corresponding w-value - pivotColIndex = basis[pivotRowIndex] - dim; - - /*the column becomes part of the basis*/ - basis[pivotRowIndex] = pivotColIndexOld; - - pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); - - if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary - GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); - basis[pivotRowIndex] = pivotColIndex; //update basis - break; - } - - } + /*start looping*/ + for (steps = 0; steps < maxloops; steps++) + { + GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); #ifdef BT_DEBUG_OSTREAM - if(DEBUGLEVEL >= 1) { - cout << "Number of loops: " << steps << endl; - cout << "Number of maximal loops: " << maxloops << endl; - } -#endif //BT_DEBUG_OSTREAM - - if(!validBasis(basis)) { - info = -1; + if (DEBUGLEVEL >= 3) + { + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM + + int pivotColIndexOld = pivotColIndex; + + /*find new column index */ + if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value + pivotColIndex = basis[pivotRowIndex] + dim; + else + //else do it the other way round and get in the corresponding w-value + pivotColIndex = basis[pivotRowIndex] - dim; + + /*the column becomes part of the basis*/ + basis[pivotRowIndex] = pivotColIndexOld; + + pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); + + if (z0Row == pivotRowIndex) + { //if z0 leaves the basis the solution is found --> one last elimination step is necessary + GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); + basis[pivotRowIndex] = pivotColIndex; //update basis + break; + } + } #ifdef BT_DEBUG_OSTREAM - if(DEBUGLEVEL >= 1) - cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; -#endif //BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + { + cout << "Number of loops: " << steps << endl; + cout << "Number of maximal loops: " << maxloops << endl; + } +#endif //BT_DEBUG_OSTREAM - return solutionVector; - } + if (!validBasis(basis)) + { + info = -1; +#ifdef BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 1) + cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; +#endif //BT_DEBUG_OSTREAM - } + return solutionVector; + } + } #ifdef BT_DEBUG_OSTREAM - if (DEBUGLEVEL >= 2) { - // cout << "A: " << A << endl; - cout << "pivotRowIndex " << pivotRowIndex << endl; - cout << "pivotColIndex " << pivotColIndex << endl; - } -#endif //BT_DEBUG_OSTREAM - - for (int i = 0; i < basis.size(); i++) + if (DEBUGLEVEL >= 2) { - solutionVector[basis[i]] = A(i,2*dim+1);//q_[i]; + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; } +#endif //BT_DEBUG_OSTREAM - info = 0; + for (int i = 0; i < basis.size(); i++) + { + solutionVector[basis[i]] = A(i, 2 * dim + 1); //q_[i]; + } - return solutionVector; - } + info = 0; - int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) { - int RowIndex = 0; - int dim = A.rows(); - btAlignedObjectArray<btVectorXu> Rows; - for (int row = 0; row < dim; row++) - { + return solutionVector; +} - btVectorXu vec(dim + 1); - vec.setZero();//, INIT, 0.) - Rows.push_back(vec); - btScalar a = A(row, pivotColIndex); - if (a > 0) { - Rows[row][0] = A(row, 2 * dim + 1) / a; - Rows[row][1] = A(row, 2 * dim) / a; - for (int j = 2; j < dim + 1; j++) - Rows[row][j] = A(row, j - 1) / a; +int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex) +{ + int RowIndex = 0; + int dim = A.rows(); + btAlignedObjectArray<btVectorXu> Rows; + for (int row = 0; row < dim; row++) + { + btVectorXu vec(dim + 1); + vec.setZero(); //, INIT, 0.) + Rows.push_back(vec); + btScalar a = A(row, pivotColIndex); + if (a > 0) + { + Rows[row][0] = A(row, 2 * dim + 1) / a; + Rows[row][1] = A(row, 2 * dim) / a; + for (int j = 2; j < dim + 1; j++) + Rows[row][j] = A(row, j - 1) / a; #ifdef BT_DEBUG_OSTREAM - // if (DEBUGLEVEL) { - // cout << "Rows(" << row << ") = " << Rows[row] << endl; + // if (DEBUGLEVEL) { + // cout << "Rows(" << row << ") = " << Rows[row] << endl; // } -#endif - } - } - - for (int i = 0; i < Rows.size(); i++) - { - if (Rows[i].nrm2() > 0.) { - - int j = 0; - for (; j < Rows.size(); j++) - { - if(i != j) - { - if(Rows[j].nrm2() > 0.) - { - btVectorXu test(dim + 1); - for (int ii=0;ii<dim+1;ii++) - { - test[ii] = Rows[j][ii] - Rows[i][ii]; - } - - //=Rows[j] - Rows[i] - if (! LexicographicPositive(test)) - break; - } - } - } - - if (j == Rows.size()) - { - RowIndex += i; - break; - } - } - } - - return RowIndex; - } - - bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v) -{ - int i = 0; - // if (DEBUGLEVEL) - // cout << "v " << v << endl; +#endif + } + } - while(i < v.size()-1 && fabs(v[i]) < btMachEps()) - i++; - if (v[i] > 0) - return true; + for (int i = 0; i < Rows.size(); i++) + { + if (Rows[i].nrm2() > 0.) + { + int j = 0; + for (; j < Rows.size(); j++) + { + if (i != j) + { + if (Rows[j].nrm2() > 0.) + { + btVectorXu test(dim + 1); + for (int ii = 0; ii < dim + 1; ii++) + { + test[ii] = Rows[j][ii] - Rows[i][ii]; + } + + //=Rows[j] - Rows[i] + if (!LexicographicPositive(test)) + break; + } + } + } + + if (j == Rows.size()) + { + RowIndex += i; + break; + } + } + } - return false; - } + return RowIndex; +} -void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) +bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v) { + int i = 0; + // if (DEBUGLEVEL) + // cout << "v " << v << endl; + + while (i < v.size() - 1 && fabs(v[i]) < btMachEps()) + i++; + if (v[i] > 0) + return true; + return false; +} + +void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) +{ btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex); #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; #endif - for (int i = 0; i < A.rows(); i++) + for (int i = 0; i < A.rows(); i++) { - if (i != pivotRowIndex) - { - for (int j = 0; j < A.cols(); j++) + if (i != pivotRowIndex) { - if (j != pivotColumnIndex) - { - btScalar v = A(i, j); - v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; - A.setElem(i, j, v); - } + for (int j = 0; j < A.cols(); j++) + { + if (j != pivotColumnIndex) + { + btScalar v = A(i, j); + v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; + A.setElem(i, j, v); + } + } } - } } #ifdef BT_DEBUG_OSTREAM cout << A << std::endl; -#endif //BT_DEBUG_OSTREAM - for (int i = 0; i < A.cols(); i++) +#endif //BT_DEBUG_OSTREAM + for (int i = 0; i < A.cols(); i++) { - A.mulElem(pivotRowIndex, i,-a); - } + A.mulElem(pivotRowIndex, i, -a); + } #ifdef BT_DEBUG_OSTREAM 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) - { - A.setElem(i, pivotColumnIndex,0); - } + if (i != pivotRowIndex) + { + A.setElem(i, pivotColumnIndex, 0); + } } #ifdef BT_DEBUG_OSTREAM 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; - for (int i = 0; i < vector.size(); i++) { - if (vector[i] < 0) { - isGreater = false; - break; - } - } - - return isGreater; - } - - bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) - { - bool isValid = true; - for (int i = 0; i < basis.size(); i++) { - if (basis[i] >= basis.size() * 2) { //then z0 is in the base - isValid = false; - break; - } - } - - return isValid; - } + bool isGreater = true; + for (int i = 0; i < vector.size(); i++) + { + if (vector[i] < 0) + { + isGreater = false; + break; + } + } + return isGreater; +} +bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) +{ + bool isValid = true; + for (int i = 0; i < basis.size(); i++) + { + if (basis[i] >= basis.size() * 2) + { //then z0 is in the base + isValid = false; + break; + } + } + + return isValid; +} |