Classes
struct	BodyData
struct	ConstraintPoint
class	ExtraJointLinkPair
struct	LinkData
class	LinkPair
Public Types
typedef std::vector < ConstraintPoint >	ConstraintPointArray
typedef intrusive_ptr < ExtraJointLinkPair >	ExtraJointLinkPairPtr
typedef std::vector< LinkData >	LinkDataArray
typedef std::vector< LinkPairPtr >	LinkPairArray
typedef intrusive_ptr< LinkPair >	LinkPairPtr
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >	rmdmatrix
Public Member Functions
bool	addCollisionCheckLinkPair (int bodyIndex1, Link link1, int bodyIndex2, Link link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
void	addConstraintForceToLink (LinkPair *linkPair, int ipair)
void	addConstraintForceToLinks ()
bool	addExtraJoint (int bodyIndex1, Link link1, int bodyIndex2, Link link2, const double link1LocalPos, const double link2LocalPos, const short jointType, const double *jointAxis)
void	calcABMForceElementsWithTestForce (BodyData &bodyData, Link *linkToApplyForce, const Vector3 &f, const Vector3 &tau)
void	calcAccelsABM (BodyData &bodyData, int constraintIndex)
void	calcAccelsMM (BodyData &bodyData, int constraintIndex)
	CFSImpl (WorldBase &world)
void	checkLCPResult (rmdmatrix &M, dvector &b, dvector &x)
void	checkMCPResult (rmdmatrix &M, dvector &b, dvector &x)
void	clearExternalForces ()
void	clearSingularPointConstraintsOfClosedLoopConnections ()
void	copySymmetricElementsOfAccelerationMatrix (Eigen::Block< rmdmatrix > &Knn, Eigen::Block< rmdmatrix > &Ktn, Eigen::Block< rmdmatrix > &Knt, Eigen::Block< rmdmatrix > &Ktt)
template<class TMatrix >
void	debugPutMatrix (const TMatrix &M, const char *name)
template<class TVector >
void	debugPutVector (const TVector &M, const char *name)
void	extractRelAccelsFromLinkPairCase1 (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int testForceIndex, int constraintIndex)
void	extractRelAccelsFromLinkPairCase2 (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int iTestForce, int iDefault, int testForceIndex, int constraintIndex)
void	extractRelAccelsFromLinkPairCase3 (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, LinkPair &linkPair, int testForceIndex, int constraintIndex)
void	extractRelAccelsOfConstraintPoints (Eigen::Block< rmdmatrix > &Kxn, Eigen::Block< rmdmatrix > &Kxt, int testForceIndex, int constraintIndex)
void	initABMForceElementsWithNoExtForce (BodyData &bodyData)
void	initBody (BodyPtr body, BodyData &bodyData)
void	initExtraJoints (int bodyIndex)
void	initialize (void)
void	initMatrices ()
void	putContactPoints ()
template<class TMatrix >
void	putMatrix (TMatrix &M, const char *name)
template<class TVector >
void	putVector (const TVector &M, const char *name)
void	setAccelCalcSkipInformation ()
void	setAccelerationMatrix ()
void	setConstantVectorAndMuBlock ()
void	setConstraintPoints (CollisionSequence &collisions)
void	setContactConstraintPoints (LinkPair &linkPair, CollisionPointSequence &collisionPoints)
void	setDefaultAccelerationVector ()
void	setExtraJointConstraintPoints (ExtraJointLinkPairPtr &linkPair)
void	setFrictionVectors (ConstraintPoint &constraintPoint)
void	solve (CollisionSequence &corbaCollisionSequence)
void	solveImpactConstraints ()
void	solveMCPByProjectedGaussSeidel (const rmdmatrix &M, const dvector &b, dvector &x)
void	solveMCPByProjectedGaussSeidelInitial (const rmdmatrix &M, const dvector &b, dvector &x, const int numIteration)
void	solveMCPByProjectedGaussSeidelMain (const rmdmatrix &M, const dvector &b, dvector &x, const int numIteration)
	~CFSImpl ()
Public Attributes
double	allowedPenetrationDepth
dvector	an0
bool	areThereImpacts
dvector	at0
dvector	b
std::vector< BodyData >	bodiesData
LinkPairArray	collisionCheckLinkPairs
std::vector< LinkPair * >	constrainedLinkPairs
dvector	contactIndexToMu
vector< ExtraJointLinkPairPtr >	extraJointLinkPairs
std::vector< int >	frictionIndexToContactIndex
double	gaussSeidelMaxRelError
int	globalNumConstraintVectors
int	globalNumContactNormalVectors
int	globalNumFrictionVectors
bool	isConstraintForceOutputMode
int	maxNumGaussSeidelIteration
dvector	mcpHi
rmdmatrix	Mlcp
double	negativeVelocityRatioForPenetration
int	numGaussSeidelInitialIteration
int	numGaussSeidelTotalCalls
int	numGaussSeidelTotalLoops
int	numGaussSeidelTotalLoopsMax
int	numUnconverged
ofstream	os
int	prevGlobalNumConstraintVectors
int	prevGlobalNumFrictionVectors
variate_generator < boost::mt19937, uniform_real<> >	randomAngle
dvector	solution
bool	useBuiltinCollisionDetector
WorldBase &	world
Static Public Attributes
static const double	PI = 3.14159265358979323846
static const double	PI_2 = 1.57079632679489661923

Detailed Description

Definition at line 115 of file ConstraintForceSolver.cpp.

Member Typedef Documentation

typedef std::vector<ConstraintPoint> hrp::CFSImpl::ConstraintPointArray

Definition at line 159 of file ConstraintForceSolver.cpp.

typedef intrusive_ptr<ExtraJointLinkPair> hrp::CFSImpl::ExtraJointLinkPairPtr

Definition at line 229 of file ConstraintForceSolver.cpp.

typedef std::vector<LinkData> hrp::CFSImpl::LinkDataArray

Definition at line 174 of file ConstraintForceSolver.cpp.

typedef std::vector<LinkPairPtr> hrp::CFSImpl::LinkPairArray

Definition at line 219 of file ConstraintForceSolver.cpp.

typedef intrusive_ptr<LinkPair> hrp::CFSImpl::LinkPairPtr

Definition at line 218 of file ConstraintForceSolver.cpp.

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> hrp::CFSImpl::rmdmatrix

Definition at line 245 of file ConstraintForceSolver.cpp.

Constructor & Destructor Documentation

CFSImpl::CFSImpl ( WorldBase & world )

Definition at line 377 of file ConstraintForceSolver.cpp.

CFSImpl::~CFSImpl ( )

Definition at line 392 of file ConstraintForceSolver.cpp.

Member Function Documentation

bool CFSImpl::addCollisionCheckLinkPair	(	int	bodyIndex1,
		Link *	link1,
		int	bodyIndex2,
		Link *	link2,
		double	muStatic,
		double	muDynamic,
		double	culling_thresh,
		double	restitution,
		double	epsilon
	)

Definition at line 401 of file ConstraintForceSolver.cpp.

void CFSImpl::addConstraintForceToLink	(	LinkPair *	linkPair,
		int	ipair
	)

Definition at line 1699 of file ConstraintForceSolver.cpp.

void CFSImpl::addConstraintForceToLinks ( )

Definition at line 1685 of file ConstraintForceSolver.cpp.

bool CFSImpl::addExtraJoint	(	int	bodyIndex1,
		Link *	link1,
		int	bodyIndex2,
		Link *	link2,
		const double *	link1LocalPos,
		const double *	link2LocalPos,
		const short	jointType,
		const double *	jointAxis
	)

Definition at line 438 of file ConstraintForceSolver.cpp.

void CFSImpl::calcABMForceElementsWithTestForce	(	BodyData &	bodyData,
		Link *	linkToApplyForce,
		const Vector3 &	f,
		const Vector3 &	tau
	)

Definition at line 1321 of file ConstraintForceSolver.cpp.

void CFSImpl::calcAccelsABM	(	BodyData &	bodyData,
		int	constraintIndex
	)

Definition at line 1346 of file ConstraintForceSolver.cpp.

void CFSImpl::calcAccelsMM	(	BodyData &	bodyData,
		int	constraintIndex
	)

Definition at line 1405 of file ConstraintForceSolver.cpp.

void CFSImpl::checkLCPResult	(	rmdmatrix &	M,
		dvector &	b,
		dvector &	x
	)

Definition at line 2055 of file ConstraintForceSolver.cpp.

void CFSImpl::checkMCPResult	(	rmdmatrix &	M,
		dvector &	b,
		dvector &	x
	)

Definition at line 2085 of file ConstraintForceSolver.cpp.

void CFSImpl::clearExternalForces ( ) [inline]

Definition at line 635 of file ConstraintForceSolver.cpp.

void CFSImpl::clearSingularPointConstraintsOfClosedLoopConnections ( )

Definition at line 1600 of file ConstraintForceSolver.cpp.

void CFSImpl::copySymmetricElementsOfAccelerationMatrix	(	Eigen::Block< rmdmatrix > &	Knn,
		Eigen::Block< rmdmatrix > &	Ktn,
		Eigen::Block< rmdmatrix > &	Knt,
		Eigen::Block< rmdmatrix > &	Ktt
	)

Definition at line 1564 of file ConstraintForceSolver.cpp.

template<class TMatrix >

void hrp::CFSImpl::debugPutMatrix	(	const TMatrix &	M,
		const char *	name
	)		`[inline]`

Definition at line 359 of file ConstraintForceSolver.cpp.

template<class TVector >

void hrp::CFSImpl::debugPutVector	(	const TVector &	M,
		const char *	name
	)		`[inline]`

Definition at line 364 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsFromLinkPairCase1	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		LinkPair &	linkPair,
		int	testForceIndex,
		int	constraintIndex
	)

Todo:: Can the follwoing equations be simplified ?

Definition at line 1468 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsFromLinkPairCase2	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		LinkPair &	linkPair,
		int	iTestForce,
		int	iDefault,
		int	testForceIndex,
		int	constraintIndex
	)

Definition at line 1504 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsFromLinkPairCase3	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		LinkPair &	linkPair,
		int	testForceIndex,
		int	constraintIndex
	)

Definition at line 1541 of file ConstraintForceSolver.cpp.

void CFSImpl::extractRelAccelsOfConstraintPoints	(	Eigen::Block< rmdmatrix > &	Kxn,
		Eigen::Block< rmdmatrix > &	Kxt,
		int	testForceIndex,
		int	constraintIndex
	)