Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3496
src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
View File

File diff suppressed because it is too large Load Diff

12
src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
View File

@@ -41,7 +41,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
*/
#ifndef _BT_LCP_H_
#define _BT_LCP_H_
@@ -49,7 +48,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
#include <stdio.h>
#include <assert.h>
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -62,16 +60,14 @@ struct btDantzigScratchMemory
btAlignedObjectArray<btScalar> delta_x;
btAlignedObjectArray<btScalar> Dell;
btAlignedObjectArray<btScalar> ell;
btAlignedObjectArray<btScalar*> Arows;
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);
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_
#endif //_BT_LCP_H_

32
src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
View File

@@ -20,30 +20,28 @@ subject to the following restrictions:
#include "btMLCPSolverInterface.h"
#include "btDantzigLCP.h"
class btDantzigSolver : public btMLCPSolverInterface
{
protected:
btScalar m_acceptableUpperLimitSolution;
btAlignedObjectArray<char> m_tempBuffer;
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;
btAlignedObjectArray<int> m_dependencies;
btDantzigScratchMemory m_scratchMemory;
public:
public:
btDantzigSolver()
:m_acceptableUpperLimitSolution(btScalar(1000))
: 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)
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();
@@ -52,14 +50,12 @@ public:
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.resize(n * n);
for (int i = 0; i < n * n; i++)
{
m_A[i] = Aptr[i];
}
m_b.resize(n);
@@ -67,7 +63,7 @@ public:
m_lo.resize(n);
m_hi.resize(n);
m_dependencies.resize(n);
for (int i=0;i<n;i++)
for (int i = 0; i < n; i++)
{
m_lo[i] = lo[i];
m_hi[i] = hi[i];
@@ -76,13 +72,12 @@ public:
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);
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++)
// printf("numAllocas = %d\n",numAllocas);
for (int i = 0; i < n; i++)
{
volatile btScalar xx = m_x[i];
if (xx != m_x[i])
@@ -98,15 +93,14 @@ public:
}
}
for (int i=0;i<n;i++)
for (int i = 0; i < n; i++)
{
x[i] = m_x[i];
}
}
return result;
}
};
#endif //BT_DANTZIG_SOLVER_H
#endif //BT_DANTZIG_SOLVER_H

468
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
View File

@@ -19,64 +19,60 @@ subject to the following restrictions:
//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
#endif //BT_DEBUG_OSTREAM
btScalar btMachEps()
{
static bool calculated=false;
static bool calculated = false;
static btScalar machEps = btScalar(1.);
if (!calculated)
{
do {
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;
} while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
// printf( "\nCalculated Machine epsilon: %G\n", machEps );
calculated = true;
}
return machEps;
}
btScalar btEpsRoot() {
btScalar btEpsRoot()
{
static btScalar epsroot = 0.;
static bool alreadyCalculated = false;
if (!alreadyCalculated) {
if (!alreadyCalculated)
{
epsroot = btSqrt(btMachEps());
alreadyCalculated = true;
}
return epsroot;
}
btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
{
steps = 0;
steps = 0;
int dim = m_q.size();
int dim = m_q.size();
#ifdef BT_DEBUG_OSTREAM
if(DEBUGLEVEL >= 1) {
cout << "Dimension = " << dim << endl;
}
#endif //BT_DEBUG_OSTREAM
if (DEBUGLEVEL >= 1)
{
cout << "Dimension = " << dim << endl;
}
#endif //BT_DEBUG_OSTREAM
btVectorXu solutionVector(2 * dim);
solutionVector.setZero();
//, INIT, 0.);
//, INIT, 0.);
btMatrixXu ident(dim, dim);
ident.setIdentity();
@@ -85,287 +81,289 @@ btScalar btEpsRoot() {
#endif
btMatrixXu mNeg = m_M.negative();
btMatrixXu A(dim, 2 * dim + 2);
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, 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);
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
#endif //BT_DEBUG_OSTREAM
// btVectorXu q_;
// q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
// 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);
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++)
for (int i = 0; i < dim; i++)
{
btScalar v =A(i,2*dim+1);
if (v<minValue)
btScalar v = A(i, 2 * dim + 1);
if (v < minValue)
{
minValue=v;
minValue = v;
pivotRowIndex = i;
}
if (v<0)
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
// 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)
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
// 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...
}
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);
/*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
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;
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;
/*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;
/*the column becomes part of the basis*/
basis[pivotRowIndex] = pivotColIndexOld;
pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
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;
}
}
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 (DEBUGLEVEL >= 1)
{
cout << "Number of loops: " << steps << endl;
cout << "Number of maximal loops: " << maxloops << endl;
}
#endif //BT_DEBUG_OSTREAM
if(!validBasis(basis)) {
info = -1;
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
if (DEBUGLEVEL >= 1)
cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
#endif //BT_DEBUG_OSTREAM
return solutionVector;
}
}
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++)
if (DEBUGLEVEL >= 2)
{
solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
// 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;
info = 0;
return solutionVector;
}
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;
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;
// if (DEBUGLEVEL) {
// cout << "Rows(" << row << ") = " << Rows[row] << endl;
// }
#endif
}
}
#endif
}
}
for (int i = 0; i < Rows.size(); i++)
{
if (Rows[i].nrm2() > 0.) {
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];
}
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;
}
}
}
//=Rows[j] - Rows[i]
if (! LexicographicPositive(test))
break;
}
}
}
if (j == Rows.size())
{
RowIndex += i;
break;
}
}
}
if (j == Rows.size())
{
RowIndex += i;
break;
}
}
}
return RowIndex;
}
return RowIndex;
}
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
{
int i = 0;
// if (DEBUGLEVEL)
// cout << "v " << v << endl;
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;
while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
i++;
if (v[i] > 0)
return true;
return false;
}
return false;
}
void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
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++)
for (int i = 0; i < A.rows(); i++)
{
if (i != pivotRowIndex)
{
for (int j = 0; j < A.cols(); j++)
if (i != pivotRowIndex)
{
if (j != pivotColumnIndex)
{
btScalar v = A(i, j);
v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
A.setElem(i, j, v);
}
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++)
#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);
}
A.mulElem(pivotRowIndex, i, -a);
}
#ifdef BT_DEBUG_OSTREAM
cout << A << std::endl;
#endif //#ifdef BT_DEBUG_OSTREAM
}
#endif //#ifdef BT_DEBUG_OSTREAM
bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
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;
}
}
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;
}
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;
}

82
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
View File

@@ -19,90 +19,84 @@ subject to the following restrictions:
//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
#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_);
}
btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
DEBUGLEVEL(DEBUGLEVEL_)
{
setSystem(M_, q_);
}
/* GETTER / SETTER */
/**
/* GETTER / SETTER */
/**
* \brief return info of solution process
*/
int getInfo() {
return info;
}
int getInfo()
{
return info;
}
/**
/**
* \brief get the number of steps until the solution was found
*/
int getSteps(void) {
return steps;
}
int getSteps(void)
{
return steps;
}
/**
/**
* \brief set system with Matrix M and vector q
*/
void setSystem(const btMatrixXu & M_, const btVectorXu & 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);
btVectorXu solve(unsigned int maxloops = 0);
virtual ~btLemkeAlgorithm() {
}
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);
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;
btMatrixXu m_M;
btVectorXu m_q;
/**
/**
* \brief number of steps until the Lemke algorithm found a solution
*/
unsigned int steps;
unsigned int steps;
/**
/**
* \brief define level of debug output
*/
int DEBUGLEVEL;
int DEBUGLEVEL;
/**
/**
* \brief did the algorithm find a solution
*
* -1 : not successful
* 0 : successful
*/
int info;
int info;
};
#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */

504
src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
View File

@@ -17,334 +17,322 @@ subject to the following restrictions:
#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;
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)
: 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)
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;
BT_PROFILE("btLemkeSolver::solveMLCP");
int n = A.rows();
if (0==n)
return true;
bool fail = false;
bool fail = false;
btVectorXu solution(n);
btVectorXu q1;
q1.resize(n);
for (int row=0;row<n;row++)
{
q1[row] = -b[row];
}
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;
// cout << "A" << endl;
// cout << A << endl;
/////////////////////////////////////
//slow matrix inversion, replace with LU decomposition
btMatrixXu A1;
btMatrixXu B(n,n);
btMatrixXu B(n, n);
{
BT_PROFILE("inverse(slow)");
A1.resize(A.rows(),A.cols());
for (int row=0;row<A.rows();row++)
A1.resize(A.rows(), A.cols());
for (int row = 0; row < A.rows(); row++)
{
for (int col=0;col<A.cols();col++)
for (int col = 0; col < A.cols(); col++)
{
A1.setElem(row,col,A(row,col));
A1.setElem(row, col, A(row, col));
}
}
btMatrixXu matrix;
matrix.resize(n,2*n);
for (int row=0;row<n;row++)
matrix.resize(n, 2 * n);
for (int row = 0; row < n; row++)
{
for (int col=0;col<n;col++)
for (int col = 0; col < n; col++)
{
matrix.setElem(row,col,A1(row,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 ratio, a;
int i, j, k;
for (i = 0; i < n; i++)
{
for (j = n; j < 2 * n; j++)
{
btScalar v = matrix(i,i);
if (btFuzzyZero(v))
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)
{
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));
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))
for (i = 0; i < n; i++)
{
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++)
a = matrix(i, i);
if (btFuzzyZero(a))
{
B.setElem(row,col,matrix(row,n+col));
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 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++)
btMatrixXu M(n * 2, n * 2);
for (int row = 0; row < n; row++)
{
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)
b1.setElem(row, 0, -b[row]);
for (int col = 0; col < n; col++)
{
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]);
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);
}
}
}
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++)
btMatrixXu Bb1 = B * b1;
// q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
btVectorXu qq;
qq.resize(n * 2);
for (int row = 0; row < n; row++)
{
x[i]=0.f;
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;
}
return !fail;
} else
{
else
{
int dimension = A.rows();
if (0==dimension)
return true;
// printf("================ solving using Lemke/Newton/Fixpoint\n");
if (0 == dimension)
return true;
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)
// printf("================ solving using Lemke/Newton/Fixpoint\n");
btVectorXu q;
q.resize(dimension);
for (int row = 0; row < dimension; row++)
{
x.setZero();
return false;
q[row] = -b[row];
}
//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)
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++)
{
if (x[i]> errorValueMax)
x[i] = solution[i + dimension];
volatile btScalar check = x[i];
if (x[i] != check)
{
fail = true;
errorIndexMax = i;
errorValueMax = x[i];
x.setZero();
return false;
}
////printf("x[i] = %f,",x[i]);
}
if (x[i]<-m_maxValue)
{
if (x[i]<errorValueMin)
//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)
{
errorIndexMin = i;
errorValueMin = x[i];
fail = true;
//printf("x[i] = %f,",x[i]);
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++)
if (fail)
{
x[i]=0.f;
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 !fail;
return true;
}
return true;
}
};
#endif //BT_LEMKE_SOLVER_H
#endif //BT_LEMKE_SOLVER_H

395
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
View File

@@ -19,10 +19,9 @@ subject to the following restrictions:
#include "LinearMath/btQuickprof.h"
#include "btSolveProjectedGaussSeidel.h"
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver),
m_fallback(0)
btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver)
: m_solver(solver),
m_fallback(0)
{
}
@@ -33,67 +32,65 @@ 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)
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);
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 numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
// int numBodies = m_tmpSolverBodyPool.size();
// 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());
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++)
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;
int firstContactConstraintOffset = dindex;
if (interleaveContactAndFriction)
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
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]);
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;
m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
}
}
} else
}
else
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
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++)
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;
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
}
}
if (!m_allConstraintPtrArray.size())
{
m_A.resize(0,0);
m_A.resize(0, 0);
m_b.resize(0);
m_x.resize(0);
m_lo.resize(0);
@@ -102,7 +99,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
}
}
if (gUseMatrixMultiply)
{
BT_PROFILE("createMLCP");
@@ -121,7 +117,7 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
bool result = true;
if (m_A.rows()==0)
if (m_A.rows() == 0)
return true;
//if using split impulse, we solve 2 separate (M)LCPs
@@ -129,28 +125,26 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
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 );
// 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(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 );
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 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;
@@ -163,36 +157,36 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
m_bSplit.resize(numConstraintRows);
m_b.setZero();
m_bSplit.setZero();
for (int i=0;i<numConstraintRows ;i++)
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;
m_b[i] = rhs / jacDiag;
m_bSplit[i] = rhsPenetration / jacDiag;
}
}
}
// btScalar* w = 0;
// int nub = 0;
// 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++)
for (int i = 0; i < numConstraintRows; i++)
{
if (0)//m_limitDependencies[i]>=0)
if (0) //m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
} else
}
else
{
m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
@@ -201,48 +195,48 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
}
//
int m=m_allConstraintPtrArray.size();
int m = m_allConstraintPtrArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies,-1);
bodyJointNodeArray.resize(numBodies, -1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
}
btMatrixXu& J3 = m_scratchJ3;
btMatrixXu& J3 = m_scratchJ3;
{
BT_PROFILE("J3.resize");
J3.resize(2*m,8);
J3.resize(2 * m, 8);
}
btMatrixXu& JinvM3 = m_scratchJInvM3;
btMatrixXu& JinvM3 = m_scratchJInvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2*m,8);
JinvM3.resize(2 * m, 8);
JinvM3.setZero();
J3.setZero();
}
int cur=0;
int cur = 0;
int rowOffset = 0;
btAlignedObjectArray<int>& ofs = m_scratchOfs;
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 c = 0;
int numRows = 0;
for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
@@ -250,14 +244,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
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 ;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
if (orgBodyA)
{
{
int slotA=-1;
int slotA = -1;
//find free jointNode slot for sbA
slotA =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
slotA = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
@@ -265,35 +259,35 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row=0;row<numRows;row++,cur++)
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();
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++)
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, 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);
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
} else
}
else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB=-1;
int slotB = -1;
//find free jointNode slot for sbA
slotB =jointNodeArray.size();
jointNodeArray.expand();//NonInitializing();
slotB = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
@@ -302,78 +296,74 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row=0;row<numRows;row++,cur++)
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();
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++)
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, 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);
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;
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);
m_A.resize(n, n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c=0;
int c = 0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
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;
// 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__;
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)
while (startJointNodeA >= 0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0<c)
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;
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]);
m_A.multiplyAdd2_p8r(JinvMrow,
Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
@@ -381,17 +371,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB>=0)
while (startJointNodeB >= 0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1<c)
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]);
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;
}
@@ -402,27 +392,25 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int row__ = 0;
int numJointRows = m_allConstraintPtrArray.size();
int jj=0;
for (;row__<numJointRows;)
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* 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 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)
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__);
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
}
row__ += infom;
jj++;
@@ -433,12 +421,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
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_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 ");
@@ -450,21 +438,21 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (int i=0;i<m_allConstraintPtrArray.size();i++)
for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse;
m_x[i] = c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
} else
}
else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
@@ -475,120 +463,116 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
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++)
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;
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;
m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv;
}
}
btMatrixXu& Minv = m_scratchMInv;
Minv.resize(6*numBodies,6*numBodies);
btMatrixXu& Minv = m_scratchMInv;
Minv.resize(6 * numBodies, 6 * numBodies);
Minv.setZero();
for (int i=0;i<numBodies;i++)
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]);
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);
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);
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++)
{
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]);
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]);
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;
btMatrixXu& J_transpose = m_scratchJTranspose;
J_transpose = J.transpose();
btMatrixXu& tmp = m_scratchTmp;
{
{
BT_PROFILE("J*Minv");
tmp = J*Minv;
tmp = J * Minv;
}
{
BT_PROFILE("J*tmp");
m_A = tmp*J_transpose;
m_A = tmp * J_transpose;
}
}
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
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_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();
// m_x.setZero();
for (int i=0;i<m_allConstraintPtrArray.size();i++)
for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i]=c.m_appliedImpulse;
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)
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());
// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
result = solveMLCP(infoGlobal);
}
@@ -596,44 +580,41 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
if (result)
{
BT_PROFILE("process MLCP results");
for (int i=0;i<m_allConstraintPtrArray.size();i++)
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;
// 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;
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);
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);
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);
// printf("m_fallback = %d\n",m_fallback);
m_fallback++;
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
}
return 0.f;
}

40
src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
View File

@@ -23,15 +23,13 @@ subject to the following restrictions:
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;
@@ -39,24 +37,23 @@ protected:
btVectorXu m_xSplit2;
btAlignedObjectArray<int> m_limitDependencies;
btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
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);
/// 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);
@@ -65,8 +62,7 @@ protected:
virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
public:
btMLCPSolver( btMLCPSolverInterface* solver);
btMLCPSolver(btMLCPSolverInterface* solver);
virtual ~btMLCPSolver();
void setMLCPSolver(btMLCPSolverInterface* solver)
@@ -83,12 +79,10 @@ public:
m_fallback = num;
}
virtual btConstraintSolverType getSolverType() const
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;
}
};
#endif //BT_MLCP_SOLVER_H
#endif //BT_MLCP_SOLVER_H

4
src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
View File

@@ -27,7 +27,7 @@ public:
}
//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;
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
#endif //BT_MLCP_SOLVER_INTERFACE_H

61
src/BulletDynamics/MLCPSolvers/btPATHSolver.h
View File

@@ -14,38 +14,35 @@ subject to the following restrictions:
*/
///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" {
extern "C"
{
#include "PATH/SimpleLCP.h"
#include "PATH/License.h"
#include "PATH/Error_Interface.h"
};
void __stdcall MyError(Void *data, Char *msg)
void __stdcall MyError(Void *data, Char *msg)
{
printf("Path Error: %s\n",msg);
printf("Path Error: %s\n", msg);
}
void __stdcall MyWarning(Void *data, Char *msg)
void __stdcall MyWarning(Void *data, Char *msg)
{
printf("Path Warning: %s\n",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");
@@ -55,17 +52,15 @@ public:
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)
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)
if (0 == numVariables)
return true;
/* - variables - the number of variables in the problem
/* - 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
@@ -78,16 +73,16 @@ public:
btAlignedObjectArray<int> rowIndices;
btAlignedObjectArray<int> colIndices;
for (int i=0;i<A.rows();i++)
for (int i = 0; i < A.rows(); i++)
{
for (int j=0;j<A.cols();j++)
for (int j = 0; j < A.cols(); j++)
{
if (A(i,j)!=0.f)
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));
rowIndices.push_back(i + 1);
colIndices.push_back(j + 1);
values.push_back(A(i, j));
}
}
}
@@ -97,19 +92,18 @@ public:
btAlignedObjectArray<double> rhs;
btAlignedObjectArray<double> upperBounds;
btAlignedObjectArray<double> lowerBounds;
for (int i=0;i<numVariables;i++)
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]);
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[] = {
static const char *gReturnMsgs[] = {
"Invalid return",
"MCP_Solved: The problem was solved",
"MCP_NoProgress: A stationary point was found",
@@ -122,16 +116,16 @@ public:
"MCP_Infeasible: Problem has no solution",
"MCP_Error: An error occurred within the code",
"MCP_LicenseError: License could not be found",
"MCP_OK"
};
"MCP_OK"};
printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
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
}
else
{
for (int i=0;i<numVariables;i++)
for (int i = 0; i < numVariables; i++)
{
x[i] = zResult[i];
//check for #NAN
@@ -139,13 +133,10 @@ public:
return false;
}
return true;
}
}
};
#endif //BT_USE_PATH
#endif //BT_USE_PATH
#endif //BT_PATH_SOLVER_H
#endif //BT_PATH_SOLVER_H

63
src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
View File

@@ -17,25 +17,22 @@ subject to the following restrictions:
#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)
: 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)
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;
@@ -46,65 +43,65 @@ public:
btAssert(A.rows() == b.rows());
int i, j, numRows = A.rows();
btScalar delta;
for (int k = 0; k <numIterations; k++)
for (int k = 0; k < numIterations; k++)
{
m_leastSquaresResidual = 0.f;
for (i = 0; i <numRows; i++)
for (i = 0; i < numRows; i++)
{
delta = 0.0f;
if (useSparsity)
{
for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
for (int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++)
{
int j = A.m_rowNonZeroElements1[i][h];
if (j != i)//skip main diagonal
if (j != i) //skip main diagonal
{
delta += A(i,j) * x[j];
delta += A(i, j) * x[j];
}
}
} else
}
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];
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 aDiag = A(i, i);
btScalar xOld = x[i];
x [i] = (b [i] - delta) / aDiag;
x[i] = (b[i] - delta) / aDiag;
btScalar s = 1.f;
if (limitDependency[i]>=0)
if (limitDependency[i] >= 0)
{
s = x[limitDependency[i]];
if (s<0)
s=1;
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;
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;
m_leastSquaresResidual += diff * diff;
}
btScalar eps = m_leastSquaresResidualThreshold;
if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
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);
printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual, k);
#endif
break;
}
}
return true;
}
};
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H