Differential D8762 Diff 28333 extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

Changeset View

Standalone View

extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

	Show All 11 Lines
	2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.			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.			3. This notice may not be removed or altered from any source distribution.
	*/			*/
	///original version written by Erwin Coumans, October 2013			///original version written by Erwin Coumans, October 2013

	#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H			#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
	#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H			#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H


	#include "btMLCPSolverInterface.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)			///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			class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
	{			{
	public:			public:
				btScalar m_leastSquaresResidualThreshold;
				btScalar m_leastSquaresResidual;

				btSolveProjectedGaussSeidel()
				: 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())			if (!A.rows())
	return true;			return true;
	//the A matrix is sparse, so compute the non-zero elements			//the A matrix is sparse, so compute the non-zero elements
	A.rowComputeNonZeroElements();			A.rowComputeNonZeroElements();

	//A is a m-n matrix, m rows, n columns			//A is a m-n matrix, m rows, n columns
	btAssert(A.rows() == b.rows());			btAssert(A.rows() == b.rows());

	int i, j, numRows = A.rows();			int i, j, numRows = A.rows();

	float delta;			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;			delta = 0.0f;
	if (useSparsity)			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];			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++)			for (j = 0; j < i; j++)
	delta += A(i,j) * x[j];			delta += A(i, j) * x[j];
	for (j = i+1; j<numRows; j++)			for (j = i + 1; j < numRows; j++)
	delta += A(i,j) * x[j];			delta += A(i, j) * x[j];
	}			}

	float 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;
	float s = 1.f;			btScalar s = 1.f;

	if (limitDependency[i]>=0)			if (limitDependency[i] >= 0)
	{			{
	s = x[limitDependency[i]];			s = x[limitDependency[i]];
	if (s<0)			if (s < 0)
	s=1;			s = 1;
	}			}

	if (x[i]<lo[i]*s)			if (x[i] < lo[i] * s)
	x[i]=lo[i]*s;			x[i] = lo[i] * s;
	if (x[i]>hi[i]*s)			if (x[i] > hi[i] * s)
	x[i]=hi[i]*s;			x[i] = hi[i] * s;
				btScalar diff = x[i] - xOld;
				m_leastSquaresResidual += diff * diff;
				}

				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);
				#endif
				break;
	}			}
	}			}
	return true;			return true;
	}			}

	};			};

	#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H			#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H