Update Files

lib/haxebullet/bullet/BulletDynamics/MLCPSolvers/btDantzigLCP.h

@@ -0,0 +1,77 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of                                          * 
+ *   The BSD-style license that is included with this library in         *
+ *   the file LICENSE-BSD.TXT.                                           *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
+satisfies one of
+	(1) x = lo, w >= 0
+	(2) x = hi, w <= 0
+	(3) lo < x < hi, w = 0
+A is a matrix of dimension n*n, everything else is a vector of size n*1.
+lo and hi can be +/- dInfinity as needed. the first `nub' variables are
+unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
+
+we restrict lo(i) <= 0 and hi(i) >= 0.
+
+the original data (A,b) may be modified by this function.
+
+if the `findex' (friction index) parameter is nonzero, it points to an array
+of index values. in this case constraints that have findex[i] >= 0 are
+special. all non-special constraints are solved for, then the lo and hi values
+for the special constraints are set:
+  hi[i] = abs( hi[i] * x[findex[i]] )
+  lo[i] = -hi[i]
+and the solution continues. this mechanism allows a friction approximation
+to be implemented. the first `nub' variables are assumed to have findex < 0.
+
+*/
+
+
+#ifndef _BT_LCP_H_
+#define _BT_LCP_H_
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct btDantzigScratchMemory
+{
+	btAlignedObjectArray<btScalar> m_scratch;
+	btAlignedObjectArray<btScalar> L;
+	btAlignedObjectArray<btScalar> d;
+	btAlignedObjectArray<btScalar> delta_w;
+	btAlignedObjectArray<btScalar> delta_x;
+	btAlignedObjectArray<btScalar> Dell;
+	btAlignedObjectArray<btScalar> ell;
+	btAlignedObjectArray<btScalar*> Arows;
+	btAlignedObjectArray<int> p;
+	btAlignedObjectArray<int> C;
+	btAlignedObjectArray<bool> state;
+};
+
+//return false if solving failed
+bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
+	int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch);
+
+
+
+#endif //_BT_LCP_H_

lib/haxebullet/bullet/BulletDynamics/MLCPSolvers/btDantzigSolver.h

@@ -0,0 +1,112 @@
+/*
+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_DANTZIG_SOLVER_H
+#define BT_DANTZIG_SOLVER_H
+
+#include "btMLCPSolverInterface.h"
+#include "btDantzigLCP.h"
+
+
+class btDantzigSolver : public btMLCPSolverInterface
+{
+protected:
+
+	btScalar m_acceptableUpperLimitSolution;
+
+	btAlignedObjectArray<char>	m_tempBuffer;
+
+	btAlignedObjectArray<btScalar> m_A;
+	btAlignedObjectArray<btScalar> m_b;
+	btAlignedObjectArray<btScalar> m_x;
+	btAlignedObjectArray<btScalar> m_lo;
+	btAlignedObjectArray<btScalar> m_hi;
+	btAlignedObjectArray<int>	m_dependencies;
+	btDantzigScratchMemory m_scratchMemory;
+public:
+
+	btDantzigSolver()
+		:m_acceptableUpperLimitSolution(btScalar(1000))
+	{
+	}
+
+	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)
+	{
+		bool result = true;
+		int n = b.rows();
+		if (n)
+		{
+			int nub = 0;
+			btAlignedObjectArray<btScalar> ww;
+			ww.resize(n);
+	
+
+			const btScalar* Aptr = A.getBufferPointer();
+			m_A.resize(n*n);
+			for (int i=0;i<n*n;i++)
+			{
+				m_A[i] = Aptr[i];
+
+			}
+
+			m_b.resize(n);
+			m_x.resize(n);
+			m_lo.resize(n);
+			m_hi.resize(n);
+			m_dependencies.resize(n);
+			for (int i=0;i<n;i++)
+			{
+				m_lo[i] = lo[i];
+				m_hi[i] = hi[i];
+				m_b[i] = b[i];
+				m_x[i] = x[i];
+				m_dependencies[i] = limitDependency[i];
+			}
+
+
+			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
+			if (!result)
+				return result;
+
+//			printf("numAllocas = %d\n",numAllocas);
+			for (int i=0;i<n;i++)
+			{
+				volatile btScalar xx = m_x[i];
+				if (xx != m_x[i])
+					return false;
+				if (x[i] >= m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+
+				if (x[i] <= -m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+			}
+
+			for (int i=0;i<n;i++)
+			{
+				x[i] = m_x[i];
+			}
+			
+		}
+
+		return result;
+	}
+};
+
+#endif //BT_DANTZIG_SOLVER_H

lib/haxebullet/bullet/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;
+  }
+
+

lib/haxebullet/bullet/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_ */

lib/haxebullet/bullet/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<6B>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

lib/haxebullet/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -0,0 +1,639 @@
+/*
+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
+
+#include "btMLCPSolver.h"
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btQuickprof.h"
+#include "btSolveProjectedGaussSeidel.h"
+
+
+btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
+:m_solver(solver),
+m_fallback(0)
+{
+}
+
+btMLCPSolver::~btMLCPSolver()
+{
+}
+
+bool gUseMatrixMultiply = false;
+bool interleaveContactAndFriction = false;
+
+btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+
+	{
+		BT_PROFILE("gather constraint data");
+
+		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
+
+
+	//	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());
+
+		int dindex = 0;
+		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
+		{
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+ 
+		///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		
+		int firstContactConstraintOffset=dindex;
+
+		if (interleaveContactAndFriction)
+		{
+			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				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_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
+					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
+				}
+			}
+		} else
+		{
+			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
+			}
+			
+		}
+
+
+		if (!m_allConstraintPtrArray.size())
+		{
+			m_A.resize(0,0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+			return 0.f;
+		}
+	}
+
+	
+	if (gUseMatrixMultiply)
+	{
+		BT_PROFILE("createMLCP");
+		createMLCP(infoGlobal);
+	}
+	else
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return 0.f;
+}
+
+bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	bool result = true;
+
+	if (m_A.rows()==0)
+		return true;
+
+	//if using split impulse, we solve 2 separate (M)LCPs
+	if (infoGlobal.m_splitImpulse)
+	{
+		btMatrixXu Acopy = m_A;
+		btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+//		printf("solve first LCP\n");
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+		if (result)
+			result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
+
+	} else
+	{
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+	}
+	return result;
+}
+
+struct btJointNode
+{
+	int jointIndex;     // pointer to enclosing dxJoint object
+	int otherBodyIndex;       // *other* body this joint is connected to
+	int nextJointNodeIndex;//-1 for null
+	int constraintRowIndex;
+};
+
+
+
+void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
+{
+	int numContactRows = interleaveContactAndFriction ? 3 : 1;
+
+	int numConstraintRows = m_allConstraintPtrArray.size();
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		m_b.setZero();
+		m_bSplit.setZero();
+		for (int i=0;i<numConstraintRows ;i++)
+		{
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
+			{
+				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i]=rhs/jacDiag;
+				m_bSplit[i] = rhsPenetration/jacDiag;
+			}
+
+		}
+	}
+
+//	btScalar* w = 0;
+//	int nub = 0;
+
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+ 
+	{
+		BT_PROFILE("init lo/ho");
+
+		for (int i=0;i<numConstraintRows;i++)
+		{
+			if (0)//m_limitDependencies[i]>=0)
+			{
+				m_lo[i] = -BT_INFINITY;
+				m_hi[i] = BT_INFINITY;
+			} else
+			{
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m=m_allConstraintPtrArray.size();
+
+	int numBodies = m_tmpSolverBodyPool.size();
+	btAlignedObjectArray<int> bodyJointNodeArray;
+	{
+		BT_PROFILE("bodyJointNodeArray.resize");
+		bodyJointNodeArray.resize(numBodies,-1);
+	}
+	btAlignedObjectArray<btJointNode> jointNodeArray;
+	{
+		BT_PROFILE("jointNodeArray.reserve");
+		jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
+	}
+
+    btMatrixXu& J3 = m_scratchJ3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2*m,8);
+	}
+    btMatrixXu& JinvM3 = m_scratchJInvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2*m,8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur=0;
+	int rowOffset = 0;
+    btAlignedObjectArray<int>& ofs = m_scratchOfs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
+	}				
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c=0;
+
+		int numRows = 0;
+
+		for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
+		{
+			ofs[c] = rowOffset;
+			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 ;
+			if (orgBodyA)
+			{
+				{
+					int slotA=-1;
+					//find free jointNode slot for sbA
+					slotA =jointNodeArray.size();
+					jointNodeArray.expand();//NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbA];
+					bodyJointNodeArray[sbA] = slotA;
+					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotA].jointIndex = c;
+					jointNodeArray[slotA].constraintRowIndex = i;
+					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
+				}
+				for (int row=0;row<numRows;row++,cur++)
+				{
+					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_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]);
+					}
+					J3.setElem(cur,3,0);
+					JinvM3.setElem(cur,3,0);
+					J3.setElem(cur,7,0);
+					JinvM3.setElem(cur,7,0);
+				}
+			} else
+			{
+				cur += numRows;
+			}
+			if (orgBodyB)
+			{
+
+				{
+					int slotB=-1;
+					//find free jointNode slot for sbA
+					slotB =jointNodeArray.size();
+					jointNodeArray.expand();//NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbB];
+					bodyJointNodeArray[sbB] = slotB;
+					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotB].jointIndex = c;
+					jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
+					jointNodeArray[slotB].constraintRowIndex = i;
+				}
+
+				for (int row=0;row<numRows;row++,cur++)
+				{
+					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_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]);
+					}
+					J3.setElem(cur,3,0);
+					JinvM3.setElem(cur,3,0);
+					J3.setElem(cur,7,0);
+					JinvM3.setElem(cur,7,0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			rowOffset+=numRows;
+
+		}
+		
+	}
+
+
+	//compute JinvM = J*invM.
+	const btScalar* JinvM = JinvM3.getBufferPointer();
+
+	const btScalar* Jptr = J3.getBufferPointer();
+	{
+		BT_PROFILE("m_A.resize");
+		m_A.resize(n,n);
+	}
+	
+	{
+		BT_PROFILE("m_A.setZero");
+		m_A.setZero();
+	}
+	int c=0;
+	{
+		int numRows = 0;
+		BT_PROFILE("Compute A");
+		for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
+		{
+			int row__ = ofs[c];
+			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 ;
+					
+			const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
+
+			{
+				int startJointNodeA = bodyJointNodeArray[sbA];
+				while (startJointNodeA>=0)
+				{
+					int j0 = jointNodeArray[startJointNodeA].jointIndex;
+					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
+					if (j0<c)
+					{
+								 
+						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
+						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]);
+					}
+					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
+				}
+			}
+
+			{
+				int startJointNodeB = bodyJointNodeArray[sbB];
+				while (startJointNodeB>=0)
+				{
+					int j1 = jointNodeArray[startJointNodeB].jointIndex;
+					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
+
+					if (j1<c)
+					{
+						int numRowsOther =  cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						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]);
+					}
+					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
+				}
+			}
+		}
+
+		{
+			BT_PROFILE("compute diagonal");
+			// compute diagonal blocks of m_A
+
+			int  row__ = 0;
+			int numJointRows = m_allConstraintPtrArray.size();
+
+			int jj=0;
+			for (;row__<numJointRows;)
+			{
+
+				//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;
+
+
+				const unsigned int infom =  row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
+				
+				const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
+				const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
+				m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
+				if (orgBodyB) 
+				{
+					m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom,  row__,row__);
+				}
+				row__ += infom;
+				jj++;
+			}
+		}
+	}
+
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for ( int i=0; i<m_A.rows(); ++i) 
+		{
+			m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm/ infoGlobal.m_timeStep);
+		}
+	}
+				   
+	///fill the upper triangle of the matrix, to make it symmetric
+	{
+		BT_PROFILE("fill the upper triangle ");
+		m_A.copyLowerToUpperTriangle();
+	}
+
+	{
+		BT_PROFILE("resize/init x");
+		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
+		if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
+		{
+			for (int i=0;i<m_allConstraintPtrArray.size();i++)
+			{
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+				m_x[i]=c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
+			}
+		} else
+		{
+			m_x.setZero();
+			m_xSplit.setZero();
+		}
+	}
+
+}
+
+void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
+{
+	int numBodies = this->m_tmpSolverBodyPool.size();
+	int numConstraintRows = m_allConstraintPtrArray.size();
+
+	m_b.resize(numConstraintRows);
+	if (infoGlobal.m_splitImpulse)
+		m_bSplit.resize(numConstraintRows);
+ 
+	m_bSplit.setZero();
+	m_b.setZero();
+
+	for (int i=0;i<numConstraintRows ;i++)
+	{
+		if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
+		{
+			m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (infoGlobal.m_splitImpulse)
+				m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
+		}
+	}
+ 
+    btMatrixXu& Minv = m_scratchMInv;
+	Minv.resize(6*numBodies,6*numBodies);
+	Minv.setZero();
+	for (int i=0;i<numBodies;i++)
+	{
+		const btSolverBody& rb = m_tmpSolverBodyPool[i];
+		const btVector3& invMass = rb.m_invMass;
+		setElem(Minv,i*6+0,i*6+0,invMass[0]);
+		setElem(Minv,i*6+1,i*6+1,invMass[1]);
+		setElem(Minv,i*6+2,i*6+2,invMass[2]);
+		btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
+ 
+		for (int r=0;r<3;r++)
+			for (int c=0;c<3;c++)
+				setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
+	}
+ 
+    btMatrixXu& J = m_scratchJ;
+	J.resize(numConstraintRows,6*numBodies);
+	J.setZero();
+ 
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+ 
+	for (int i=0;i<numConstraintRows;i++)
+	{
+
+		m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+		m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
+ 
+		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_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_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]);
+		}
+	}
+ 
+    btMatrixXu& J_transpose = m_scratchJTranspose;
+	J_transpose= J.transpose();
+
+    btMatrixXu& tmp = m_scratchTmp;
+
+	{
+		{
+			BT_PROFILE("J*Minv");
+			tmp = J*Minv;
+
+		}
+		{
+			BT_PROFILE("J*tmp");
+			m_A = tmp*J_transpose;
+		}
+	}
+
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for ( int i=0; i<m_A.rows(); ++i) 
+		{
+			m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	m_x.resize(numConstraintRows);
+	if (infoGlobal.m_splitImpulse)
+		m_xSplit.resize(numConstraintRows);
+//	m_x.setZero();
+
+	for (int i=0;i<m_allConstraintPtrArray.size();i++)
+	{
+		const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+		m_x[i]=c.m_appliedImpulse;
+		if (infoGlobal.m_splitImpulse)
+			m_xSplit[i] = c.m_appliedPushImpulse;
+	}
+
+}
+
+
+btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	bool result = true;
+	{
+		BT_PROFILE("solveMLCP");
+//		printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
+		result = solveMLCP(infoGlobal);
+	}
+
+	//check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
+	if (result)
+	{
+		BT_PROFILE("process MLCP results");
+		for (int i=0;i<m_allConstraintPtrArray.size();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;
+
+				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+ 
+				{
+					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)
+				{
+					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];
+				}
+				
+			}
+		}
+	}
+	else
+	{
+	//	printf("m_fallback = %d\n",m_fallback);
+		m_fallback++;
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	}
+
+	return 0.f;
+}
+
+

lib/haxebullet/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -0,0 +1,94 @@
+/*
+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_MLCP_SOLVER_H
+#define BT_MLCP_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "LinearMath/btMatrixX.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+class btMLCPSolver : public btSequentialImpulseConstraintSolver
+{
+
+protected:
+	
+	btMatrixXu m_A;
+	btVectorXu m_b;
+	btVectorXu m_x;
+	btVectorXu m_lo;
+	btVectorXu m_hi;
+	
+	///when using 'split impulse' we solve two separate (M)LCPs
+	btVectorXu m_bSplit;
+	btVectorXu m_xSplit;
+	btVectorXu m_bSplit1;
+	btVectorXu m_xSplit2;
+
+	btAlignedObjectArray<int> m_limitDependencies;
+	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
+	btMLCPSolverInterface* m_solver;
+	int m_fallback;
+
+    /// The following scratch variables are not stateful -- contents are cleared prior to each use.
+    /// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+    /// that multiple instances of the solver can be run in parallel.
+    btMatrixXu m_scratchJ3;
+    btMatrixXu m_scratchJInvM3;
+    btAlignedObjectArray<int> m_scratchOfs;
+    btMatrixXu m_scratchMInv;
+    btMatrixXu m_scratchJ;
+    btMatrixXu m_scratchJTranspose;
+    btMatrixXu m_scratchTmp;
+
+	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);
+
+	//return true is it solves the problem successfully
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+public:
+
+	btMLCPSolver(	 btMLCPSolverInterface* solver);
+	virtual ~btMLCPSolver();
+
+	void setMLCPSolver(btMLCPSolverInterface* solver)
+	{
+		m_solver = solver;
+	}
+
+	int getNumFallbacks() const
+	{
+		return m_fallback;
+	}
+	void setNumFallbacks(int num)
+	{
+		m_fallback = num;
+	}
+
+	virtual btConstraintSolverType	getSolverType() const
+	{
+		return BT_MLCP_SOLVER;
+	}
+
+};
+
+
+#endif //BT_MLCP_SOLVER_H

lib/haxebullet/bullet/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h

@@ -0,0 +1,33 @@
+/*
+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_MLCP_SOLVER_INTERFACE_H
+#define BT_MLCP_SOLVER_INTERFACE_H
+
+#include "LinearMath/btMatrixX.h"
+
+class btMLCPSolverInterface
+{
+public:
+	virtual ~btMLCPSolverInterface()
+	{
+	}
+
+	//return true is it solves the problem successfully
+	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)=0;
+};
+
+#endif //BT_MLCP_SOLVER_INTERFACE_H

lib/haxebullet/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h

@@ -0,0 +1,151 @@
+/*
+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_PATH_SOLVER_H
+#define BT_PATH_SOLVER_H
+
+//#define BT_USE_PATH
+#ifdef BT_USE_PATH
+
+extern "C" {
+#include "PATH/SimpleLCP.h"
+#include "PATH/License.h"
+#include "PATH/Error_Interface.h"
+};
+  void __stdcall MyError(Void *data, Char *msg)
+{
+	printf("Path Error: %s\n",msg);
+}
+  void __stdcall MyWarning(Void *data, Char *msg)
+{
+	printf("Path Warning: %s\n",msg);
+}
+
+Error_Interface e;
+
+
+
+#include "btMLCPSolverInterface.h"
+#include "Dantzig/lcp.h"
+
+class btPathSolver : public btMLCPSolverInterface
+{
+public:
+
+	btPathSolver()
+	{
+		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
+		e.error_data = 0;
+		e.warning = MyWarning;
+		e.error = MyError;
+		Error_SetInterface(&e);
+	}
+
+
+	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)
+	{
+		MCP_Termination status;
+		
+
+		int numVariables = b.rows();
+		if (0==numVariables)
+			return true;
+
+			/*	 - variables - the number of variables in the problem
+			- m_nnz - the number of nonzeros in the M matrix
+			- m_i - a vector of size m_nnz containing the row indices for M
+			- m_j - a vector of size m_nnz containing the column indices for M
+			- m_ij - a vector of size m_nnz containing the data for M
+			- q - a vector of size variables
+			- lb - a vector of size variables containing the lower bounds on x
+			- ub - a vector of size variables containing the upper bounds on x
+			*/
+		btAlignedObjectArray<double> values;
+		btAlignedObjectArray<int> rowIndices;
+		btAlignedObjectArray<int> colIndices;
+
+		for (int i=0;i<A.rows();i++)
+		{
+			for (int j=0;j<A.cols();j++)
+			{
+				if (A(i,j)!=0.f)
+				{
+					//add 1, because Path starts at 1, instead of 0
+					rowIndices.push_back(i+1);
+					colIndices.push_back(j+1);
+					values.push_back(A(i,j));
+				}
+			}
+		}
+		int numNonZero = rowIndices.size();
+		btAlignedObjectArray<double> zResult;
+		zResult.resize(numVariables);
+		btAlignedObjectArray<double> rhs;
+		btAlignedObjectArray<double> upperBounds;
+		btAlignedObjectArray<double> lowerBounds;
+		for (int i=0;i<numVariables;i++)
+		{
+			upperBounds.push_back(hi[i]);
+			lowerBounds.push_back(lo[i]);
+			rhs.push_back(-b[i]);
+		}
+
+
+		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
+
+		if (status != MCP_Solved)
+		{
+			static const char* gReturnMsgs[] = {
+				"Invalid return",
+				"MCP_Solved: The problem was solved",
+				"MCP_NoProgress: A stationary point was found",
+				"MCP_MajorIterationLimit: Major iteration limit met",
+				"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
+				"MCP_TimeLimit: Ran out of time",
+				"MCP_UserInterrupt: Control-C, typically",
+				"MCP_BoundError: Problem has a bound error",
+				"MCP_DomainError: Could not find starting point",
+				"MCP_Infeasible: Problem has no solution",
+				"MCP_Error: An error occurred within the code",
+				"MCP_LicenseError: License could not be found",
+				"MCP_OK"
+			};
+
+			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
+			printf("using Projected Gauss Seidel fallback\n");
+			
+			return false;
+		} else
+		{
+			for (int i=0;i<numVariables;i++)
+			{
+				x[i] = zResult[i];
+				//check for #NAN
+				if (x[i] != zResult[i])
+					return false;
+			}
+			return true;
+
+		}
+
+	}
+};
+
+#endif //BT_USE_PATH
+
+
+#endif //BT_PATH_SOLVER_H

lib/haxebullet/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -0,0 +1,110 @@
+/*
+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_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+
+
+#include "btMLCPSolverInterface.h"
+
+///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
+class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
+{
+
+public:
+
+	btScalar m_leastSquaresResidualThreshold;
+	btScalar m_leastSquaresResidual;
+
+	btSolveProjectedGaussSeidel()
+		:m_leastSquaresResidualThreshold(0),
+		m_leastSquaresResidual(0)
+	{
+	}
+
+	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 (!A.rows())
+			return true;
+		//the A matrix is sparse, so compute the non-zero elements
+		A.rowComputeNonZeroElements();
+
+		//A is a m-n matrix, m rows, n columns
+		btAssert(A.rows() == b.rows());
+
+		int i, j, numRows = A.rows();
+	
+		btScalar delta;
+
+		for (int k = 0; k <numIterations; k++)
+		{
+			m_leastSquaresResidual = 0.f;
+			for (i = 0; i <numRows; i++)
+			{
+				delta = 0.0f;
+				if (useSparsity)
+				{
+					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
+					{
+						int j = A.m_rowNonZeroElements1[i][h];
+						if (j != i)//skip main diagonal
+						{
+							delta += A(i,j) * x[j];
+						}
+					}
+				} else
+				{
+					for (j = 0; j <i; j++) 
+						delta += A(i,j) * x[j];
+					for (j = i+1; j<numRows; j++) 
+						delta += A(i,j) * x[j];
+				}
+
+				btScalar aDiag = A(i,i);
+				btScalar xOld = x[i];
+				x [i] = (b [i] - delta) / aDiag;
+				btScalar s = 1.f;
+
+				if (limitDependency[i]>=0)
+				{
+					s = x[limitDependency[i]];
+					if (s<0)
+						s=1;
+				}
+			
+				if (x[i]<lo[i]*s)
+					x[i]=lo[i]*s;
+				if (x[i]>hi[i]*s)
+					x[i]=hi[i]*s;
+				btScalar diff = x[i] - xOld;
+				m_leastSquaresResidual += diff*diff;
+			}
+
+			btScalar eps  = m_leastSquaresResidualThreshold;
+			if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
+			{
+#ifdef VERBOSE_PRINTF_RESIDUAL
+				printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k);
+#endif
+				break;
+			}
+		}
+		return true;
+	}
+
+};
+
+#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H