diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/AccelerationBasedImplicitIntegrationScheme.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/AccelerationBasedImplicitIntegrationScheme.cpp index a3cb39311ae..9488cb2c10b 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/AccelerationBasedImplicitIntegrationScheme.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/AccelerationBasedImplicitIntegrationScheme.cpp @@ -27,6 +27,7 @@ #include #include #include +#include using sofa::simulation::mechanicalvisitor::MechanicalGetNonDiagonalMassesCountVisitor; @@ -46,8 +47,13 @@ void AccelerationBasedImplicitIntegrationScheme::doSetupIntegrationStep(const co // This is only there for lagrangian based simulation, to make sure we start using the real pose // instead of the free pos (same for velocity) - m_vop->v_eq(m_vResult, core::vec_id::write_access::velocity); - m_vop->v_eq(m_xResult, core::vec_id::write_access::position); + simulation::mechanicalvisitor::MechanicalVOpVisitor copyPoseInFreePose(params, m_xResult, core::vec_id::write_access::position); + simulation::mechanicalvisitor::MechanicalVOpVisitor copyVelInFreeVel(params, m_vResult, core::vec_id::write_access::velocity); + copyVelInFreeVel.setMapped(true); + copyPoseInFreePose.setMapped(true); + this->getContext()->executeVisitor(©PoseInFreePose); + this->getContext()->executeVisitor(©VelInFreeVel); + // Deal with higher order integration scheme const Size order = getIntegrationSchemeTimeOrder(); diff --git a/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/VelocityBasedImplicitIntegrationScheme.cpp b/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/VelocityBasedImplicitIntegrationScheme.cpp index 18ecc22bb96..070e2025dc3 100644 --- a/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/VelocityBasedImplicitIntegrationScheme.cpp +++ b/Sofa/framework/Simulation/Core/src/sofa/simulation/integrationscheme/VelocityBasedImplicitIntegrationScheme.cpp @@ -28,6 +28,7 @@ #include #include using sofa::simulation::mechanicalvisitor::MechanicalGetNonDiagonalMassesCountVisitor; +#include namespace sofa::simulation::integrationscheme { @@ -65,9 +66,12 @@ void VelocityBasedImplicitIntegrationScheme::doSetupIntegrationStep(const core:: // This is only there for lagrangian based simulation, to make sure we start using the real pose // instead of the free pos (same for velocity) - - m_vop->v_eq(m_vResult, core::vec_id::write_access::velocity); - m_vop->v_eq(m_xResult, core::vec_id::write_access::position); + simulation::mechanicalvisitor::MechanicalVOpVisitor copyPoseInFreePose(params, m_xResult, core::vec_id::write_access::position); + simulation::mechanicalvisitor::MechanicalVOpVisitor copyVelInFreeVel(params, m_vResult, core::vec_id::write_access::velocity); + copyVelInFreeVel.setMapped(true); + copyPoseInFreePose.setMapped(true); + this->getContext()->executeVisitor(©PoseInFreePose); + this->getContext()->executeVisitor(©VelInFreeVel); // Deal with higher order integration scheme const Size order = getIntegrationSchemeTimeOrder();