TEST(FastUpdate, BlockMatrixAdd)
{
typedef double Scalar;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
std::vector<size_t> N_list, M_list;
N_list.push_back(0);
N_list.push_back(10);
N_list.push_back(2);
M_list.push_back(10);
M_list.push_back(20);
M_list.push_back(4);
for (size_t n=0; n<N_list.size(); ++n) {
for (size_t m=0; m<M_list.size(); ++m) {
const size_t N = N_list[n];
const size_t M = M_list[m];
matrix_t A(N,N), B(N,M), C(M,N), D(M,M);
matrix_t E(N,N), F(N,M), G(M,N), H(M,M);
alps::ResizableMatrix<Scalar> invA(N,N), BigMatrix(N+M, N+M, 0);//, invBigMatrix2(N+M, N+M, 0);
randomize_matrix(A, 100);//100 is a seed
randomize_matrix(B, 200);
randomize_matrix(C, 300);
randomize_matrix(D, 400);
if (N>0) {
invA = A.inverse();
} else {
invA.destructive_resize(0,0);
}
copy_block(A,0,0,BigMatrix,0,0,N,N);
copy_block(B,0,0,BigMatrix,0,N,N,M);
copy_block(C,0,0,BigMatrix,N,0,M,N);
copy_block(D,0,0,BigMatrix,N,N,M,M);
const Scalar det_rat = compute_det_ratio_up<Scalar>(B, C, D, invA);
ASSERT_TRUE(std::abs(det_rat-determinant(BigMatrix)/A.determinant())<1E-8)
<< "N=" << N << " M=" << M << " " << std::abs(det_rat-determinant(BigMatrix)) << "/" << std::abs(det_rat)<<"="
<< std::abs(det_rat-determinant(BigMatrix)/A.determinant());
const Scalar det_rat2 = compute_inverse_matrix_up(B, C, D, invA);
ASSERT_TRUE(std::abs(det_rat-det_rat2)<1E-8) << "N=" << N << " M=" << M;
ASSERT_TRUE(norm_square(inverse(BigMatrix)-invA)<1E-8) << "N=" << N << " M=" << M;
}
}
}
matrix inv(const matrix& A)
{ static StopWatch watch("inv(matrix)");
watch.start();
int N = A.nRows();
myassert(N > 0);
myassert(N == A.nCols());
matrix invA(A); //destructible copy
int ldA = A.nRows(); //leading dimension
std::vector<int> iPivot(N); //pivot info
int info; //error code in return
//LU decomposition (in place):
zgetrf_(&N, &N, invA.data(), &ldA, iPivot.data(), &info);
if(info<0) { logPrintf("Argument# %d to LAPACK LU decomposition routine ZGETRF is invalid.\n", -info); stackTraceExit(1); }
if(info>0) { logPrintf("LAPACK LU decomposition routine ZGETRF found input matrix to be singular at the %d'th step.\n", info); stackTraceExit(1); }
//Compute inverse in place:
int lWork = (64+1)*N;
std::vector<complex> work(lWork);
zgetri_(&N, invA.data(), &ldA, iPivot.data(), work.data(), &lWork, &info);
if(info<0) { logPrintf("Argument# %d to LAPACK matrix inversion routine ZGETRI is invalid.\n", -info); stackTraceExit(1); }
if(info>0) { logPrintf("LAPACK matrix inversion routine ZGETRI found input matrix to be singular at the %d'th step.\n", info); stackTraceExit(1); }
watch.stop();
return invA;
}
// triangles = warped triangles
Mat warpImage(vector<vector<Point2f> > triangles, Mat srcIm, vector<Mat> affines)
{
Mat warpedIm = srcIm.clone();
//*
for (int i = 0; i < warpedIm.rows; i++)
for (int j = 0; j < warpedIm.cols; j++)
warpedIm.at<Vec3b>(i,j) = Vec3b(0,0,0);
//*/
for (int i = 0; i < warpedIm.cols; i++)//
{
for (int j = 0; j < warpedIm.rows; j++)
{
// get warped triangle belonged to
int idx = getAffineIndex(triangles, i,j);
if (idx == -1)
{
//cout << "bad triangle index found" << endl;
continue;
}
Mat curA = affines[idx];
//*
// affine mat: [ R | T ] inv: [ R^-1 | -R^-1 T]
// [ 0 0 0 | 1 ] inv: [ 0 0 0 | 1 ]
Mat R(2, 2, CV_64F); // NOT JUST ROTATION: rotation, scaling, shear
R.at<double>(0,0) = curA.at<double>(0,0);
R.at<double>(0,1) = curA.at<double>(0,1);
R.at<double>(1,0) = curA.at<double>(1,0);
R.at<double>(1,1) = curA.at<double>(1,1);
Mat invR = R.inv(DECOMP_SVD);
Mat invA(3, 3, CV_64F, 0.); // for (3,0-2)
// R^-1
invA.at<double>(0,0) = invR.at<double>(0,0);
invA.at<double>(0,1) = invR.at<double>(0,1);
invA.at<double>(1,0) = invR.at<double>(1,0);
invA.at<double>(1,1) = invR.at<double>(1,1);
// -R^-1 T
invA.at<double>(0,2) = -invR.at<double>(0,0)* curA.at<double>(0,2) -invR.at<double>(0,1)* curA.at<double>(1,2);
invA.at<double>(1,2) = -invR.at<double>(1,0)* curA.at<double>(0,2) -invR.at<double>(1,1)* curA.at<double>(1,2);
// 1
//invA.at<double>(2,2) = 1.;
//*/
//Mat invA = curA.inv(DECOMP_SVD);
//
double x(i), y(j);
double srcJ = invA.at<double>(0,0)*x + invA.at<double>(0,1)*y + invA.at<double>(0,2);
double srcI = invA.at<double>(1,0)*x + invA.at<double>(1,1)*y + invA.at<double>(1,2);
if (srcI < 0 || srcJ < 0)
warpedIm.at<Vec3b>(j, i) =Vec3b(0,0,0);//srcIm.at<Vec3b>(j,i);//i, j); // hack
else
warpedIm.at<Vec3b>(j, i) = srcIm.at<Vec3b>(srcI,srcJ);
}
}
//drawAllTriangles(warpedIm.clone(), triangles, "post warp", false);
//waitKey();
return warpedIm;
}
void LinearSolver::greens_function(int I, VECTOR & G_I)
{
SPAR_MAT * A = NULL;
initialize_matrix();
initialize_inverse();
G_I.reset(nVariables_);
VECTOR & x = G_I;
if (solverType_ == ITERATIVE_SOLVE_SYMMETRIC) {
DENS_VEC b(nVariables_); b = 0.0; b(I) = 1.0;
if (parallel_) {
A = new PAR_SPAR_MAT(LammpsInterface::instance()->world(), *matrixSparse_);
}
else {
A = new SPAR_MAT(*matrixSparse_);
}
const DIAG_MAT & PC = matrixDiagonal_;
int niter = maxIterations_;
double tol = tol_;
int convergence = CG(*A, x, b, PC, niter, tol);
if (convergence>0) {
stringstream ss;
ss << "CG greens_function solve did not converge,";
ss << " iterations: " << niter;
ss << " residual: " << tol;
throw ATC_Error(ss.str());
}
}
else if (solverType_ == ITERATIVE_SOLVE) {
DENS_VEC b(nVariables_); b = 0.0; b(I) = 1.0;
// VECTOR & bb = b;
if (parallel_) {
A = new PAR_SPAR_MAT(LammpsInterface::instance()->world(), *matrixSparse_);
}
else {
A = new SPAR_MAT(*matrixSparse_);
}
// const DENS_MAT A = matrixSparse_->dense_copy();
const DIAG_MAT & PC = matrixDiagonal_;
int iterations = maxIterations_;
int restarts = maxRestarts_;
double tol = tol_;
DENS_MAT H(maxRestarts_+1, maxRestarts_);
DENS_VEC xx(nVariables_);
int convergence = GMRES(*A, xx, b, PC, H, restarts, iterations, tol);
if (convergence>0) {
stringstream ss;
ss << "GMRES greens_function solve did not converge,";
ss << " iterations: " << iterations;
ss << " residual: " << tol;
throw ATC_Error(ss.str());
}
x.copy(xx.ptr(),xx.nRows());
}
else {
const DENS_MAT & invA = matrixInverse_;
if (constraintHandlerType_ == CONDENSE_CONSTRAINTS) {
set<int>::const_iterator itr;
for (itr = fixedSet_.begin(); itr != fixedSet_.end(); itr++) {
int ii = *itr;
x(ii) = 0;
}
itr = freeSet_.find(I);
if (itr !=freeSet_.end() ) {
int j = freeGlobalToCondensedMap_[I];
int i = 0;
for (itr = freeSet_.begin(); itr != freeSet_.end(); itr++,i++) {
int ii = *itr;
x(ii) = invA(j,i);
}
}
}
else {
for (int i = 0; i < nVariables_; ++i) x(i) = invA(I,i);
}
}
delete A;
}
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
}
BSplineReduction<Real,TVector>::BSplineReduction (int numCtrlPoints,
const TVector* ctrlPoints, int degree, Real fraction, int& outNumCtrlPoints,
TVector*& outCtrlPoints)
{
// Check for valid input. If invalid, return a "null" curve.
assertion(numCtrlPoints >= 2, "Invalid input\n");
assertion(ctrlPoints != 0, "Invalid input\n");
assertion(1 <= degree && degree < numCtrlPoints, "Invalid input\n");
if (numCtrlPoints < 2 || !ctrlPoints || degree < 1
|| degree >= numCtrlPoints)
{
outNumCtrlPoints = 0;
outCtrlPoints = 0;
return;
}
// Clamp the number of control points to [degree+1,quantity-1].
outNumCtrlPoints = (int)(fraction*numCtrlPoints);
if (outNumCtrlPoints > numCtrlPoints)
{
outNumCtrlPoints = numCtrlPoints;
outCtrlPoints = new1<TVector>(outNumCtrlPoints);
memcpy(outCtrlPoints, ctrlPoints, numCtrlPoints*sizeof(TVector));
return;
}
if (outNumCtrlPoints < degree + 1)
{
outNumCtrlPoints = degree + 1;
}
// Allocate output control points and set all to the zero vector.
outCtrlPoints = new1<TVector>(outNumCtrlPoints);
// Set up basis function parameters. Function 0 corresponds to the
// output curve. Function 1 corresponds to the input curve.
mDegree = degree;
mQuantity[0] = outNumCtrlPoints;
mQuantity[1] = numCtrlPoints;
for (int j = 0; j <= 1; ++j)
{
mNumKnots[j] = mQuantity[j] + mDegree + 1;
mKnot[j] = new1<Real>(mNumKnots[j]);
int i;
for (i = 0; i <= mDegree; ++i)
{
mKnot[j][i] = (Real)0;
}
Real factor = ((Real)1)/(Real)(mQuantity[j] - mDegree);
for (/**/; i < mQuantity[j]; ++i)
{
mKnot[j][i] = (i-mDegree)*factor;
}
for (/**/; i < mNumKnots[j]; i++)
{
mKnot[j][i] = (Real)1;
}
}
// Construct matrix A (depends only on the output basis function).
Real value, tmin, tmax;
int i0, i1;
mBasis[0] = 0;
mBasis[1] = 0;
BandedMatrix<Real> A(mQuantity[0], mDegree, mDegree);
for (i0 = 0; i0 < mQuantity[0]; ++i0)
{
mIndex[0] = i0;
tmax = MaxSupport(0, i0);
for (i1 = i0; i1 <= i0 + mDegree && i1 < mQuantity[0]; ++i1)
{
mIndex[1] = i1;
tmin = MinSupport(0, i1);
value = Integrate1<Real>::RombergIntegral(8, tmin, tmax,
Integrand, this);
A(i0, i1) = value;
A(i1, i0) = value;
}
}
// Construct A^{-1}.
GMatrix<Real> invA(mQuantity[0], mQuantity[0]);
bool solved = LinearSystem<Real>().Invert(A, invA);
assertion(solved, "Failed to solve linear system\n");
WM5_UNUSED(solved);
// Construct B (depends on both input and output basis functions).
mBasis[1] = 1;
GMatrix<Real> B(mQuantity[0], mQuantity[1]);
for (i0 = 0; i0 < mQuantity[0]; ++i0)
{
mIndex[0] = i0;
Real tmin0 = MinSupport(0, i0);
Real tmax0 = MaxSupport(0, i0);
for (i1 = 0; i1 < mQuantity[1]; ++i1)
{
mIndex[1] = i1;
Real tmin1 = MinSupport(1, i1);
Real tmax1 = MaxSupport(1, i1);
Intersector1<Real> intr(tmin0, tmax0, tmin1, tmax1);
intr.Find();
int quantity = intr.GetNumIntersections();
if (quantity == 2)
{
value = Integrate1<Real>::RombergIntegral(8,
intr.GetIntersection(0), intr.GetIntersection(1),
Integrand, this);
B[i0][i1] = value;
}
else
{
B[i0][i1] = (Real)0;
}
}
}
// Construct A^{-1}*B.
GMatrix<Real> prod = invA*B;
// Construct the control points for the least-squares curve.
memset(outCtrlPoints,0,outNumCtrlPoints*sizeof(TVector));
for (i0 = 0; i0 < mQuantity[0]; ++i0)
{
for (i1 = 0; i1 < mQuantity[1]; ++i1)
{
outCtrlPoints[i0] += ctrlPoints[i1]*prod[i0][i1];
}
}
}
