Added Dantzig MLCP solver option from Open Dynamics Engine (trying to avoid naming/linking conflicts in case ODE and Bullet is used together)

If an MLCP solver fails, use PGS/SI fallback, add a boolean return value for 'solve' method

src/BulletDynamics/MLCPSolvers/btDantzigLCP.h

@@ -0,0 +1,61 @@
+/*************************************************************************
+ *                                                                       *
+ * 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"
+
+//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);
+
+
+
+#endif //_BT_LCP_H_

src/BulletDynamics/MLCPSolvers/btDantzigSolver.h

@@ -0,0 +1,103 @@
+/*
+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;
+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)
+		{
+			btScalar* AA = (btScalar*) A.getBufferPointer();
+			btScalar* bb = (btScalar* ) b.getBufferPointer();
+			btScalar* xx = (btScalar*) x.getBufferPointer();
+			btScalar* llo = (btScalar*) lo.getBufferPointer();
+			btScalar* hhi = (btScalar*) hi.getBufferPointer();
+			int* findex = (int*) &limitDependency[0];
+			int nub = 0;
+			btAlignedObjectArray<btScalar> ww;
+			ww.resize(n);
+		
+
+
+			const btScalar* Aptr = A.getBufferPointer();
+
+			for (int i=0;i<n*n;i++)
+			{
+				AA[i] = Aptr[i];
+			}
+			for (int i=0;i<n;i++)
+			{
+				llo[i] = lo[i];
+				hhi[i] = hi[i];
+				bb[i] = b[i];
+				xx[i] = x[i];
+			}
+
+			extern int numAllocas;
+			numAllocas = 0;
+
+			result = btSolveDantzigLCP (n,AA,xx,bb,&ww[0],nub,llo,hhi,findex);
+			
+//			printf("numAllocas = %d\n",numAllocas);
+			for (int i=0;i<n;i++)
+			{
+				x[i] = xx[i];
+
+				//test for NAN
+				if (x[i] != xx[i])
+				{
+					return false;
+				}
+				if (x[i] >= m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+
+				if (x[i] <= -m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+				
+			}
+			
+		}
+
+		return result;
+	}
+};
+
+#endif //BT_DANTZIG_SOLVER_H

src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -115,10 +115,9 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 	return 0.f;
 }
 
-void btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 {
-	 m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
-
+	 return m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
 }
 
 struct btJointNode
@@ -400,7 +399,8 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 			}
 		}
 	}
-				
+
+	///todo: use proper cfm values from the constraints (getInfo2)
 	if (1)
 	{
 		// add cfm to the diagonal of m_A
@@ -520,6 +520,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 
 	if (1)
 	{
+		///todo: use proper cfm values from the constraints (getInfo2)
 		// add cfm to the diagonal of m_A
 		for ( int i=0; i<m_A.rows(); ++i) 
 		{
@@ -543,13 +544,15 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 
 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());
-		solveMLCP(infoGlobal);
+		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_allConstraintArray.size();i++)
@@ -570,9 +573,11 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
 			}
 		}
 	}
-
-
-	//btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	else
+	{
+		printf("fallback to btSequentialImpulseConstraintSolver\n");
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	}
 
 	return 0.f;
 }

src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -43,7 +43,8 @@ protected:
 	virtual void createMLCP(const btContactSolverInfo& infoGlobal);
 	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
 
-	virtual void solveMLCP(const btContactSolverInfo& infoGlobal);
+	//return true is it solves the problem successfully
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
 
 public:
 

src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h

@@ -26,7 +26,8 @@ public:
 	{
 	}
 
-	virtual void 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;
+	//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

src/BulletDynamics/MLCPSolvers/btPATHSolver.h

@@ -56,14 +56,14 @@ public:
 	}
 
 
-	virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = 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)
 	{
 		MCP_Termination status;
 		
 
 		int numVariables = b.rows();
 		if (0==numVariables)
-			return;
+			return true;
 
 			/*	 - variables - the number of variables in the problem
 			- m_nnz - the number of nonzeros in the M matrix
@@ -126,14 +126,20 @@ public:
 			};
 
 			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
-			printf("using Projected Gauss Seidel instead\n");
-			//x = Solve_GaussSeidel(A,b,lo,hi,limitDependencies,infoGlobal.m_numIterations);
+			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;
+
 		}
 
 	}

src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -23,7 +23,7 @@ subject to the following restrictions:
 class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
 {
 public:
-	virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = 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)
 	{
 		//A is a m-n matrix, m rows, n columns
 		btAssert(A.rows() == b.rows());