diff --git a/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.h b/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.h index 7b2afa1a710..dfa29262eb2 100644 --- a/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.h +++ b/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.h @@ -114,6 +114,7 @@ class TetrahedronDiffusionFEMForceField : public core::behavior::ForceField, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + SingleLink, MechObject, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_mecaObj; protected: diff --git a/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.inl b/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.inl index 341bbb7c0f3..39360bfc9bb 100644 --- a/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.inl +++ b/Sofa/Component/Diffusion/src/sofa/component/diffusion/TetrahedronDiffusionFEMForceField.inl @@ -128,6 +128,7 @@ TetrahedronDiffusionFEMForceField::TetrahedronDiffusionFEMForceField( d_tagMeshMechanics(initData(&d_tagMeshMechanics, std::string("meca"),"tagMechanics","Tag of the Mechanical Object.")), d_drawConduc( initData(&d_drawConduc, (bool)false, "drawConduc","To display conductivity map.")) , l_topology(initLink("topology", "link to the topology container")) + , l_mecaObj(initLink("mecaObj", "link to the 3D mechanical MechanicalObject")) , m_topology(nullptr) { this->f_listening.setValue(true); @@ -198,12 +199,29 @@ void TetrahedronDiffusionFEMForceField::init() } /// Get the mechanical object containing the mesh position in 3D - core::objectmodel::Tag mechanicalTag(d_tagMeshMechanics.getValue()); - this->getContext()->get(mechanicalObject, mechanicalTag,sofa::core::objectmodel::BaseContext::SearchUp); - if (mechanicalObject==nullptr) + /// Priority is the explicit link, with a fallback to historical tag-based lookup. + if (!l_mecaObj.empty()) { - msg_error() << "cannot find the mechanical object named '" << mechanicalObject << msgendl; - return; + mechanicalObject = l_mecaObj.get(); + if (mechanicalObject == nullptr) + { + msg_error() << "No MechanicalState found at mecaObj link path '" << l_mecaObj.getLinkedPath() << "'."; + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + } + else + { + core::objectmodel::Tag mechanicalTag(d_tagMeshMechanics.getValue()); + this->getContext()->get(mechanicalObject, mechanicalTag, sofa::core::objectmodel::BaseContext::SearchUp); + if (mechanicalObject == nullptr) + { + msg_error() << "No MechanicalState found. Set 'mecaObj' link, or provide a valid 'tagMechanics' (current value: '" + << d_tagMeshMechanics.getValue() << "')."; + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + msg_info() << "Using legacy tagMechanics lookup to find MechanicalState with tag '" << d_tagMeshMechanics.getValue() << "'."; } if(d_transverseAnisotropyRatio.getValue()!=1.0) diff --git a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.cpp b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.cpp index f76c68e98f8..b87c620df36 100644 --- a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.cpp +++ b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.cpp @@ -24,16 +24,241 @@ #include #include #include +#include +#include +#include namespace sofa::component::linearsolver::iterative { +using CRSd = linearalgebra::CompressedRowSparseMatrix; +using CRS3d = linearalgebra::CompressedRowSparseMatrix>; +using FVd = linearalgebra::FullVector; + +// Preconditioned CG on an assembled CRS matrix (both CRS and CRS). +// +// Mirrors the generic PCGLinearSolver::solve() in the .inl, but drives the +// preconditioner directly on the assembled matrix (precond->invert(M) once, then +// precond->solve(M, z, r) per iteration) instead of the GraphScattered +// setRHS/solveSystem/dispatch plumbing, which does not exist for a CRS +// preconditioner such as SSORPreconditioner. The stopping test follows the +// generic version: r_norm = compared against tolerance * . +// +// The body is inlined into each specialisation of solve() below: it relies on the +// protected members (cgstep_*, d_graph, newton_iter, TempVectorContainer) of the +// solver, so it cannot live in a free helper, and a member template would leak +// into the GraphScattered specialisation which must keep the .inl implementation. + +// --------------------------------------------------------------------------- +// CompressedRowSparseMatrix specialisations +// --------------------------------------------------------------------------- +template<> +void PCGLinearSolver::checkLinearSystem() +{ + this->template doCheckLinearSystem< + sofa::component::linearsystem::MatrixLinearSystem>(); +} + +template<> +void PCGLinearSolver::ensureRequiredLinearSystemType() +{ + // MatrixLinearSystem is the expected assembled system — nothing to reject. +} + +template<> +void PCGLinearSolver::bwdInit() +{ + if (this->isComponentStateInvalid()) + return; + // No PreconditionedMatrixFreeSystem linking is needed for the CRS pipeline. +} + +template<> +void PCGLinearSolver::solve(CRSd& M, FVd& x, FVd& b) +{ + using MLSolver = MatrixLinearSolver; + + // Resolve the preconditioner (may be null -> plain CG). + MLSolver* precond = nullptr; + if (l_preconditioner.get() != nullptr && d_use_precond.getValue()) + { + precond = dynamic_cast(l_preconditioner.get()); + if (!precond) + msg_warning() << "Preconditioner '" << l_preconditioner->getName() + << "' is not a MatrixLinearSolver " + << "— running unpreconditioned."; + else + precond->invert(M); // factorise/refresh against the current matrix + } + + std::map>& graph = *d_graph.beginEdit(); + newton_iter++; + char name[256]; + snprintf(name, sizeof(name), "Error %d", newton_iter); + sofa::type::vector& graph_error = graph[std::string(name)]; + + const core::ExecParams* params = core::execparams::defaultInstance(); + typename Inherit::TempVectorContainer vtmp(this, params, M, x, b); + FVd& r = *vtmp.createTempVector(); + FVd& w = *vtmp.createTempVector(); + FVd& s = *vtmp.createTempVector(); + // createTempVector() default-constructs an empty FullVector; size it to the + // system before use, otherwise the preconditioner writes out of bounds. + r.resize(M.rowSize()); + w.resize(M.rowSize()); + s.resize(M.rowSize()); + + const SReal b_norm = b.dot(b); + const SReal tol = d_tolerance.getValue() * b_norm; + + r = M * x; + cgstep_beta(r, b, SReal(-1)); // r = b - M*x + + if (precond) precond->solve(M, w, r); // w = M^{-1} r + else w = r; + + SReal r_norm = r.dot(w); // r . M^{-1} r + graph_error.push_back(r_norm / b_norm); + + unsigned iter = 1; + while ((iter <= d_maxIter.getValue()) && (r_norm > tol)) + { + s = M * w; + const SReal dtq = w.dot(s); + const SReal alpha = r_norm / dtq; + cgstep_alpha(x, w, alpha); // x += alpha w + cgstep_alpha(r, s, -alpha); // r -= alpha (M w) + + if (precond) precond->solve(M, s, r); + else s = r; + + const SReal deltaOld = r_norm; + r_norm = r.dot(s); + graph_error.push_back(r_norm / b_norm); + const SReal beta = r_norm / deltaOld; + cgstep_beta(w, s, beta); // w = s + beta w + iter++; + } + + d_graph.endEdit(); + vtmp.deleteTempVector(&r); + vtmp.deleteTempVector(&w); + vtmp.deleteTempVector(&s); + sofa::helper::AdvancedTimer::valSet("PCG iterations", iter); +} + +// --------------------------------------------------------------------------- +// CompressedRowSparseMatrix> specialisations +// --------------------------------------------------------------------------- +template<> +void PCGLinearSolver::checkLinearSystem() +{ + this->template doCheckLinearSystem< + sofa::component::linearsystem::MatrixLinearSystem>(); +} + +template<> +void PCGLinearSolver::ensureRequiredLinearSystemType() +{ + // MatrixLinearSystem is the expected assembled system — nothing to reject. +} + +template<> +void PCGLinearSolver::bwdInit() +{ + if (this->isComponentStateInvalid()) + return; + // No PreconditionedMatrixFreeSystem linking is needed for the CRS pipeline. +} + +template<> +void PCGLinearSolver::solve(CRS3d& M, FVd& x, FVd& b) +{ + using MLSolver = MatrixLinearSolver; + + // Resolve the preconditioner (may be null -> plain CG). + MLSolver* precond = nullptr; + if (l_preconditioner.get() != nullptr && d_use_precond.getValue()) + { + precond = dynamic_cast(l_preconditioner.get()); + if (!precond) + msg_warning() << "Preconditioner '" << l_preconditioner->getName() + << "' is not a MatrixLinearSolver " + << "— running unpreconditioned."; + else + precond->invert(M); // factorise/refresh against the current matrix + } + + std::map>& graph = *d_graph.beginEdit(); + newton_iter++; + char name[256]; + snprintf(name, sizeof(name), "Error %d", newton_iter); + sofa::type::vector& graph_error = graph[std::string(name)]; + + const core::ExecParams* params = core::execparams::defaultInstance(); + typename Inherit::TempVectorContainer vtmp(this, params, M, x, b); + FVd& r = *vtmp.createTempVector(); + FVd& w = *vtmp.createTempVector(); + FVd& s = *vtmp.createTempVector(); + // createTempVector() default-constructs an empty FullVector; size it to the + // system before use, otherwise the preconditioner writes out of bounds. + r.resize(M.rowSize()); + w.resize(M.rowSize()); + s.resize(M.rowSize()); + + const SReal b_norm = b.dot(b); + const SReal tol = d_tolerance.getValue() * b_norm; + + r = M * x; + cgstep_beta(r, b, SReal(-1)); // r = b - M*x + + if (precond) precond->solve(M, w, r); // w = M^{-1} r + else w = r; + + SReal r_norm = r.dot(w); // r . M^{-1} r + graph_error.push_back(r_norm / b_norm); + + unsigned iter = 1; + while ((iter <= d_maxIter.getValue()) && (r_norm > tol)) + { + s = M * w; + const SReal dtq = w.dot(s); + const SReal alpha = r_norm / dtq; + cgstep_alpha(x, w, alpha); // x += alpha w + cgstep_alpha(r, s, -alpha); // r -= alpha (M w) + + if (precond) precond->solve(M, s, r); + else s = r; + + const SReal deltaOld = r_norm; + r_norm = r.dot(s); + graph_error.push_back(r_norm / b_norm); + const SReal beta = r_norm / deltaOld; + cgstep_beta(w, s, beta); // w = s + beta w + iter++; + } + + d_graph.endEdit(); + vtmp.deleteTempVector(&r); + vtmp.deleteTempVector(&w); + vtmp.deleteTempVector(&s); + sofa::helper::AdvancedTimer::valSet("PCG iterations", iter); +} + +// --------------------------------------------------------------------------- +// Factory registration + explicit instantiations +// --------------------------------------------------------------------------- void registerPCGLinearSolver(sofa::core::ObjectFactory* factory) { - factory->registerObjects(core::ObjectRegistrationData("Linear solver using the preconditioned conjugate gradient iterative algorithm.") - .add< PCGLinearSolver >()); + factory->registerObjects(core::ObjectRegistrationData( + "Linear solver using the preconditioned conjugate gradient iterative algorithm.") + .add< PCGLinearSolver >() + .add< PCGLinearSolver >() + .add< PCGLinearSolver >()); } template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver; +template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver; +template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver; } // namespace sofa::component::linearsolver::iterative diff --git a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.h b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.h index 38ddcd331b3..026c00c92a0 100644 --- a/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.h +++ b/Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/PCGLinearSolver.h @@ -24,6 +24,9 @@ #include #include +#include +#include +#include #include #include @@ -100,8 +103,80 @@ inline void PCGLinearSolver inline void PCGLinearSolver::cgstep_alpha(Vector& x,Vector& p, Real alpha); +// ----------------------------------------------------------------------------- +// CompressedRowSparseMatrix (CRS) specialisations. +// +// The generic (GraphScattered) implementation in the .inl targets a matrix-free +// system and is incompatible with an assembled CRS system on three points, so +// each is overridden in the .cpp: +// * checkLinearSystem() -> the generic one auto-creates a +// PreconditionedMatrixFreeSystem; +// CRS needs a plain MatrixLinearSystem. +// * ensureRequiredLinearSystemType() -> the generic one rejects anything that is +// not a PreconditionedMatrixFreeSystem. +// * solve() -> the generic precond path drives the +// preconditioner through the GraphScattered +// setRHS/solveSystem/dispatch plumbing; for a +// CRS preconditioner (e.g. SSORPreconditioner) +// we call precond->invert(M)/precond->solve() +// directly on the assembled matrix. +// This makes PCGLinearSolver usable with an assembled (CRS) linear system, hence +// with LinearSolverConstraintCorrection, which requires one. +// ----------------------------------------------------------------------------- +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix, + linearalgebra::FullVector>::solve( + linearalgebra::CompressedRowSparseMatrix&, + linearalgebra::FullVector&, + linearalgebra::FullVector&); + +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix, + linearalgebra::FullVector>::checkLinearSystem(); + +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix, + linearalgebra::FullVector>::ensureRequiredLinearSystemType(); + +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix, + linearalgebra::FullVector>::bwdInit(); + +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix>, + linearalgebra::FullVector>::solve( + linearalgebra::CompressedRowSparseMatrix>&, + linearalgebra::FullVector&, + linearalgebra::FullVector&); + +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix>, + linearalgebra::FullVector>::checkLinearSystem(); + +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix>, + linearalgebra::FullVector>::ensureRequiredLinearSystemType(); + +template<> +void PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix>, + linearalgebra::FullVector>::bwdInit(); + #if !defined(SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_PCGLINEARSOLVER_CPP) -template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver; +extern template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver; +extern template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix, + linearalgebra::FullVector>; +extern template class SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API PCGLinearSolver< + linearalgebra::CompressedRowSparseMatrix>, + linearalgebra::FullVector>; #endif // !defined(SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_PCGLINEARSOLVER_CPP) } // namespace sofa::component::linearsolver::iterative