diff --git a/CMakeLists.txt b/CMakeLists.txt index 3fcf51ae4b8..0d323aefe6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -127,9 +127,6 @@ option(SOFA_WITH_DEVTOOLS "Compile with developement extra features." ON) ### Threading option(SOFA_WITH_THREADING "Compile sofa with thread-safetiness support (PARTIAL/EXPERIMENTAL)" ON) -### Experimental -option(SOFA_WITH_EXPERIMENTAL_FEATURES "Compile sofa with exprimental features regarding sparse matrices treatments under mappings" OFF) - ### Testing option(SOFA_BUILD_TESTS "Compile the automatic tests for Sofa, along with the gtest library." ON) if(SOFA_BUILD_TESTS) diff --git a/SofaKernel/SofaFramework/CMakeLists.txt b/SofaKernel/SofaFramework/CMakeLists.txt index 117430b9252..f96a872bf91 100644 --- a/SofaKernel/SofaFramework/CMakeLists.txt +++ b/SofaKernel/SofaFramework/CMakeLists.txt @@ -42,12 +42,6 @@ else() set(SOFA_WITH_THREADING_ "0") endif() -if(SOFA_WITH_EXPERIMENTAL_FEATURES) - set(SOFA_WITH_EXPERIMENTAL_FEATURES_ "1") -else() - set(SOFA_WITH_EXPERIMENTAL_FEATURES_ "0") -endif() - if(SOFA_WITH_DEPRECATED_COMPONENTS) set(SOFA_WITH_DEPRECATED_COMPONENTS_ "1") else() @@ -175,7 +169,6 @@ set(SOFA_BUILD_OPTIONS_SRC sharedlibrary_defines.h build_option_deprecated_components.h build_option_dump_visitor.h - build_option_experimental_features.h build_option_opengl.h build_option_threading.h ) diff --git a/SofaKernel/SofaFramework/build_option_experimental_features.h.in b/SofaKernel/SofaFramework/build_option_experimental_features.h.in deleted file mode 100644 index b6077bac3b9..00000000000 --- a/SofaKernel/SofaFramework/build_option_experimental_features.h.in +++ /dev/null @@ -1,27 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture, development version * -* (c) 2006-2018 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 . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_CONFIG_BUILD_OPTION_EXPERIMENTAL_FEATURES_H -#define SOFA_CONFIG_BUILD_OPTION_EXPERIMENTAL_FEATURES_H - -#define SOFA_WITH_EXPERIMENTAL_FEATURES() ${SOFA_WITH_EXPERIMENTAL_FEATURES_} - -#endif diff --git a/SofaKernel/framework/sofa/core/behavior/BaseMechanicalState.h b/SofaKernel/framework/sofa/core/behavior/BaseMechanicalState.h index c5845e2c37e..c36213d0f23 100644 --- a/SofaKernel/framework/sofa/core/behavior/BaseMechanicalState.h +++ b/SofaKernel/framework/sofa/core/behavior/BaseMechanicalState.h @@ -22,7 +22,6 @@ #ifndef SOFA_CORE_BEHAVIOR_BASEMECHANICALSTATE_H #define SOFA_CORE_BEHAVIOR_BASEMECHANICALSTATE_H -#include #include #include @@ -222,11 +221,9 @@ class SOFA_CORE_API BaseMechanicalState : public virtual BaseState /// build the jacobian of the constraint in a baseMatrix virtual void getConstraintJacobian(const ConstraintParams* params, sofa::defaulttype::BaseMatrix* J,unsigned int & off) = 0; -#if SOFA_WITH_EXPERIMENTAL_FEATURES() == 1 + /// fill the jacobian matrix (of the constraints) with identity blocks on the provided list of nodes(dofs) virtual void buildIdentityBlocksInJacobian(const sofa::helper::vector& list_n, core::MatrixDerivId &mID) = 0; -#endif - class ConstraintBlock { diff --git a/SofaKernel/framework/sofa/simulation/MechanicalVisitor.h b/SofaKernel/framework/sofa/simulation/MechanicalVisitor.h index c0350520319..2c5735e4cad 100644 --- a/SofaKernel/framework/sofa/simulation/MechanicalVisitor.h +++ b/SofaKernel/framework/sofa/simulation/MechanicalVisitor.h @@ -2077,6 +2077,7 @@ class SOFA_SIMULATION_CORE_API MechanicalAccumulateMatrixDeriv : public BaseMech bool reverseOrder; }; + /** Apply the constraints as filters to the given vector. This works for simple independent constraints, like maintaining a fixed point. */ diff --git a/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.h b/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.h index 0ee416d0f5b..4c90ba18c35 100644 --- a/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.h +++ b/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.h @@ -23,8 +23,6 @@ #define SOFA_COMPONENT_MECHANICALOBJECT_H #include "config.h" -#include - #include #include @@ -147,9 +145,7 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState Data< VecDeriv > vfree; ///< free velocity coordinates of the degrees of freedom Data< VecCoord > x0; ///< rest position coordinates of the degrees of freedom Data< MatrixDeriv > c; ///< constraints applied to the degrees of freedom -#if(SOFA_WITH_EXPERIMENTAL_FEATURES()==1) Data< MatrixDeriv > m; ///< mappingJacobian applied to the degrees of freedom -#endif Data< VecCoord > reset_position; ///< reset position coordinates of the degrees of freedom Data< VecDeriv > reset_velocity; ///< reset velocity coordinates of the degrees of freedom #endif @@ -308,8 +304,6 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState /// @} - - /// @name Integration related methods /// @{ @@ -376,9 +370,8 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState virtual void resetConstraint(const core::ConstraintParams* cparams) override; virtual void getConstraintJacobian(const core::ConstraintParams* cparams, sofa::defaulttype::BaseMatrix* J,unsigned int & off) override; -#if(SOFA_WITH_EXPERIMENTAL_FEATURES()==1) + virtual void buildIdentityBlocksInJacobian(const sofa::helper::vector& list_n, core::MatrixDerivId &mID) override; -#endif /// @} /// @name Debug diff --git a/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.inl b/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.inl index 08fc97d64d1..b0b3deb129c 100644 --- a/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.inl +++ b/SofaKernel/modules/SofaBaseMechanics/MechanicalObject.inl @@ -22,8 +22,6 @@ #ifndef SOFA_COMPONENT_MECHANICALOBJECT_INL #define SOFA_COMPONENT_MECHANICALOBJECT_INL -#include - #include #include #include @@ -87,9 +85,7 @@ MechanicalObject::MechanicalObject() , vfree(initData(&vfree, "free_velocity", "free velocity coordinates of the degrees of freedom")) , x0(initData(&x0, "rest_position", "rest position coordinates of the degrees of freedom")) , c(initData(&c, "constraint", "constraints applied to the degrees of freedom")) -#if(SOFA_WITH_EXPERIMENTAL_FEATURES()==1) , m(initData(&m, "mappingJacobian", "mappingJacobian applied to the degrees of freedom")) -#endif , reset_position(initData(&reset_position, "reset_position", "reset position coordinates of the degrees of freedom")) , reset_velocity(initData(&reset_velocity, "reset_velocity", "reset velocity coordinates of the degrees of freedom")) , restScale(initData(&restScale, (SReal)1.0, "restScale", "optional scaling of rest position coordinates (to simulated pre-existing internal tension).(default = 1.0)")) @@ -149,9 +145,7 @@ MechanicalObject::MechanicalObject() setVecDeriv(core::VecDerivId::freeVelocity().index, &vfree); setVecDeriv(core::VecDerivId::resetVelocity().index, &reset_velocity); setVecMatrixDeriv(core::MatrixDerivId::constraintJacobian().index, &c); -#if(SOFA_WITH_EXPERIMENTAL_FEATURES()==1) setVecMatrixDeriv(core::MatrixDerivId::mappingJacobian().index, &m); -#endif // These vectors are set as modified as they are mandatory in the MechanicalObject. x .forceSet(); @@ -2544,12 +2538,10 @@ void MechanicalObject::resetConstraint(const core::ConstraintParams* MatrixDeriv *c = c_data.beginEdit(cParams); c->clear(); c_data.endEdit(cParams); -#if(SOFA_WITH_EXPERIMENTAL_FEATURES()==1) Data& m_data = *this->write(core::MatrixDerivId::mappingJacobian()); MatrixDeriv *m = m_data.beginEdit(cParams); m->clear(); m_data.endEdit(cParams); -#endif } template @@ -2580,7 +2572,6 @@ void MechanicalObject::getConstraintJacobian(const core::ConstraintPa off += this->getSize() * N; } -#if(SOFA_WITH_EXPERIMENTAL_FEATURES()==1) template void MechanicalObject::buildIdentityBlocksInJacobian(const sofa::helper::vector& list_n, core::MatrixDerivId &mID) { @@ -2607,9 +2598,6 @@ void MechanicalObject::buildIdentityBlocksInJacobian(const sofa::help cMatrix->endEdit(); } -#endif - - template std::list< core::behavior::BaseMechanicalState::ConstraintBlock > MechanicalObject::constraintBlocks( const std::list &indices) const diff --git a/examples/Components/animationloop/MechanicalMatrixMapper.html b/examples/Components/animationloop/MechanicalMatrixMapper.html new file mode 100644 index 00000000000..1fb6314e604 --- /dev/null +++ b/examples/Components/animationloop/MechanicalMatrixMapper.html @@ -0,0 +1,26 @@ + + + + + +
+ +

Mechanical matrix mapper

+ +

In this scene we demonstrate how to use the MechanicalMatrixMapper component. This component allows to map mechanical matrices (Stiffness, Mass) through a mapping. +

This is needed in SOFA scenes having these two following particularities: +

-- There are using a direct solver (e.g. SparseLDLSolver) that, unlike + iterative solvers, need to build the mechanical matrices. +

-- They involves ForceFields that implement addKToMatrix (i.e. that compute internal forces such as e.g. TetrahedronFEMForceField, + TetrahedronHyperElasticityFEMForceField, but not ConstantForceField which only contributes to the right-hand side) and that + ARE USED UNDER mappings. In this scene, the HexaHedronFEMForceField is used under a SubsetMultiMapping and a RigidMapping), and the MechanicalMatrixMapper is needed to map that ForceField to the degrees of freedom closer to the root in the scene graph. + +

Without this component, such a scene either crashes or gives unlogical behaviour. + +
+ +

+ + + + diff --git a/examples/Components/animationloop/MechanicalMatrixMapper.pyscn b/examples/Components/animationloop/MechanicalMatrixMapper.pyscn new file mode 100644 index 00000000000..570db1e550d --- /dev/null +++ b/examples/Components/animationloop/MechanicalMatrixMapper.pyscn @@ -0,0 +1,48 @@ +import Sofa + +def createScene(rootNode): + + rootNode.createObject('RequiredPlugin', name='ModelOrderReduction') + rootNode.createObject('VisualStyle', displayFlags='showCollisionModels hideMappings showForceFields') + + meshDivision = rootNode.createChild('meshDivision') + meshDivision.createObject('RegularGridTopology', name='topology', n=[2, 2, 4] , min=[-1, -1, -2], max=[1, 1, 2]) + meshOfStructure = meshDivision.createObject('Mesh',name='beamMesh', src="@topology") + meshDivision.createObject('MechanicalObject', template='Vec3d') + boxFixed= meshDivision.createObject('BoxROI',template="Vec3d", name='box_roi_fix',box=[-1, -1, -2.05, 1, 1, -1.5] , position="@beamMesh.position" ) + boxDeformable= meshDivision.createObject('BoxROI',template="Vec3d", name='box_roi_deformable',box=[-1, -1, -2.05, 1, 1, 1.5], position="@beamMesh.position" ) + boxRigid= meshDivision.createObject('BoxROI',template="Vec3d", name='box_roi_rigid',box=[-1, -1, 1.500001, 1, 1, 2.05], position="@beamMesh.position") + + SolverNode= rootNode.createChild('SolverNode') + SolverNode.createObject('EulerImplicit',verbose='false') + SolverNode.createObject('SparseLDLSolver', name="ldlsolveur") + SolverNode.createObject('MechanicalMatrixMapper', template='Vec3d,Rigid', object1='@./deformablePartNode/beamPart1Mech', object2='@./RigidNode/rigid1', nodeToParse='@./deformablePartNode/FEMNode') + + +##### Deformable Part of the BEAM (Main Body) + deformablePartNode= SolverNode.createChild('deformablePartNode') + deformablePartNode.createObject('MechanicalObject', template='Vec3d',name='beamPart1Mech', position="@"+boxDeformable.getPathName()+".pointsInROI") + +##### Rigid Part of the BEAM (Top) + RigidNode= SolverNode.createChild('RigidNode') + RigidNode.createObject("MechanicalObject",template="Rigid",name="rigid1", position=[0, 0, 2, 0, 0, 0, 1], showObject=True, showObjectScale=0.5) + RigidifiedNode= RigidNode.createChild('RigidifiedNode') + + RigidifiedNode.createObject("MechanicalObject",name="rigidMecha",template="Vec3d", position="@"+boxRigid.getPathName()+".pointsInROI") + RigidifiedNode.createObject("RigidMapping",globalToLocalCoords="true") + +##### COMBINED BEAM + FEMNode= deformablePartNode.createChild('FEMNode') + RigidifiedNode.addChild(FEMNode) + + FEMNode.createObject('Mesh',name='meshInput',src="@"+meshOfStructure.getPathName()) + FEMNode.createObject('MechanicalObject', template='Vec3d',name='beamMecha') + FEMNode.createObject('HexahedronFEMForceField', name='HexaFF', src="@meshInput", poissonRatio=0.49, youngModulus=2000) + FEMNode.createObject('RestShapeSpringsForceField', name='restShapeFF', points="@"+boxFixed.getPathName()+'.indices', stiffness=10000, angularStiffness=10000) + FEMNode.createObject('UniformMass', totalmass=0.1) + FEMNode.createObject('ConstantForceField', name='xMoins', indices=15, forces=[1000, 0, 0]) + FEMNode.createObject("SubsetMultiMapping",name="subsetMapping",template="Vec3d,Vec3d", input="@../beamPart1Mech @../../RigidNode/RigidifiedNode/rigidMecha",output="@./beamMecha", indexPairs="0 0 0 1 0 2 0 3 0 4 0 5 0 6 0 7 0 8 0 9 0 10 0 11 1 0 1 1 1 2 1 3") + + return rootNode + + diff --git a/modules/SofaGeneralAnimationLoop/CMakeLists.txt b/modules/SofaGeneralAnimationLoop/CMakeLists.txt index 8b886f08729..403c5fb7da5 100644 --- a/modules/SofaGeneralAnimationLoop/CMakeLists.txt +++ b/modules/SofaGeneralAnimationLoop/CMakeLists.txt @@ -12,14 +12,17 @@ set(SOURCE_FILES list(APPEND HEADER_FILES MultiStepAnimationLoop.h - MultiTagAnimationLoop.h ) + MultiTagAnimationLoop.h + MechanicalMatrixMapper.h + MechanicalMatrixMapper.inl) list(APPEND SOURCE_FILES MultiStepAnimationLoop.cpp - MultiTagAnimationLoop.cpp ) + MultiTagAnimationLoop.cpp + MechanicalMatrixMapper.cpp) add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES}) -target_link_libraries(${PROJECT_NAME} PUBLIC SofaSimulationCommon) +target_link_libraries(${PROJECT_NAME} PUBLIC SofaSimulationCommon SofaBaseLinearSolver) set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DSOFA_BUILD_GENERAL_ANIMATION_LOOP") set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER "${HEADER_FILES}") diff --git a/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.cpp b/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.cpp new file mode 100644 index 00000000000..7b88e7b4b15 --- /dev/null +++ b/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.cpp @@ -0,0 +1,77 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1 * +* (c) 2006-2018 MGH, INRIA, USTL, UJF, CNRS * +* * +* This library 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 library 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 library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* SOFA :: Modules * +* * +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include "MechanicalMatrixMapper.inl" +#include +#include +#include + +namespace sofa +{ + +namespace component +{ + +namespace interactionforcefield +{ + +using namespace sofa::defaulttype; + +//////////////////////////////////////////// FACTORY ////////////////////////////////////////////// +SOFA_DECL_CLASS(MechanicalMatrixMapper) + +int MechanicalMatrixMapperClass = core::RegisterObject("This component allows to map the stiffness (and mass) matrix through a mapping.") +#ifdef SOFA_WITH_FLOAT + .add< MechanicalMatrixMapper >() + .add< MechanicalMatrixMapper >() + .add< MechanicalMatrixMapper >() + .add< MechanicalMatrixMapper >() +#endif +#ifdef SOFA_WITH_DOUBLE + .add< MechanicalMatrixMapper >(true) + .add< MechanicalMatrixMapper >(true) + .add< MechanicalMatrixMapper >(true) + .add< MechanicalMatrixMapper >(true) +#endif + ; +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +#ifdef SOFA_WITH_DOUBLE +template class MechanicalMatrixMapper; +template class MechanicalMatrixMapper; +template class MechanicalMatrixMapper; +template class MechanicalMatrixMapper; +#endif +#ifdef SOFA_WITH_FLOAT +template class MechanicalMatrixMapper; +template class MechanicalMatrixMapper; +template class MechanicalMatrixMapper; +template class MechanicalMatrixMapper; +#endif + +} // namespace forcefield + +} // namespace component + +} // namespace sofa diff --git a/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.h b/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.h new file mode 100644 index 00000000000..1671527ddfa --- /dev/null +++ b/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.h @@ -0,0 +1,281 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1 * +* (c) 2006-2018 MGH, INRIA, USTL, UJF, CNRS * +* * +* This library 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 library 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 library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* SOFA :: Modules * +* * +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#ifndef SOFA_COMPONENT_ANIMATIONLOOP_MECHANICALMATRIXMAPPER_H +#define SOFA_COMPONENT_ANIMATIONLOOP_MECHANICALMATRIXMAPPER_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace sofa +{ + +namespace component +{ + +namespace interactionforcefield +{ + + +class SOFA_GENERAL_ANIMATION_LOOP_API MechanicalAccumulateJacobian : public simulation::BaseMechanicalVisitor +{ +public: + MechanicalAccumulateJacobian(const core::ConstraintParams* _cparams, core::MultiMatrixDerivId _res) + : simulation::BaseMechanicalVisitor(_cparams) + , res(_res) + , cparams(_cparams) + { + + } + + virtual void bwdMechanicalMapping(simulation::Node* node, core::BaseMapping* map) + { + ctime_t t0 = begin(node, map); + map->applyJT(cparams, res, res); + end(node, map, t0); + } + + /// Return a class name for this visitor + /// Only used for debugging / profiling purposes + virtual const char* getClassName() const { return "MechanicalAccumulateJacobian"; } + + virtual bool isThreadSafe() const + { + return false; + } + // This visitor must go through all mechanical mappings, even if isMechanical flag is disabled + virtual bool stopAtMechanicalMapping(simulation::Node* /*node*/, core::BaseMapping* /*map*/) + { + return false; // !map->isMechanical(); + } + +#ifdef SOFA_DUMP_VISITOR_INFO + void setReadWriteVectors() + { + } +#endif + +protected: + core::MultiMatrixDerivId res; + const sofa::core::ConstraintParams *cparams; +}; + +using sofa::core::objectmodel::BaseObject ; +using sofa::component::linearsolver::CompressedRowSparseMatrix ; +using sofa::core::behavior::MixedInteractionForceField ; +using sofa::core::behavior::BaseForceField ; +using sofa::core::behavior::BaseMass ; +using sofa::core::behavior::BaseMechanicalState ; +using sofa::core::behavior::MultiMatrixAccessor ; +using sofa::component::linearsolver::DefaultMultiMatrixAccessor ; +using sofa::defaulttype::BaseMatrix ; +using sofa::core::MechanicalParams ; +using sofa::core::objectmodel::ComponentState ; + + +/** + * \brief This component allows to map mechanical matrices (Stiffness, Mass) through a mapping. + * + * This is needed in SOFA scenes having these two following particularities: + * - There are using a direct solver (e.g. SparseLDLSolver) that, unlike + * iterative solvers, need to build the mechanical matrices. + * - They involves ForceFields that implement addKToMatrix (i.e. that compute internal forces such as e.g. TetrahedronFEMForceField, + * TetrahedronHyperElasticityFEMForceField, but not ConstantForceField which only contributes to the right-hand side) and that + * ARE USED UNDER mappings. + * Without this component, such a scene either crashes or gives unlogical behaviour. + * + * The component supports the case of subsetMultiMappings that map from one to two mechanical objects. + * An example using this component can be found in examples/Components/animationLoop/MechanicalMatrixMapper.pyscn +*/ +template +class SOFA_GENERAL_ANIMATION_LOOP_API MechanicalMatrixMapper : public MixedInteractionForceField +{ +public: + SOFA_CLASS(SOFA_TEMPLATE2(MechanicalMatrixMapper, TDataTypes1, TDataTypes2), SOFA_TEMPLATE2(MixedInteractionForceField, TDataTypes1, TDataTypes2)); + + typedef MixedInteractionForceField Inherit; + + // Vec3 + typedef TDataTypes1 DataTypes1; + typedef typename DataTypes1::VecCoord VecCoord1; + typedef typename DataTypes1::VecDeriv VecDeriv1; + typedef typename DataTypes1::Coord Coord1; + typedef typename DataTypes1::Deriv Deriv1; + typedef typename DataTypes1::Real Real1; + typedef typename DataTypes1::MatrixDeriv MatrixDeriv1; + typedef Data DataMatrixDeriv1; + typedef typename DataTypes1::MatrixDeriv::RowConstIterator MatrixDeriv1RowConstIterator; + typedef typename DataTypes1::MatrixDeriv::ColConstIterator MatrixDeriv1ColConstIterator; + static const unsigned int DerivSize1 = Deriv1::total_size; + typedef Data DataVecCoord1; + typedef Data DataVecDeriv1; + + // Rigid + typedef TDataTypes2 DataTypes2; + typedef typename DataTypes2::VecCoord VecCoord2; + typedef typename DataTypes2::VecDeriv VecDeriv2; + typedef typename DataTypes2::Coord Coord2; + typedef typename DataTypes2::Deriv Deriv2; + typedef typename DataTypes2::Real Real2; + typedef typename DataTypes2::MatrixDeriv MatrixDeriv2; + typedef Data DataMatrixDeriv2; + typedef typename DataTypes2::MatrixDeriv::RowConstIterator MatrixDeriv2RowConstIterator; + typedef typename DataTypes2::MatrixDeriv::ColConstIterator MatrixDeriv2ColConstIterator; + static const unsigned int DerivSize2 = Deriv2::total_size; + typedef Data DataVecCoord2; + typedef Data DataVecDeriv2; + +protected: + + SingleLink < MechanicalMatrixMapper, sofa::simulation::Node , BaseLink::FLAG_STOREPATH > l_nodeToParse; + + SingleLink < MechanicalMatrixMapper, sofa::core::behavior::BaseMechanicalState , BaseLink::FLAG_NONE > l_mechanicalState; + SingleLink < MechanicalMatrixMapper, sofa::core::behavior::BaseMass , BaseLink::FLAG_NONE > l_mappedMass; + MultiLink < MechanicalMatrixMapper, sofa::core::behavior::BaseForceField, BaseLink::FLAG_NONE > l_forceField; + + Data> d_forceFieldList; + + MechanicalMatrixMapper() ; + +public: + + ////////////////////////// Inherited from BaseObject ////////////////////// + virtual void init() override; + /////////////////////////////////////////////////////////////////////////// + + ////////////////////////// Inherited from ForceField ////////////////////// + virtual void addForce(const MechanicalParams* mparams, + DataVecDeriv1& f1, + DataVecDeriv2& f2, + const DataVecCoord1& x1, + const DataVecCoord2& x2, + const DataVecDeriv1& v1, + const DataVecDeriv2& v2) override; + + virtual void addDForce(const MechanicalParams* mparams, + DataVecDeriv1& df1, + DataVecDeriv2& df2, + const DataVecDeriv1& dx1, + const DataVecDeriv2& dx2) override; + + virtual void addKToMatrix(const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix ) override; + + virtual double getPotentialEnergy(const MechanicalParams* mparams, + const DataVecCoord1& x1, const DataVecCoord2& x2) const override; + /////////////////////////////////////////////////////////////////////////// + + + /** + * \brief Walk recursively through a node and its children linking with their forcefield + * \param node : from which node it start + * \param massName : won't link with given mass component + */ + void parseNode(sofa::simulation::Node *node ,std::string massName); + + +protected: + /** + * \brief This method computes the mapping jacobian. + * + */ + void accumulateJacobians(const MechanicalParams* mparams); + + /** + * \brief This method fills the jacobian matrix (of the mapping) with identity blocks on the provided list of nodes(dofs) + * + * + */ + virtual void buildIdentityBlocksInJacobian(core::behavior::BaseMechanicalState* mstate, sofa::core::MatrixDerivId Id); + + /** + * \brief This method encapsulates the jacobian accumulation to allow for specific optimisations in child classes + * + * + */ + virtual void accumulateJacobiansOptimized(const MechanicalParams* mparams); + + /** + * \brief This method adds the mass matrix to the system + * + * + */ + virtual void addMassToSystem(const MechanicalParams* mparams, const DefaultMultiMatrixAccessor* KAccessor); + + /** + * \brief This method does not do anything in this class but is used in child classes. + * + * + */ + virtual void addPrecomputedMassToSystem(const MechanicalParams* mparams,const unsigned int mstateSize,const Eigen::SparseMatrix &Jeig, Eigen::SparseMatrix& JtKJeig); + + /** + * \brief This method performs the copy of the jacobian matrix of the first mstate to the eigen sparse format + * + * + */ + virtual void optimizeAndCopyMappingJacobianToEigenFormat1(const typename DataTypes1::MatrixDeriv& J, Eigen::SparseMatrix& Jeig); + + /** + * \brief This method performs the copy of the jacobian matrix of the second mstate (not mandatory) to the eigen sparse format + * + * + */ + virtual void optimizeAndCopyMappingJacobianToEigenFormat2(const typename DataTypes2::MatrixDeriv& J, Eigen::SparseMatrix& Jeig); + + ////////////////////////// Inherited attributes //////////////////////////// + /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html + /// Bring inherited attributes and function in the current lookup context. + /// otherwise any access to the base::attribute would require + /// the "this->" approach. + using MixedInteractionForceField::f_printLog ; + using MixedInteractionForceField::mstate1 ; + using MixedInteractionForceField::mstate2 ; + using MixedInteractionForceField::getContext ; + using BaseObject::m_componentstate ; + //////////////////////////////////////////////////////////////////////////// + +}; + +} // namespace interactionforcefield + +} // namespace component + +} // namespace sofa + +#endif // SOFA_COMPONENT_ANIMATIONLOOP_MECHANICALMATRIXMAPPER_H diff --git a/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.inl b/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.inl new file mode 100644 index 00000000000..0c8646dbd91 --- /dev/null +++ b/modules/SofaGeneralAnimationLoop/MechanicalMatrixMapper.inl @@ -0,0 +1,521 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1 * +* (c) 2006-2018 MGH, INRIA, USTL, UJF, CNRS * +* * +* This library 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 library 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 library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* SOFA :: Modules * +* * +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#ifndef SOFA_COMPONENT_ANIMATIONLOOP_MECHANICALMATRIXMAPPER_INL +#define SOFA_COMPONENT_ANIMATIONLOOP_MECHANICALMATRIXMAPPER_INL + +#include "MechanicalMatrixMapper.h" +#include +#include +#include +#include + +// accumulate jacobian +#include +#include +#include +#include + +// verify timing +#include + +// Eigen Sparse Matrix +#include + +namespace sofa +{ + +namespace component +{ + +namespace interactionforcefield +{ + +template +MechanicalMatrixMapper::MechanicalMatrixMapper() + : + d_forceFieldList(initData(&d_forceFieldList,"forceFieldList","List of ForceField Names to work on (by default will take all)")), + l_nodeToParse(initLink("nodeToParse","link to the node on which the component will work, from this link the mechanicalState/mass/forceField links will be made")), + l_mechanicalState(initLink("mechanicalState","The mechanicalState with which the component will work on (filled automatically during init)")), + l_mappedMass(initLink("mass","mass with which the component will work on (filled automatically during init)")), + l_forceField(initLink("forceField","The ForceField(s) attached to this node (filled automatically during init)")) +{ +} + +template +void MechanicalMatrixMapper::init() +{ + if(m_componentstate==ComponentState::Valid){ + msg_warning() << "Calling an already fully initialized component. You should use reinit instead." ; + } + + if(l_nodeToParse.get() == NULL) + { + msg_error() << " failed to initialized -> missing/wrong link " << l_nodeToParse.getName() << " : " << l_nodeToParse.getLinkedPath() << sendl; + m_componentstate = ComponentState::Invalid ; + return; + } + + sofa::core::behavior::BaseInteractionForceField::init(); + + if (mstate1.get() == NULL || mstate2.get() == NULL) + { + msg_error() << " failed to initialized -> missing/wrong link " << mstate1.getName() << " or " << mstate2.getName() << sendl; + m_componentstate = ComponentState::Invalid ; + return; + } + + + // Add link to mass & and get mass component name to rm it from forcefields + std::string massName; + if (l_nodeToParse.get()->mass) + { + l_mappedMass.add(l_nodeToParse.get()->mass,l_nodeToParse.get()->mass->getPathName()); + massName.append(l_nodeToParse.get()->mass->getName()); + } + + // Add link to mechanical + if (l_nodeToParse.get()->mechanicalState) + { + l_mechanicalState.add(l_nodeToParse.get()->mechanicalState,l_nodeToParse.get()->mechanicalState->getPathName()); + } + else + { + msg_error() << ": no mechanical object to link to for this node path: " << l_nodeToParse.getPath(); + m_componentstate = ComponentState::Invalid ; + return; + } + + // Parse l_nodeToParse to find & link with the forcefields + parseNode(l_nodeToParse.get(),massName); + + if (l_forceField.size() == 0) + { + msg_error() << ": no forcefield to link to for this node path: " << l_nodeToParse.getPath(); + m_componentstate = ComponentState::Invalid ; + return; + } + + m_componentstate = ComponentState::Valid ; +} + +template +void MechanicalMatrixMapper::parseNode(sofa::simulation::Node *node,std::string massName) +{ + bool empty = d_forceFieldList.getValue().empty(); + for(unsigned int i=0; iforceField.size(); i++) + { + if (node->forceField[i]->name != massName) + { + bool found = true; + if (!empty) + found = (std::find(d_forceFieldList.getValue().begin(), d_forceFieldList.getValue().end(), node->forceField[i]->getName()) != d_forceFieldList.getValue().end()); + + if(found) + { + l_forceField.add(node->forceField[i],node->forceField[i]->getPathName()); + } + } + } + for (sofa::simulation::Node::ChildIterator it = node->child.begin(), itend = node->child.end(); it != itend; ++it) + { + parseNode(it->get(),massName); + } + return; +} + +template +void MechanicalMatrixMapper::buildIdentityBlocksInJacobian(core::behavior::BaseMechanicalState* mstate, sofa::core::MatrixDerivId Id) +{ + sofa::helper::vector list; + for (unsigned int i=0; igetSize(); i++) + list.push_back(i); + mstate->buildIdentityBlocksInJacobian(list, Id); +} + +template +void MechanicalMatrixMapper::accumulateJacobiansOptimized(const MechanicalParams* mparams) +{ + this->accumulateJacobians(mparams); +} + +template +void MechanicalMatrixMapper::accumulateJacobians(const MechanicalParams* mparams) +{ + const core::ExecParams* eparams = dynamic_cast( mparams ); + core::ConstraintParams cparams = core::ConstraintParams(*eparams); + + sofa::core::MatrixDerivId Id= sofa::core::MatrixDerivId::mappingJacobian(); + core::objectmodel::BaseContext* context = this->getContext(); + simulation::Node* gnode = dynamic_cast(context); + simulation::MechanicalResetConstraintVisitor(&cparams).execute(context); + buildIdentityBlocksInJacobian(l_mechanicalState,Id); + + MechanicalAccumulateJacobian(&cparams, core::MatrixDerivId::mappingJacobian()).execute(gnode); +} + +template +void copyKToEigenFormat(CompressedRowSparseMatrix< T >* K, Eigen::SparseMatrix& Keig) +{ + Keig.resize(K->nRow,K->nRow); + std::vector< Eigen::Triplet > tripletList; + tripletList.reserve(K->colsValue.size()); + + int row; + for (unsigned int it_rows_k=0; it_rows_k < K->rowIndex.size() ; it_rows_k ++) + { + row = K->rowIndex[it_rows_k] ; + typename CompressedRowSparseMatrix::Range rowRange( K->rowBegin[it_rows_k], K->rowBegin[it_rows_k+1] ); + for(sofa::defaulttype::BaseVector::Index xj = rowRange.begin() ; xj < rowRange.end() ; xj++ ) // for each non-null block + { + int col = K->colsIndex[xj]; // block column + const T& k = K->colsValue[xj]; // non-null element of the matrix + tripletList.push_back(Eigen::Triplet(row,col,k)); + } + } + Keig.setFromTriplets(tripletList.begin(), tripletList.end()); +} +template +static void copyMappingJacobianToEigenFormat(const typename InputFormat::MatrixDeriv& J, Eigen::SparseMatrix& Jeig) +{ + typedef typename InputFormat::MatrixDeriv::RowConstIterator RowConstIterator; + typedef typename InputFormat::MatrixDeriv::ColConstIterator ColConstIterator; + typedef typename InputFormat::Deriv Deriv; + int DerivSize = InputFormat::Deriv::total_size; + int nbRowsJ = Jeig.rows(); + int maxRowIndex = 0, maxColIndex = 0; + std::vector< Eigen::Triplet > tripletListJ; + + for (RowConstIterator rowIt = J.begin(); rowIt != J.end(); ++rowIt) + { + int rowIndex = rowIt.index(); + if (rowIndex>maxRowIndex) + maxRowIndex = rowIndex; + for (ColConstIterator colIt = rowIt.begin(); colIt != rowIt.end(); ++colIt) + { + int colIndex = colIt.index(); + Deriv elemVal = colIt.val(); + for (int i=0;i(rowIndex,DerivSize*colIndex + i,elemVal[i])); + if (colIndex>maxColIndex) + maxColIndex = colIndex; + } + } + } + Jeig.resize(nbRowsJ,DerivSize*(maxColIndex+1)); + Jeig.reserve(J.size()); + Jeig.setFromTriplets(tripletListJ.begin(), tripletListJ.end()); +} +template +void MechanicalMatrixMapper::optimizeAndCopyMappingJacobianToEigenFormat1(const typename DataTypes1::MatrixDeriv& J, Eigen::SparseMatrix& Jeig) +{ + copyMappingJacobianToEigenFormat(J, Jeig); +} + +template +void MechanicalMatrixMapper::optimizeAndCopyMappingJacobianToEigenFormat2(const typename DataTypes2::MatrixDeriv& J, Eigen::SparseMatrix& Jeig) +{ + copyMappingJacobianToEigenFormat(J, Jeig); +} + +template +void MechanicalMatrixMapper::addMassToSystem(const MechanicalParams* mparams, const DefaultMultiMatrixAccessor* KAccessor) +{ + if (l_mappedMass != NULL) + { + l_mappedMass->addMToMatrix(mparams, KAccessor); + } + else + { + msg_info() << "There is no mappedMass"; + } +} + +template +void MechanicalMatrixMapper::addPrecomputedMassToSystem(const MechanicalParams* mparams, const unsigned int mstateSize,const Eigen::SparseMatrix &Jeig, Eigen::SparseMatrix &JtKJeig) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(mstateSize); + SOFA_UNUSED(Jeig); + SOFA_UNUSED(JtKJeig); +} + +template +void MechanicalMatrixMapper::addKToMatrix(const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) +{ + if(m_componentstate != ComponentState::Valid) + return ; + + sofa::helper::system::thread::CTime *timer; + double timeScale, time, totime ; + timeScale = 1000.0 / (double)sofa::helper::system::thread::CTime::getTicksPerSec(); + + totime = (double)timer->getTime(); + + sofa::core::behavior::MechanicalState* ms1 = this->getMState1(); + sofa::core::behavior::MechanicalState* ms2 = this->getMState2(); + + + sofa::core::behavior::BaseMechanicalState* bms1 = this->getMechModel1(); + sofa::core::behavior::BaseMechanicalState* bms2 = this->getMechModel2(); + + + MultiMatrixAccessor::MatrixRef mat11 = matrix->getMatrix(mstate1); + MultiMatrixAccessor::MatrixRef mat22 = matrix->getMatrix(mstate2); + MultiMatrixAccessor::InteractionMatrixRef mat12 = matrix->getMatrix(mstate1, mstate2); + MultiMatrixAccessor::InteractionMatrixRef mat21 = matrix->getMatrix(mstate2, mstate1); + + + /////////////////////////// STEP 1 //////////////////////////////////// + /* -------------------------------------------------------------------------- */ + /* compute jacobians using generic implementation */ + /* -------------------------------------------------------------------------- */ + time= (double)timer->getTime(); + accumulateJacobiansOptimized(mparams); + msg_info(this) <<" accumulate J : "<<( (double)timer->getTime() - time)*timeScale<<" ms"; + + /////////////////////////// STEP 2 //////////////////////////////////// + /* -------------------------------------------------------------------------- */ + /* compute the stiffness K of the forcefield and put it in a rowsparseMatrix */ + /* get the stiffness matrix from the mapped ForceField */ + /* TODO: use the template of the FF for Real */ + /* -------------------------------------------------------------------------- */ + + + /////////////////////// GET K //////////////////////////////////////// + CompressedRowSparseMatrix< Real1 >* K = new CompressedRowSparseMatrix< Real1 > ( ); + + K->resizeBloc( 3*l_mechanicalState->getSize() , 3*l_mechanicalState->getSize()); + K->clear(); + DefaultMultiMatrixAccessor* KAccessor; + KAccessor = new DefaultMultiMatrixAccessor; + KAccessor->addMechanicalState( l_mechanicalState ); + KAccessor->setGlobalMatrix(K); + KAccessor->setupMatrices(); + + time= (double)timer->getTime(); + + for(unsigned int i=0; iaddKToMatrix(mparams, KAccessor); + } + + addMassToSystem(mparams,KAccessor); + + if (!K) + { + msg_error(this) << "matrix of the force-field system not found"; + return; + } + + msg_info(this)<<" time addKtoMatrix K : "<<( (double)timer->getTime() - time)*timeScale<<" ms"; + + /////////////////////// COMPRESS K /////////////////////////////////// + time= (double)timer->getTime(); + K->compress(); + msg_info(this) << " time compress K : "<<( (double)timer->getTime() - time)*timeScale<<" ms"; + + //------------------------------------------------------------------------------ + + time = (double)timer->getTime(); + Eigen::SparseMatrix Keig; + copyKToEigenFormat(K,Keig); + msg_info(this)<<" time set Keig : "<<( (double)timer->getTime() - time)*timeScale<<" ms"; + + + /////////////////////// COPY J1 AND J2 IN EIGEN FORMAT ////////////////////////////////////// + double startTime= (double)timer->getTime(); + sofa::core::MultiMatrixDerivId c = sofa::core::MatrixDerivId::mappingJacobian(); + const MatrixDeriv1 &J1 = c[ms1].read()->getValue(); + const MatrixDeriv2 &J2 = c[ms2].read()->getValue(); + + Eigen::SparseMatrix J1eig; + Eigen::SparseMatrix J2eig; + J1eig.resize(K->nRow, J1.begin().row().size()*DerivSize1); + unsigned int nbColsJ1 = 0, nbColsJ2 = 0; + + optimizeAndCopyMappingJacobianToEigenFormat1(J1, J1eig); + if (bms1 != bms2) + { + double startTime2= (double)timer->getTime(); + J2eig.resize(K->nRow, J2.begin().row().size()*DerivSize2); + optimizeAndCopyMappingJacobianToEigenFormat2(J2, J2eig); + msg_info(this)<<" time set J2eig alone : "<<( (double)timer->getTime() - startTime2)*timeScale<<" ms"; + } + + msg_info(this)<<" time getJ + set J1eig (and potentially J2eig) : "<<( (double)timer->getTime() - startTime)*timeScale<<" ms"; + startTime= (double)timer->getTime(); + + /////////////////////////// STEP 4 //////////////////////////////////// + /* -------------------------------------------------------------------------- */ + /* perform the multiplication with [J1t J2t] * K * [J1 J2] */ + /* -------------------------------------------------------------------------- */ + nbColsJ1 = J1eig.cols(); + if (bms1 != bms2) + { + nbColsJ2 = J2eig.cols(); + } + Eigen::SparseMatrix J1tKJ1eigen(nbColsJ1,nbColsJ1); + + J1tKJ1eigen = J1eig.transpose()*Keig*J1eig; + + msg_info(this)<<" time compute J1tKJ1eigen alone : "<<( (double)timer->getTime() - startTime)*timeScale<<" ms"; + + Eigen::SparseMatrix J2tKJ2eigen(nbColsJ2,nbColsJ2); + Eigen::SparseMatrix J1tKJ2eigen(nbColsJ1,nbColsJ2); + Eigen::SparseMatrix J2tKJ1eigen(nbColsJ2,nbColsJ1); + + if (bms1 != bms2) + { + double startTime2= (double)timer->getTime(); + J2tKJ2eigen = J2eig.transpose()*Keig*J2eig; + J1tKJ2eigen = J1eig.transpose()*Keig*J2eig; + J2tKJ1eigen = J2eig.transpose()*Keig*J1eig; + msg_info(this)<<" time compute J1tKJ2eigen J2TKJ2 and J2tKJ1 : "<<( (double)timer->getTime() - startTime2)*timeScale<<" ms"; + + } + + //-------------------------------------------------------------------------------------------------------------------- + + msg_info(this)<<" time compute all JtKJeigen with J1eig and J2eig : "<<( (double)timer->getTime() - startTime)*timeScale<<" ms"; + //int row; + unsigned int mstateSize = l_mechanicalState->getSize(); + addPrecomputedMassToSystem(mparams,mstateSize,J1eig,J1tKJ1eigen); + int offset,offrow, offcol; + startTime= (double)timer->getTime(); + offset = mat11.offset; + for (int k=0; k::InnerIterator it(J1tKJ1eigen,k); it; ++it) + { + mat11.matrix->add(offset + it.row(),offset + it.col(), it.value()); + } + msg_info(this)<<" time copy J1tKJ1eigen back to J1tKJ1 in CompressedRowSparse : "<<( (double)timer->getTime() - startTime)*timeScale<<" ms"; + + if (bms1 != bms2) + { + startTime= (double)timer->getTime(); + offset = mat22.offset; + for (int k=0; k::InnerIterator it(J2tKJ2eigen,k); it; ++it) + { + mat22.matrix->add(offset + it.row(),offset + it.col(), it.value()); + } + msg_info(this)<<" time copy J2tKJ2eigen back to J2tKJ2 in CompressedRowSparse : "<<( (double)timer->getTime() - startTime)*timeScale<<" ms"; + startTime= (double)timer->getTime(); + offrow = mat12.offRow; + offcol = mat12.offCol; + for (int k=0; k::InnerIterator it(J1tKJ2eigen,k); it; ++it) + { + mat22.matrix->add(offrow + it.row(),offcol + it.col(), it.value()); + } + msg_info(this)<<" time copy J1tKJ2eigen back to J1tKJ2 in CompressedRowSparse : "<<( (double)timer->getTime() - startTime)*timeScale<<" ms"; + startTime= (double)timer->getTime(); + offrow = mat21.offRow; + offcol = mat21.offCol; + for (int k=0; k::InnerIterator it(J2tKJ1eigen,k); it; ++it) + { + mat21.matrix->add(offrow + it.row(),offcol + it.col(), it.value()); + } + msg_info(this)<<" time copy J2tKJ1eigen back to J2tKJ1 in CompressedRowSparse : "<<( (double)timer->getTime() - startTime)*timeScale<<" ms"; + + } + + msg_info(this)<<" total time compute J() * K * J: "<<( (double)timer->getTime() - totime)*timeScale<<" ms"; + delete KAccessor; + delete K; + + + if(f_printLog.getValue()) + sout << "EXIT addKToMatrix\n" << sendl; + + + const core::ExecParams* eparams = dynamic_cast( mparams ); + core::ConstraintParams cparams = core::ConstraintParams(*eparams); + + core::objectmodel::BaseContext* context = this->getContext(); + simulation::MechanicalResetConstraintVisitor(&cparams).execute(context); + +} + +// Even though it does nothing, this method has to be implemented +// since it's a pure virtual in parent class +template +void MechanicalMatrixMapper::addForce(const MechanicalParams* mparams, + DataVecDeriv1& f1, + DataVecDeriv2& f2, + const DataVecCoord1& x1, + const DataVecCoord2& x2, + const DataVecDeriv1& v1, + const DataVecDeriv2& v2) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(f1); + SOFA_UNUSED(f2); + SOFA_UNUSED(x1); + SOFA_UNUSED(x2); + SOFA_UNUSED(v1); + SOFA_UNUSED(v2); +} + +// Even though it does nothing, this method has to be implemented +// since it's a pure virtual in parent class +template +void MechanicalMatrixMapper::addDForce(const MechanicalParams* mparams, + DataVecDeriv1& df1, + DataVecDeriv2& df2, + const DataVecDeriv1& dx1, + const DataVecDeriv2& dx2) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(df1); + SOFA_UNUSED(df2); + SOFA_UNUSED(dx1); + SOFA_UNUSED(dx2); +} + +// Even though it does nothing, this method has to be implemented +// since it's a pure virtual in parent class +template +double MechanicalMatrixMapper::getPotentialEnergy(const MechanicalParams* mparams, + const DataVecCoord1& x1, + const DataVecCoord2& x2) const +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(x1); + SOFA_UNUSED(x2); + + return 0.0; +} + +} // namespace forcefield + +} // namespace component + +} // namespace sofa + +#endif