diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/MLCPSolvers')
7 files changed, 926 insertions, 86 deletions
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 3bf7b5c1311..986f2148701 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -821,14 +821,15 @@ void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1) /* declare variables - Z matrix, p and q vectors, etc */ btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; const btScalar *ell; - int lskip2,lskip3,i,j; + int lskip2,i,j; +// int lskip3; /* special handling for L and B because we're solving L1 *transpose* */ L = L + (n-1)*(lskip1+1); B = B + n-1; lskip1 = -lskip1; /* compute lskip values */ lskip2 = 2*lskip1; - lskip3 = 3*lskip1; + //lskip3 = 3*lskip1; /* compute all 4 x 1 blocks of X */ for (i=0; i <= n-4; i+=4) { /* compute all 4 x 1 block of X, from rows i..i+4-1 */ @@ -1199,9 +1200,9 @@ struct btLCP int *const m_findex, *const m_p, *const m_C; btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, - btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d, + btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, btScalar *_Dell, btScalar *_ell, btScalar *_tmp, - bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows); + bool *_state, int *_findex, int *p, int *c, btScalar **Arows); int getNub() const { return m_nub; } void transfer_i_to_C (int i); void transfer_i_to_N (int i) { m_nN++; } // because we can assume C and N span 1:i-1 @@ -1224,9 +1225,9 @@ struct btLCP btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, - btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d, + btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, btScalar *_Dell, btScalar *_ell, btScalar *_tmp, - bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows): + bool *_state, int *_findex, int *p, int *c, btScalar **Arows): m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0), # ifdef BTROWPTRS m_A(Arows), @@ -1234,8 +1235,8 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc m_A(_Adata), #endif m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi), - m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp), - m_state(_state), m_findex(_findex), m_p(_p), m_C(_C) + m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp), + m_state(_state), m_findex(_findex), m_p(p), m_C(c) { { btSetZero (m_x,m_n); diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp new file mode 100644 index 00000000000..1f4015c7c72 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp @@ -0,0 +1,371 @@ +/* Copyright (C) 2004-2013 MBSim Development Team + +Code was converted for the Bullet Continuous Collision Detection and Physics Library + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +//The original version is here +//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 +//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 + +btScalar btMachEps() +{ + static bool calculated=false; + static btScalar machEps = btScalar(1.); + if (!calculated) + { + 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; + } + return machEps; +} + +btScalar btEpsRoot() { + + static btScalar epsroot = 0.; + static bool alreadyCalculated = false; + + if (!alreadyCalculated) { + epsroot = btSqrt(btMachEps()); + alreadyCalculated = true; + } + return epsroot; +} + + + + btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/) +{ + + + steps = 0; + + int dim = m_q.size(); +#ifdef BT_DEBUG_OSTREAM + if(DEBUGLEVEL >= 1) { + cout << "Dimension = " << dim << endl; + } +#endif //BT_DEBUG_OSTREAM + + btVectorXu solutionVector(2 * dim); + solutionVector.setZero(); + + //, INIT, 0.); + + btMatrixXu ident(dim, dim); + ident.setIdentity(); +#ifdef BT_DEBUG_OSTREAM + cout << m_M << std::endl; +#endif + + btMatrixXu mNeg = m_M.negative(); + + 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, 2 * dim, dim - 1, 2 * dim, -1.f); + 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 + + + // 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); + + int pivotRowIndex = -1; + btScalar minValue = 1e30f; + bool greaterZero = true; + for (int i=0;i<dim;i++) + { + btScalar v =A(i,2*dim+1); + if (v<minValue) + { + minValue=v; + pivotRowIndex = i; + } + 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 + +#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 + + 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... + } + + /*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; + } + + } +#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; +#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++) + { + solutionVector[basis[i]] = A(i,2*dim+1);//q_[i]; + } + + info = 0; + + return solutionVector; + } + + 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; + // } +#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; + + 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++) + { + if (i != pivotRowIndex) + { + 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++) + { + A.mulElem(pivotRowIndex, i,-a); + } +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif //#ifdef BT_DEBUG_OSTREAM + + for (int i = 0; i < A.rows(); i++) + { + if (i != pivotRowIndex) + { + A.setElem(i, pivotColumnIndex,0); + } + } +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif //#ifdef BT_DEBUG_OSTREAM + } + + 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; + } + + diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h new file mode 100644 index 00000000000..7555cd9d207 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h @@ -0,0 +1,108 @@ +/* Copyright (C) 2004-2013 MBSim Development Team + +Code was converted for the Bullet Continuous Collision Detection and Physics Library + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +//The original version is here +//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 (Kilian Grundl) +//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h +//STL/std::vector replaced by btAlignedObjectArray + + + +#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_ +#define BT_NUMERICS_LEMKE_ALGORITHM_H_ + +#include "LinearMath/btMatrixX.h" + + +#include <vector> //todo: replace by btAlignedObjectArray + +class btLemkeAlgorithm +{ +public: + + + btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) : + DEBUGLEVEL(DEBUGLEVEL_) + { + setSystem(M_, q_); + } + + /* GETTER / SETTER */ + /** + * \brief return info of solution process + */ + int getInfo() { + return info; + } + + /** + * \brief get the number of steps until the solution was found + */ + int getSteps(void) { + return steps; + } + + + + /** + * \brief set system with Matrix M and vector q + */ + void setSystem(const btMatrixXu & M_, const btVectorXu & q_) + { + m_M = M_; + m_q = q_; + } + /***************************************************/ + + /** + * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) + */ + btVectorXu solve(unsigned int maxloops = 0); + + virtual ~btLemkeAlgorithm() { + } + +protected: + int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex); + bool LexicographicPositive(const btVectorXu & v); + void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis); + bool greaterZero(const btVectorXu & vector); + bool validBasis(const btAlignedObjectArray<int>& basis); + + btMatrixXu m_M; + btVectorXu m_q; + + /** + * \brief number of steps until the Lemke algorithm found a solution + */ + unsigned int steps; + + /** + * \brief define level of debug output + */ + int DEBUGLEVEL; + + /** + * \brief did the algorithm find a solution + * + * -1 : not successful + * 0 : successful + */ + int info; +}; + + +#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */ diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h new file mode 100644 index 00000000000..98484c37964 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h @@ -0,0 +1,350 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_LEMKE_SOLVER_H +#define BT_LEMKE_SOLVER_H + + +#include "btMLCPSolverInterface.h" +#include "btLemkeAlgorithm.h" + + + + +///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " +///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time. +///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team +class btLemkeSolver : public btMLCPSolverInterface +{ +protected: + +public: + + btScalar m_maxValue; + int m_debugLevel; + int m_maxLoops; + bool m_useLoHighBounds; + + + + btLemkeSolver() + :m_maxValue(100000), + m_debugLevel(0), + m_maxLoops(1000), + m_useLoHighBounds(true) + { + } + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) + { + + if (m_useLoHighBounds) + { + + BT_PROFILE("btLemkeSolver::solveMLCP"); + int n = A.rows(); + if (0==n) + return true; + + bool fail = false; + + btVectorXu solution(n); + btVectorXu q1; + q1.resize(n); + for (int row=0;row<n;row++) + { + q1[row] = -b[row]; + } + + // cout << "A" << endl; + // cout << A << endl; + + ///////////////////////////////////// + + //slow matrix inversion, replace with LU decomposition + btMatrixXu A1; + btMatrixXu B(n,n); + { + BT_PROFILE("inverse(slow)"); + A1.resize(A.rows(),A.cols()); + for (int row=0;row<A.rows();row++) + { + for (int col=0;col<A.cols();col++) + { + A1.setElem(row,col,A(row,col)); + } + } + + btMatrixXu matrix; + matrix.resize(n,2*n); + for (int row=0;row<n;row++) + { + for (int col=0;col<n;col++) + { + matrix.setElem(row,col,A1(row,col)); + } + } + + + btScalar ratio,a; + int i,j,k; + for(i = 0; i < n; i++){ + for(j = n; j < 2*n; j++){ + if(i==(j-n)) + matrix.setElem(i,j,1.0); + else + matrix.setElem(i,j,0.0); + } + } + for(i = 0; i < n; i++){ + for(j = 0; j < n; j++){ + if(i!=j) + { + btScalar v = matrix(i,i); + if (btFuzzyZero(v)) + { + a = 0.000001f; + } + ratio = matrix(j,i)/matrix(i,i); + for(k = 0; k < 2*n; k++){ + matrix.addElem(j,k,- ratio * matrix(i,k)); + } + } + } + } + for(i = 0; i < n; i++){ + a = matrix(i,i); + if (btFuzzyZero(a)) + { + a = 0.000001f; + } + btScalar invA = 1.f/a; + for(j = 0; j < 2*n; j++){ + matrix.mulElem(i,j,invA); + } + } + + + + + + for (int row=0;row<n;row++) + { + for (int col=0;col<n;col++) + { + B.setElem(row,col,matrix(row,n+col)); + } + } + } + + btMatrixXu b1(n,1); + + btMatrixXu M(n*2,n*2); + for (int row=0;row<n;row++) + { + b1.setElem(row,0,-b[row]); + for (int col=0;col<n;col++) + { + btScalar v =B(row,col); + M.setElem(row,col,v); + M.setElem(n+row,n+col,v); + M.setElem(n+row,col,-v); + M.setElem(row,n+col,-v); + + } + } + + btMatrixXu Bb1 = B*b1; +// q = [ (-B*b1 - lo)' (hi + B*b1)' ]' + + btVectorXu qq; + qq.resize(n*2); + for (int row=0;row<n;row++) + { + qq[row] = -Bb1(row,0)-lo[row]; + qq[n+row] = Bb1(row,0)+hi[row]; + } + + btVectorXu z1; + + btMatrixXu y1; + y1.resize(n,1); + btLemkeAlgorithm lemke(M,qq,m_debugLevel); + { + BT_PROFILE("lemke.solve"); + lemke.setSystem(M,qq); + z1 = lemke.solve(m_maxLoops); + } + for (int row=0;row<n;row++) + { + y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]); + } + btMatrixXu y1_b1(n,1); + for (int i=0;i<n;i++) + { + y1_b1.setElem(i,0,y1(i,0)-b1(i,0)); + } + + btMatrixXu x1; + + x1 = B*(y1_b1); + + for (int row=0;row<n;row++) + { + solution[row] = x1(row,0);//n]; + } + + int errorIndexMax = -1; + int errorIndexMin = -1; + float errorValueMax = -1e30; + float errorValueMin = 1e30; + + for (int i=0;i<n;i++) + { + x[i] = solution[i]; + volatile btScalar check = x[i]; + if (x[i] != check) + { + //printf("Lemke result is #NAN\n"); + x.setZero(); + return false; + } + + //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver + //we need to figure out why it happens, and fix it, or detect it properly) + if (x[i]>m_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]<errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } + } + } + if (fail) + { + int m_errorCountTimes = 0; + if (errorIndexMin<0) + errorValueMin = 0.f; + if (errorIndexMax<0) + errorValueMax = 0.f; + m_errorCountTimes++; + // printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); + for (int i=0;i<n;i++) + { + x[i]=0.f; + } + } + return !fail; + } else + + { + int dimension = A.rows(); + if (0==dimension) + return true; + +// printf("================ solving using Lemke/Newton/Fixpoint\n"); + + btVectorXu q; + q.resize(dimension); + for (int row=0;row<dimension;row++) + { + q[row] = -b[row]; + } + + btLemkeAlgorithm lemke(A,q,m_debugLevel); + + + lemke.setSystem(A,q); + + btVectorXu solution = lemke.solve(m_maxLoops); + + //check solution + + bool fail = false; + int errorIndexMax = -1; + int errorIndexMin = -1; + float errorValueMax = -1e30; + float errorValueMin = 1e30; + + for (int i=0;i<dimension;i++) + { + x[i] = solution[i+dimension]; + volatile btScalar check = x[i]; + if (x[i] != check) + { + x.setZero(); + return false; + } + + //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver + //we need to figure out why it happens, and fix it, or detect it properly) + if (x[i]>m_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]<errorValueMin) + { + errorIndexMin = i; + errorValueMin = x[i]; + fail = true; + //printf("x[i] = %f,",x[i]); + } + } + } + if (fail) + { + static int errorCountTimes = 0; + if (errorIndexMin<0) + errorValueMin = 0.f; + if (errorIndexMax<0) + errorValueMax = 0.f; + printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++); + for (int i=0;i<dimension;i++) + { + x[i]=0.f; + } + } + + + return !fail; + } + return true; + + } + +}; + +#endif //BT_LEMKE_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 0635b958bed..6688694a928 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -44,8 +44,8 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2; - int numBodies = m_tmpSolverBodyPool.size(); - m_allConstraintArray.resize(0); + // int numBodies = m_tmpSolverBodyPool.size(); + m_allConstraintPtrArray.resize(0); m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); @@ -53,7 +53,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int dindex = 0; for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } @@ -65,14 +65,14 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, { for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; - m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact)); m_limitDependencies[dindex++] = findex +firstContactConstraintOffset; if (numFrictionPerContact==2) { - m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); m_limitDependencies[dindex++] = findex+firstContactConstraintOffset; } } @@ -80,19 +80,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, { for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++) { - m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]); + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset; } } - if (!m_allConstraintArray.size()) + if (!m_allConstraintPtrArray.size()) { m_A.resize(0,0); m_b.resize(0); @@ -156,7 +156,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numContactRows = interleaveContactAndFriction ? 3 : 1; - int numConstraintRows = m_allConstraintArray.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); int n = numConstraintRows; { BT_PROFILE("init b (rhs)"); @@ -166,11 +166,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_bSplit.setZero(); for (int i=0;i<numConstraintRows ;i++) { - btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv; + btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; if (!btFuzzyZero(jacDiag)) { - btScalar rhs = m_allConstraintArray[i].m_rhs; - btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration; + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; m_b[i]=rhs/jacDiag; m_bSplit[i] = rhsPenetration/jacDiag; } @@ -178,8 +178,8 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } } - btScalar* w = 0; - int nub = 0; +// btScalar* w = 0; +// int nub = 0; m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); @@ -195,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_hi[i] = BT_INFINITY; } else { - m_lo[i] = m_allConstraintArray[i].m_lowerLimit; - m_hi[i] = m_allConstraintArray[i].m_upperLimit; + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; } } } // - int m=m_allConstraintArray.size(); + int m=m_allConstraintPtrArray.size(); int numBodies = m_tmpSolverBodyPool.size(); btAlignedObjectArray<int> bodyJointNodeArray; @@ -213,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) btAlignedObjectArray<btJointNode> jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintArray.size()); + jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); } static btMatrixXu J3; @@ -235,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { BT_PROFILE("ofs resize"); ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintArray.size()); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); } { BT_PROFILE("Compute J and JinvM"); @@ -243,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) int numRows = 0; - for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++) + for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++) { ofs[c] = rowOffset; - int sbA = m_allConstraintArray[i].m_solverBodyIdA; - int sbB = m_allConstraintArray[i].m_solverBodyIdB; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; @@ -268,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } for (int row=0;row<numRows;row++,cur++) { - btVector3 normalInvMass = m_allConstraintArray[i+row].m_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMass[r]); JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); } @@ -305,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) for (int row=0;row<numRows;row++,cur++) { - btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMassB[r]); JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); } @@ -349,13 +349,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numRows = 0; BT_PROFILE("Compute A"); - for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++) + for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++) { int row__ = ofs[c]; - int sbA = m_allConstraintArray[i].m_solverBodyIdA; - int sbB = m_allConstraintArray[i].m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; @@ -371,7 +371,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; + size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; //printf("%d joint i %d and j0: %d: ",count++,i,j0); m_A.multiplyAdd2_p8r ( JinvMrow, Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); @@ -390,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (j1<c) { int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintArray[cj1].m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; + size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); } @@ -404,15 +404,15 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) // compute diagonal blocks of m_A int row__ = 0; - int numJointRows = m_allConstraintArray.size(); + int numJointRows = m_allConstraintPtrArray.size(); int jj=0; for (;row__<numJointRows;) { - int sbA = m_allConstraintArray[row__].m_solverBodyIdA; - int sbB = m_allConstraintArray[row__].m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; @@ -453,9 +453,9 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) { - for (int i=0;i<m_allConstraintArray.size();i++) + for (int i=0;i<m_allConstraintPtrArray.size();i++) { - const btSolverConstraint& c = m_allConstraintArray[i]; + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; m_x[i]=c.m_appliedImpulse; m_xSplit[i] = c.m_appliedPushImpulse; } @@ -471,7 +471,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) { int numBodies = this->m_tmpSolverBodyPool.size(); - int numConstraintRows = m_allConstraintArray.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); m_b.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) @@ -482,11 +482,11 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) for (int i=0;i<numConstraintRows ;i++) { - if (m_allConstraintArray[i].m_jacDiagABInv) + if (m_allConstraintPtrArray[i]->m_jacDiagABInv) { - m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; + m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; } } @@ -517,28 +517,28 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) for (int i=0;i<numConstraintRows;i++) { - m_lo[i] = m_allConstraintArray[i].m_lowerLimit; - m_hi[i] = m_allConstraintArray[i].m_upperLimit; + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA; - int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB; + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { - setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]); + setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { - setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]); + setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); } } @@ -573,9 +573,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_xSplit.resize(numConstraintRows); // m_x.setZero(); - for (int i=0;i<m_allConstraintArray.size();i++) + for (int i=0;i<m_allConstraintPtrArray.size();i++) { - const btSolverConstraint& c = m_allConstraintArray[i]; + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; m_x[i]=c.m_appliedImpulse; if (infoGlobal.m_splitImpulse) m_xSplit[i] = c.m_appliedPushImpulse; @@ -597,27 +597,33 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod if (result) { BT_PROFILE("process MLCP results"); - for (int i=0;i<m_allConstraintArray.size();i++) + for (int i=0;i<m_allConstraintPtrArray.size();i++) { { - btSolverConstraint& c = m_allConstraintArray[i]; + btSolverConstraint& c = *m_allConstraintPtrArray[i]; int sbA = c.m_solverBodyIdA; int sbB = c.m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; - solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]); - solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]); + { + btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse; + c.m_appliedImpulse = m_x[i]; + solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } + if (infoGlobal.m_splitImpulse) { - solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]); - solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]); + btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse; + solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); c.m_appliedPushImpulse = m_xSplit[i]; } - c.m_appliedImpulse = m_x[i]; + } } } @@ -629,4 +635,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod } return 0.f; -}
\ No newline at end of file +} + + diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h index 31e8eb88ba5..43e85445bad 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h @@ -39,13 +39,15 @@ protected: btVectorXu m_xSplit2; btAlignedObjectArray<int> m_limitDependencies; - btConstraintArray m_allConstraintArray; + btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; btScalar m_cfm; virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual void createMLCP(const btContactSolverInfo& infoGlobal); virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index d2ad54d21a8..77cc57c6e0e 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -62,7 +62,7 @@ public: } float aDiag = A(i,i); - x [i] = (b [i] - delta) / A(i,i); + x [i] = (b [i] - delta) / aDiag; float s = 1.f; if (limitDependency[i]>=0) |