Upgrade Bullet to version 2.83.

I tried to carefully preserve all patches since the last upgrade.

Improves T47195, cloth collision detection bug.

Differential Revision: https://developer.blender.org/D1739

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)