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

added the btNNCGConstraintSolver, based on the paper "Nonsmooth Nonlinear Conjugate Gradient Method for interactive

Browse Source 
contact force problems". The solver needs a lot of iterations, before the quality goes up (~ 1000)
Thanks to Gabor PUHR for the contribution!
Improved the btLemkeSolver.
Remove the sparse optimizations from the btMatrixX.h, replace it with explicit call to rowComputeNonZeroElements (only used in the btSolveProjectedGaussSeidel), it was likely slowing things down, without being useful.
Re-enable SIMD in the solver (was accidently disabled in Bullet 2.82 release)
This commit is contained in:
erwin.coumans@gmail.com
2013-10-31 06:17:08 +00:00
parent 6ca948e22f
commit 644d01d231
11 changed files with 794 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files

164
src/LinearMath/btMatrixX.h
View File

@@ -113,10 +113,13 @@ struct btVectorX
}
void setZero()
{
// for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
btSetZero(&m_storage[0],m_storage.size());
if (m_storage.size())
{
// for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
btSetZero(&m_storage[0],m_storage.size());
}
}
const T& operator[] (int index) const
{
@@ -158,8 +161,7 @@ struct btMatrixX
int m_setElemOperations;
btAlignedObjectArray<T> m_storage;
btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
T* getBufferPointerWritable()
{
@@ -196,7 +198,6 @@ struct btMatrixX
BT_PROFILE("m_storage.resize");
m_storage.resize(rows*cols);
}
clearSparseInfo();
}
int cols() const
{
@@ -231,14 +232,6 @@ struct btMatrixX
void setElem(int row,int col, T val)
{
m_setElemOperations++;
if (val)
{
if (m_storage[col+row*m_cols]==0.f)
{
m_rowNonZeroElements1[row].push_back(col);
m_colNonZeroElements[col].push_back(row);
}
}
m_storage[row*m_cols+col] = val;
}
@@ -256,11 +249,10 @@ struct btMatrixX
void copyLowerToUpperTriangle()
{
int count=0;
for (int row=0;row<m_rowNonZeroElements1.size();row++)
for (int row=0;row<rows();row++)
{
for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
for (int col=0;col<row;col++)
{
int col = m_rowNonZeroElements1[row][j];
setElem(col,row, (*this)(row,col));
count++;
@@ -274,16 +266,6 @@ struct btMatrixX
return m_storage[col+row*m_cols];
}
void clearSparseInfo()
{
BT_PROFILE("clearSparseInfo=0");
m_rowNonZeroElements1.resize(m_rows);
m_colNonZeroElements.resize(m_cols);
for (int i=0;i<m_rows;i++)
m_rowNonZeroElements1[i].resize(0);
for (int j=0;j<m_cols;j++)
m_colNonZeroElements[j].resize(0);
}
void setZero()
{
@@ -294,10 +276,6 @@ struct btMatrixX
//for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
}
{
BT_PROFILE("clearSparseInfo=0");
clearSparseInfo();
}
}
void setIdentity()
@@ -327,28 +305,9 @@ struct btMatrixX
printf("\n---------------------\n");
}
void printNumZeros(const char* msg)
{
printf("%s: ",msg);
int numZeros = 0;
for (int i=0;i<m_storage.size();i++)
if (m_storage[i]==0)
numZeros++;
int total = m_cols*m_rows;
int computedNonZero = total-numZeros;
int nonZero = 0;
for (int i=0;i<m_colNonZeroElements.size();i++)
nonZero += m_colNonZeroElements[i].size();
btAssert(computedNonZero==nonZero);
if(computedNonZero!=nonZero)
{
printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
}
//printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
}
/*
void rowComputeNonZeroElements()
void rowComputeNonZeroElements() const
{
m_rowNonZeroElements1.resize(rows());
for (int i=0;i<rows();i++)
@@ -363,13 +322,11 @@ struct btMatrixX
}
}
}
*/
btMatrixX transpose() const
{
//transpose is optimized for sparse matrices
btMatrixX tr(m_cols,m_rows);
tr.setZero();
#if 0
for (int i=0;i<m_cols;i++)
for (int j=0;j<m_rows;j++)
{
@@ -379,37 +336,13 @@ struct btMatrixX
tr.setElem(i,j,v);
}
}
#else
for (int i=0;i<m_colNonZeroElements.size();i++)
for (int h=0;h<m_colNonZeroElements[i].size();h++)
{
int j = m_colNonZeroElements[i][h];
T v = (*this)(j,i);
tr.setElem(i,j,v);
}
#endif
return tr;
}
void sortRowIndexArrays()
{
for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
{
m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
}
}
void sortColIndexArrays()
{
for (int i=0;i<m_colNonZeroElements[i].size();i++)
{
m_colNonZeroElements[i].quickSort(btIntSortPredicate());
}
}
btMatrixX operator*(const btMatrixX& other)
{
//btMatrixX*btMatrixX implementation, optimized for sparse matrices
//btMatrixX*btMatrixX implementation, brute force
btAssert(cols() == other.rows());
btMatrixX res(rows(),other.cols());
@@ -417,76 +350,18 @@ struct btMatrixX
// BT_PROFILE("btMatrixX mul");
for (int j=0; j < res.cols(); ++j)
{
//int numZero=other.m_colNonZeroElements[j].size();
//if (numZero)
{
for (int i=0; i < res.rows(); ++i)
//for (int g = 0;g<m_colNonZeroElements[j].size();g++)
{
T dotProd=0;
T dotProd2=0;
int waste=0,waste2=0;
bool doubleWalk = false;
if (doubleWalk)
{
int numRows = m_rowNonZeroElements1[i].size();
int numOtherCols = other.m_colNonZeroElements[j].size();
for (int ii=0;ii<numRows;ii++)
{
int vThis=m_rowNonZeroElements1[i][ii];
}
for (int ii=0;ii<numOtherCols;ii++)
{
int vOther = other.m_colNonZeroElements[j][ii];
}
int indexRow = 0;
int indexOtherCol = 0;
while (indexRow < numRows && indexOtherCol < numOtherCols)
{
int vThis=m_rowNonZeroElements1[i][indexRow];
int vOther = other.m_colNonZeroElements[j][indexOtherCol];
if (vOther==vThis)
{
dotProd += (*this)(i,vThis) * other(vThis,j);
}
if (vThis<vOther)
{
indexRow++;
} else
{
indexOtherCol++;
}
}
} else
{
bool useOtherCol = true;
if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
{
useOtherCol=true;
}
if (!useOtherCol )
{
for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
for (int v=0;v<rows();v++)
{
int v = other.m_colNonZeroElements[j][q];
T w = (*this)(i,v);
if (w!=0.f)
{
dotProd+=w*other(v,j);
}
}
}
else
{
for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
{
int v=m_rowNonZeroElements1[i][q];
T w = (*this)(i,v);
if (other(v,j)!=0.f)
{
@@ -593,10 +468,9 @@ struct btMatrixX
btMatrixX negative()
{
btMatrixX neg(rows(),cols());
for (int i=0;i<m_colNonZeroElements.size();i++)
for (int h=0;h<m_colNonZeroElements[i].size();h++)
for (int i=0;i<rows();i++)
for (int j=0;j<cols();j++)
{
int j = m_colNonZeroElements[i][h];
T v = (*this)(i,j);
neg.setElem(i,j,-v);
}
@@ -625,7 +499,7 @@ std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
{
for (int j=0;j<mat.cols();j++)
{
os << std::setw(10) << mat(i,j);
os << std::setw(12) << mat(i,j);
}
if (i!=mat.rows()-1)
os << std::endl << " ";
@@ -643,7 +517,7 @@ std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
//printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++)
{
os << std::setw(10) << mat[i];
os << std::setw(12) << mat[i];
if (i!=mat.rows()-1)
os << std::endl << " ";
}