示例#1
0
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...;
}