diff --git a/python3/cosserat/examples/testRigidDistanceMapping.py b/python3/cosserat/examples/testRigidDistanceMapping.py index 91ad612e..31dd7e91 100644 --- a/python3/cosserat/examples/testRigidDistanceMapping.py +++ b/python3/cosserat/examples/testRigidDistanceMapping.py @@ -144,7 +144,7 @@ def createScene(rootNode): rigidBaseNode2 = solverNode.addChild('rigidBase2') rigidBaseNode2.addObject('MechanicalObject', name="rigidState", template='Rigid3d', position="2 0. 0 0 0 0 1", showObject=True, showObjectScale=1.0) - # position="2 4 1 0.7071 0 0.7071 0", + # position="2 4 1 0.7071 0 0.7071 0", rigidBaseNode2.addObject('RestShapeSpringsForceField', name='rigid3', stiffness=0.2e1, template="Rigid3d", angularStiffness=0.2e1, external_points=0, mstate="@rigidState", points=0) @@ -161,17 +161,17 @@ def createScene(rootNode): distanceMapping = True if distanceMapping: distanceNode.addObject('RigidDistanceMapping', input1=rigidBaseNode2.getLinkPath(), - input2=rigidBaseNode1.getLinkPath(), newVersionOfFrameComputation='1', - output=distanceMo.getLinkPath(), first_point=[0], second_point=[0], name='distanceMap1') + input2=rigidBaseNode1.getLinkPath(), newVersionOfFrameComputation='1', + output=distanceMo.getLinkPath(), first_point=[0], second_point=[0], name='distanceMap1') solverNode.addObject('MechanicalMatrixMapper', template='Rigid3d,Rigid3d', object1=rigidBaseNode1.getLinkPath(), object2=rigidBaseNode2.getLinkPath(), name='mapper1', nodeToParse=distanceNode.getLinkPath()) distanceNode.addObject('CosseratNeedleSlidingConstraint', name='constraintMappingConstraint', - template="Rigid3d", useDirections=np.array([0, 1, 1, 0, 0, 0])) + template="Rigid3d", useDirections=np.array([0, 1, 1, 0, 0, 0])) else: distanceNode.addObject('RigidRigidMapping', name="interRigidMap", globalToLocalCoords='0', template="Rigid3d,Rigid3d") solverNode.addObject(Animation(rigidBaseNode1)) - return rootNode + return rootNode \ No newline at end of file diff --git a/src/CosseratPlugin/mapping/RigidDistanceMapping.h b/src/CosseratPlugin/mapping/RigidDistanceMapping.h index e0646378..e4c38276 100644 --- a/src/CosseratPlugin/mapping/RigidDistanceMapping.h +++ b/src/CosseratPlugin/mapping/RigidDistanceMapping.h @@ -54,132 +54,132 @@ template class RigidDistanceMapping : public core::Multi2Mapping { public: - SOFA_CLASS(SOFA_TEMPLATE3(RigidDistanceMapping, TIn1,TIn2, TOut), SOFA_TEMPLATE3(core::Multi2Mapping, TIn1, TIn2, TOut) ); - typedef core::Multi2Mapping Inherit; - - /// Input Model Type - typedef TIn1 In1; - typedef TIn2 In2; - - /// Output Model Type - typedef TOut Out; - - typedef typename In1::Coord Coord1 ; - typedef typename In1::Deriv Deriv1 ; - typedef typename In1::VecCoord In1VecCoord; - typedef typename In1::VecDeriv In1VecDeriv; - typedef typename In1::MatrixDeriv In1MatrixDeriv; - typedef Data In1DataVecCoord; - typedef Data In1DataVecDeriv; - typedef Data In1DataMatrixDeriv; - - typedef typename In2::Coord::value_type Real ; - typedef typename In2::Coord Coord2 ; - typedef typename In2::Deriv Deriv2 ; - typedef typename In2::VecCoord In2VecCoord; - typedef typename In2::VecDeriv In2VecDeriv; - typedef typename In2::MatrixDeriv In2MatrixDeriv; - typedef Data In2DataVecCoord; - typedef Data In2DataVecDeriv; - typedef Data In2DataMatrixDeriv; - typedef type::Mat<4,4,Real> Mat4x4; - - typedef Out OutDataTypes; - typedef type::Quat Rot; - typedef typename Out::VecCoord OutVecCoord; - typedef typename Out::Coord OutCoord; - typedef typename Out::Deriv OutDeriv; - typedef typename Out::VecDeriv OutVecDeriv; - typedef typename Out::MatrixDeriv OutMatrixDeriv; - typedef Data OutDataVecCoord; - typedef Data OutDataVecDeriv; - typedef Data OutDataMatrixDeriv; - - typedef typename SolidTypes::Transform Transform ; - typedef typename SolidTypes< Real>::SpatialVector SpatialVector ; - - enum { N=OutDataTypes::spatial_dimensions }; - typedef type::Mat Mat; - typedef type::Vec Vector ; - Mat rotation; + SOFA_CLASS(SOFA_TEMPLATE3(RigidDistanceMapping, TIn1,TIn2, TOut), SOFA_TEMPLATE3(core::Multi2Mapping, TIn1, TIn2, TOut) ); + typedef core::Multi2Mapping Inherit; + + /// Input Model Type + typedef TIn1 In1; + typedef TIn2 In2; + + /// Output Model Type + typedef TOut Out; + + typedef typename In1::Coord Coord1 ; + typedef typename In1::Deriv Deriv1 ; + typedef typename In1::VecCoord In1VecCoord; + typedef typename In1::VecDeriv In1VecDeriv; + typedef typename In1::MatrixDeriv In1MatrixDeriv; + typedef Data In1DataVecCoord; + typedef Data In1DataVecDeriv; + typedef Data In1DataMatrixDeriv; + + typedef typename In2::Coord::value_type Real ; + typedef typename In2::Coord Coord2 ; + typedef typename In2::Deriv Deriv2 ; + typedef typename In2::VecCoord In2VecCoord; + typedef typename In2::VecDeriv In2VecDeriv; + typedef typename In2::MatrixDeriv In2MatrixDeriv; + typedef Data In2DataVecCoord; + typedef Data In2DataVecDeriv; + typedef Data In2DataMatrixDeriv; + typedef type::Mat<4,4,Real> Mat4x4; + + typedef Out OutDataTypes; + typedef type::Quat Rot; + typedef typename Out::VecCoord OutVecCoord; + typedef typename Out::Coord OutCoord; + typedef typename Out::Deriv OutDeriv; + typedef typename Out::VecDeriv OutVecDeriv; + typedef typename Out::MatrixDeriv OutMatrixDeriv; + typedef Data OutDataVecCoord; + typedef Data OutDataVecDeriv; + typedef Data OutDataMatrixDeriv; + + typedef typename SolidTypes::Transform Transform ; + typedef typename SolidTypes< Real>::SpatialVector SpatialVector ; + + enum { N=OutDataTypes::spatial_dimensions }; + typedef type::Mat Mat; + typedef type::Vec Vector ; + Mat rotation; protected: - Data > d_index1 ; - Data > d_index2 ; - Data d_max ; - Data d_min ; - Data d_radius ; - Data d_color; - Data > d_index; - Data d_debug ; - Data d_newVersionOfFrameComputation; - - core::State* m_toModel; + Data > d_index1 ; + Data > d_index2 ; + Data d_max ; + Data d_min ; + Data d_radius ; + Data d_color; + Data > d_index; + Data d_debug ; + Data d_newVersionOfFrameComputation; + + core::State* m_toModel; protected: - /// Constructor - RigidDistanceMapping() ; - /// Destructor - ~RigidDistanceMapping() override = default; - sofa::Index m_minInd{}; + /// Constructor + RigidDistanceMapping() ; + /// Destructor + ~RigidDistanceMapping() override = default; + sofa::Index m_minInd{}; public: - /**********************SOFA METHODS**************************/ - void init() override; - void reinit() override; - void draw(const core::visual::VisualParams* /*vparams*/) override {} - - /**********************MAPPING METHODS**************************/ - void apply( - const core::MechanicalParams* /* mparams */, const type::vector& dataVecOutPos, - const type::vector& dataVecIn1Pos , - const type::vector& dataVecIn2Pos) override; - - void applyJ( - const core::MechanicalParams* /* mparams */, const type::vector< OutDataVecDeriv*>& dataVecOutVel, - const type::vector& dataVecIn1Vel, - const type::vector& dataVecIn2Vel) override; - - //ApplyJT Force - void applyJT( - const core::MechanicalParams* /* mparams */, const type::vector< In1DataVecDeriv*>& dataVecOut1Force, - const type::vector< In2DataVecDeriv*>& dataVecOut2RootForce, - const type::vector& dataVecInForce) override; - - void applyDJT(const core::MechanicalParams* /*mparams*/, core::MultiVecDerivId /*inForce*/, core::ConstMultiVecDerivId /*outForce*/) override{} - - /// This method must be reimplemented by all mappings if they need to support constraints. - void applyJT( - const core::ConstraintParams* cparams , const type::vector< In1DataMatrixDeriv*>& dataMatOut1Const , - const type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , - const type::vector& dataMatInConst) override; - - int computeTransform(Transform &global_H0_local, Transform &global_H1_local, Transform &local0_H_local1, - Quat &local_R_local0, const Coord1 &x1, const Coord2 &x2); - - int computeTransform2(unsigned int edgeInList, Transform &global_H_local0, Transform &global_H_local1, - const OutVecCoord &x); - - void getDOFtoLocalTransform(unsigned int edgeInList, Transform &DOF0_H_local0, Transform &DOF1_H_local1) + /**********************SOFA METHODS**************************/ + void init() override; + void reinit() override; + void draw(const core::visual::VisualParams* /*vparams*/) override {} + + /**********************MAPPING METHODS**************************/ + void apply( + const core::MechanicalParams* /* mparams */, const type::vector& dataVecOutPos, + const type::vector& dataVecIn1Pos , + const type::vector& dataVecIn2Pos) override; + + void applyJ( + const core::MechanicalParams* /* mparams */, const type::vector< OutDataVecDeriv*>& dataVecOutVel, + const type::vector& dataVecIn1Vel, + const type::vector& dataVecIn2Vel) override; + + //ApplyJT Force + void applyJT( + const core::MechanicalParams* /* mparams */, const type::vector< In1DataVecDeriv*>& dataVecOut1Force, + const type::vector< In2DataVecDeriv*>& dataVecOut2RootForce, + const type::vector& dataVecInForce) override; + + void applyDJT(const core::MechanicalParams* /*mparams*/, core::MultiVecDerivId /*inForce*/, core::ConstMultiVecDerivId /*outForce*/) override{} + + /// This method must be reimplemented by all mappings if they need to support constraints. + void applyJT( + const core::ConstraintParams* cparams , const type::vector< In1DataMatrixDeriv*>& dataMatOut1Const , + const type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , + const type::vector& dataMatInConst) override; + + int computeTransform(Transform &global_H0_local, Transform &global_H1_local, Transform &local0_H_local1, + Quat &local_R_local0, const Coord1 &x1, const Coord2 &x2); + + int computeTransform2(unsigned int edgeInList, Transform &global_H_local0, Transform &global_H_local1, + const OutVecCoord &x); + + void getDOFtoLocalTransform(unsigned int edgeInList, Transform &DOF0_H_local0, Transform &DOF1_H_local1) + { + // @Todo re-implement this function + /*if (d_dofsAndBeamsAligned.getValue()) { - // @Todo re-implement this function - /*if (d_dofsAndBeamsAligned.getValue()) - { - DOF0_H_local0.clear(); - DOF1_H_local1.clear(); - return; - }*/ - - /*DOF0_H_local0 = d_DOF0TransformNode0.getValue()[edgeInList]; - DOF1_H_local1 = d_DOF1TransformNode1.getValue()[edgeInList];*/ - } + DOF0_H_local0.clear(); + DOF1_H_local1.clear(); + return; + }*/ + + /*DOF0_H_local0 = d_DOF0TransformNode0.getValue()[edgeInList]; + DOF1_H_local1 = d_DOF1TransformNode1.getValue()[edgeInList];*/ + } public: - type::vector mVect_global_H_input2Frame; - type::vector mVect_input1Frame_H_input2Frame; - std::vector m_vecRotation; - OutVecCoord pointsR0; + type::vector mVect_global_H_input2Frame; + type::vector mVect_input1Frame_H_input2Frame; + std::vector m_vecRotation; + OutVecCoord pointsR0; }; -} // namespace sofa::component::mapping +} // namespace sofa::component::mapping \ No newline at end of file diff --git a/src/CosseratPlugin/mapping/RigidDistanceMapping.inl b/src/CosseratPlugin/mapping/RigidDistanceMapping.inl index af142757..522e0643 100644 --- a/src/CosseratPlugin/mapping/RigidDistanceMapping.inl +++ b/src/CosseratPlugin/mapping/RigidDistanceMapping.inl @@ -37,128 +37,128 @@ namespace sofa::component::mapping { - using sofa::core::objectmodel::BaseContext ; - using sofa::helper::AdvancedTimer; - using sofa::helper::WriteAccessor; - using sofa::defaulttype::SolidTypes ; - using sofa::type::RGBAColor; +using sofa::core::objectmodel::BaseContext ; +using sofa::helper::AdvancedTimer; +using sofa::helper::WriteAccessor; +using sofa::defaulttype::SolidTypes ; +using sofa::type::RGBAColor; template RigidDistanceMapping::RigidDistanceMapping() : m_toModel(NULL) - , d_index1(initData(&d_index1, "first_point", "index of the first model \n")) - , d_index2(initData(&d_index2, "second_point", "index of the second model \n")) - , d_max(initData(&d_max, (Real)1.0e-2, "max", "the maximum of the deformation.\n")) - , d_min(initData(&d_min, (Real)0.0, "min", "the minimum of the deformation.\n")) - , d_radius(initData(&d_radius, (Real)3.0, "radius", "the axis in which we want to show the deformation.\n")) - , d_color(initData(&d_color, type::Vec4f (1, 0., 1., 0.8) ,"color", "The default beam color")) - , d_index(initData(&d_index, "index", "if this parameter is false, you draw the beam with color " - "according to the force apply to each beam")) - , d_debug(initData(&d_debug, false, "debug", "show debug output.\n")) - ,d_newVersionOfFrameComputation(initData(&d_newVersionOfFrameComputation, false, "newVersionOfFrameComputation", "if true, the frame is computed with the new version of the code")) + , d_index1(initData(&d_index1, "first_point", "index of the first model \n")) + , d_index2(initData(&d_index2, "second_point", "index of the second model \n")) + , d_max(initData(&d_max, (Real)1.0e-2, "max", "the maximum of the deformation.\n")) + , d_min(initData(&d_min, (Real)0.0, "min", "the minimum of the deformation.\n")) + , d_radius(initData(&d_radius, (Real)3.0, "radius", "the axis in which we want to show the deformation.\n")) + , d_color(initData(&d_color, type::Vec4f (1, 0., 1., 0.8) ,"color", "The default beam color")) + , d_index(initData(&d_index, "index", "if this parameter is false, you draw the beam with color " + "according to the force apply to each beam")) + , d_debug(initData(&d_debug, false, "debug", "show debug output.\n")) + ,d_newVersionOfFrameComputation(initData(&d_newVersionOfFrameComputation, false, "newVersionOfFrameComputation", "if true, the frame is computed with the new version of the code")) { - d_debug.setValue(false); - - this->addUpdateCallback("updateMappedIndices", {&d_index1, &d_index2}, [this](const core::DataTracker& t) - { - SOFA_UNUSED(t); - this->init(); - // Resize the output MechanicalObject - // TO DO: This callback is developped specifically to answer changes made during a navigation - // scene of a Cosserat coaxial model. At the moment, the dynamic rediscretisation required by - // this scenario is done entirely on the curvilinear abscissas of the Cosserat mapping component. - // A better implementation would be to delegate the discretisation to a topology component, and - // handle 'topological' changes (on the Mechanical Object in output of this mapping) with the - // appropriate topology modifications API. - core::State* toModel = this->getToModels()[0]; - toModel->resize(m_minInd); - this->Inherit::init(); - - return sofa::core::objectmodel::ComponentState::Valid; - }, {}); + d_debug.setValue(false); + + this->addUpdateCallback("updateMappedIndices", {&d_index1, &d_index2}, [this](const core::DataTracker& t) + { + SOFA_UNUSED(t); + this->init(); + // Resize the output MechanicalObject + // TO DO: This callback is developped specifically to answer changes made during a navigation + // scene of a Cosserat coaxial model. At the moment, the dynamic rediscretisation required by + // this scenario is done entirely on the curvilinear abscissas of the Cosserat mapping component. + // A better implementation would be to delegate the discretisation to a topology component, and + // handle 'topological' changes (on the Mechanical Object in output of this mapping) with the + // appropriate topology modifications API. + core::State* toModel = this->getToModels()[0]; + toModel->resize(m_minInd); + this->Inherit::init(); + + return sofa::core::objectmodel::ComponentState::Valid; + }, {}); } template void RigidDistanceMapping::init() { - if(this->getFromModels1().empty() || this->getFromModels2().empty() || this->getToModels().empty()) - { - msg_error() << "Error while initializing ; input getFromModels1/getFromModels2/output not found" ; - return; - } - - const type::vector &m1Indices = d_index1.getValue(); - const type::vector &m2Indices = d_index2.getValue(); - - m_minInd = std::min(m1Indices.size(), m2Indices.size()); - if (m_minInd == 0) { - msg_info("") << " The size of the indices must not be equal to zero" ; - return; - } + if(this->getFromModels1().empty() || this->getFromModels2().empty() || this->getToModels().empty()) + { + msg_error() << "Error while initializing ; input getFromModels1/getFromModels2/output not found" ; + return; + } + + const type::vector &m1Indices = d_index1.getValue(); + const type::vector &m2Indices = d_index2.getValue(); + + m_minInd = std::min(m1Indices.size(), m2Indices.size()); + if (m_minInd == 0) { + msg_info("") << " The size of the indices must not be equal to zero" ; + return; + } } template void RigidDistanceMapping::reinit() { - // TO DO: This method is implemented as a quickfix, to only trigger an artificial call - // to the component's callback from a Python code. This is only a quickfix as the correct - // way to do this would be to implement a Python binding for the callback method. - - // Checking the componentState, to trigger a callback if other data fields (specifically - // d_index1 or d_index2) were changed - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; + // TO DO: This method is implemented as a quickfix, to only trigger an artificial call + // to the component's callback from a Python code. This is only a quickfix as the correct + // way to do this would be to implement a Python binding for the callback method. + + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_index1 or d_index2) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; } template void RigidDistanceMapping::apply( - const core::MechanicalParams* /* mparams */, const type::vector& dataVecOutPos, - const type::vector& dataVecIn1Pos , - const type::vector& dataVecIn2Pos) + const core::MechanicalParams* /* mparams */, const type::vector& dataVecOutPos, + const type::vector& dataVecIn1Pos , + const type::vector& dataVecIn2Pos) { - if(dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) - return; + if(dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) + return; - // Checking the componentState, to trigger a callback if other data fields (specifically - // d_index1 or d_index2) were changed - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_index1 or d_index2) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; - ///Do Apply - //We need only one input In model and input Root model (if present) - const In1VecCoord& input1MOPos = dataVecIn1Pos[0]->getValue(); - const In2VecCoord& input2MOPos = dataVecIn2Pos[0]->getValue(); + ///Do Apply + //We need only one input In model and input Root model (if present) + const In1VecCoord& input1MOPos = dataVecIn1Pos[0]->getValue(); + const In2VecCoord& input2MOPos = dataVecIn2Pos[0]->getValue(); - OutVecCoord& outputMOPos = *dataVecOutPos[0]->beginEdit(); - outputMOPos.resize(m_minInd); - pointsR0.resize(m_minInd); + OutVecCoord& outputMOPos = *dataVecOutPos[0]->beginEdit(); + outputMOPos.resize(m_minInd); + pointsR0.resize(m_minInd); - auto &m1Indices = d_index1.getValue(); - auto &m2Indices = d_index2.getValue(); + auto &m1Indices = d_index1.getValue(); + auto &m2Indices = d_index2.getValue(); - size_t baseIndex = 0; // index of the first point of the beam, add this to the data - mVect_global_H_input2Frame.clear(); - mVect_input1Frame_H_input2Frame.clear(); + size_t baseIndex = 0; // index of the first point of the beam, add this to the data + mVect_global_H_input2Frame.clear(); + mVect_input1Frame_H_input2Frame.clear(); - m_vecRotation.clear(); + m_vecRotation.clear(); - for (sofa::Index pid=0; pid::apply( * global_H_input2Frame.getOrientation() * will simply return the position of input2Frame (child frame) with respect to the global * frame (parent frame), that is : [1, 0, 0, 0, 0, 0, 1] - */ - Transform global_H_input2Frame = Transform(In2::getCPos(input2MOPos[tm2]), In2::getCRot(input2MOPos[tm2])); - // We compute a similar Transform for input1Frame - Transform global_H_input1Frame = Transform(In1::getCPos(input1MOPos[tm1]), In1::getCRot(input1MOPos[tm1])); + */ + Transform global_H_input2Frame = Transform(In2::getCPos(input2MOPos[tm2]), In2::getCRot(input2MOPos[tm2])); + // We compute a similar Transform for input1Frame + Transform global_H_input1Frame = Transform(In1::getCPos(input1MOPos[tm1]), In1::getCRot(input1MOPos[tm1])); - /* Now, we compute a new Transform from the local coordinates of input1Frame to the local + /* Now, we compute a new Transform from the local coordinates of input1Frame to the local * coordinates of input2Frame. This Transform can then be used directly to set the output * MechanicalObject position and orientation (which are the difference in position and * orientation between input1Frame and input2Frame, expressed in input2Frame) @@ -229,305 +229,305 @@ void RigidDistanceMapping::apply( * a Transform in which the parent frame is transform1's parent frame, and the child frame is * transform2's child frame. To make sense, transform1's child frame and transform2's parent frame * should be the same. - */ - Transform input2Frame_H_input1Frame = global_H_input2Frame.inversed() * global_H_input1Frame; - - // Updating the output - outCenter = input2Frame_H_input1Frame.getOrigin(); - outOri = input2Frame_H_input1Frame.getOrientation(); - outOri.normalize(); - outputMOPos[pid] = OutCoord(outCenter,outOri); - - // Saving the computed Transforms to be used in the rest of the mapping methods - mVect_global_H_input2Frame.push_back(global_H_input2Frame); - mVect_input1Frame_H_input2Frame.push_back(input2Frame_H_input1Frame); - } - else - { - outCenter = (In2::getCPos(input2MOPos[tm2]) - In1::getCPos(input1MOPos[tm1])); - outOri = (In1::getCRot(input1MOPos[tm1]).inverse()*In2::getCRot(input2MOPos[tm2])); - outOri.normalize(); - outputMOPos[pid] = OutCoord(outCenter,outOri); - } - - if (d_debug.getValue()) - { - std::cout << " in1 :" << input1MOPos[tm1] << std::endl; - std::cout << " in2 :" << input2MOPos[tm2] << std::endl; - std::cout << " out :" << outputMOPos[pid] << std::endl; - } + */ + Transform input2Frame_H_input1Frame = global_H_input2Frame.inversed() * global_H_input1Frame; + + // Updating the output + outCenter = input2Frame_H_input1Frame.getOrigin(); + outOri = input2Frame_H_input1Frame.getOrientation(); + outOri.normalize(); + outputMOPos[pid] = OutCoord(outCenter,outOri); + + // Saving the computed Transforms to be used in the rest of the mapping methods + mVect_global_H_input2Frame.push_back(global_H_input2Frame); + mVect_input1Frame_H_input2Frame.push_back(input2Frame_H_input1Frame); + } + else + { + outCenter = (In2::getCPos(input2MOPos[tm2]) - In1::getCPos(input1MOPos[tm1])); + outOri = (In1::getCRot(input1MOPos[tm1]).inverse()*In2::getCRot(input2MOPos[tm2])); + outOri.normalize(); + outputMOPos[pid] = OutCoord(outCenter,outOri); + } + + if (d_debug.getValue()) + { + std::cout << " in1 :" << input1MOPos[tm1] << std::endl; + std::cout << " in2 :" << input2MOPos[tm2] << std::endl; + std::cout << " out :" << outputMOPos[pid] << std::endl; } - dataVecOutPos[0]->endEdit(); + } + dataVecOutPos[0]->endEdit(); } template void RigidDistanceMapping:: applyJ( - const core::MechanicalParams* /* mparams */, const type::vector< OutDataVecDeriv*>& dataVecOutVel, - const type::vector& dataVecIn1Vel, - const type::vector& dataVecIn2Vel) { + const core::MechanicalParams* /* mparams */, const type::vector< OutDataVecDeriv*>& dataVecOutVel, + const type::vector& dataVecIn1Vel, + const type::vector& dataVecIn2Vel) { + + if(dataVecOutVel.empty() || dataVecIn1Vel.empty() ||dataVecIn2Vel.empty() ) + return; + + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_index1 or d_index2) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; - if(dataVecOutVel.empty() || dataVecIn1Vel.empty() ||dataVecIn2Vel.empty() ) - return; + Vector omega; - // Checking the componentState, to trigger a callback if other data fields (specifically - // d_index1 or d_index2) were changed - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; + const In1VecDeriv& input1MOVelocities = dataVecIn1Vel[0]->getValue(); + const In2VecDeriv& input2MOVelocities = dataVecIn2Vel[0]->getValue(); + OutVecDeriv& outputVelocities = *dataVecOutVel[0]->beginEdit(); - Vector omega; + const auto &m1Indices = d_index1.getValue(); + const auto &m2Indices = d_index2.getValue(); - const In1VecDeriv& input1MOVelocities = dataVecIn1Vel[0]->getValue(); - const In2VecDeriv& input2MOVelocities = dataVecIn2Vel[0]->getValue(); - OutVecDeriv& outputVelocities = *dataVecOutVel[0]->beginEdit(); + SpatialVector vDOF1, vDOF2; - const auto &m1Indices = d_index1.getValue(); - const auto &m2Indices = d_index2.getValue(); + for (sofa::Index index = 0; index < m_minInd; index++) + { + if (d_newVersionOfFrameComputation.getValue()) + { + // Let's compute the velocity of the input1Frame in the local coordinates of the input2Frame + + // Getting the velocities of input1Frame and input2Frame in global coordinates + auto input1MOVel = getVCenter(input1MOVelocities[m1Indices[index]]); + auto input2MOVel = getVCenter(input2MOVelocities[m2Indices[index]]); + + // The angular velocity `omega`, of the input1Frame with respect to the input2Frame in global frame is : + auto omega = getVOrientation(input1MOVelocities[m1Indices[index]]) - getVOrientation(input2MOVelocities[m2Indices[index]]); + // auto omega = mVect_input1Frame_H_input2Frame[index].getOrientation().rotate(getVOrientation(input1MOVelocities[m1Indices[index]])) ; - SpatialVector vDOF1, vDOF2; + // V1_2 = 1_R_2 * V1_1 + omega x p1 + // auto input1MOVelin2 = mVect_global_H_input2Frame[index].getOrientation().rotate(input2MOVel - input1MOVel) + cross(omega, getVCenter(input1MOVelocities[m1Indices[index]])); + auto diffVelIn2 = mVect_global_H_input2Frame[index].getOrientation().inverseRotate(input1MOVel - input2MOVel); // + cross(omega, mVect_input1Frame_H_input2Frame[index].getOrigin()); - for (sofa::Index index = 0; index < m_minInd; index++) + // Due to the fact that the velocity of input2Frame in its frame is zero, we can write : + getVCenter(outputVelocities[index]) = diffVelIn2; + getVOrientation(outputVelocities[index]) = mVect_global_H_input2Frame[index].getOrientation().inverseRotate(omega) ; + } + else { - if (d_newVersionOfFrameComputation.getValue()) - { - // Let's compute the velocity of the input1Frame in the local coordinates of the input2Frame - - // Getting the velocities of input1Frame and input2Frame in global coordinates - auto input1MOVel = getVCenter(input1MOVelocities[m1Indices[index]]); - auto input2MOVel = getVCenter(input2MOVelocities[m2Indices[index]]); - - // The angular velocity `omega`, of the input1Frame with respect to the input2Frame in global frame is : - auto omega = getVOrientation(input1MOVelocities[m1Indices[index]]) - getVOrientation(input2MOVelocities[m2Indices[index]]); - // auto omega = mVect_input1Frame_H_input2Frame[index].getOrientation().rotate(getVOrientation(input1MOVelocities[m1Indices[index]])) ; - - // V1_2 = 1_R_2 * V1_1 + omega x p1 - // auto input1MOVelin2 = mVect_global_H_input2Frame[index].getOrientation().rotate(input2MOVel - input1MOVel) + cross(omega, getVCenter(input1MOVelocities[m1Indices[index]])); - auto diffVelIn2 = mVect_global_H_input2Frame[index].getOrientation().inverseRotate(input1MOVel - input2MOVel); // + cross(omega, mVect_input1Frame_H_input2Frame[index].getOrigin()); - - // Due to the fact that the velocity of input2Frame in its frame is zero, we can write : - getVCenter(outputVelocities[index]) = diffVelIn2; - getVOrientation(outputVelocities[index]) = mVect_global_H_input2Frame[index].getOrientation().inverseRotate(omega) ; - } - else - { - getVCenter(outputVelocities[index]) = getVCenter(input2MOVelocities[m2Indices[index]]) - getVCenter(input1MOVelocities[m1Indices[index]]); - getVOrientation(outputVelocities[index]) = getVOrientation(input2MOVelocities[m2Indices[index]]) - getVOrientation(input1MOVelocities[m1Indices[index]]) ; - } + getVCenter(outputVelocities[index]) = getVCenter(input2MOVelocities[m2Indices[index]]) - getVCenter(input1MOVelocities[m1Indices[index]]); + getVOrientation(outputVelocities[index]) = getVOrientation(input2MOVelocities[m2Indices[index]]) - getVOrientation(input1MOVelocities[m1Indices[index]]) ; } + } - dataVecOutVel[0]->endEdit(); + dataVecOutVel[0]->endEdit(); - if (d_debug.getValue()) - std::cout << " =====> outVel[m1Indices[index]] : " << outputVelocities << std::endl; + if (d_debug.getValue()) + std::cout << " =====> outVel[m1Indices[index]] : " << outputVelocities << std::endl; } template void RigidDistanceMapping:: applyJT( - const core::MechanicalParams* /*mparams*/, const type::vector< In1DataVecDeriv*>& dataVecOut1Force, - const type::vector< In2DataVecDeriv*>& dataVecOut2Force, - const type::vector& dataVecInForce) { + const core::MechanicalParams* /*mparams*/, const type::vector< In1DataVecDeriv*>& dataVecOut1Force, + const type::vector< In2DataVecDeriv*>& dataVecOut2Force, + const type::vector& dataVecInForce) { - if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) - return; + if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) + return; - // Checking the componentState, to trigger a callback if other data fields (specifically - // d_index1 or d_index2) were changed - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_index1 or d_index2) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; - const OutVecDeriv& outputForces = dataVecInForce[0]->getValue(); + const OutVecDeriv& outputForces = dataVecInForce[0]->getValue(); - In1VecDeriv& input1MOForces = *dataVecOut1Force[0]->beginEdit(); - In2VecDeriv& input2MOForces = *dataVecOut2Force[0]->beginEdit(); + In1VecDeriv& input1MOForces = *dataVecOut1Force[0]->beginEdit(); + In2VecDeriv& input2MOForces = *dataVecOut2Force[0]->beginEdit(); - //@todo implementation of force modification - const auto &m1Indices = d_index1.getValue(); - const auto &m2Indices = d_index2.getValue(); + //@todo implementation of force modification + const auto &m1Indices = d_index1.getValue(); + const auto &m2Indices = d_index2.getValue(); - // Safety check - // TO DO: is it necessary to raise a warning or an error? - if (outputForces.size() != m_minInd) - return; + // Safety check + // TO DO: is it necessary to raise a warning or an error? + if (outputForces.size() != m_minInd) + return; - for (sofa::Index outputMOIndex = 0; outputMOIndex < m_minInd; outputMOIndex++) + for (sofa::Index outputMOIndex = 0; outputMOIndex < m_minInd; outputMOIndex++) + { + if (d_newVersionOfFrameComputation.getValue()) { - if (d_newVersionOfFrameComputation.getValue()) - { - // The outputForces are computed in the local frames of the input2Frame - - // Computing the forces of the input1Frame and input2Frame in the global coordinates - Transform global_H_input2Frame = mVect_global_H_input2Frame[outputMOIndex]; - auto force = global_H_input2Frame.getOrientation().rotate(getVCenter(outputForces[outputMOIndex])); - auto angForce = getVOrientation(outputForces[outputMOIndex]); // + cross(force,mVect_input1Frame_H_input2Frame[outputMOIndex].getOrigin()); - - getVCenter( input1MOForces[m1Indices[outputMOIndex]]) += force; - getVOrientation(input1MOForces[m1Indices[outputMOIndex]]) += angForce; - getVCenter( input2MOForces[m2Indices[outputMOIndex]]) -= force; - getVOrientation(input2MOForces[m2Indices[outputMOIndex]]) -= angForce; - } - else - { - getVCenter(input1MOForces[m1Indices[outputMOIndex]]) -= getVCenter(outputForces[outputMOIndex]); - getVOrientation(input1MOForces[m1Indices[outputMOIndex]]) -= getVOrientation(outputForces[outputMOIndex]); - getVCenter(input2MOForces[m2Indices[outputMOIndex]]) += getVCenter(outputForces[outputMOIndex]); - getVOrientation(input2MOForces[m2Indices[outputMOIndex]]) += getVOrientation(outputForces[outputMOIndex]); - } + // The outputForces are computed in the local frames of the input2Frame + + // Computing the forces of the input1Frame and input2Frame in the global coordinates + Transform global_H_input2Frame = mVect_global_H_input2Frame[outputMOIndex]; + auto force = global_H_input2Frame.getOrientation().rotate(getVCenter(outputForces[outputMOIndex])); + auto angForce = getVOrientation(outputForces[outputMOIndex]); // + cross(force,mVect_input1Frame_H_input2Frame[outputMOIndex].getOrigin()); + + getVCenter( input1MOForces[m1Indices[outputMOIndex]]) += force; + getVOrientation(input1MOForces[m1Indices[outputMOIndex]]) += angForce; + getVCenter( input2MOForces[m2Indices[outputMOIndex]]) -= force; + getVOrientation(input2MOForces[m2Indices[outputMOIndex]]) -= angForce; } + else + { + getVCenter(input1MOForces[m1Indices[outputMOIndex]]) -= getVCenter(outputForces[outputMOIndex]); + getVOrientation(input1MOForces[m1Indices[outputMOIndex]]) -= getVOrientation(outputForces[outputMOIndex]); + getVCenter(input2MOForces[m2Indices[outputMOIndex]]) += getVCenter(outputForces[outputMOIndex]); + getVOrientation(input2MOForces[m2Indices[outputMOIndex]]) += getVOrientation(outputForces[outputMOIndex]); + } + } - dataVecOut1Force[0]->endEdit(); - dataVecOut2Force[0]->endEdit(); + dataVecOut1Force[0]->endEdit(); + dataVecOut2Force[0]->endEdit(); } //___________________________________________________________________________ template void RigidDistanceMapping::applyJT( - const core::ConstraintParams*/*cparams*/ , const type::vector< In1DataMatrixDeriv*>& dataMatOut1Const, - const type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , - const type::vector& dataMatInConst) + const core::ConstraintParams*/*cparams*/ , const type::vector< In1DataMatrixDeriv*>& dataMatOut1Const, + const type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , + const type::vector& dataMatInConst) { - if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty() ) - return; + if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty() ) + return; + + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_index1 or d_index2) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + + In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the reference frame 1 + In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the reference frame 2 + const OutMatrixDeriv& in = dataMatInConst[0]->getValue(); // input constraints defined on the mapped frames + + const auto &m1Indices = d_index1.getValue(); + const auto &m2Indices = d_index2.getValue(); + typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + + for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) + { + typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); + typename In1MatrixDeriv::RowIterator o1 = out1.writeLine(rowIt.index()); // we store the constraint number + typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); + + int outputMOIndex = colIt.index(); + + //We compute the inputMO indices + auto input1MOIndex = m1Indices[outputMOIndex]; + auto input2MOIndex = m2Indices[outputMOIndex]; - // Checking the componentState, to trigger a callback if other data fields (specifically - // d_index1 or d_index2) were changed - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; + Deriv1 direction1; + Deriv2 direction2; - In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the reference frame 1 - In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the reference frame 2 - const OutMatrixDeriv& in = dataMatInConst[0]->getValue(); // input constraints defined on the mapped frames + const OutDeriv valueConst_ = colIt.val(); - const auto &m1Indices = d_index1.getValue(); - const auto &m2Indices = d_index2.getValue(); - typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + if (d_newVersionOfFrameComputation.getValue()) + { + Transform global_H_input2Frame = mVect_global_H_input2Frame[outputMOIndex]; + auto force = global_H_input2Frame.getOrientation().rotate(getVCenter(valueConst_)); + auto angForce = getVOrientation(valueConst_); //+ cross(force,mVect_input1Frame_H_input2Frame[outputMOIndex].getOrigin()); + In1::setDPos(direction1, force); + In1::setDRot(direction1, angForce); + In2::setDPos(direction2, -force); + In2::setDRot(direction2, -angForce); + } + else + { + // Compute the mapped Constraint on the beam nodes + In1::setDPos(direction1,-getVCenter(valueConst_)); + In1::setDRot(direction1,-getVOrientation(valueConst_)); + In2::setDPos(direction2,getVCenter(valueConst_)); + In2::setDRot(direction2,getVOrientation(valueConst_)); + } - for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) + if (d_debug.getValue()) { - typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename In1MatrixDeriv::RowIterator o1 = out1.writeLine(rowIt.index()); // we store the constraint number - typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); - - int outputMOIndex = colIt.index(); - - //We compute the inputMO indices - auto input1MOIndex = m1Indices[outputMOIndex]; - auto input2MOIndex = m2Indices[outputMOIndex]; - - Deriv1 direction1; - Deriv2 direction2; - - const OutDeriv valueConst_ = colIt.val(); - - if (d_newVersionOfFrameComputation.getValue()) - { - Transform global_H_input2Frame = mVect_global_H_input2Frame[outputMOIndex]; - auto force = global_H_input2Frame.getOrientation().rotate(getVCenter(valueConst_)); - auto angForce = getVOrientation(valueConst_); //+ cross(force,mVect_input1Frame_H_input2Frame[outputMOIndex].getOrigin()); - In1::setDPos(direction1, force); - In1::setDRot(direction1, angForce); - In2::setDPos(direction2, -force); - In2::setDRot(direction2, -angForce); - } - else - { - // Compute the mapped Constraint on the beam nodes - In1::setDPos(direction1,-getVCenter(valueConst_)); - In1::setDRot(direction1,-getVOrientation(valueConst_)); - In2::setDPos(direction2,getVCenter(valueConst_)); - In2::setDRot(direction2,getVOrientation(valueConst_)); - } - - if (d_debug.getValue()) - { - printf("1. ======================================================================================= \n"); - std::cout << "Constraint " << rowIt.index() << " ==> outputMOIndex: "<< outputMOIndex << std::endl; - std::cout << "input1MOIndex " << input1MOIndex << " ==> input2MOIndex "<< input2MOIndex << std::endl; - std::cout << "valueConst_: "<< valueConst_ << std::endl; - std::cout << "direction1: " << direction1 << std::endl; - std::cout << "direction2: " << direction2 << std::endl; - } - - o1.addCol(input1MOIndex, direction1); - o2.addCol(input2MOIndex, direction2); + printf("1. ======================================================================================= \n"); + std::cout << "Constraint " << rowIt.index() << " ==> outputMOIndex: "<< outputMOIndex << std::endl; + std::cout << "input1MOIndex " << input1MOIndex << " ==> input2MOIndex "<< input2MOIndex << std::endl; + std::cout << "valueConst_: "<< valueConst_ << std::endl; + std::cout << "direction1: " << direction1 << std::endl; + std::cout << "direction2: " << direction2 << std::endl; } - dataMatOut1Const[0]->endEdit(); - dataMatOut2Const[0]->endEdit(); + o1.addCol(input1MOIndex, direction1); + o2.addCol(input2MOIndex, direction2); + } + + dataMatOut1Const[0]->endEdit(); + dataMatOut2Const[0]->endEdit(); } template int RigidDistanceMapping::computeTransform(Transform &global_H_local1, - Transform &global_H_local2, - Transform &local1_H_local2, - Quat &local1_R_local2, - const Coord1 &x1, const Coord2 &x2) + Transform &global_H_local2, + Transform &local1_H_local2, + Quat &local1_R_local2, + const Coord1 &x1, const Coord2 &x2) { - /// 1. Get the indices of element and nodes - // @todo: check if we need to do this every time ! + /// 1. Get the indices of element and nodes + // @todo: check if we need to do this every time ! - /// 2. Computes the optional rigid transformation of DOF0_Transform_node0 and DOF1_Transform_node1 - // @todo: This part depend on the previous step, so it should be done in the same loop - Transform OBJ0_H_local0 = Transform(type::Vec3(0,0,0), Rot::identity()); - Transform OBJ1_H_local1 = Transform(type::Vec3(0,0,0), Rot::identity()); - //getDOFtoLocalTransform(edgeInList, DOF0_H_local0, DOF1_H_local1); + /// 2. Computes the optional rigid transformation of DOF0_Transform_node0 and DOF1_Transform_node1 + // @todo: This part depend on the previous step, so it should be done in the same loop + Transform OBJ0_H_local0 = Transform(type::Vec3(0,0,0), Rot::identity()); + Transform OBJ1_H_local1 = Transform(type::Vec3(0,0,0), Rot::identity()); + //getDOFtoLocalTransform(edgeInList, DOF0_H_local0, DOF1_H_local1); - /// 3. Computes the transformation global To local for both nodes - Transform global_H_OBJ0(x1.getCenter(), x1.getOrientation()); - Transform global_H_OBJ1(x2.getCenter(), x2.getOrientation()); + /// 3. Computes the transformation global To local for both nodes + Transform global_H_OBJ0(x1.getCenter(), x1.getOrientation()); + Transform global_H_OBJ1(x2.getCenter(), x2.getOrientation()); - /// - add a optional transformation - global_H_local1 = global_H_OBJ0*OBJ0_H_local0; - global_H_local2 = global_H_OBJ1*OBJ1_H_local1; + /// - add a optional transformation + global_H_local1 = global_H_OBJ0*OBJ0_H_local0; + global_H_local2 = global_H_OBJ1*OBJ1_H_local1; - /// 4. Compute the local frame - /// SIMPLIFICATION: local = local0: - local1_R_local2.clear(); + /// 4. Compute the local frame + /// SIMPLIFICATION: local = local0: + local1_R_local2.clear(); - global_H_OBJ0.set(type::Vec3(0,0,0), x1.getOrientation()); - global_H_OBJ1.set(type::Vec3(0,0,0), x2.getOrientation()); + global_H_OBJ0.set(type::Vec3(0,0,0), x1.getOrientation()); + global_H_OBJ1.set(type::Vec3(0,0,0), x2.getOrientation()); - /// - rotation due to the optional transformation - global_H_local1 = global_H_OBJ0*OBJ0_H_local0; - global_H_local2 = global_H_OBJ1*OBJ1_H_local1; + /// - rotation due to the optional transformation + global_H_local1 = global_H_OBJ0*OBJ0_H_local0; + global_H_local2 = global_H_OBJ1*OBJ1_H_local1; -// global_H_local1 = global_H_local1; - sofa::type::Quat local0_R_local1 = local1_H_local2.getOrientation(); - Transform local0_HR_local1(type::Vec3(0,0,0), local0_R_local1); + // global_H_local1 = global_H_local1; + sofa::type::Quat local0_R_local1 = local1_H_local2.getOrientation(); + Transform local0_HR_local1(type::Vec3(0,0,0), local0_R_local1); - global_H_local2 = global_H_local1 * local0_HR_local1.inversed(); + global_H_local2 = global_H_local1 * local0_HR_local1.inversed(); - return 1; - } + return 1; +} template int RigidDistanceMapping::computeTransform2(unsigned int edgeInList, - Transform &global_H_local0, - Transform &global_H_local1, - const OutVecCoord &x) + Transform &global_H_local0, + Transform &global_H_local1, + const OutVecCoord &x) { - /// 1. Get the indices of element and nodes - unsigned int node0Idx=0, node1Idx=0; - /*if ( getNodeIndices( edgeInList, node0Idx, node1Idx ) == -1) - { - dmsg_error() << "[computeTransform2] Error in getNodeIndices(). (Aborting)" ; - return -1; - }*/ - - /// 2. Computes the optional rigid transformation of DOF0_Transform_node0 and DOF1_Transform_node1 - Transform DOF0_H_local0, DOF1_H_local1; - getDOFtoLocalTransform(edgeInList, DOF0_H_local0, DOF1_H_local1); - - /// 3. Computes the transformation global To local for both nodes - Transform global_H_DOF0(x[node0Idx].getCenter(),x[node0Idx].getOrientation()); - Transform global_H_DOF1(x[node1Idx].getCenter(),x[node1Idx].getOrientation()); - /// - add a optional transformation - global_H_local0 = global_H_DOF0*DOF0_H_local0; - global_H_local1 = global_H_DOF1*DOF1_H_local1; - - return 1; /// no error - } -} // namespace sofa::components::mapping + /// 1. Get the indices of element and nodes + unsigned int node0Idx=0, node1Idx=0; + /*if ( getNodeIndices( edgeInList, node0Idx, node1Idx ) == -1) + { + dmsg_error() << "[computeTransform2] Error in getNodeIndices(). (Aborting)" ; + return -1; + }*/ + + /// 2. Computes the optional rigid transformation of DOF0_Transform_node0 and DOF1_Transform_node1 + Transform DOF0_H_local0, DOF1_H_local1; + getDOFtoLocalTransform(edgeInList, DOF0_H_local0, DOF1_H_local1); + + /// 3. Computes the transformation global To local for both nodes + Transform global_H_DOF0(x[node0Idx].getCenter(),x[node0Idx].getOrientation()); + Transform global_H_DOF1(x[node1Idx].getCenter(),x[node1Idx].getOrientation()); + /// - add a optional transformation + global_H_local0 = global_H_DOF0*DOF0_H_local0; + global_H_local1 = global_H_DOF1*DOF1_H_local1; + + return 1; /// no error +} +} // namespace sofa::components::mapping \ No newline at end of file