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