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

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