remove a lot of warnings (more todo in demos and serialization code)
This commit is contained in:
Erwin Coumans
@@ -821,14 +821,15 @@ 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,lskip3,i,j;
|
||||
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;
|
||||
//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 */
|
||||
|
||||
@@ -1,112 +1,112 @@
|
||||
/*
|
||||
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
|
||||
/*
|
||||
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
|
||||
|
||||
@@ -1,350 +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<6B>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
|
||||
/*
|
||||
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<6B>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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,93 +1,93 @@
|
||||
/*
|
||||
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;
|
||||
btScalar m_cfm;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
btScalar getCfm() const
|
||||
{
|
||||
return m_cfm;
|
||||
}
|
||||
void setCfm(btScalar cfm)
|
||||
{
|
||||
m_cfm = cfm;
|
||||
}
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const
|
||||
{
|
||||
return BT_MLCP_SOLVER;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_MLCP_SOLVER_H
|
||||
/*
|
||||
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;
|
||||
btScalar m_cfm;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
btScalar getCfm() const
|
||||
{
|
||||
return m_cfm;
|
||||
}
|
||||
void setCfm(btScalar cfm)
|
||||
{
|
||||
m_cfm = cfm;
|
||||
}
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const
|
||||
{
|
||||
return BT_MLCP_SOLVER;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_MLCP_SOLVER_H
|
||||
|
||||
@@ -1,33 +1,33 @@
|
||||
/*
|
||||
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
|
||||
/*
|
||||
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
|
||||
|
||||
@@ -1,151 +1,151 @@
|
||||
/*
|
||||
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
|
||||
/*
|
||||
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
|
||||
|
||||
@@ -1,86 +1,86 @@
|
||||
/*
|
||||
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:
|
||||
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();
|
||||
|
||||
float delta;
|
||||
|
||||
for (int k = 0; k <numIterations; k++)
|
||||
{
|
||||
for (i = 0; i <numRows; i++)
|
||||
{
|
||||
delta = 0.0f;
|
||||
if (useSparsity)
|
||||
{
|
||||
for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
|
||||
{
|
||||
int 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];
|
||||
}
|
||||
|
||||
float aDiag = A(i,i);
|
||||
x [i] = (b [i] - delta) / A(i,i);
|
||||
float 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;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
|
||||
/*
|
||||
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:
|
||||
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();
|
||||
|
||||
float delta;
|
||||
|
||||
for (int k = 0; k <numIterations; k++)
|
||||
{
|
||||
for (i = 0; i <numRows; i++)
|
||||
{
|
||||
delta = 0.0f;
|
||||
if (useSparsity)
|
||||
{
|
||||
for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
|
||||
{
|
||||
int 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];
|
||||
}
|
||||
|
||||
float aDiag = A(i,i);
|
||||
x [i] = (b [i] - delta) / aDiag;
|
||||
float 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;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
|
||||
|
||||
Reference in New Issue
Block a user