summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h')
-rw-r--r--thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h350
1 files changed, 0 insertions, 350 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
deleted file mode 100644
index 98484c3796..0000000000
--- a/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
+++ /dev/null
@@ -1,350 +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
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-18 16:39:10 +0000