void MinresSolver::solve_schur(AssembledSystem::vec& x,
const AssembledSystem& sys,
const AssembledSystem::vec& b,
real damping) const {
// unconstrained velocity
vec tmp(sys.m);
response->solve(tmp, b.head(sys.m));
x.head( sys.m ) = tmp;
if( sys.n ) {
::schur<response_type> A(sys, *response, damping);
vec rhs = b.tail(sys.n) - sys.J * x.head(sys.m);
vec lambda = x.tail(sys.n);
typedef ::minres<SReal> solver_type;
solver_type::params p = params(rhs);
solver_type::solve(lambda, A, rhs, p);
// constraint velocity correction
response->solve(tmp, sys.J.transpose() * lambda );
x.head( sys.m ) += tmp;
x.tail( sys.n ) = lambda;
report("minres (schur)", p );
}
}
void LDLTSolver::solve(AssembledSystem::vec& res,
const AssembledSystem& sys,
const AssembledSystem::vec& rhs) const {
assert( res.size() == sys.size() );
assert( rhs.size() == sys.size() );
vec Pv = (sys.P * rhs.head(sys.m));
typedef AssembledSystem::dmat dmat;
if( debug.getValue() ){
cerr<<"LDLTSolver::solve, rhs = " << rhs.transpose() << endl;
cerr<<"LDLTSolver::solve, Pv = " << Pv.transpose() << endl;
cerr<<"LDLTSolver::solve, H = " << endl << dmat(sys.H) << endl;
}
// in place solve
Pv = pimpl->Hinv.solve( Pv );
if( debug.getValue() ){
cerr<<"LDLTSolver::solve, free motion solution = " << Pv.transpose() << endl;
cerr<<"LDLTSolver::solve, verification = " << (sys.H * Pv).transpose() << endl;
cerr<<"LDLTSolver::solve, sys.m = " << sys.m << ", sys.n = " << sys.n << ", rhs.size = " << rhs.size() << endl;
}
res.head( sys.m ) = sys.P * Pv;
if( sys.n ) {
vec tmp = rhs.tail( sys.n ) - pimpl->HinvPJT.transpose() * rhs.head( sys.m );
// lambdas
res.tail( sys.n ) = pimpl->schur.solve( tmp );
// constraint forces
res.head( sys.m ) += sys.P * (pimpl->HinvPJT * res.tail( sys.n));
if( debug.getValue() ){
cerr<<"LDLTSolver::solve, free motion constraint error= " << -tmp.transpose() << endl;
cerr<<"LDLTSolver::solve, lambda = " << res.tail(sys.n).transpose() << endl;
cerr<<"LDLTSolver::solve, constraint forces = " << (sys.P * (pimpl->HinvPJT * res.tail( sys.n))).transpose() << endl;
}
}
}
static void write_qp(std::ofstream& out,
const AssembledSystem& sys,
const AssembledSystem::vec& rhs) {
const char endl = '\n';
out << sys.m << ' ' << sys.n << endl;
out << sys.H << endl;
out << -rhs.head(sys.m).transpose() << endl;
out << sys.P << endl;
if( sys.n ) {
out << sys.J << endl;
out << rhs.tail(sys.n).transpose() << endl;
// TODO unilateral mask !
}
}
void CompliantLDLTPreconditioner::apply( AssembledSystem::vec& res, const AssembledSystem::vec& v )
{
res.resize( v.size() );
res.head(preconditioner.rows()) = preconditioner.solve( v.head(preconditioner.rows()) );
res.tail( v.size()-preconditioner.rows() ) = v.tail( v.size()-preconditioner.rows() ); // in case of dofs have been added, like mouse...;
}