コード例 #1
0
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 );
	}


}
コード例 #2
0
ファイル: AnalysisSolver.cpp プロジェクト: fredroy/sofa
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 !
    }

}
コード例 #3
0
ファイル: LDLTSolver.cpp プロジェクト: mhdsedighi/SOFA
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;
        }
    }
	
} 
コード例 #4
0
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...;
}