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

Changeset View

Standalone View

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

	Show All 13 Lines
	*/			*/

	//The original version is here			//The original version is here
	//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc			//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
	//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)			//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)
	//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h			//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
	//STL/std::vector replaced by btAlignedObjectArray			//STL/std::vector replaced by btAlignedObjectArray



	#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_			#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
	#define BT_NUMERICS_LEMKE_ALGORITHM_H_			#define BT_NUMERICS_LEMKE_ALGORITHM_H_

	#include "LinearMath/btMatrixX.h"			#include "LinearMath/btMatrixX.h"


	#include <vector> //todo: replace by btAlignedObjectArray			#include <vector> //todo: replace by btAlignedObjectArray

	class btLemkeAlgorithm			class btLemkeAlgorithm
	{			{
	public:			public:
				btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int& DEBUGLEVEL_ = 0) : DEBUGLEVEL(DEBUGLEVEL_)

	btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
	DEBUGLEVEL(DEBUGLEVEL_)
	{			{
	setSystem(M_, q_);			setSystem(M_, q_);
	}			}

	/* GETTER / SETTER */			/* GETTER / SETTER */
	/**			/**
	* \brief return info of solution process			* \brief return info of solution process
	*/			*/
	int getInfo() {			int getInfo()
				{
	return info;			return info;
	}			}

	/**			/**
	* \brief get the number of steps until the solution was found			* \brief get the number of steps until the solution was found
	*/			*/
	int getSteps(void) {			int getSteps(void)
				{
	return steps;			return steps;
	}			}



	/**			/**
	* \brief set system with Matrix M and vector q			* \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_M = M_;
	m_q = q_;			m_q = q_;
	}			}
	/***************************************************/			/***************************************************/

	/**			/**
	* \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd)			* \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:			protected:
	int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex);			int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex);
	bool LexicographicPositive(const btVectorXu & v);			bool LexicographicPositive(const btVectorXu& v);
	void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);			void GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
	bool greaterZero(const btVectorXu & vector);			bool greaterZero(const btVectorXu& vector);
	bool validBasis(const btAlignedObjectArray<int>& basis);			bool validBasis(const btAlignedObjectArray<int>& basis);

	btMatrixXu m_M;			btMatrixXu m_M;
	btVectorXu m_q;			btVectorXu m_q;

	/**			/**
	* \brief number of steps until the Lemke algorithm found a solution			* \brief number of steps until the Lemke algorithm found a solution
	*/			*/
	unsigned int steps;			unsigned int steps;

	/**			/**
	* \brief define level of debug output			* \brief define level of debug output
	*/			*/
	int DEBUGLEVEL;			int DEBUGLEVEL;

	/**			/**
	* \brief did the algorithm find a solution			* \brief did the algorithm find a solution
	*			*
	* -1 : not successful			* -1 : not successful
	* 0 : successful			* 0 : successful
	*/			*/
	int info;			int info;
	};			};


	#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */			#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */