/******************************************************************************
* SOFA, Simulation Open-Framework Architecture *
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU Lesser General Public License as published by *
* the Free Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: contact@sofa-framework.org *
******************************************************************************/
#include <SofaMiscCollision/SolverMerger.h>
#include <sofa/helper/FnDispatcher.h>
#include <sofa/helper/FnDispatcher.inl>
#include <sofa/component/odesolver/forward/EulerSolver.h>
#include <sofa/component/odesolver/forward/RungeKutta4Solver.h>
#include <sofa/component/odesolver/backward/StaticSolver.h>
#include <sofa/component/odesolver/backward/EulerImplicitSolver.h>
#include <sofa/component/linearsolver/iterative/CGLinearSolver.h>
#include <sofa/component/constraint/lagrangian/solver/LCPConstraintSolver.h>
namespace sofa::component::collision
{
using sofa::core::behavior::OdeSolver;
using sofa::core::behavior::BaseLinearSolver;
using sofa::core::behavior::ConstraintSolver;
SolverSet::SolverSet(core::behavior::OdeSolver::SPtr o,
core::behavior::BaseLinearSolver::SPtr l,
core::behavior::ConstraintSolver::SPtr c) :
odeSolver(o),linearSolver(l),constraintSolver(c)
{}
namespace solvermergers
{
/// Create a new object which type is the template parameter, and
/// copy all its data fields values.
/// This function is meant to be used for ODE solvers and constraint solvers
template<class SolverType>
typename SolverType::SPtr copySolver(const SolverType& s)
{
const SolverType* src = &s;
typename SolverType::SPtr res = sofa::core::objectmodel::New<SolverType>();
for (auto* dataField : src->getDataFields())
{
msg_error_when(dataField == nullptr, "SolverMerger::copySolver") << "Found nullptr data field from " << src->getName();
if (auto* d = res->findData(dataField->getName()))
d->copyValueFrom(dataField);
}
return res;
}
ConstraintSolver::SPtr createConstraintSolver(OdeSolver* solver1, OdeSolver* solver2)
{
ConstraintSolver* csolver1 = nullptr;
if (solver1!=nullptr)
{
solver1->getContext()->get(csolver1, core::objectmodel::BaseContext::SearchDown);
}
ConstraintSolver* csolver2 = nullptr;
if (solver2!=nullptr)
{
solver2->getContext()->get(csolver2, core::objectmodel::BaseContext::SearchDown);
}
if (!csolver1 && !csolver2)
{
//no constraint solver associated to any ODE solver
return nullptr;
}
if (!csolver1)
{
//first ODE solver does not have any constraint solver. The second is copied to be shared with the first
if (auto* cs=dynamic_cast<constraint::lagrangian::solver::LCPConstraintSolver*>(csolver2))
return copySolver<constraint::lagrangian::solver::LCPConstraintSolver>(*cs);
}
else if (!csolver2)
{
//second ODE solver does not have any constraint solver. The first is copied to be shared with the second
if (auto* cs=dynamic_cast<constraint::lagrangian::solver::LCPConstraintSolver*>(csolver1))
return copySolver<constraint::lagrangian::solver::LCPConstraintSolver>(*cs);
}
else
{
//both ODE solvers have an associated constraint solver
if (auto* lcp1 = dynamic_cast<constraint::lagrangian::solver::LCPConstraintSolver*>(csolver1))
if (auto* lcp2 = dynamic_cast<constraint::lagrangian::solver::LCPConstraintSolver*>(csolver2))
{
constraint::lagrangian::solver::LCPConstraintSolver::SPtr newSolver = sofa::core::objectmodel::New<constraint::lagrangian::solver::LCPConstraintSolver>();
newSolver->initial_guess.setValue(lcp1->initial_guess.getValue() | lcp2->initial_guess.getValue());