summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h')
-rw-r--r--thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h350
1 files changed, 350 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h
new file mode 100644
index 0000000000..98484c3796
--- /dev/null
+++ b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h
@@ -0,0 +1,350 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_LEMKE_SOLVER_H
+#define BT_LEMKE_SOLVER_H
+
+
+#include "btMLCPSolverInterface.h"
+#include "btLemkeAlgorithm.h"
+
+
+
+
+///The btLemkeSolver is based on "Fast Implementation of Lemke’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
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 23:49:33 +0000