int main() {
scanf("%d%d%d", &n, &p, &m);
a.set(n, p); b.set(p, m); c.set(n, m);
a.read(); b.read(); c.read();
if(check()) puts("Yes");
else printf("No\n%d %d\n%d\n", ansr, ansc, ans);
}
void innerVoronoiProjectionANDNormal(RigidBody& rb, Mat<float>& pointL, Mat<float>& normalL)
{
//given an inner point, let us find out its closest projection on the surface of the OBB :
//and the normal to the corresponding surface : the normal that is directed on the outside.
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);
float distance[6];
Mat<float> tempProj[6];
int idxMin=3;
float distMin=1e3f;
int line = 1;
for(int k=3;k--;)
{
tempProj[k] = pointL;
tempProj[k].set( min.get(line,1), line,1);
tempProj[k+3] = pointL;
tempProj[k+3].set( max.get(line,1), line,1);
distance[k] = norme2(pointL-tempProj[k]);
distance[k+3] = norme2(pointL-tempProj[k+3]);
if(distMin > distance[k])
{
distMin = distance[k];
idxMin = k;
}
if(distMin > distance[k+3])
{
distMin = distance[k+3];
idxMin = k+3;
}
line++;
}
pointL = tempProj[idxMin];
normalL = Mat<float>(0.0f,3,1);
switch(idxMin)
{
case 0 :
//it is min on the z face :
normalL.set( -1.0f,3,1);
break;
case 1 :
//it is min on the y face :
normalL.set( -1.0f,2,1);
break;
case 2 :
//it is min on the x face :
normalL.set( -1.0f,1,1);
break;
case 3 :
//it is max on the z face :
normalL.set( 1.0f,3,1);
break;
case 4 :
//it is max on the y face :
normalL.set( 1.0f,2,1);
break;
case 5 :
//it is max on the x face :
normalL.set( 1.0f,1,1);
break;
}
}
Mat get(int a) {
Mat r;
r.set();
return pwd(r, a);
}
bool SpeechRec::ProcessOffline(data_format inpf, data_format outpf, void *inpSig, int sigNBytes, Mat<float> *inpMat, Mat<float> *outMat)
{
assert((int)inpf < (int)outpf);
assert(outMat || outpf == dfStrings);
assert(inpMat || inpf == dfWaveform);
Mat<float> *paramMat = 0;
Mat<float> *posteriorsMat = 0;
// waveform -> parameters
int nFrames;
if(inpf == dfWaveform)
{
if(!ConvertWaveformFormat(waveFormat, inpSig, sigNBytes, &waveform, &waveformLen))
return false;
nFrames = (waveformLen > actualParams->GetVectorSize() ? (waveformLen - actualParams->GetVectorSize()) / actualParams->GetStep() + 1 : 1);
actualParams->AddWaveform(waveform, waveformLen);
if(outpf == dfParams)
{
paramMat = outMat;
}
else
{
paramMat = new Mat<float>;
if(!paramMat)
{
MERROR("Insufficient memory\n");
return false;
}
}
if(actualParams->GetNParams() != paramMat->columns() || nFrames != paramMat->rows())
paramMat->init(nFrames, actualParams->GetNParams());
int fr = 0;
while(actualParams->GetFeatures(params))
{
FrameBasedNormalization(params, actualParams->GetNParams());
paramMat->set(fr, fr, 0, actualParams->GetNParams() - 1, params);
fr++;
}
if(outpf == dfParams)
return true;
}
// sentence based normalization
if(inpf == dfWaveform || inpf == dfParams)
{
if(inpf == dfParams)
paramMat = inpMat;
if(paramMat->columns() < actualParams->GetNParams())
{
MERROR("Invalid dimensionality of parameter vectors\n");
return false;
}
else if(paramMat->columns() > actualParams->GetNParams())
{
Mat<float> *tmpMat = new Mat<float>;
tmpMat->init(paramMat->rows(), actualParams->GetNParams());
tmpMat->copy(*paramMat, 0, paramMat->rows() - 1, 0, actualParams->GetNParams() - 1,
0, paramMat->rows() - 1, 0, actualParams->GetNParams() - 1);
delete paramMat;
paramMat = tmpMat;
inpMat = paramMat;
}
SentenceBasedNormalization(paramMat);
}
// parameters -> posteriors
if(outpf == dfPosteriors && !mTrapsEnabled)
{
MERROR("The 'traps' module have to be enabled for generating posteriors\n");
return false;
}
if((inpf == dfWaveform || inpf == dfParams) && mTrapsEnabled)
{
if(inpf == dfParams)
paramMat = inpMat;
if(outpf == dfPosteriors)
{
posteriorsMat = outMat;
}
else
{
posteriorsMat = new Mat<float>;
if(!posteriorsMat)
{
if(inpf != dfParams)
delete paramMat;
MERROR("Insufficient memory\n");
return false;
}
}
nFrames = paramMat->rows();
if(TR.GetNumOuts() != posteriorsMat->columns() || nFrames != posteriorsMat->rows())
posteriorsMat->init(nFrames, TR.GetNumOuts());
// first part - initialization
int i;
int trapShift = TR.GetTrapShift();
int nparams = actualParams->GetNParams();
if(nFrames >= trapShift)
{
TR.CalcFeaturesBunched((float *)paramMat->getMem(), posteriors, trapShift, false);
}
else
{
sCopy(nFrames * paramMat->columns(), params, (float *)paramMat->getMem());
for(i = nFrames; i < TR.GetTrapShift(); i++)
paramMat->extr(nFrames - 1, nFrames - 1, 0, nparams - 1, params + i * nparams);
TR.CalcFeaturesBunched(params, posteriors, trapShift, false);
}
// second part - main block
if(nFrames > trapShift)
TR.CalcFeaturesBunched((float *)paramMat->getMem() + trapShift * nparams, (float *)posteriorsMat->getMem(), nFrames - trapShift);
// last part - termination
int n = (nFrames > trapShift ? trapShift : nFrames);
for(i = 0; i < n; i++)
paramMat->extr(nFrames - 1, nFrames - 1, 0, nparams - 1, params + i * nparams);
TR.CalcFeaturesBunched(params, (float *)posteriorsMat->getMem() + (nFrames - n) * posteriorsMat->columns(), n);
// softening function: posteriors -> posteriors/log. posteriors
int nPost = posteriorsMat->columns();
for(i = 0; i < nFrames; i++)
{
posteriorsMat->extr(i, i, 0, nPost - 1, posteriors);
int j;
for(j = 0; j < nPost; j++)
posteriors[j] = (*postSoftFunc)(posteriors[j], postSoftArg1, postSoftArg2, postSoftArg3);
posteriorsMat->set(i, i, 0, nPost - 1, posteriors);
}
if(inpf != dfParams)
delete paramMat;
if(outpf == dfPosteriors)
return true;
}
// posteriors -> strings
if(inpf == dfWaveform || inpf == dfParams || inpf == dfPosteriors)
{
if(inpf == dfPosteriors || (inpf == dfParams && !mTrapsEnabled))
posteriorsMat = inpMat;
nFrames = posteriorsMat->rows();
int nPost = posteriorsMat->columns(); // TR.GetNumOuts()
// softening function: posteriors -> log. posteriors
int i;
for(i = 0; i < nFrames; i++)
{
posteriorsMat->extr(i, i, 0, nPost - 1, posteriors);
int j;
for(j = 0; j < nPost; j++)
posteriors[j] = (*decSoftFunc)(posteriors[j], decSoftArg1, decSoftArg2, decSoftArg3);
posteriorsMat->set(i, i, 0, nPost - 1, posteriors);
}
// log posteriors -> strings
for(i = 0; i < nFrames; i++)
{
posteriorsMat->extr(i, i, 0, nPost - 1, posteriors);
DE->ProcessFrame(posteriors);
}
if(inpf != dfPosteriors)
delete posteriorsMat;
}
return true;
}
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())
{
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();
}
void IterativeImpulseBasedConstraintSolverStrategy::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 )
{
float clampingVal = 1e20f;
float clampingValQ = 1e20f;
Mat<float> qdotminus(qdot);
this->dt = dt;
Mat<float> tempInvMFext( dt*(invM * Fext) ) ;
std::vector<float> constraintsNormeCdotAfterImpulses(c.size(),1.0f);
std::vector<Mat<float> > constraintsVAfterImpulses;
bool continuer = true;
int nbrIteration = 0;
while( continuer)
{
computeConstraintsANDJacobian(c,q,qdot,invM);
constraintsImpulses.clear();
constraintsImpulses.resize(constraintsC.size());
if( nbrIteration == 0)
{
constraintsVAfterImpulses = constraintsV;
}
int nbrConstraintsSolved = 0;
for(int k=0;k<constraintsC.size();k++)
{
if(constraintsNormeCdotAfterImpulses[k] >= 0.0f)
{
Mat<float> tConstraintsJacobian( transpose( constraintsJacobians[k] ) );
Mat<float> A( constraintsJacobians[k] * constraintsInvM[k] * tConstraintsJacobian );
//Construct the inverse matrix :
//---------------------------
Mat<float> invA( invGJ(A) );
//Construct b and compute the solution.
//----------------------------------
Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobians[k]*constraintsV[k] + constraintsOffsets[k]) ) );
//-----------------------------------
//Solution Impulse :
//------------------------------------
constraintsImpulses[k] = tConstraintsJacobian * tempLambda;
Mat<float> udot( constraintsInvM[k]*constraintsImpulses[k]);
//------------------------------------
if(isnanM(udot))
{
std::cout << " ITERATIVE SOLVER :: udot :: NAN ISSUE : " << std::endl;
transpose(udot).afficher();
udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn());
}
/*
for(int i=1;i<=udot.getLine();i++)
{
if(udot.get(i,1) > clampingVal)
{
udot.set( clampingVal,i,1);
}
}
*/
//---------------------------------
// UPDATE OF THE VELOCITY :
//---------------------------------
std::vector<int> indexes = constraintsIndexes[k];
//constraintsVAfterImpulses[k] = constraintsV[k]-udot;
constraintsVAfterImpulses[k] = constraintsV[k]+udot;
std::cout << " UDOT : ids : " << indexes[0] << " and " << indexes[1] << std::endl;
transpose(udot).afficher();
for(int kk=0;kk<=1;kk++)
{
for(int i=1;i<=6;i++)
{
qdot.set( constraintsVAfterImpulses[k].get(kk*6+i,1), indexes[kk]*6+i, 1);
}
}
//---------------------------------
/*
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);
}
}
*/
}
else
{
std::cout << "CONSTRAINTS : " << k << "/" << constraintsC.size() << " : has been solved for." << std::endl;
}
}
for(int k=0;k<constraintsC.size();k++)
{
//#ifdef debuglvl3
std::cout << "SOME VERIFICATION ON : J*qdot + c = 0 : k = " << k << std::endl;
Mat<float> cdot( constraintsJacobians[k]*constraintsVAfterImpulses[k]+constraintsOffsets[k]);
transpose(cdot).afficher();
constraintsNormeCdotAfterImpulses[k] = (transpose(cdot)*cdot).get(1,1);
std::cout << "NORME : "<< k << " : " << constraintsNormeCdotAfterImpulses[k] << std::endl;
if( constraintsNormeCdotAfterImpulses[k] <= 1e-20f)
{
nbrConstraintsSolved++;
}
//#endif
}
if(nbrConstraintsSolved == constraintsC.size() )
{
continuer = false;
std::cout << " IterativeImpulseBasedConstraintsSolver::Solve :: Constraints solved : " << nbrConstraintsSolved << " / " << constraintsC.size() << " :: ENDED CLEANLY." << std::endl;
}
else
{
continuer = true;
nbrIteration++;
if(nbrIteration > this->nbrIterationSolver)
{
continuer = false;
std::cout << " IterativeImpulseBasedConstraintsSolver::Solve :: Constraints solved : " << nbrConstraintsSolved << " / " << constraintsC.size() << " :: ENDED WITH UNRESOLVED CONSTRAINTS." << std::endl;
}
}
//--------------------------------------
#ifdef debuglvl4
std::cout << " Qdot+ : " << std::endl;
transpose(qdot).afficher();
#endif
}
wrappingUp(c,q,qdot);
}
/* 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
}
