Example #1
0
bool equals(const Mat<float>& a, const Mat<float>& b, float precision)
{
	if(a.getLine() == b.getLine() && a.getColumn() == b.getColumn())
	{
		for(int i=a.getLine();i--;)
		{
			for(int j=a.getColumn();j--;)
			{
				float vala = a.get(i+1,j+1);
				float valb = b.get(i+1,j+1);
				if( vala+precision < valb || vala-precision > valb)
				{
					return false;
				}
			}
		}
	}
	else
	{
		std::cerr << "EQUALS : matrices a and b are not of the same sizes." << std::endl;
		return false;
	}
		
	return true;
}
Mat<float> reshapeV(const Mat<float>& m)
{
	int line = m.getLine();
	int column = m.getColumn();
	Mat<float> r(line*column,1);
	
	for(int i=1;i<=line;i++)
	{
		for(int j=1;j<=column;j++)
		{
			r.set( m.get(i,j), (i-1)*column+j, 1);
		}
	}
	
	return r;
}
void SimultaneousImpulseBasedConstraintSolverStrategy::Solve(float dt, std::vector<std::unique_ptr<IConstraint> >& c, Mat<float>& q, Mat<float>& qdot, SparseMat<float>& invM, SparseMat<float>& S, const Mat<float>& Fext )
{
	//std::cout << "STATE :" << std::endl;
	//q.afficher();
	
	Mat<float> qdotminus(qdot);
	this->dt = dt;
	//computeConstraintsJacobian(c);
	Mat<float> tempInvMFext( dt*(invM * Fext) ) ;
	//qdot += tempInvMFext;
	//computeConstraintsJacobian(c,q,qdot);
	computeConstraintsANDJacobian(c,q,qdot);
	
	//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
	//std::cout << "Constraints : norme  = " << norme2(C) << std::endl;
	//C.afficher();
	
	Mat<float> tConstraintsJacobian( transpose(constraintsJacobian) );
	//std::cout << "t Constraints Jacobian :" << std::endl;
	//tConstraintsJacobian.afficher();
	
	
	//PREVIOUS METHOD :
	//--------------------------------
	//David Baraff 96 STAR.pdf Interactive Simulation of Rigid Body Dynamics in Computer Graphics : Lagrange Multipliers Method :
	//Construct A :
	/*
	Mat<float> A( (-1.0f)*tConstraintsJacobian );
	Mat<float> M( invGJ( invM.SM2mat() ) );
	A = operatorL( M, A);
	A = operatorC( A , operatorL( constraintsJacobian, Mat<float>((float)0,constraintsJacobian.getLine(), constraintsJacobian.getLine()) ) );
	*/
	//----------------------------
	Mat<float> A( constraintsJacobian * invM.SM2mat() * tConstraintsJacobian );
	
	//---------------------------
	Mat<float> invA( invGJ(A) );//invM*tConstraintsJacobian ) * constraintsJacobian );
	
	//Construct b and compute the solution.
	//----------------------------------
	//Mat<float> tempLambda( invA * operatorC( Mat<float>((float)0,invA.getLine()-constraintsJacobian.getLine(),1) , (-1.0f)*(constraintsJacobian*(invM*Fext) + offset) ) );
	//-----------------------------------
	Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobian*tempInvMFext + offset) ) );
	//-----------------------------------
	
	//Solutions :
	//------------------------------------
	//lambda = extract( &tempLambda, qdot.getLine()+1, 1, tempLambda.getLine(), 1);
	//if(isnanM(lambda))
	//	lambda = Mat<float>(0.0f,lambda.getLine(),lambda.getColumn());
	//Mat<float> udot( extract(  &tempLambda, 1,1, qdot.getLine(), 1) );
	//------------------------------------
	lambda = tempLambda;
	Mat<float> udot(  tConstraintsJacobian * tempLambda);
	//------------------------------------
	
	if(isnanM(udot))
		udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn());
	
	
	float clampingVal = 1e4f;
	for(int i=1;i<=udot.getLine();i++)
	{
		if(udot.get(i,1) > clampingVal)
		{
			udot.set( clampingVal,i,1);
		}
	}
	
#ifdef debuglvl1	
	std::cout << " SOLUTIONS : udot and lambda/Pc : " << std::endl;
	transpose(udot).afficher();
	transpose(lambda).afficher();
	transpose( tConstraintsJacobian*lambda).afficher();
#endif	
	//Assumed model :
	//qdot = tempInvMFext + dt*extract(  &tempLambda, 1,1, qdot.getLine(), 1);
	//qdot = tempInvMFext + udot;
	qdot += tempInvMFext + invM*udot;
	//qdot += invM*udot;
	//qdot += tempInvMFext + udot;
	float clampingValQ = 1e3f;
	for(int i=1;i<=qdot.getLine();i++)
	{
		if( fabs_(qdot.get(i,1)) > clampingValQ)
		{
			qdot.set( clampingValQ * fabs_(qdot.get(i,1))/qdot.get(i,1),i,1);
		}
	}
	//qdot = udot;
	
	//Assumed model if the update of the integration is applied after that constraints solver :
	//qdot += dt*extract(  &tempLambda, 1,1, qdot.getLine(), 1);//+tempInvMFext
	
	Mat<float> t( dt*( S*qdot ) );	
	
	float clampQuat = 1e-1f;
	float idxQuat = 3;
	while(idxQuat < t.getLine())
	{
		for(int i=1;i<4;i++)
		{
			if( fabs_(t.get(idxQuat+i,1)) > clampQuat)
			{
				t.set( clampQuat*(t.get(idxQuat+i,1))/t.get(idxQuat+i,1), idxQuat+i,1);
			}
		}
		
		idxQuat += 7;
	}
	
	//the update is done by the update via an accurate integration and we must construct q and qdot at every step
	//q += t;
	
	//--------------------------------------	
	//let us normalize each quaternion :
	/*
	idxQuat = 3;
	while(idxQuat < q.getLine())
	{
		float scaler = q.get( idxQuat+4,1);
		
		if(scaler != 0.0f)
		{
			for(int i=1;i<=4;i++)
			{
				q.set( q.get(idxQuat+i,1)/scaler, idxQuat+i,1);
			}
		}
		
		idxQuat += 7;
	}
	*/
	
	//--------------------------------------
	
#ifdef debuglvl2	
	//std::cout << " computed Pc : " << std::endl;
	//(tConstraintsJacobian*tempLambda).afficher();
	//std::cout << " q+ : " << std::endl;
	//transpose(q).afficher();
	std::cout << " qdot+ : " << std::endl;
	transpose(qdot).afficher();
	std::cout << " qdotminus : " << std::endl;
	transpose(qdotminus).afficher();
#endif

#ifdef debuglvl3	
	std::cout << "SOME VERIFICATION ON : J*qdot + c = 0 : " << std::endl;
	transpose(constraintsJacobian*qdot+offset).afficher();
	
	
	float normC = (transpose(C)*C).get(1,1);
	Mat<float> Cdot( constraintsJacobian*qdot);
	float normCdot = (transpose(Cdot)*Cdot).get(1,1);
	float normQdot = (transpose(qdot)*qdot).get(1,1);
	
	//rs->ltadd(std::string("normC"),normC);
	//rs->ltadd(std::string("normCdot"),normCdot);
	rs->ltadd(std::string("normQdot"),normQdot);
	char name[5];
	for(int i=1;i<=t.getLine();i++)
	{
		sprintf(name,"dq%d",i);
		rs->ltadd(std::string(name), t.get(i,1));
	}
	rs->tWriteFileTABLE();
#endif	
	//END OF PREVIOUS METHOD :
	//--------------------------------
	
	//--------------------------------
	//--------------------------------
	//Second Method :
	/*
	//According to Tonge Richar's Physucs For Game pdf :
	Mat<float> tempLambda( (-1.0f)*invGJ( constraintsJacobian*invM.SM2mat()*tConstraintsJacobian)*constraintsJacobian*qdot );
	qdot += invM*tConstraintsJacobian*tempLambda;
	
	
	//qdot += tempInvMFext;	//contraints not satisfied.
	
	//qdot += tempInvMFext;	//qdot+ = qdot- + dt*M-1Fext;
	//Mat<float> qdotreal( qdot + dt*extract(  &tempLambda, 1,1, qdot.getLine(), 1) );	//qdotreal = qdot+ + Pc;
	
	Mat<float> t( dt*( S*qdot ) );
	q += t;
	*/
	//--------------------------------
	//--------------------------------
	//End of second method...
	//--------------------------------
	
	
	
	//--------------------------------
	//--------------------------------
	//THIRD METHOD :
	//--------------------------------
	//With reference to A Unified Framework for Rigid Body Dynamics Chap. 4.6.2.Simultaneous Force-based methods :
	//which refers to Bara96 :
	/*
	Mat<float> iM(invM.SM2mat());
	Mat<float> b((-1.0f)*constraintsJacobian*iM*Fext+offset);
	Mat<float> tempLambda( invGJ( constraintsJacobian*iM*tConstraintsJacobian) * b );
	//Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda+Fext)); 
	Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda)); 
	qdot += dt*qdoubledot;
	
	
	//qdot += tempInvMFext;	//contraints not satisfied.
	
	//qdot += tempInvMFext;	//qdot+ = qdot- + dt*M-1Fext;
	//Mat<float> qdotreal( qdot + dt*extract(  &tempLambda, 1,1, qdot.getLine(), 1) );	//qdotreal = qdot+ + Pc;
	
	Mat<float> t( dt*( S*qdot ) );
	q += t;
	
	std::cout << " computed Pc : " << std::endl;
	(tConstraintsJacobian*tempLambda).afficher();
	std::cout << " q+ : " << std::endl;
	q.afficher();
	std::cout << " qdot+ : " << std::endl;
	qdot.afficher();
	*/
	//END OF THIRD METHOD :
	//--------------------------------
	
	
	
	
	//S.print();
	
	//std::cout << " computed Pc : " << std::endl;
	//(tConstraintsJacobian*lambda).afficher();
	//std::cout << " delta state = S * qdotreal : " << std::endl; 
	//t.afficher();
	//std::cout << " S & qdotreal : " << std::endl;
	//S.print();
	//qdot.afficher();
	//std::cout << "invM*Fext : " << std::endl;
	//tempInvMFext.afficher();
	
	
	//temp.afficher();
	//(constraintsJacobian*(invM*Fext)).afficher();
	//(invM*Fext).afficher();
	//std::cout << " A : " << std::endl;
	//A.afficher();
    //std::cout << " SVD A*tA :  S : " << std::endl;
    //SVD<float> instanceSVD(A*transpose(A));
    //instanceSVD.getS().afficher();
	//std::cout << " invA : " << std::endl;
	//invA.afficher();
	
	//std::cout << " LAMBDA : " << std::endl;
	//lambda.afficher();
	
	//std::cout << " qdot+ & qdot- : " << std::endl;
	//qdot.afficher();
	//qdotminus.afficher();
	
	//std::cout << " q+ : " << std::endl;
	//q.afficher();
#ifdef debuglvl4	
	//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
	std::cout << "tConstraints : norme  = " << norme2(C) << std::endl;
	transpose(C).afficher();
	std::cout << "Cdot : " << std::endl;
	(constraintsJacobian*qdot).afficher();
	std::cout << " JACOBIAN : " << std::endl;
	//transpose(constraintsJacobian).afficher();
	constraintsJacobian.afficher();
	std::cout << " Qdot+ : " << std::endl;
	transpose(qdot).afficher();
#endif	
	//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
	//std::cout << "Constraints : norme  = " << norme2(C) << std::endl;
	//C.afficher();
	
}
/*	FORCE BASED : */
void SimultaneousImpulseBasedConstraintSolverStrategy::SolveForceBased(float dt, std::vector<std::unique_ptr<IConstraint> >& c, Mat<float>& q, Mat<float>& qdot, SparseMat<float>& invM, SparseMat<float>& S, const Mat<float>& Fext )
{	
	Mat<float> qdotminus(qdot);
	this->dt = dt;
	Mat<float> tempInvMFext( dt*(invM * Fext) ) ;


	computeConstraintsANDJacobian(c,q,qdot);
	
	Mat<float> tConstraintsJacobian( transpose(constraintsJacobian) );
	Mat<float> A( constraintsJacobian * invM.SM2mat() * tConstraintsJacobian );
	
	//---------------------------
	Mat<float> invA( invGJ(A) );
	
	//Construct b and compute the solution.
	//-----------------------------------
	Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobian*tempInvMFext + offset) ) );
	//-----------------------------------
	
	//Solutions :
	//------------------------------------
	lambda = tempLambda;
	Mat<float> udot(  tConstraintsJacobian * tempLambda);
	//------------------------------------
	
	if(isnanM(udot))
		udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn());
	
	
	float clampingVal = 1e4f;
	for(int i=1;i<=udot.getLine();i++)
	{
		if(udot.get(i,1) > clampingVal)
		{
			udot.set( clampingVal,i,1);
		}
	}
	
#ifdef debuglvl1	
	std::cout << " SOLUTIONS : udot and lambda/Pc : " << std::endl;
	transpose(udot).afficher();
	transpose(lambda).afficher();
	transpose( tConstraintsJacobian*lambda).afficher();
#endif	
	//Assumed model :
	qdot += tempInvMFext + dt*(invM*udot);
	float clampingValQ = 1e3f;
	for(int i=1;i<=qdot.getLine();i++)
	{
		if( fabs_(qdot.get(i,1)) > clampingValQ)
		{
			qdot.set( clampingValQ * fabs_(qdot.get(i,1))/qdot.get(i,1),i,1);
		}
	}
	
	//--------------------------------------
	
#ifdef debuglvl2	
	//std::cout << " computed Pc : " << std::endl;
	//(tConstraintsJacobian*tempLambda).afficher();
	//std::cout << " q+ : " << std::endl;
	//transpose(q).afficher();
	std::cout << " qdot+ : " << std::endl;
	transpose(qdot).afficher();
	std::cout << " qdotminus : " << std::endl;
	transpose(qdotminus).afficher();
#endif

	//END OF PREVIOUS METHOD :
	//--------------------------------
	
	
#ifdef debuglvl4	
	//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
	std::cout << "tConstraints : norme  = " << norme2(C) << std::endl;
	transpose(C).afficher();
	std::cout << "Cdot : " << std::endl;
	(constraintsJacobian*qdot).afficher();
	std::cout << " JACOBIAN : " << std::endl;
	//transpose(constraintsJacobian).afficher();
	constraintsJacobian.afficher();
	std::cout << " Qdot+ : " << std::endl;
	transpose(qdot).afficher();
#endif	
	
	
}