Remove unused Bullet module and thirdparty code

It has been disabled in `master` since one year (#45852) and our plan
is for Bullet, and possibly other thirdparty physics engines, to be
implemented via GDExtension so that they can be selected by the users
who need them.

11 files changed, 0 insertions, 4136 deletions

diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
deleted file mode 100644
index 98ecdc0794..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
+++ /dev/null
@@ -1,2158 +0,0 @@
-/*************************************************************************
-*                                                                       *
-* 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 EITHER:                                  *
-*   (1) The GNU Lesser General Public License as published by the Free  *
-*       Software Foundation; either version 2.1 of the License, or (at  *
-*       your option) any later version. The text of the GNU Lesser      *
-*       General Public License is included with this library in the     *
-*       file LICENSE.TXT.                                               *
-*   (2) 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.                     *
-*                                                                       *
-*************************************************************************/
-
-/*
-
-
-THE ALGORITHM
--------------
-
-solve A*x = b+w, with x and w subject to certain LCP conditions.
-each x(i),w(i) must lie on one of the three line segments in the following
-diagram. each line segment corresponds to one index set :
-
-     w(i)
-     /|\      |           :
-      |       |           :
-      |       |i in N     :
-  w>0 |       |state[i]=0 :
-      |       |           :
-      |       |           :  i in C
-  w=0 +       +-----------------------+
-      |                   :           |
-      |                   :           |
-  w<0 |                   :           |i in N
-      |                   :           |state[i]=1
-      |                   :           |
-      |                   :           |
-      +-------|-----------|-----------|----------> x(i)
-             lo           0           hi
-
-the Dantzig algorithm proceeds as follows:
-  for i=1:n
-    * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or
-      negative towards the line. as this is done, the other (x(j),w(j))
-      for j<i are constrained to be on the line. if any (x,w) reaches the
-      end of a line segment then it is switched between index sets.
-    * i is added to the appropriate index set depending on what line segment
-      it hits.
-
-we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit
-simpler, because the starting point for x(i),w(i) is always on the dotted
-line x=0 and x will only ever increase in one direction, so it can only hit
-two out of the three line segments.
-
-
-NOTES
------
-
-this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m".
-the implementation is split into an LCP problem object (btLCP) and an LCP
-driver function. most optimization occurs in the btLCP object.
-
-a naive implementation of the algorithm requires either a lot of data motion
-or a lot of permutation-array lookup, because we are constantly re-ordering
-rows and columns. to avoid this and make a more optimized algorithm, a
-non-trivial data structure is used to represent the matrix A (this is
-implemented in the fast version of the btLCP object).
-
-during execution of this algorithm, some indexes in A are clamped (set C),
-some are non-clamped (set N), and some are "don't care" (where x=0).
-A,x,b,w (and other problem vectors) are permuted such that the clamped
-indexes are first, the unclamped indexes are next, and the don't-care
-indexes are last. this permutation is recorded in the array `p'.
-initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped,
-the corresponding elements of p are swapped.
-
-because the C and N elements are grouped together in the rows of A, we can do
-lots of work with a fast dot product function. if A,x,etc were not permuted
-and we only had a permutation array, then those dot products would be much
-slower as we would have a permutation array lookup in some inner loops.
-
-A is accessed through an array of row pointers, so that element (i,j) of the
-permuted matrix is A[i][j]. this makes row swapping fast. for column swapping
-we still have to actually move the data.
-
-during execution of this algorithm we maintain an L*D*L' factorization of
-the clamped submatrix of A (call it `AC') which is the top left nC*nC
-submatrix of A. there are two ways we could arrange the rows/columns in AC.
-
-(1) AC is always permuted such that L*D*L' = AC. this causes a problem
-when a row/column is removed from C, because then all the rows/columns of A
-between the deleted index and the end of C need to be rotated downward.
-this results in a lot of data motion and slows things down.
-(2) L*D*L' is actually a factorization of a *permutation* of AC (which is
-itself a permutation of the underlying A). this is what we do - the
-permutation is recorded in the vector C. call this permutation A[C,C].
-when a row/column is removed from C, all we have to do is swap two
-rows/columns and manipulate C.
-
-*/
-
-#include "btDantzigLCP.h"
-
-#include <string.h>  //memcpy
-
-bool s_error = false;
-
-//***************************************************************************
-// code generation parameters
-
-#define btLCP_FAST  // use fast btLCP object
-
-// option 1 : matrix row pointers (less data copying)
-#define BTROWPTRS
-#define BTATYPE btScalar **
-#define BTAROW(i) (m_A[i])
-
-// option 2 : no matrix row pointers (slightly faster inner loops)
-//#define NOROWPTRS
-//#define BTATYPE btScalar *
-//#define BTAROW(i) (m_A+(i)*m_nskip)
-
-#define BTNUB_OPTIMIZATIONS
-
-/* solve L*X=B, with B containing 1 right hand sides.
- * L is an n*n lower triangular matrix with ones on the diagonal.
- * L is stored by rows and its leading dimension is lskip.
- * B is an n*1 matrix that contains the right hand sides.
- * B is stored by columns and its leading dimension is also lskip.
- * B is overwritten with X.
- * this processes blocks of 2*2.
- * if this is in the factorizer source file, n must be a multiple of 2.
- */
-
-static void btSolveL1_1(const btScalar *L, btScalar *B, int n, int lskip1)
-{
-	/* declare variables - Z matrix, p and q vectors, etc */
-	btScalar Z11, m11, Z21, m21, p1, q1, p2, *ex;
-	const btScalar *ell;
-	int i, j;
-	/* compute all 2 x 1 blocks of X */
-	for (i = 0; i < n; i += 2)
-	{
-		/* compute all 2 x 1 block of X, from rows i..i+2-1 */
-		/* set the Z matrix to 0 */
-		Z11 = 0;
-		Z21 = 0;
-		ell = L + i * lskip1;
-		ex = B;
-		/* the inner loop that computes outer products and adds them to Z */
-		for (j = i - 2; j >= 0; j -= 2)
-		{
-			/* compute outer product and add it to the Z matrix */
-			p1 = ell[0];
-			q1 = ex[0];
-			m11 = p1 * q1;
-			p2 = ell[lskip1];
-			m21 = p2 * q1;
-			Z11 += m11;
-			Z21 += m21;
-			/* compute outer product and add it to the Z matrix */
-			p1 = ell[1];
-			q1 = ex[1];
-			m11 = p1 * q1;
-			p2 = ell[1 + lskip1];
-			m21 = p2 * q1;
-			/* advance pointers */
-			ell += 2;
-			ex += 2;
-			Z11 += m11;
-			Z21 += m21;
-			/* end of inner loop */
-		}
-		/* compute left-over iterations */
-		j += 2;
-		for (; j > 0; j--)
-		{
-			/* compute outer product and add it to the Z matrix */
-			p1 = ell[0];
-			q1 = ex[0];
-			m11 = p1 * q1;
-			p2 = ell[lskip1];
-			m21 = p2 * q1;
-			/* advance pointers */
-			ell += 1;
-			ex += 1;
-			Z11 += m11;
-			Z21 += m21;
-		}
-		/* finish computing the X(i) block */
-		Z11 = ex[0] - Z11;
-		ex[0] = Z11;
-		p1 = ell[lskip1];
-		Z21 = ex[1] - Z21 - p1 * Z11;
-		ex[1] = Z21;
-		/* end of outer loop */
-	}
-}
-
-/* solve L*X=B, with B containing 2 right hand sides.
- * L is an n*n lower triangular matrix with ones on the diagonal.
- * L is stored by rows and its leading dimension is lskip.
- * B is an n*2 matrix that contains the right hand sides.
- * B is stored by columns and its leading dimension is also lskip.
- * B is overwritten with X.
- * this processes blocks of 2*2.
- * if this is in the factorizer source file, n must be a multiple of 2.
- */
-
-static void btSolveL1_2(const btScalar *L, btScalar *B, int n, int lskip1)
-{
-	/* declare variables - Z matrix, p and q vectors, etc */
-	btScalar Z11, m11, Z12, m12, Z21, m21, Z22, m22, p1, q1, p2, q2, *ex;
-	const btScalar *ell;
-	int i, j;
-	/* compute all 2 x 2 blocks of X */
-	for (i = 0; i < n; i += 2)
-	{
-		/* compute all 2 x 2 block of X, from rows i..i+2-1 */
-		/* set the Z matrix to 0 */
-		Z11 = 0;
-		Z12 = 0;
-		Z21 = 0;
-		Z22 = 0;
-		ell = L + i * lskip1;
-		ex = B;
-		/* the inner loop that computes outer products and adds them to Z */
-		for (j = i - 2; j >= 0; j -= 2)
-		{
-			/* compute outer product and add it to the Z matrix */
-			p1 = ell[0];
-			q1 = ex[0];
-			m11 = p1 * q1;
-			q2 = ex[lskip1];
-			m12 = p1 * q2;
-			p2 = ell[lskip1];
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z12 += m12;
-			Z21 += m21;
-			Z22 += m22;
-			/* compute outer product and add it to the Z matrix */
-			p1 = ell[1];
-			q1 = ex[1];
-			m11 = p1 * q1;
-			q2 = ex[1 + lskip1];
-			m12 = p1 * q2;
-			p2 = ell[1 + lskip1];
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			/* advance pointers */
-			ell += 2;
-			ex += 2;
-			Z11 += m11;
-			Z12 += m12;
-			Z21 += m21;
-			Z22 += m22;
-			/* end of inner loop */
-		}
-		/* compute left-over iterations */
-		j += 2;
-		for (; j > 0; j--)
-		{
-			/* compute outer product and add it to the Z matrix */
-			p1 = ell[0];
-			q1 = ex[0];
-			m11 = p1 * q1;
-			q2 = ex[lskip1];
-			m12 = p1 * q2;
-			p2 = ell[lskip1];
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			/* advance pointers */
-			ell += 1;
-			ex += 1;
-			Z11 += m11;
-			Z12 += m12;
-			Z21 += m21;
-			Z22 += m22;
-		}
-		/* finish computing the X(i) block */
-		Z11 = ex[0] - Z11;
-		ex[0] = Z11;
-		Z12 = ex[lskip1] - Z12;
-		ex[lskip1] = Z12;
-		p1 = ell[lskip1];
-		Z21 = ex[1] - Z21 - p1 * Z11;
-		ex[1] = Z21;
-		Z22 = ex[1 + lskip1] - Z22 - p1 * Z12;
-		ex[1 + lskip1] = Z22;
-		/* end of outer loop */
-	}
-}
-
-void btFactorLDLT(btScalar *A, btScalar *d, int n, int nskip1)
-{
-	int i, j;
-	btScalar sum, *ell, *dee, dd, p1, p2, q1, q2, Z11, m11, Z21, m21, Z22, m22;
-	if (n < 1) return;
-
-	for (i = 0; i <= n - 2; i += 2)
-	{
-		/* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
-		btSolveL1_2(A, A + i * nskip1, i, nskip1);
-		/* scale the elements in a 2 x i block at A(i,0), and also */
-		/* compute Z = the outer product matrix that we'll need. */
-		Z11 = 0;
-		Z21 = 0;
-		Z22 = 0;
-		ell = A + i * nskip1;
-		dee = d;
-		for (j = i - 6; j >= 0; j -= 6)
-		{
-			p1 = ell[0];
-			p2 = ell[nskip1];
-			dd = dee[0];
-			q1 = p1 * dd;
-			q2 = p2 * dd;
-			ell[0] = q1;
-			ell[nskip1] = q2;
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z21 += m21;
-			Z22 += m22;
-			p1 = ell[1];
-			p2 = ell[1 + nskip1];
-			dd = dee[1];
-			q1 = p1 * dd;
-			q2 = p2 * dd;
-			ell[1] = q1;
-			ell[1 + nskip1] = q2;
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z21 += m21;
-			Z22 += m22;
-			p1 = ell[2];
-			p2 = ell[2 + nskip1];
-			dd = dee[2];
-			q1 = p1 * dd;
-			q2 = p2 * dd;
-			ell[2] = q1;
-			ell[2 + nskip1] = q2;
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z21 += m21;
-			Z22 += m22;
-			p1 = ell[3];
-			p2 = ell[3 + nskip1];
-			dd = dee[3];
-			q1 = p1 * dd;
-			q2 = p2 * dd;
-			ell[3] = q1;
-			ell[3 + nskip1] = q2;
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z21 += m21;
-			Z22 += m22;
-			p1 = ell[4];
-			p2 = ell[4 + nskip1];
-			dd = dee[4];
-			q1 = p1 * dd;
-			q2 = p2 * dd;
-			ell[4] = q1;
-			ell[4 + nskip1] = q2;
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z21 += m21;
-			Z22 += m22;
-			p1 = ell[5];
-			p2 = ell[5 + nskip1];
-			dd = dee[5];
-			q1 = p1 * dd;
-			q2 = p2 * dd;
-			ell[5] = q1;
-			ell[5 + nskip1] = q2;
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z21 += m21;
-			Z22 += m22;
-			ell += 6;
-			dee += 6;
-		}
-		/* compute left-over iterations */
-		j += 6;
-		for (; j > 0; j--)
-		{
-			p1 = ell[0];
-			p2 = ell[nskip1];
-			dd = dee[0];
-			q1 = p1 * dd;
-			q2 = p2 * dd;
-			ell[0] = q1;
-			ell[nskip1] = q2;
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m22 = p2 * q2;
-			Z11 += m11;
-			Z21 += m21;
-			Z22 += m22;
-			ell++;
-			dee++;
-		}
-		/* solve for diagonal 2 x 2 block at A(i,i) */
-		Z11 = ell[0] - Z11;
-		Z21 = ell[nskip1] - Z21;
-		Z22 = ell[1 + nskip1] - Z22;
-		dee = d + i;
-		/* factorize 2 x 2 block Z,dee */
-		/* factorize row 1 */
-		dee[0] = btRecip(Z11);
-		/* factorize row 2 */
-		sum = 0;
-		q1 = Z21;
-		q2 = q1 * dee[0];
-		Z21 = q2;
-		sum += q1 * q2;
-		dee[1] = btRecip(Z22 - sum);
-		/* done factorizing 2 x 2 block */
-		ell[nskip1] = Z21;
-	}
-	/* compute the (less than 2) rows at the bottom */
-	switch (n - i)
-	{
-		case 0:
-			break;
-
-		case 1:
-			btSolveL1_1(A, A + i * nskip1, i, nskip1);
-			/* scale the elements in a 1 x i block at A(i,0), and also */
-			/* compute Z = the outer product matrix that we'll need. */
-			Z11 = 0;
-			ell = A + i * nskip1;
-			dee = d;
-			for (j = i - 6; j >= 0; j -= 6)
-			{
-				p1 = ell[0];
-				dd = dee[0];
-				q1 = p1 * dd;
-				ell[0] = q1;
-				m11 = p1 * q1;
-				Z11 += m11;
-				p1 = ell[1];
-				dd = dee[1];
-				q1 = p1 * dd;
-				ell[1] = q1;
-				m11 = p1 * q1;
-				Z11 += m11;
-				p1 = ell[2];
-				dd = dee[2];
-				q1 = p1 * dd;
-				ell[2] = q1;
-				m11 = p1 * q1;
-				Z11 += m11;
-				p1 = ell[3];
-				dd = dee[3];
-				q1 = p1 * dd;
-				ell[3] = q1;
-				m11 = p1 * q1;
-				Z11 += m11;
-				p1 = ell[4];
-				dd = dee[4];
-				q1 = p1 * dd;
-				ell[4] = q1;
-				m11 = p1 * q1;
-				Z11 += m11;
-				p1 = ell[5];
-				dd = dee[5];
-				q1 = p1 * dd;
-				ell[5] = q1;
-				m11 = p1 * q1;
-				Z11 += m11;
-				ell += 6;
-				dee += 6;
-			}
-			/* compute left-over iterations */
-			j += 6;
-			for (; j > 0; j--)
-			{
-				p1 = ell[0];
-				dd = dee[0];
-				q1 = p1 * dd;
-				ell[0] = q1;
-				m11 = p1 * q1;
-				Z11 += m11;
-				ell++;
-				dee++;
-			}
-			/* solve for diagonal 1 x 1 block at A(i,i) */
-			Z11 = ell[0] - Z11;
-			dee = d + i;
-			/* factorize 1 x 1 block Z,dee */
-			/* factorize row 1 */
-			dee[0] = btRecip(Z11);
-			/* done factorizing 1 x 1 block */
-			break;
-
-			//default: *((char*)0)=0;  /* this should never happen! */
-	}
-}
-
-/* solve L*X=B, with B containing 1 right hand sides.
- * L is an n*n lower triangular matrix with ones on the diagonal.
- * L is stored by rows and its leading dimension is lskip.
- * B is an n*1 matrix that contains the right hand sides.
- * B is stored by columns and its leading dimension is also lskip.
- * B is overwritten with X.
- * this processes blocks of 4*4.
- * if this is in the factorizer source file, n must be a multiple of 4.
- */
-
-void btSolveL1(const btScalar *L, btScalar *B, int n, int lskip1)
-{
-	/* declare variables - Z matrix, p and q vectors, etc */
-	btScalar Z11, Z21, Z31, Z41, p1, q1, p2, p3, p4, *ex;
-	const btScalar *ell;
-	int lskip2, lskip3, i, j;
-	/* compute lskip values */
-	lskip2 = 2 * 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 */
-		/* set the Z matrix to 0 */
-		Z11 = 0;
-		Z21 = 0;
-		Z31 = 0;
-		Z41 = 0;
-		ell = L + i * lskip1;
-		ex = B;
-		/* the inner loop that computes outer products and adds them to Z */
-		for (j = i - 12; j >= 0; j -= 12)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			p2 = ell[lskip1];
-			p3 = ell[lskip2];
-			p4 = ell[lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[1];
-			q1 = ex[1];
-			p2 = ell[1 + lskip1];
-			p3 = ell[1 + lskip2];
-			p4 = ell[1 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[2];
-			q1 = ex[2];
-			p2 = ell[2 + lskip1];
-			p3 = ell[2 + lskip2];
-			p4 = ell[2 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[3];
-			q1 = ex[3];
-			p2 = ell[3 + lskip1];
-			p3 = ell[3 + lskip2];
-			p4 = ell[3 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[4];
-			q1 = ex[4];
-			p2 = ell[4 + lskip1];
-			p3 = ell[4 + lskip2];
-			p4 = ell[4 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[5];
-			q1 = ex[5];
-			p2 = ell[5 + lskip1];
-			p3 = ell[5 + lskip2];
-			p4 = ell[5 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[6];
-			q1 = ex[6];
-			p2 = ell[6 + lskip1];
-			p3 = ell[6 + lskip2];
-			p4 = ell[6 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[7];
-			q1 = ex[7];
-			p2 = ell[7 + lskip1];
-			p3 = ell[7 + lskip2];
-			p4 = ell[7 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[8];
-			q1 = ex[8];
-			p2 = ell[8 + lskip1];
-			p3 = ell[8 + lskip2];
-			p4 = ell[8 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[9];
-			q1 = ex[9];
-			p2 = ell[9 + lskip1];
-			p3 = ell[9 + lskip2];
-			p4 = ell[9 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[10];
-			q1 = ex[10];
-			p2 = ell[10 + lskip1];
-			p3 = ell[10 + lskip2];
-			p4 = ell[10 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* load p and q values */
-			p1 = ell[11];
-			q1 = ex[11];
-			p2 = ell[11 + lskip1];
-			p3 = ell[11 + lskip2];
-			p4 = ell[11 + lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* advance pointers */
-			ell += 12;
-			ex += 12;
-			/* end of inner loop */
-		}
-		/* compute left-over iterations */
-		j += 12;
-		for (; j > 0; j--)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			p2 = ell[lskip1];
-			p3 = ell[lskip2];
-			p4 = ell[lskip3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			Z21 += p2 * q1;
-			Z31 += p3 * q1;
-			Z41 += p4 * q1;
-			/* advance pointers */
-			ell += 1;
-			ex += 1;
-		}
-		/* finish computing the X(i) block */
-		Z11 = ex[0] - Z11;
-		ex[0] = Z11;
-		p1 = ell[lskip1];
-		Z21 = ex[1] - Z21 - p1 * Z11;
-		ex[1] = Z21;
-		p1 = ell[lskip2];
-		p2 = ell[1 + lskip2];
-		Z31 = ex[2] - Z31 - p1 * Z11 - p2 * Z21;
-		ex[2] = Z31;
-		p1 = ell[lskip3];
-		p2 = ell[1 + lskip3];
-		p3 = ell[2 + lskip3];
-		Z41 = ex[3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31;
-		ex[3] = Z41;
-		/* end of outer loop */
-	}
-	/* compute rows at end that are not a multiple of block size */
-	for (; i < n; i++)
-	{
-		/* compute all 1 x 1 block of X, from rows i..i+1-1 */
-		/* set the Z matrix to 0 */
-		Z11 = 0;
-		ell = L + i * lskip1;
-		ex = B;
-		/* the inner loop that computes outer products and adds them to Z */
-		for (j = i - 12; j >= 0; j -= 12)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[1];
-			q1 = ex[1];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[2];
-			q1 = ex[2];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[3];
-			q1 = ex[3];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[4];
-			q1 = ex[4];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[5];
-			q1 = ex[5];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[6];
-			q1 = ex[6];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[7];
-			q1 = ex[7];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[8];
-			q1 = ex[8];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[9];
-			q1 = ex[9];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[10];
-			q1 = ex[10];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* load p and q values */
-			p1 = ell[11];
-			q1 = ex[11];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* advance pointers */
-			ell += 12;
-			ex += 12;
-			/* end of inner loop */
-		}
-		/* compute left-over iterations */
-		j += 12;
-		for (; j > 0; j--)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			/* compute outer product and add it to the Z matrix */
-			Z11 += p1 * q1;
-			/* advance pointers */
-			ell += 1;
-			ex += 1;
-		}
-		/* finish computing the X(i) block */
-		Z11 = ex[0] - Z11;
-		ex[0] = Z11;
-	}
-}
-
-/* solve L^T * x=b, with b containing 1 right hand side.
- * L is an n*n lower triangular matrix with ones on the diagonal.
- * L is stored by rows and its leading dimension is lskip.
- * b is an n*1 matrix that contains the right hand side.
- * b is overwritten with x.
- * this processes blocks of 4.
- */
-
-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, 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;
-	/* 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 */
-		/* set the Z matrix to 0 */
-		Z11 = 0;
-		Z21 = 0;
-		Z31 = 0;
-		Z41 = 0;
-		ell = L - i;
-		ex = B;
-		/* the inner loop that computes outer products and adds them to Z */
-		for (j = i - 4; j >= 0; j -= 4)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			p2 = ell[-1];
-			p3 = ell[-2];
-			p4 = ell[-3];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m31 = p3 * q1;
-			m41 = p4 * q1;
-			ell += lskip1;
-			Z11 += m11;
-			Z21 += m21;
-			Z31 += m31;
-			Z41 += m41;
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[-1];
-			p2 = ell[-1];
-			p3 = ell[-2];
-			p4 = ell[-3];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m31 = p3 * q1;
-			m41 = p4 * q1;
-			ell += lskip1;
-			Z11 += m11;
-			Z21 += m21;
-			Z31 += m31;
-			Z41 += m41;
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[-2];
-			p2 = ell[-1];
-			p3 = ell[-2];
-			p4 = ell[-3];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m31 = p3 * q1;
-			m41 = p4 * q1;
-			ell += lskip1;
-			Z11 += m11;
-			Z21 += m21;
-			Z31 += m31;
-			Z41 += m41;
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[-3];
-			p2 = ell[-1];
-			p3 = ell[-2];
-			p4 = ell[-3];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m31 = p3 * q1;
-			m41 = p4 * q1;
-			ell += lskip1;
-			ex -= 4;
-			Z11 += m11;
-			Z21 += m21;
-			Z31 += m31;
-			Z41 += m41;
-			/* end of inner loop */
-		}
-		/* compute left-over iterations */
-		j += 4;
-		for (; j > 0; j--)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			p2 = ell[-1];
-			p3 = ell[-2];
-			p4 = ell[-3];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			m21 = p2 * q1;
-			m31 = p3 * q1;
-			m41 = p4 * q1;
-			ell += lskip1;
-			ex -= 1;
-			Z11 += m11;
-			Z21 += m21;
-			Z31 += m31;
-			Z41 += m41;
-		}
-		/* finish computing the X(i) block */
-		Z11 = ex[0] - Z11;
-		ex[0] = Z11;
-		p1 = ell[-1];
-		Z21 = ex[-1] - Z21 - p1 * Z11;
-		ex[-1] = Z21;
-		p1 = ell[-2];
-		p2 = ell[-2 + lskip1];
-		Z31 = ex[-2] - Z31 - p1 * Z11 - p2 * Z21;
-		ex[-2] = Z31;
-		p1 = ell[-3];
-		p2 = ell[-3 + lskip1];
-		p3 = ell[-3 + lskip2];
-		Z41 = ex[-3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31;
-		ex[-3] = Z41;
-		/* end of outer loop */
-	}
-	/* compute rows at end that are not a multiple of block size */
-	for (; i < n; i++)
-	{
-		/* compute all 1 x 1 block of X, from rows i..i+1-1 */
-		/* set the Z matrix to 0 */
-		Z11 = 0;
-		ell = L - i;
-		ex = B;
-		/* the inner loop that computes outer products and adds them to Z */
-		for (j = i - 4; j >= 0; j -= 4)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			ell += lskip1;
-			Z11 += m11;
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[-1];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			ell += lskip1;
-			Z11 += m11;
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[-2];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			ell += lskip1;
-			Z11 += m11;
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[-3];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			ell += lskip1;
-			ex -= 4;
-			Z11 += m11;
-			/* end of inner loop */
-		}
-		/* compute left-over iterations */
-		j += 4;
-		for (; j > 0; j--)
-		{
-			/* load p and q values */
-			p1 = ell[0];
-			q1 = ex[0];
-			/* compute outer product and add it to the Z matrix */
-			m11 = p1 * q1;
-			ell += lskip1;
-			ex -= 1;
-			Z11 += m11;
-		}
-		/* finish computing the X(i) block */
-		Z11 = ex[0] - Z11;
-		ex[0] = Z11;
-	}
-}
-
-void btVectorScale(btScalar *a, const btScalar *d, int n)
-{
-	btAssert(a && d && n >= 0);
-	for (int i = 0; i < n; i++)
-	{
-		a[i] *= d[i];
-	}
-}
-
-void btSolveLDLT(const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip)
-{
-	btAssert(L && d && b && n > 0 && nskip >= n);
-	btSolveL1(L, b, n, nskip);
-	btVectorScale(b, d, n);
-	btSolveL1T(L, b, n, nskip);
-}
-
-//***************************************************************************
-
-// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
-// A is nskip. this only references and swaps the lower triangle.
-// if `do_fast_row_swaps' is nonzero and row pointers are being used, then
-// rows will be swapped by exchanging row pointers. otherwise the data will
-// be copied.
-
-static void btSwapRowsAndCols(BTATYPE A, int n, int i1, int i2, int nskip,
-							  int do_fast_row_swaps)
-{
-	btAssert(A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
-			 nskip >= n && i1 < i2);
-
-#ifdef BTROWPTRS
-	btScalar *A_i1 = A[i1];
-	btScalar *A_i2 = A[i2];
-	for (int i = i1 + 1; i < i2; ++i)
-	{
-		btScalar *A_i_i1 = A[i] + i1;
-		A_i1[i] = *A_i_i1;
-		*A_i_i1 = A_i2[i];
-	}
-	A_i1[i2] = A_i1[i1];
-	A_i1[i1] = A_i2[i1];
-	A_i2[i1] = A_i2[i2];
-	// swap rows, by swapping row pointers
-	if (do_fast_row_swaps)
-	{
-		A[i1] = A_i2;
-		A[i2] = A_i1;
-	}
-	else
-	{
-		// Only swap till i2 column to match A plain storage variant.
-		for (int k = 0; k <= i2; ++k)
-		{
-			btScalar tmp = A_i1[k];
-			A_i1[k] = A_i2[k];
-			A_i2[k] = tmp;
-		}
-	}
-	// swap columns the hard way
-	for (int j = i2 + 1; j < n; ++j)
-	{
-		btScalar *A_j = A[j];
-		btScalar tmp = A_j[i1];
-		A_j[i1] = A_j[i2];
-		A_j[i2] = tmp;
-	}
-#else
-	btScalar *A_i1 = A + i1 * nskip;
-	btScalar *A_i2 = A + i2 * nskip;
-	for (int k = 0; k < i1; ++k)
-	{
-		btScalar tmp = A_i1[k];
-		A_i1[k] = A_i2[k];
-		A_i2[k] = tmp;
-	}
-	btScalar *A_i = A_i1 + nskip;
-	for (int i = i1 + 1; i < i2; A_i += nskip, ++i)
-	{
-		btScalar tmp = A_i2[i];
-		A_i2[i] = A_i[i1];
-		A_i[i1] = tmp;
-	}
-	{
-		btScalar tmp = A_i1[i1];
-		A_i1[i1] = A_i2[i2];
-		A_i2[i2] = tmp;
-	}
-	btScalar *A_j = A_i2 + nskip;
-	for (int j = i2 + 1; j < n; A_j += nskip, ++j)
-	{
-		btScalar tmp = A_j[i1];
-		A_j[i1] = A_j[i2];
-		A_j[i2] = tmp;
-	}
-#endif
-}
-
-// swap two indexes in the n*n LCP problem. i1 must be <= i2.
-
-static void btSwapProblem(BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo,
-						  btScalar *hi, int *p, bool *state, int *findex,
-						  int n, int i1, int i2, int nskip,
-						  int do_fast_row_swaps)
-{
-	btScalar tmpr;
-	int tmpi;
-	bool tmpb;
-	btAssert(n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
-	if (i1 == i2) return;
-
-	btSwapRowsAndCols(A, n, i1, i2, nskip, do_fast_row_swaps);
-
-	tmpr = x[i1];
-	x[i1] = x[i2];
-	x[i2] = tmpr;
-
-	tmpr = b[i1];
-	b[i1] = b[i2];
-	b[i2] = tmpr;
-
-	tmpr = w[i1];
-	w[i1] = w[i2];
-	w[i2] = tmpr;
-
-	tmpr = lo[i1];
-	lo[i1] = lo[i2];
-	lo[i2] = tmpr;
-
-	tmpr = hi[i1];
-	hi[i1] = hi[i2];
-	hi[i2] = tmpr;
-
-	tmpi = p[i1];
-	p[i1] = p[i2];
-	p[i2] = tmpi;
-
-	tmpb = state[i1];
-	state[i1] = state[i2];
-	state[i2] = tmpb;
-
-	if (findex)
-	{
-		tmpi = findex[i1];
-		findex[i1] = findex[i2];
-		findex[i2] = tmpi;
-	}
-}
-
-//***************************************************************************
-// btLCP manipulator object. this represents an n*n LCP problem.
-//
-// two index sets C and N are kept. each set holds a subset of
-// the variable indexes 0..n-1. an index can only be in one set.
-// initially both sets are empty.
-//
-// the index set C is special: solutions to A(C,C)\A(C,i) can be generated.
-
-//***************************************************************************
-// fast implementation of btLCP. see the above definition of btLCP for
-// interface comments.
-//
-// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is
-// permuted as the other vectors/matrices are permuted.
-//
-// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have
-// contiguous indexes. the don't-care indexes follow N.
-//
-// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are
-// added or removed from the set C the factorization is updated.
-// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A.
-// the leading dimension of the matrix L is always `nskip'.
-//
-// at the start there may be other indexes that are unbounded but are not
-// included in `nub'. btLCP will permute the matrix so that absolutely all
-// unbounded vectors are at the start. thus there may be some initial
-// permutation.
-//
-// the algorithms here assume certain patterns, particularly with respect to
-// index transfer.
-
-#ifdef btLCP_FAST
-
-struct btLCP
-{
-	const int m_n;
-	const int m_nskip;
-	int m_nub;
-	int m_nC, m_nN;                                                         // size of each index set
-	BTATYPE const m_A;                                                      // A rows
-	btScalar *const m_x, *const m_b, *const m_w, *const m_lo, *const m_hi;  // permuted LCP problem data
-	btScalar *const m_L, *const m_d;                                        // L*D*L' factorization of set C
-	btScalar *const m_Dell, *const m_ell, *const m_tmp;
-	bool *const m_state;
-	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 *_Dell, btScalar *_ell, btScalar *_tmp,
-		  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
-	void transfer_i_from_N_to_C(int i);
-	void transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch);
-	int numC() const { return m_nC; }
-	int numN() const { return m_nN; }
-	int indexC(int i) const { return i; }
-	int indexN(int i) const { return i + m_nC; }
-	btScalar Aii(int i) const { return BTAROW(i)[i]; }
-	btScalar AiC_times_qC(int i, btScalar *q) const { return btLargeDot(BTAROW(i), q, m_nC); }
-	btScalar AiN_times_qN(int i, btScalar *q) const { return btLargeDot(BTAROW(i) + m_nC, q + m_nC, m_nN); }
-	void pN_equals_ANC_times_qC(btScalar *p, btScalar *q);
-	void pN_plusequals_ANi(btScalar *p, int i, int sign = 1);
-	void pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q);
-	void pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q);
-	void solve1(btScalar *a, int i, int dir = 1, int only_transfer = 0);
-	void unpermute();
-};
-
-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 *_Dell, btScalar *_ell, btScalar *_tmp,
-			 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),
-#else
-																			 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)
-{
-	{
-		btSetZero(m_x, m_n);
-	}
-
-	{
-#ifdef BTROWPTRS
-		// make matrix row pointers
-		btScalar *aptr = _Adata;
-		BTATYPE A = m_A;
-		const int n = m_n, nskip = m_nskip;
-		for (int k = 0; k < n; aptr += nskip, ++k) A[k] = aptr;
-#endif
-	}
-
-	{
-		int *p = m_p;
-		const int n = m_n;
-		for (int k = 0; k < n; ++k) p[k] = k;  // initially unpermuted
-	}
-
-	/*
-  // for testing, we can do some random swaps in the area i > nub
-  {
-    const int n = m_n;
-    const int nub = m_nub;
-    if (nub < n) {
-    for (int k=0; k<100; k++) {
-      int i1,i2;
-      do {
-        i1 = dRandInt(n-nub)+nub;
-        i2 = dRandInt(n-nub)+nub;
-      }
-      while (i1 > i2); 
-      //printf ("--> %d %d\n",i1,i2);
-      btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,n,i1,i2,m_nskip,0);
-    }
-  }
-  */
-
-	// permute the problem so that *all* the unbounded variables are at the
-	// start, i.e. look for unbounded variables not included in `nub'. we can
-	// potentially push up `nub' this way and get a bigger initial factorization.
-	// note that when we swap rows/cols here we must not just swap row pointers,
-	// as the initial factorization relies on the data being all in one chunk.
-	// variables that have findex >= 0 are *not* considered to be unbounded even
-	// if lo=-inf and hi=inf - this is because these limits may change during the
-	// solution process.
-
-	{
-		int *findex = m_findex;
-		btScalar *lo = m_lo, *hi = m_hi;
-		const int n = m_n;
-		for (int k = m_nub; k < n; ++k)
-		{
-			if (findex && findex[k] >= 0) continue;
-			if (lo[k] == -BT_INFINITY && hi[k] == BT_INFINITY)
-			{
-				btSwapProblem(m_A, m_x, m_b, m_w, lo, hi, m_p, m_state, findex, n, m_nub, k, m_nskip, 0);
-				m_nub++;
-			}
-		}
-	}
-
-	// if there are unbounded variables at the start, factorize A up to that
-	// point and solve for x. this puts all indexes 0..nub-1 into C.
-	if (m_nub > 0)
-	{
-		const int nub = m_nub;
-		{
-			btScalar *Lrow = m_L;
-			const int nskip = m_nskip;
-			for (int j = 0; j < nub; Lrow += nskip, ++j) memcpy(Lrow, BTAROW(j), (j + 1) * sizeof(btScalar));
-		}
-		btFactorLDLT(m_L, m_d, nub, m_nskip);
-		memcpy(m_x, m_b, nub * sizeof(btScalar));
-		btSolveLDLT(m_L, m_d, m_x, nub, m_nskip);
-		btSetZero(m_w, nub);
-		{
-			int *C = m_C;
-			for (int k = 0; k < nub; ++k) C[k] = k;
-		}
-		m_nC = nub;
-	}
-
-	// permute the indexes > nub such that all findex variables are at the end
-	if (m_findex)
-	{
-		const int nub = m_nub;
-		int *findex = m_findex;
-		int num_at_end = 0;
-		for (int k = m_n - 1; k >= nub; k--)
-		{
-			if (findex[k] >= 0)
-			{
-				btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, findex, m_n, k, m_n - 1 - num_at_end, m_nskip, 1);
-				num_at_end++;
-			}
-		}
-	}
-
-	// print info about indexes
-	/*
-  {
-    const int n = m_n;
-    const int nub = m_nub;
-    for (int k=0; k<n; k++) {
-      if (k<nub) printf ("C");
-      else if (m_lo[k]==-BT_INFINITY && m_hi[k]==BT_INFINITY) printf ("c");
-      else printf (".");
-    }
-    printf ("\n");
-  }
-  */
-}
-
-void btLCP::transfer_i_to_C(int i)
-{
-	{
-		if (m_nC > 0)
-		{
-			// ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
-			{
-				const int nC = m_nC;
-				btScalar *const Ltgt = m_L + nC * m_nskip, *ell = m_ell;
-				for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j];
-			}
-			const int nC = m_nC;
-			m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC));
-		}
-		else
-		{
-			m_d[0] = btRecip(BTAROW(i)[i]);
-		}
-
-		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1);
-
-		const int nC = m_nC;
-		m_C[nC] = nC;
-		m_nC = nC + 1;  // nC value is outdated after this line
-	}
-}
-
-void btLCP::transfer_i_from_N_to_C(int i)
-{
-	{
-		if (m_nC > 0)
-		{
-			{
-				btScalar *const aptr = BTAROW(i);
-				btScalar *Dell = m_Dell;
-				const int *C = m_C;
-#ifdef BTNUB_OPTIMIZATIONS
-				// if nub>0, initial part of aptr unpermuted
-				const int nub = m_nub;
-				int j = 0;
-				for (; j < nub; ++j) Dell[j] = aptr[j];
-				const int nC = m_nC;
-				for (; j < nC; ++j) Dell[j] = aptr[C[j]];
-#else
-				const int nC = m_nC;
-				for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]];
-#endif
-			}
-			btSolveL1(m_L, m_Dell, m_nC, m_nskip);
-			{
-				const int nC = m_nC;
-				btScalar *const Ltgt = m_L + nC * m_nskip;
-				btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
-				for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
-			}
-			const int nC = m_nC;
-			m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC));
-		}
-		else
-		{
-			m_d[0] = btRecip(BTAROW(i)[i]);
-		}
-
-		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1);
-
-		const int nC = m_nC;
-		m_C[nC] = nC;
-		m_nN--;
-		m_nC = nC + 1;  // nC value is outdated after this line
-	}
-
-	// @@@ TO DO LATER
-	// if we just finish here then we'll go back and re-solve for
-	// delta_x. but actually we can be more efficient and incrementally
-	// update delta_x here. but if we do this, we wont have ell and Dell
-	// to use in updating the factorization later.
-}
-
-void btRemoveRowCol(btScalar *A, int n, int nskip, int r)
-{
-	btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n);
-	if (r >= n - 1) return;
-	if (r > 0)
-	{
-		{
-			const size_t move_size = (n - r - 1) * sizeof(btScalar);
-			btScalar *Adst = A + r;
-			for (int i = 0; i < r; Adst += nskip, ++i)
-			{
-				btScalar *Asrc = Adst + 1;
-				memmove(Adst, Asrc, move_size);
-			}
-		}
-		{
-			const size_t cpy_size = r * sizeof(btScalar);
-			btScalar *Adst = A + r * nskip;
-			for (int i = r; i < (n - 1); ++i)
-			{
-				btScalar *Asrc = Adst + nskip;
-				memcpy(Adst, Asrc, cpy_size);
-				Adst = Asrc;
-			}
-		}
-	}
-	{
-		const size_t cpy_size = (n - r - 1) * sizeof(btScalar);
-		btScalar *Adst = A + r * (nskip + 1);
-		for (int i = r; i < (n - 1); ++i)
-		{
-			btScalar *Asrc = Adst + (nskip + 1);
-			memcpy(Adst, Asrc, cpy_size);
-			Adst = Asrc - 1;
-		}
-	}
-}
-
-void btLDLTAddTL(btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar> &scratch)
-{
-	btAssert(L && d && a && n > 0 && nskip >= n);
-
-	if (n < 2) return;
-	scratch.resize(2 * nskip);
-	btScalar *W1 = &scratch[0];
-
-	btScalar *W2 = W1 + nskip;
-
-	W1[0] = btScalar(0.0);
-	W2[0] = btScalar(0.0);
-	for (int j = 1; j < n; ++j)
-	{
-		W1[j] = W2[j] = (btScalar)(a[j] * SIMDSQRT12);
-	}
-	btScalar W11 = (btScalar)((btScalar(0.5) * a[0] + 1) * SIMDSQRT12);
-	btScalar W21 = (btScalar)((btScalar(0.5) * a[0] - 1) * SIMDSQRT12);
-
-	btScalar alpha1 = btScalar(1.0);
-	btScalar alpha2 = btScalar(1.0);
-
-	{
-		btScalar dee = d[0];
-		btScalar alphanew = alpha1 + (W11 * W11) * dee;
-		btAssert(alphanew != btScalar(0.0));
-		dee /= alphanew;
-		btScalar gamma1 = W11 * dee;
-		dee *= alpha1;
-		alpha1 = alphanew;
-		alphanew = alpha2 - (W21 * W21) * dee;
-		dee /= alphanew;
-		//btScalar gamma2 = W21 * dee;
-		alpha2 = alphanew;
-		btScalar k1 = btScalar(1.0) - W21 * gamma1;
-		btScalar k2 = W21 * gamma1 * W11 - W21;
-		btScalar *ll = L + nskip;
-		for (int p = 1; p < n; ll += nskip, ++p)
-		{
-			btScalar Wp = W1[p];
-			btScalar ell = *ll;
-			W1[p] = Wp - W11 * ell;
-			W2[p] = k1 * Wp + k2 * ell;
-		}
-	}
-
-	btScalar *ll = L + (nskip + 1);
-	for (int j = 1; j < n; ll += nskip + 1, ++j)
-	{
-		btScalar k1 = W1[j];
-		btScalar k2 = W2[j];
-
-		btScalar dee = d[j];
-		btScalar alphanew = alpha1 + (k1 * k1) * dee;
-		btAssert(alphanew != btScalar(0.0));
-		dee /= alphanew;
-		btScalar gamma1 = k1 * dee;
-		dee *= alpha1;
-		alpha1 = alphanew;
-		alphanew = alpha2 - (k2 * k2) * dee;
-		dee /= alphanew;
-		btScalar gamma2 = k2 * dee;
-		dee *= alpha2;
-		d[j] = dee;
-		alpha2 = alphanew;
-
-		btScalar *l = ll + nskip;
-		for (int p = j + 1; p < n; l += nskip, ++p)
-		{
-			btScalar ell = *l;
-			btScalar Wp = W1[p] - k1 * ell;
-			ell += gamma1 * Wp;
-			W1[p] = Wp;
-			Wp = W2[p] - k2 * ell;
-			ell -= gamma2 * Wp;
-			W2[p] = Wp;
-			*l = ell;
-		}
-	}
-}
-
-#define _BTGETA(i, j) (A[i][j])
-//#define _GETA(i,j) (A[(i)*nskip+(j)])
-#define BTGETA(i, j) ((i > j) ? _BTGETA(i, j) : _BTGETA(j, i))
-
-inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip)
-{
-	return nskip * 2 * sizeof(btScalar);
-}
-
-void btLDLTRemove(btScalar **A, const int *p, btScalar *L, btScalar *d,
-				  int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar> &scratch)
-{
-	btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
-			 n1 >= n2 && nskip >= n1);
-#ifdef BT_DEBUG
-	for (int i = 0; i < n2; ++i)
-		btAssert(p[i] >= 0 && p[i] < n1);
-#endif
-
-	if (r == n2 - 1)
-	{
-		return;  // deleting last row/col is easy
-	}
-	else
-	{
-		size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip);
-		btAssert(LDLTAddTL_size % sizeof(btScalar) == 0);
-		scratch.resize(nskip * 2 + n2);
-		btScalar *tmp = &scratch[0];
-		if (r == 0)
-		{
-			btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size);
-			const int p_0 = p[0];
-			for (int i = 0; i < n2; ++i)
-			{
-				a[i] = -BTGETA(p[i], p_0);
-			}
-			a[0] += btScalar(1.0);
-			btLDLTAddTL(L, d, a, n2, nskip, scratch);
-		}
-		else
-		{
-			btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size);
-			{
-				btScalar *Lcurr = L + r * nskip;
-				for (int i = 0; i < r; ++Lcurr, ++i)
-				{
-					btAssert(d[i] != btScalar(0.0));
-					t[i] = *Lcurr / d[i];
-				}
-			}
-			btScalar *a = t + r;
-			{
-				btScalar *Lcurr = L + r * nskip;
-				const int *pp_r = p + r, p_r = *pp_r;
-				const int n2_minus_r = n2 - r;
-				for (int i = 0; i < n2_minus_r; Lcurr += nskip, ++i)
-				{
-					a[i] = btLargeDot(Lcurr, t, r) - BTGETA(pp_r[i], p_r);
-				}
-			}
-			a[0] += btScalar(1.0);
-			btLDLTAddTL(L + r * nskip + r, d + r, a, n2 - r, nskip, scratch);
-		}
-	}
-
-	// snip out row/column r from L and d
-	btRemoveRowCol(L, n2, nskip, r);
-	if (r < (n2 - 1)) memmove(d + r, d + r + 1, (n2 - r - 1) * sizeof(btScalar));
-}
-
-void btLCP::transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch)
-{
-	{
-		int *C = m_C;
-		// remove a row/column from the factorization, and adjust the
-		// indexes (black magic!)
-		int last_idx = -1;
-		const int nC = m_nC;
-		int j = 0;
-		for (; j < nC; ++j)
-		{
-			if (C[j] == nC - 1)
-			{
-				last_idx = j;
-			}
-			if (C[j] == i)
-			{
-				btLDLTRemove(m_A, C, m_L, m_d, m_n, nC, j, m_nskip, scratch);
-				int k;
-				if (last_idx == -1)
-				{
-					for (k = j + 1; k < nC; ++k)
-					{
-						if (C[k] == nC - 1)
-						{
-							break;
-						}
-					}
-					btAssert(k < nC);
-				}
-				else
-				{
-					k = last_idx;
-				}
-				C[k] = C[j];
-				if (j < (nC - 1)) memmove(C + j, C + j + 1, (nC - j - 1) * sizeof(int));
-				break;
-			}
-		}
-		btAssert(j < nC);
-
-		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, i, nC - 1, m_nskip, 1);
-
-		m_nN++;
-		m_nC = nC - 1;  // nC value is outdated after this line
-	}
-}
-
-void btLCP::pN_equals_ANC_times_qC(btScalar *p, btScalar *q)
-{
-	// we could try to make this matrix-vector multiplication faster using
-	// outer product matrix tricks, e.g. with the dMultidotX() functions.
-	// but i tried it and it actually made things slower on random 100x100
-	// problems because of the overhead involved. so we'll stick with the
-	// simple method for now.
-	const int nC = m_nC;
-	btScalar *ptgt = p + nC;
-	const int nN = m_nN;
-	for (int i = 0; i < nN; ++i)
-	{
-		ptgt[i] = btLargeDot(BTAROW(i + nC), q, nC);
-	}
-}
-
-void btLCP::pN_plusequals_ANi(btScalar *p, int i, int sign)
-{
-	const int nC = m_nC;
-	btScalar *aptr = BTAROW(i) + nC;
-	btScalar *ptgt = p + nC;
-	if (sign > 0)
-	{
-		const int nN = m_nN;
-		for (int j = 0; j < nN; ++j) ptgt[j] += aptr[j];
-	}
-	else
-	{
-		const int nN = m_nN;
-		for (int j = 0; j < nN; ++j) ptgt[j] -= aptr[j];
-	}
-}
-
-void btLCP::pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q)
-{
-	const int nC = m_nC;
-	for (int i = 0; i < nC; ++i)
-	{
-		p[i] += s * q[i];
-	}
-}
-
-void btLCP::pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q)
-{
-	const int nC = m_nC;
-	btScalar *ptgt = p + nC, *qsrc = q + nC;
-	const int nN = m_nN;
-	for (int i = 0; i < nN; ++i)
-	{
-		ptgt[i] += s * qsrc[i];
-	}
-}
-
-void btLCP::solve1(btScalar *a, int i, int dir, int only_transfer)
-{
-	// the `Dell' and `ell' that are computed here are saved. if index i is
-	// later added to the factorization then they can be reused.
-	//
-	// @@@ question: do we need to solve for entire delta_x??? yes, but
-	//     only if an x goes below 0 during the step.
-
-	if (m_nC > 0)
-	{
-		{
-			btScalar *Dell = m_Dell;
-			int *C = m_C;
-			btScalar *aptr = BTAROW(i);
-#ifdef BTNUB_OPTIMIZATIONS
-			// if nub>0, initial part of aptr[] is guaranteed unpermuted
-			const int nub = m_nub;
-			int j = 0;
-			for (; j < nub; ++j) Dell[j] = aptr[j];
-			const int nC = m_nC;
-			for (; j < nC; ++j) Dell[j] = aptr[C[j]];
-#else
-			const int nC = m_nC;
-			for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]];
-#endif
-		}
-		btSolveL1(m_L, m_Dell, m_nC, m_nskip);
-		{
-			btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
-			const int nC = m_nC;
-			for (int j = 0; j < nC; ++j) ell[j] = Dell[j] * d[j];
-		}
-
-		if (!only_transfer)
-		{
-			btScalar *tmp = m_tmp, *ell = m_ell;
-			{
-				const int nC = m_nC;
-				for (int j = 0; j < nC; ++j) tmp[j] = ell[j];
-			}
-			btSolveL1T(m_L, tmp, m_nC, m_nskip);
-			if (dir > 0)
-			{
-				int *C = m_C;
-				btScalar *tmp = m_tmp;
-				const int nC = m_nC;
-				for (int j = 0; j < nC; ++j) a[C[j]] = -tmp[j];
-			}
-			else
-			{
-				int *C = m_C;
-				btScalar *tmp = m_tmp;
-				const int nC = m_nC;
-				for (int j = 0; j < nC; ++j) a[C[j]] = tmp[j];
-			}
-		}
-	}
-}
-
-void btLCP::unpermute()
-{
-	// now we have to un-permute x and w
-	{
-		memcpy(m_tmp, m_x, m_n * sizeof(btScalar));
-		btScalar *x = m_x, *tmp = m_tmp;
-		const int *p = m_p;
-		const int n = m_n;
-		for (int j = 0; j < n; ++j) x[p[j]] = tmp[j];
-	}
-	{
-		memcpy(m_tmp, m_w, m_n * sizeof(btScalar));
-		btScalar *w = m_w, *tmp = m_tmp;
-		const int *p = m_p;
-		const int n = m_n;
-		for (int j = 0; j < n; ++j) w[p[j]] = tmp[j];
-	}
-}
-
-#endif  // btLCP_FAST
-
-//***************************************************************************
-// an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
-
-bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b,
-					   btScalar *outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratchMem)
-{
-	s_error = false;
-
-	//	printf("btSolveDantzigLCP n=%d\n",n);
-	btAssert(n > 0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
-	btAssert(outer_w);
-
-#ifdef BT_DEBUG
-	{
-		// check restrictions on lo and hi
-		for (int k = 0; k < n; ++k)
-			btAssert(lo[k] <= 0 && hi[k] >= 0);
-	}
-#endif
-
-	// if all the variables are unbounded then we can just factor, solve,
-	// and return
-	if (nub >= n)
-	{
-		int nskip = (n);
-		btFactorLDLT(A, outer_w, n, nskip);
-		btSolveLDLT(A, outer_w, b, n, nskip);
-		memcpy(x, b, n * sizeof(btScalar));
-
-		return !s_error;
-	}
-
-	const int nskip = (n);
-	scratchMem.L.resize(n * nskip);
-
-	scratchMem.d.resize(n);
-
-	btScalar *w = outer_w;
-	scratchMem.delta_w.resize(n);
-	scratchMem.delta_x.resize(n);
-	scratchMem.Dell.resize(n);
-	scratchMem.ell.resize(n);
-	scratchMem.Arows.resize(n);
-	scratchMem.p.resize(n);
-	scratchMem.C.resize(n);
-
-	// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
-	scratchMem.state.resize(n);
-
-	// create LCP object. note that tmp is set to delta_w to save space, this
-	// optimization relies on knowledge of how tmp is used, so be careful!
-	btLCP lcp(n, nskip, nub, A, x, b, w, lo, hi, &scratchMem.L[0], &scratchMem.d[0], &scratchMem.Dell[0], &scratchMem.ell[0], &scratchMem.delta_w[0], &scratchMem.state[0], findex, &scratchMem.p[0], &scratchMem.C[0], &scratchMem.Arows[0]);
-	int adj_nub = lcp.getNub();
-
-	// loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
-	// LCP conditions then i is added to the appropriate index set. otherwise
-	// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
-	// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
-	// while driving x(i) we maintain the LCP conditions on the other variables
-	// 0..i-1. we do this by watching out for other x(i),w(i) values going
-	// outside the valid region, and then switching them between index sets
-	// when that happens.
-
-	bool hit_first_friction_index = false;
-	for (int i = adj_nub; i < n; ++i)
-	{
-		s_error = false;
-		// the index i is the driving index and indexes i+1..n-1 are "dont care",
-		// i.e. when we make changes to the system those x's will be zero and we
-		// don't care what happens to those w's. in other words, we only consider
-		// an (i+1)*(i+1) sub-problem of A*x=b+w.
-
-		// if we've hit the first friction index, we have to compute the lo and
-		// hi values based on the values of x already computed. we have been
-		// permuting the indexes, so the values stored in the findex vector are
-		// no longer valid. thus we have to temporarily unpermute the x vector.
-		// for the purposes of this computation, 0*infinity = 0 ... so if the
-		// contact constraint's normal force is 0, there should be no tangential
-		// force applied.
-
-		if (!hit_first_friction_index && findex && findex[i] >= 0)
-		{
-			// un-permute x into delta_w, which is not being used at the moment
-			for (int j = 0; j < n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j];
-
-			// set lo and hi values
-			for (int k = i; k < n; ++k)
-			{
-				btScalar wfk = scratchMem.delta_w[findex[k]];
-				if (wfk == 0)
-				{
-					hi[k] = 0;
-					lo[k] = 0;
-				}
-				else
-				{
-					hi[k] = btFabs(hi[k] * wfk);
-					lo[k] = -hi[k];
-				}
-			}
-			hit_first_friction_index = true;
-		}
-
-		// thus far we have not even been computing the w values for indexes
-		// greater than i, so compute w[i] now.
-		w[i] = lcp.AiC_times_qC(i, x) + lcp.AiN_times_qN(i, x) - b[i];
-
-		// if lo=hi=0 (which can happen for tangential friction when normals are
-		// 0) then the index will be assigned to set N with some state. however,
-		// set C's line has zero size, so the index will always remain in set N.
-		// with the "normal" switching logic, if w changed sign then the index
-		// would have to switch to set C and then back to set N with an inverted
-		// state. this is pointless, and also computationally expensive. to
-		// prevent this from happening, we use the rule that indexes with lo=hi=0
-		// will never be checked for set changes. this means that the state for
-		// these indexes may be incorrect, but that doesn't matter.
-
-		// see if x(i),w(i) is in a valid region
-		if (lo[i] == 0 && w[i] >= 0)
-		{
-			lcp.transfer_i_to_N(i);
-			scratchMem.state[i] = false;
-		}
-		else if (hi[i] == 0 && w[i] <= 0)
-		{
-			lcp.transfer_i_to_N(i);
-			scratchMem.state[i] = true;
-		}
-		else if (w[i] == 0)
-		{
-			// this is a degenerate case. by the time we get to this test we know
-			// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
-			// and similarly that hi > 0. this means that the line segment
-			// corresponding to set C is at least finite in extent, and we are on it.
-			// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
-			lcp.solve1(&scratchMem.delta_x[0], i, 0, 1);
-
-			lcp.transfer_i_to_C(i);
-		}
-		else
-		{
-			// we must push x(i) and w(i)
-			for (;;)
-			{
-				int dir;
-				btScalar dirf;
-				// find direction to push on x(i)
-				if (w[i] <= 0)
-				{
-					dir = 1;
-					dirf = btScalar(1.0);
-				}
-				else
-				{
-					dir = -1;
-					dirf = btScalar(-1.0);
-				}
-
-				// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
-				lcp.solve1(&scratchMem.delta_x[0], i, dir);
-
-				// note that delta_x[i] = dirf, but we wont bother to set it
-
-				// compute: delta_w = A*delta_x ... note we only care about
-				// delta_w(N) and delta_w(i), the rest is ignored
-				lcp.pN_equals_ANC_times_qC(&scratchMem.delta_w[0], &scratchMem.delta_x[0]);
-				lcp.pN_plusequals_ANi(&scratchMem.delta_w[0], i, dir);
-				scratchMem.delta_w[i] = lcp.AiC_times_qC(i, &scratchMem.delta_x[0]) + lcp.Aii(i) * dirf;
-
-				// find largest step we can take (size=s), either to drive x(i),w(i)
-				// to the valid LCP region or to drive an already-valid variable
-				// outside the valid region.
-
-				int cmd = 1;  // index switching command
-				int si = 0;   // si = index to switch if cmd>3
-				btScalar s = -w[i] / scratchMem.delta_w[i];
-				if (dir > 0)
-				{
-					if (hi[i] < BT_INFINITY)
-					{
-						btScalar s2 = (hi[i] - x[i]) * dirf;  // was (hi[i]-x[i])/dirf	// step to x(i)=hi(i)
-						if (s2 < s)
-						{
-							s = s2;
-							cmd = 3;
-						}
-					}
-				}
-				else
-				{
-					if (lo[i] > -BT_INFINITY)
-					{
-						btScalar s2 = (lo[i] - x[i]) * dirf;  // was (lo[i]-x[i])/dirf	// step to x(i)=lo(i)
-						if (s2 < s)
-						{
-							s = s2;
-							cmd = 2;
-						}
-					}
-				}
-
-				{
-					const int numN = lcp.numN();
-					for (int k = 0; k < numN; ++k)
-					{
-						const int indexN_k = lcp.indexN(k);
-						if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0)
-						{
-							// don't bother checking if lo=hi=0
-							if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
-							btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k];
-							if (s2 < s)
-							{
-								s = s2;
-								cmd = 4;
-								si = indexN_k;
-							}
-						}
-					}
-				}
-
-				{
-					const int numC = lcp.numC();
-					for (int k = adj_nub; k < numC; ++k)
-					{
-						const int indexC_k = lcp.indexC(k);
-						if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY)
-						{
-							btScalar s2 = (lo[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k];
-							if (s2 < s)
-							{
-								s = s2;
-								cmd = 5;
-								si = indexC_k;
-							}
-						}
-						if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY)
-						{
-							btScalar s2 = (hi[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k];
-							if (s2 < s)
-							{
-								s = s2;
-								cmd = 6;
-								si = indexC_k;
-							}
-						}
-					}
-				}
-
-				//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
-				//			     "C->NL","C->NH"};
-				//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
-
-				// if s <= 0 then we've got a problem. if we just keep going then
-				// we're going to get stuck in an infinite loop. instead, just cross
-				// our fingers and exit with the current solution.
-				if (s <= btScalar(0.0))
-				{
-					//          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
-					if (i < n)
-					{
-						btSetZero(x + i, n - i);
-						btSetZero(w + i, n - i);
-					}
-					s_error = true;
-					break;
-				}
-
-				// apply x = x + s * delta_x
-				lcp.pC_plusequals_s_times_qC(x, s, &scratchMem.delta_x[0]);
-				x[i] += s * dirf;
-
-				// apply w = w + s * delta_w
-				lcp.pN_plusequals_s_times_qN(w, s, &scratchMem.delta_w[0]);
-				w[i] += s * scratchMem.delta_w[i];
-
-				//        void *tmpbuf;
-				// switch indexes between sets if necessary
-				switch (cmd)
-				{
-					case 1:  // done
-						w[i] = 0;
-						lcp.transfer_i_to_C(i);
-						break;
-					case 2:  // done
-						x[i] = lo[i];
-						scratchMem.state[i] = false;
-						lcp.transfer_i_to_N(i);
-						break;
-					case 3:  // done
-						x[i] = hi[i];
-						scratchMem.state[i] = true;
-						lcp.transfer_i_to_N(i);
-						break;
-					case 4:  // keep going
-						w[si] = 0;
-						lcp.transfer_i_from_N_to_C(si);
-						break;
-					case 5:  // keep going
-						x[si] = lo[si];
-						scratchMem.state[si] = false;
-						lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch);
-						break;
-					case 6:  // keep going
-						x[si] = hi[si];
-						scratchMem.state[si] = true;
-						lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch);
-						break;
-				}
-
-				if (cmd <= 3) break;
-			}  // for (;;)
-		}      // else
-
-		if (s_error)
-		{
-			break;
-		}
-	}  // for (int i=adj_nub; i<n; ++i)
-
-	lcp.unpermute();
-
-	return !s_error;
-}
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigLCP.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigLCP.h
deleted file mode 100644
index 8d9b2a13e9..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigLCP.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*************************************************************************
- *                                                                       *
- * 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_
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigSolver.h
deleted file mode 100644
index 1f669751ce..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btDantzigSolver.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
deleted file mode 100644
index 954ffaed75..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
+++ /dev/null
@@ -1,369 +0,0 @@
-/* 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/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
deleted file mode 100644
index 3c6bf72a23..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/* 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/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h
deleted file mode 100644
index f18c4ea41b..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h
+++ /dev/null
@@ -1,338 +0,0 @@
-/*
-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/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
deleted file mode 100644
index ed4e0b686d..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
+++ /dev/null
@@ -1,620 +0,0 @@
-/*
-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;
-	//Minv.printMatrix("Minv=");
-	{
-		{
-			BT_PROFILE("J*Minv");
-			tmp = J * Minv;
-		}
-		{
-			BT_PROFILE("J*tmp");
-			m_A = tmp * J_transpose;
-		}
-	}
-	//J.printMatrix("J");
-	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;
-}
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.h
deleted file mode 100644
index 510ae59e58..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
deleted file mode 100644
index 6b0465b88d..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h
deleted file mode 100644
index 7f8eec3f6e..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
-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
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
deleted file mode 100644
index c3f4ec3997..0000000000
--- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
-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++)
-					{
-						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