Added a Lemke MLCP solver, extracted from the MBSim project, and re-licensed under the zlib license

with permission of the original author. 
The Lemke implementation is not fully working yet:
1) we need to convert the lo-high LCP problem into a problem without the lo/high
2) we need to sort out the remaining instabilities, and report a failure if the max loopcount is reached etc.
We replaced the fmatvec library with our own LinearMath/btMatrixX.h, and STL std::vector with btAlignedObjectArray

Removed some warnings/potential issues: use fuzzyZero instead of isZero, and some warnings, 
related to this issue 756

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -812,7 +812,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 
 	///avoid collision response between two static objects
-	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
+	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
 		return;
 
 	int rollingFriction=1;

src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp

@@ -0,0 +1,368 @@
+/* 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"
+
+
+
+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;
+  }
+
+

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
+//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
+//STL/std::vector replaced by btAlignedObjectArray
+
+
+
+#ifndef NUMERICS_LEMKE_ALGORITHM_H_
+#define 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 /* NUMERICS_LEMKE_ALGORITHM_H_ */

src/BulletDynamics/MLCPSolvers/btLemkeSolver.h

@@ -0,0 +1,127 @@
+/*
+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"
+
+
+class btLemkeSolver : public btMLCPSolverInterface
+{
+public:
+	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)
+	{
+		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];
+		}
+		
+		int debugLevel=0;
+		btLemkeAlgorithm lemke(A,q,debugLevel);
+		
+		
+		lemke.setSystem(A,q);
+		
+		int maxloops = 10000;
+		btVectorXu solution = lemke.solve(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]>100000)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-10000)
+			{
+				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;
+			}
+		}
+#if 0
+		if (lemke.getInfo()<0)
+		{
+			printf("Lemke found no solution, info = %d\n",lemke.getInfo());
+		} else
+		{
+			printf("Lemke info = %d, found a solution in %d steps\n",lemke.getInfo(),lemke.getSteps());
+			//printf("Lemke found a solution\n");
+			for (int i=0;i<dimension;i++)
+			{
+				x[i] = solution(i+dimension);
+			}
+		}
+#endif
+
+		return !fail;
+		
+	}
+
+};
+
+#endif //BT_LEMKE_SOLVER_H

src/LinearMath/btMatrixX.h

@@ -20,6 +20,12 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+//#define BT_DEBUG_OSTREAM
+#ifdef BT_DEBUG_OSTREAM
+#include <ostream>
+#include <iomanip>      // std::setw
+#endif //BT_DEBUG_OSTREAM
+
 class btIntSortPredicate
 {
 	public:
@@ -30,6 +36,118 @@ class btIntSortPredicate
 };
 
 
+template <typename T>
+struct btVectorX
+{
+	btAlignedObjectArray<T>	m_storage;
+	
+	btVectorX()
+	{
+	}
+	btVectorX(int numRows)
+	{
+		m_storage.resize(numRows);
+	}
+	
+	void resize(int rows)
+	{
+		m_storage.resize(rows);
+	}
+	int cols() const
+	{
+		return 1;
+	}
+	int rows() const
+	{
+		return m_storage.size();
+	}
+	int size() const
+	{
+		return rows();
+	}
+	
+	T nrm2() const
+	{
+		T norm = T(0);
+		
+		int nn = rows();
+		
+		{
+			if (nn == 1)
+			{
+				norm = btFabs((*this)[0]);
+			}
+			else
+			{
+				T scale = 0.0;
+				T ssq = 1.0;
+				
+				/* The following loop is equivalent to this call to the LAPACK
+				 auxiliary routine:   CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
+				
+				for (int ix=0;ix<nn;ix++)
+				{
+					if ((*this)[ix] != 0.0)
+					{
+						T absxi = btFabs((*this)[ix]);
+						if (scale < absxi)
+						{
+							T temp;
+							temp = scale / absxi;
+							ssq = ssq * (temp * temp) + 1.0;
+							scale = absxi;
+						}
+						else
+						{
+							T temp;
+							temp = absxi / scale;
+							ssq += temp * temp;
+						}
+					}
+				}
+				norm = scale * sqrt(ssq);
+			}
+		}
+		return norm;
+		
+	}
+	void	setZero()
+	{
+		//	for (int i=0;i<m_storage.size();i++)
+		//		m_storage[i]=0;
+		//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
+		btSetZero(&m_storage[0],m_storage.size());
+	}
+	const T& operator[] (int index) const
+	{
+		return m_storage[index];
+	}
+	
+	T& operator[] (int index)
+	{
+		return m_storage[index];
+	}
+	
+	T* getBufferPointerWritable()
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	
+	const T* getBufferPointer() const
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	
+};
+/*
+ template <typename T>
+ void setElem(btMatrixX<T>& mat, int row, int col, T val)
+ {
+ mat.setElem(row,col,val);
+ }
+ */
+
+
 template <typename T> 
 struct btMatrixX
 {
@@ -109,21 +227,7 @@ struct btMatrixX
 		}
 	}
 	
-	void copyLowerToUpperTriangle()
+	
-	{
-		int count=0;
-		for (int row=0;row<m_rowNonZeroElements1.size();row++)
-		{
-			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
-			{
-				int col = m_rowNonZeroElements1[row][j];
-				setElem(col,row, (*this)(row,col));
-				count++;
-
-			}
-		}
-		//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
-	}
 	void setElem(int row,int col, T val)
 	{
 		m_setElemOperations++;
@@ -134,9 +238,37 @@ struct btMatrixX
 				m_rowNonZeroElements1[row].push_back(col);
 				m_colNonZeroElements[col].push_back(row);
 			}
-			m_storage[row*m_cols+col] = val;
 		}
+		m_storage[row*m_cols+col] = val;
 	}
+	
+	void mulElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		//mul doesn't change sparsity info
+
+		m_storage[row*m_cols+col] *= val;
+	}
+	
+	
+	
+	
+	void copyLowerToUpperTriangle()
+	{
+		int count=0;
+		for (int row=0;row<m_rowNonZeroElements1.size();row++)
+		{
+			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
+			{
+				int col = m_rowNonZeroElements1[row][j];
+				setElem(col,row, (*this)(row,col));
+				count++;
+				
+			}
+		}
+		//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
+	}
+	
 	const T& operator() (int row,int col) const
 	{
 		return m_storage[col+row*m_cols];
@@ -167,6 +299,19 @@ struct btMatrixX
 			clearSparseInfo();
 		}
 	}
+	
+	void setIdentity()
+	{
+		btAssert(rows() == cols());
+		
+		setZero();
+		for (int row=0;row<rows();row++)
+		{
+			setElem(row,row,1);
+		}
+	}
+
+	
 
 	void	printMatrix(const char* msg)
 	{
@@ -404,73 +549,62 @@ struct btMatrixX
 			bb += 8;
 		}
 	}
-
+	
-};
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
-
-template <typename T> 
-struct btVectorX
-{
-	btAlignedObjectArray<T>	m_storage;
-
-	btVectorX()
 	{
+		int numRows = rowend+1-rowstart;
+		int numCols = colend+1-colstart;
+		
+		for (int row=0;row<numRows;row++)
+		{
+			for (int col=0;col<numCols;col++)
+			{
+				setElem(rowstart+row,colstart+col,value);
+			}
+		}
 	}
-	btVectorX(int numRows)
+	
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
 	{
-		m_storage.resize(numRows);
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block(row,col));
+			}
+		}
 	}
-
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
-	void resize(int rows)
 	{
-		m_storage.resize(rows);
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block[row]);
+			}
+		}
 	}
-	int cols() const
+	
+	
+	btMatrixX negative()
 	{
-		return 1;
+		btMatrixX neg(rows(),cols());
-	}
+		for (int i=0;i<m_colNonZeroElements.size();i++)
-	int rows() const
+			for (int h=0;h<m_colNonZeroElements[i].size();h++)
-	{
+			{
-		return m_storage.size();
+				int j = m_colNonZeroElements[i][h];
-	}
+				T v = (*this)(i,j);
-	int size() const
+				neg.setElem(i,j,-v);
-	{
+			}
-		return rows();
+		return neg;
-	}
-	void	setZero()
-	{
-	//	for (int i=0;i<m_storage.size();i++)
-	//		m_storage[i]=0;
-		//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
-		btSetZero(&m_storage[0],m_storage.size());
-	}
-	const T& operator[] (int index) const
-	{
-		return m_storage[index];
-	}
-
-	T& operator[] (int index)
-	{
-		return m_storage[index];
-	}
-
-	T* getBufferPointerWritable() 
-	{
-		return m_storage.size() ? &m_storage[0] : 0;
-	}
-
-	const T* getBufferPointer() const
-	{
-		return m_storage.size() ? &m_storage[0] : 0;
 	}
 
 };
-/*
+
-template <typename T> 
-void setElem(btMatrixX<T>& mat, int row, int col, T val)
-{
-	mat.setElem(row,col,val);
-}
-*/
 
 
 typedef btMatrixX<float> btMatrixXf;
@@ -480,6 +614,30 @@ typedef btMatrixX<double> btMatrixXd;
 typedef btVectorX<double> btVectorXd;
 
 
+#ifdef BT_DEBUG_OSTREAM
+template <typename T> 
+std::ostream& operator<< (std::ostream& os, btMatrixX<T>& mat)
+	{
+		
+		os << " [";
+		//printf("%s ---------------------\n",msg);
+		for (int i=0;i<mat.rows();i++)
+		{
+			for (int j=0;j<mat.cols();j++)
+			{
+				os << std::setw(10) << mat(i,j);
+			}
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		os << " ]";
+		//printf("\n---------------------\n");
+
+		return os;
+	}
+
+#endif //BT_DEBUG_OSTREAM
+
 
 inline void setElem(btMatrixXd& mat, int row, int col, double val)
 {

src/LinearMath/btVector3.h

@@ -297,7 +297,7 @@ public:
 	SIMD_FORCE_INLINE btVector3& normalize() 
 	{
 		
-		btAssert(length() != btScalar(0));
+		btAssert(!fuzzyZero());
 
 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)		
         // dot product first
@@ -685,6 +685,7 @@ public:
 		return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
 	}
 
+
 	SIMD_FORCE_INLINE bool fuzzyZero() const 
 	{
 		return length2() < SIMD_EPSILON;
@@ -950,9 +951,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
 
 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
 {
-	btVector3 norm = *this;
+	btVector3 nrm = *this;
 
-	return norm.normalize();
+	return nrm.normalize();
 } 
 
 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
@@ -1010,21 +1011,21 @@ SIMD_FORCE_INLINE   long    btVector3::maxDot( const btVector3 *array, long arra
     if( array_count < scalar_cutoff )	
 #endif
     {
-        btScalar maxDot = -SIMD_INFINITY;
+        btScalar maxDot1 = -SIMD_INFINITY;
         int i = 0;
         int ptIndex = -1;
         for( i = 0; i < array_count; i++ )
         {
             btScalar dot = array[i].dot(*this);
             
-            if( dot > maxDot )
+            if( dot > maxDot1 )
             {
-                maxDot = dot;
+                maxDot1 = dot;
                 ptIndex = i;
             }
         }
         
-        dotOut = maxDot;
+        dotOut = maxDot1;
         return ptIndex;
     }
 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)