C++ Mat::getLine方法代码示例

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())
	{
//.........这里部分代码省略.........

/*	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	
	
	
}