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

/*
	Objective function value calculator:
	Takes Doc feature matrix and Word feature matrix, as well as ground truth counts,
	return objective function value and a vector of error value for each case.
	The objective function is 
		F = acc_error + doc_reg + word_reg 
			= sum_{(doc_id, word_id, cnt):train_vec} (cnt - D(doc_id)*W(word_id))^2 + 0.5*lambda*(|D|^2 + |W|^2)
*/
double CalcObj(const Mat &D, const Mat &W, const vector<Triplet> &train_vec, const int &begin_idx, const int &end_idx, const double &lambda, const double &mean_cnt, Mat &error, Mat &pred_out){
	double acc_error = 0.0, doc_reg = 0.0, word_reg = 0.0;
	error = Mat(end_idx-begin_idx+1, 1, 0); pred_out = Mat(end_idx-begin_idx+1, 1, 0);
	for (int i = begin_idx; i <= end_idx; i++){
		int doc_id = train_vec[i].doc_id, word_id = train_vec[i].word_id;
		double cnt = train_vec[i].cnt;
		
		// calculate the prediction: inner product of D(doc_id), W(word_id)
		double predict = 0.0;
		for (int j=0; j < D.get_m(); j++){
			double dd = D.get(doc_id, j), ww = W.get(word_id, j);
			predict += (dd*ww);
			doc_reg += sqr(dd);
			word_reg += sqr(ww);
		}
		pred_out[i-begin_idx][0] = predict + mean_cnt;
		error[i - begin_idx ][0] = pred_out[i-begin_idx][0] - train_vec[i].cnt;
		// calculate the error between predict and ground truth cnt
		acc_error += sqr(pred_out[i-begin_idx][0] - train_vec[i].cnt);
	}
	// Objective function value
	double F = acc_error + 0.5*lambda*(doc_reg + word_reg);

	return F;
}

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;
}

	/*
		Substraction. Throw exception if their dimensions don't match.
	*/
	Mat operator -(const Mat& rhs){
		if (rhs.get_n()!=n || rhs.get_m()!=m){
			throw invalid_argument("Matrix dimensions are not consistent when trying '+' two matrices");
			return Mat();
		}
		Mat res = Mat(*this);
		for (int i=0; i<res.get_n(); i++){
			for (int j=0; j<res.get_m(); j++)
				res[i][j] -= rhs.get(i, j);
		}
		return res;
	}

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;
}

Mat<float> closestPointLOfBOXGivenPointL(RigidBody& rb, const Mat<float>& pointL)
{
	BoxShape& box = (BoxShape&)rb.getShapeReference();
	Mat<float> min((float)0,3,1);
	Mat<float> max((float)0,3,1);
	
	min.set( -box.getHeight()/2.0f, 1,1);
	min.set( -box.getWidth()/2.0f, 2,1);
	min.set( -box.getDepth()/2.0f, 3,1);
	max.set( -min.get(1,1), 1,1);
	max.set( -min.get(2,1), 2,1);
	max.set( -min.get(3,1), 3,1);
	
	
	Mat<float> clpL(3,1);
	//compute the closest point by projecting the point to the closest face using Voronoi regions approach.
	bool change = false;
	for(int i=1;i<=3;i++)
	{
		
		float v = pointL.get(i,1);
		if( v < min.get(i,1))
		{
			v = min.get(i,1);
			change = true;
		}
		if( v > max.get(i,1))
		{
			v = max.get(i,1);
			change = true;
		}
		
		clpL.set( v, i,1);
	}
	if(!change)
	{
		//then it means that the given point is to be inside the BoxShape :
#ifdef debug
std::cout << "COLLISION DETECTOR : CLPLL : inside point : " << std::endl;
operatorL(operatorL(clpL,pointL), rb.getPointInWorld(pointL)).afficher();
#endif			
	}
	
	return clpL;
}  

    QString FightSheet::generateData()
    {
        Style *s = new Style( object->get("style").toInt(), keeper );

        setReportFile(s->get("name").toLower() + ".html");
        setReportPath("/../share/reports/fightsheet");

        Wrestler *red = new Wrestler(object->get("red").toInt(), keeper);
        Geo *geo_r = new Geo(red->get("geo").toInt(), keeper);
        Wrestler *blue = new Wrestler(object->get("blue").toInt(), keeper);
        Geo *geo_b = new Geo(blue->get("geo").toInt(), keeper);
        Weight *wh = new Weight( object->get("weight").toInt(), keeper );

        Competition *c = new Competition(object->get("competition").toInt(), keeper);

        Mat *mat = new Mat(object->get("mat").toInt(), keeper);
        CRound *cround = new CRound(object->get("cround").toInt(), keeper);
        CTour *ctour = new CTour(object->get("ctour").toInt(), keeper);

        QDate start = QDate::fromString(c->get("start"), QString("yyyy-MM-dd"));
        QDate stop = QDate::fromString(c->get("stop"), QString("yyyy-MM-dd"));

        QString date;
        if ( start.month() == stop.month() ) {
            date = QString("%1-%2/%3/%4").arg(start.day()).arg(stop.day()).arg(start.month()).arg(start.year());
        } else {
            date = start.toString("dd/MM/yyyy") + " - " + stop.toString("dd/MM/yyyy");
        }

        vars.insert("{red.title}", red->get("title"));
        vars.insert("{red.geo}", geo_r->get("title"));
        vars.insert("{blue.title}", blue->get("title"));
        vars.insert("{blue.geo}", geo_b->get("title"));
        vars.insert("{style}", s->get("name"));
        vars.insert("{weight}", wh->get("title"));
        vars.insert("{competition}", c->get("title"));
        vars.insert("{mat}", mat->get("title"));
        vars.insert("{round}", cround->get("title"));
        vars.insert("{place}", ctour->get("title"));
        vars.insert("{num}", object->get("num"));
        vars.insert("{date}", date);

        TQueryMap opt;
        opt.insert("competition", keeper->prepareParam(Equal, c->get("id")));

        QList<QVariant> id_list;
        if ( red->get("id").toInt() > 0 )
            id_list << red->get("id");

        if ( blue->get("id").toInt() > 0 )
            id_list << blue->get("id");

        opt.insert("wrestler", keeper->prepareParam(And | InSet, id_list ));
        opt.insert("style", keeper->prepareParam(And | Equal, s->get("id")));
        opt.insert("weight", keeper->prepareParam(And | Equal, wh->get("id")));


        QList<QVariant> list = keeper->getListOfFields(OCompetitionWrestler,
                                                       QStringList() << "wrestler" << "sorder", opt);
        for( int i = 0; i < list.size(); i++)
        {
            QStringList row = list.at(i).toStringList();
            if ( red->get("id").toInt() == row.at(0).toInt() )
                vars.insert("{red.num}", row.at(1));
            if ( blue->get("id").toInt() == row.at(0).toInt() )
                vars.insert("{blue.num}", row.at(1));
        }

        delete red;
        delete geo_r;
        delete geo_b;
        delete blue;
        delete s;
        delete wh;
        delete mat;
        delete cround;
        delete ctour;
        delete c;

        return applyTemplateVars(loadTemplate(reportFile), vars);
    }

int main() {
  { // Test lower_bound and min_max
  using jcui::algorithm::lower_bound;
  using jcui::algorithm::min_max;
  {
    int xa[] = {};
    int ya[] = {};
    vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
    int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
    eq(0, index, 0);
    eq(0, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 0);
  }
  {
    int xa[] = {3, 4, 5, 6};
    int ya[] = {5, 4, 3, 1};
    vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
    int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
    eq(1, index, 1);
    eq(1, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 4);
  }
  {
    int xa[] = {3, 3, 5, 6};
    int ya[] = {5, 4, 3, 1};
    vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
    int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
    eq(2, index, 2);
    eq(2, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 4);
  }
  {
    int xa[] = {13, 14, 15, 16};
    int ya[] = {5, 4, 3, 1};
    vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
    int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
    eq(3, index, 0);
    eq(3, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 13);
  }
  {
    int xa[] = {3, 4, 5, 6};
    int ya[] = {15, 14, 13, 11};
    vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
    int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
    eq(4, index, 4);
    eq(4, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 11);
  }
  }
  
  { // For SparseMat
    using jcui::algorithm::SparseMat;
    {
      SparseMat<int> a(100, 100), b(100, 100);
      SparseMat<int> c = a * b;
      eq(c.get(3, 10), 0);
      eq(c.get(0, 0), 0);
    }
    {
      // a = [1, 2; 3, 0], b = [6, 0, 0; 0, 1, 4], a * b = [6, 2, 8; 18, 0, 0]
      SparseMat<int> a(100, 100), b(100, 100);
      a.set(0, 0, 1);
      a.set(0, 1, 2);
      a.set(1, 0, 3);
      b.set(0, 0, 6);
      b.set(1, 1, 1);
      b.set(1, 2, 4);
      SparseMat<int> c = a * b;
      eq(c.get(0, 0), 6);
      eq(c.get(0, 1), 2);
      eq(c.get(0, 2), 8);
      eq(c.get(1, 0), 18);
      eq(c.get(1, 1), 0);
      eq(c.get(1, 2), 0);
    }
    {
      // a = [1, 2; 3, 0]
      SparseMat<long> a(100, 100);
      a.set(0, 0, 1);
      a.set(0, 1, 2);
      a.set(1, 0, 3);
      SparseMat<long> c = pow(a, 17);
      eq(c.get(0, 0), 77431669L);
      eq(c.get(0, 1), 51708494L);
      eq(c.get(1, 0), 77562741L);
      eq(c.get(1, 1), 51577422L);
    }
    {
      // a = [1, 2; 3, 0]
      int N = 10;
      SparseMat<float> a(N, N);
      for (int i = 0; i < N; ++i) {
	a.set(i, i, 0.9f);
	a.set(i, (i + 1) % N, 0.05f);
	a.set(i, (i + N - 1) % N, 0.05f);
      }
      SparseMat<float> c = pow(a, 20000);
      for (int i = 0; i < N; ++i) {
	for (int j = 0; j < N; ++j) {
	  eq(fabs(c.get(i, j) - 0.1f) < 1e-3f, true);
	}
      }
    }
    {
//.........这里部分代码省略.........

	/*
		Copy constructor. Deep copy.
	*/
	Mat(const Mat &cp){
		InitMat(cp.get_n(),cp.get_m());
		for (int i=0; i<n; i++)
			for (int j=0; j<m; j++)
				arr[i][j] = cp.get(i,j);
	}

void SpeechRec::SentenceBasedNormalization(Mat<float> *mat)
{
//        mat->saveAscii("c:\\before");

	// sentence mean and variance normalization
	bool mean_norm = C.GetBool("offlinenorm", "sent_mean_norm");
	bool var_norm = C.GetBool("offlinenorm", "sent_var_norm");

	if(mean_norm || var_norm)
	{
		Mat<float> mean;

		// mean calcualtion
		mat->sumColumns(mean);
		mean.div((float)mat->rows());

		// mean norm
		int i, j;
		for(i = 0; i < mat->columns(); i++)
			mat->sub(0, mat->rows() - 1, i, i, mean.get(0, i));

		if(var_norm)
		{
			// variance calculation
			Mat<float> var;
			var.init(mean.rows(), mean.columns());
			var.set(0.0f);
			for(i = 0; i < mat->columns(); i++)
			{
				for(j = 0; j < mat->rows(); j++)
				{
					float v = mat->get(j, i);
					var.add(0, i, v * v);
				}
			}
			var.div((float)mat->rows());
			var.sqrt();

			// lower threshold
			float lowerThr = C.GetFloat("melbanks", "sent_std_thr");
			var.lowerLimit(lowerThr);

			// variance norm
			for(i = 0; i < mat->columns(); i++)
				mat->mul(0, mat->rows() - 1, i, i, 1.0f / var.get(0, i));

			// add mean if not mean norm
			if(!mean_norm)
			{
				for(i = 0; i < mat->columns(); i++)
					mat->add(0, mat->rows() - 1, i, i, mean.get(0, i));
			}
		}
	}

	// sentence maximum normalization
	bool max_norm = C.GetBool("offlinenorm", "sent_max_norm");
	bool channel_max_norm = C.GetBool("offlinenorm", "sent_chmax_norm");

	if(max_norm || channel_max_norm)
	{
		Mat<float> max;
		max.init(1, mat->columns());
		max.set(-9999.9f);
		int i, j;

		for(i = 0; i < mat->columns(); i++)
		{
			for(j = 0; j < mat->rows(); j++)
			{
				float v = mat->get(j, i);
				if(v > max.get(0, i))
				{
				   max.set(0, i, v);
				}
			}
		}

		// global sentence maximum normalization
		if(max_norm)
		{
			float global_max = -9999.9f;
			for(i = 0; i < max.columns(); i++)
			{
				if(max.get(0, i) > global_max)
				{
				   global_max = max.get(0, i);
				}
				max.set(global_max);
			}
		}

		for(i = 0; i < mat->columns(); i++)
		{
			for(j = 0; j < mat->rows(); j++)
			{
	            		mat->set(j, i, mat->get(j, i) - max.get(0, i));
			}
		}
	}
//.........这里部分代码省略.........

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

void IterativeImpulseBasedConstraintSolverStrategy::computeConstraintsANDJacobian(std::vector<std::unique_ptr<IConstraint> >& c, const Mat<float>& q, const Mat<float>& qdot, const SparseMat<float>& invM)
{
	//-------------------------------------
	//-------------------------------------
	//-------------------------------------
	
	size_t size = c.size();
	int n = sim->simulatedObjects.size();
	float baumgarteBAS = 0.0f;//1e-1f;
	float baumgarteC = -2e0f;
	float baumgarteH = 0.0f;//1e-1f;
	
	//---------------
	//	RESETTING :
	constraintsC.clear();
	constraintsJacobians.clear();
	constraintsOffsets.clear();
	constraintsIndexes.clear();
	constraintsInvM.clear();
	constraintsV.clear();
	//----------------------
	
	if( size > 0)
	{
		
		for(int k=0;k<size;k++)
		{
			int idA = ( c[k]->rbA.getID() );
			int idB = ( c[k]->rbB.getID() );
			std::vector<int> indexes(2);
			//indexes are set during the creation of the simulation and they begin at 0.
			indexes[0] = idA;
			indexes[1] = idB;
			
			constraintsIndexes.push_back( indexes );
			
			//---------------------------
			//Constraint :
			c[k]->computeJacobians();
			
			
			Mat<float> tJA(c[k]->getJacobianA());
			Mat<float> tJB(c[k]->getJacobianB());
		
			
			Mat<float> tC(c[k]->getConstraint());
			constraintsC.push_back( tC );
	
			int nbrlineJ = tJA.getLine();
			Mat<float> JacobianAB( operatorL(tJA, tJB)  );
			constraintsJacobians.push_back( JacobianAB );
			
			//----------------------------------------
			//BAUMGARTE STABILIZATION
			//----------------------------------------
			//Contact offset :
			if( c[k]->getType() == CTContactConstraint)
			{
				//----------------------------------------
				//SLOP METHOD :
				/*
				float slop = 1e0f;
				float pdepth = ((ContactConstraint*)(c[k].get()))->penetrationDepth;
				std::cout << " ITERATIVE SOLVER :: CONTACT : pDepth = " << pdepth << std::endl;
				tC *= baumgarteC/this->dt * fabs_(fabs_(pdepth)-slop);			
				*/
				//----------------------------------------
				
				//----------------------------------------
				//DEFAULT METHOD :
				tC *= baumgarteC/this->dt;
				//----------------------------------------
				
				//----------------------------------------
				//METHOD 2 :
				/*
				float restitFactor = ( (ContactConstraint*) (c[k].get()) )->getRestitutionFactor();
				std::cout << " ITERATIVE SOLVER :: CONTACT : restitFactor = " << restitFactor << std::endl;
				Mat<float> Vrel( ( (ContactConstraint*) (c[k].get()) )->getRelativeVelocity() );
				Mat<float> normal( ( (ContactConstraint*) (c[k].get()) )->getNormalVector() );
				std::cout << " ITERATIVE SOLVER :: CONTACT : Normal vector : " << std::endl;
				transpose(normal).afficher();
				tC +=  restitFactor * transpose(Vrel)*normal; 
				*/
				//----------------------------------------
				
				std::cout << " ITERATIVE SOLVER :: CONTACT : Contact Constraint : " << std::endl;
				transpose(tC).afficher();
				std::cout << " ITERATIVE SOLVER :: CONTACT : Relative Velocity vector : " << std::endl;
				transpose(( (ContactConstraint*) (c[k].get()) )->getRelativeVelocity()).afficher();
				//std::cout << " ITERATIVE SOLVER :: CONTACT : First derivative of Contact Constraint : " << std::endl;
				//(transpose(tJA)*).afficher();
					
			}
			//BAS JOINT :
			if( c[k]->getType() == CTBallAndSocketJoint)
			{
				tC *= baumgarteBAS/this->dt;
			}
			
//.........这里部分代码省略.........

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