From 3b856ad09859170a1fe0f7199486041a651e2afd Mon Sep 17 00:00:00 2001 From: EulalieCoevoet Date: Thu, 26 Oct 2023 10:25:39 +0200 Subject: [PATCH] [mapping] new component (BeamProjectionDifferenceMultiMapping) (#113) * [mapping] new component (BeamProjectionDifferenceMultiMapping) * [mapping] change files' header --- ...amProjectionDifferenceMultiMappingTest.cpp | 141 +++++ CMakeLists.txt | 3 + .../BeamProjectionDifferenceMultiMapping.py | 87 +++ .../BeamProjectionDifferenceMultiMapping.cpp | 43 ++ .../BeamProjectionDifferenceMultiMapping.h | 206 +++++++ .../BeamProjectionDifferenceMultiMapping.inl | 565 ++++++++++++++++++ 6 files changed, 1045 insertions(+) create mode 100644 BeamAdapter_test/component/mapping/BeamProjectionDifferenceMultiMappingTest.cpp create mode 100644 examples/python3/component/BeamProjectionDifferenceMultiMapping.py create mode 100644 src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.cpp create mode 100644 src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h create mode 100644 src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl diff --git a/BeamAdapter_test/component/mapping/BeamProjectionDifferenceMultiMappingTest.cpp b/BeamAdapter_test/component/mapping/BeamProjectionDifferenceMultiMappingTest.cpp new file mode 100644 index 000000000..251d81940 --- /dev/null +++ b/BeamAdapter_test/component/mapping/BeamProjectionDifferenceMultiMappingTest.cpp @@ -0,0 +1,141 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include +using sofa::testing::BaseSimulationTest; + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace beamadapter::test { + +using namespace sofa::core; +using namespace sofa::component; +using sofa::type::Vec; +using sofa::type::Mat; +using sofa::core::objectmodel::New; + + +/** Test suite for BeamProjectionDifferenceMultiMapping. +The test cases are defined in the #Test_Cases member group. + */ +template +struct BeamProjectionDifferenceMultiMappingTest : public sofa::Multi2Mapping_test<_BeamProjectionDifferenceMultiMapping> +{ + + typedef _BeamProjectionDifferenceMultiMapping BeamProjectionDifferenceMultiMapping; + typedef sofa::Multi2Mapping_test Inherit; + + typedef typename BeamProjectionDifferenceMultiMapping::In1 In1DataTypes; + typedef typename BeamProjectionDifferenceMultiMapping::In2 In2DataTypes; + typedef statecontainer::MechanicalObject In1MechanicalObject; + typedef statecontainer::MechanicalObject In2MechanicalObject; + + typedef typename BeamProjectionDifferenceMultiMapping::Out OutDataTypes; + typedef statecontainer::MechanicalObject OutMechanicalObject; + + typedef typename In1DataTypes::VecCoord In1VecCoord; + typedef typename In2DataTypes::VecCoord In2VecCoord; + typedef typename OutDataTypes::VecCoord OutVecCoord; + + + void SetUp() override + { + } + + + BeamProjectionDifferenceMultiMapping* m_mapping; + sofa::component::fem::BeamInterpolation* m_interpolation; + sofa::component::topology::container::constant::MeshTopology* m_topology; + + + BeamProjectionDifferenceMultiMappingTest() + { + this->setupScene(); + this->errorMax = 10; + + m_mapping = static_cast( this->mapping ); + } + + + /** @name Test_Cases + For each of these cases, we can test if the mapping work + */ + bool test_oneBeam_twoParticles() + { + const int Nin1=1, Nin2=2; + this->in1Dofs.resize(Nin1); + this->in2Dofs.resize(Nin2); + this->outDofs->resize(Nin1); + + // parent position + In1VecCoord xin1(Nin1); + In1DataTypes::set( xin1[0], 0.5, 0., 0.); + + // the beam + In2VecCoord xin2(Nin2); + In2DataTypes::set( xin2[0], 0., 0., 0. ); + In2DataTypes::set( xin2[1], 1., 0., 0. ); + + // expected mapped values + OutVecCoord expectedChildCoords(Nin1); + OutDataTypes::set( expectedChildCoords[0], 0., 0., 0.); + + m_topology = sofa::modeling::addNew(this->parentsIn2).get(); + m_topology->addEdge(0,1); + m_mapping->l_in2Topology.set(m_topology); + m_interpolation = sofa::modeling::addNew>(this->parentsIn2).get(); + m_mapping->l_interpolation.set(m_interpolation); + m_mapping->d_indices.setValue(sofa::vector{0}); + sofa::type::vector directions{0, 1, 1, 0, 0, 0, 0}; + m_mapping->d_directions.setValue(directions); + + return this->runTest(sofa::vector{xin1}, sofa::vector{xin2}, expectedChildCoords); + } +}; + + + +// Define the list of types to instanciate. We do not necessarily need to test all combinations. +using ::testing::Types; +typedef Types< +beamadapter::mapping::BeamProjectionDifferenceMultiMapping +> DataTypes; // the types to instanciate. + +// Test suite for all the instanciations +TYPED_TEST_SUITE(BeamProjectionDifferenceMultiMappingTest, DataTypes); + +TYPED_TEST( BeamProjectionDifferenceMultiMappingTest, oneBeam_twoParticles ) +{ + // child coordinates given directly in parent frame + ASSERT_TRUE(this->test_oneBeam_twoParticles()); +} + +} // namespace diff --git a/CMakeLists.txt b/CMakeLists.txt index f1455dab3..d476908a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,8 @@ set(HEADER_FILES ${BEAMADAPTER_SRC}/component/mapping/AdaptiveBeamMapping.inl ${BEAMADAPTER_SRC}/component/mapping/BeamLengthMapping.h ${BEAMADAPTER_SRC}/component/mapping/BeamLengthMapping.inl + ${BEAMADAPTER_SRC}/component/mapping/BeamProjectionDifferenceMultiMapping.h + ${BEAMADAPTER_SRC}/component/mapping/BeamProjectionDifferenceMultiMapping.inl ${BEAMADAPTER_SRC}/component/mapping/MultiAdaptiveBeamMapping.h ${BEAMADAPTER_SRC}/component/mapping/MultiAdaptiveBeamMapping.inl @@ -94,6 +96,7 @@ set(SOURCE_FILES ${BEAMADAPTER_SRC}/component/mapping/AdaptiveBeamMapping.cpp ${BEAMADAPTER_SRC}/component/mapping/BeamLengthMapping.cpp + ${BEAMADAPTER_SRC}/component/mapping/BeamProjectionDifferenceMultiMapping.cpp ${BEAMADAPTER_SRC}/component/mapping/MultiAdaptiveBeamMapping.cpp ${BEAMADAPTER_SRC}/component/model/BaseRodSectionMaterial.cpp diff --git a/examples/python3/component/BeamProjectionDifferenceMultiMapping.py b/examples/python3/component/BeamProjectionDifferenceMultiMapping.py new file mode 100644 index 000000000..e6ae96504 --- /dev/null +++ b/examples/python3/component/BeamProjectionDifferenceMultiMapping.py @@ -0,0 +1,87 @@ +def createScene(rootnode): + settings = rootnode.addChild('Settings') + settings.addObject('RequiredPlugin', name='BeamAdapter') # Needed to use components [BeamProjectionDifferenceMultiMapping] + settings.addObject('RequiredPlugin', name='Sofa.Component.AnimationLoop') # Needed to use components [FreeMotionAnimationLoop] + settings.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Correction') # Needed to use components [GenericConstraintCorrection] + settings.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Solver') # Needed to use components [GenericConstraintSolver] + settings.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Projective') # Needed to use components [FixedConstraint] + settings.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct') # Needed to use components [SparseLDLSolver] + settings.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + settings.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward') # Needed to use components [EulerImplicitSolver] + settings.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.Spring') # Needed to use components [RestShapeSpringsForceField] + settings.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + settings.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Dynamic') # Needed to use components [EdgeSetTopologyContainer,PointSetTopologyContainer] + settings.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + settings.addObject('RequiredPlugin', name='Sofa.GUI.Component') # Needed to use components [AttachBodyButtonSetting] + + rootnode.addObject('VisualStyle', displayFlags='showBehavior showVisual') + rootnode.addObject('AttachBodyButtonSetting', stiffness=0.1) + rootnode.gravity.value = [0, -9810, 0] + rootnode.dt.value = 0.01 + + rootnode.addObject('FreeMotionAnimationLoop') + rootnode.addObject('GenericConstraintSolver', maxIterations=1000, tolerance=1e-3) + + simulation = rootnode.addChild('Simulation') + simulation.addObject('EulerImplicitSolver') + simulation.addObject('SparseLDLSolver', template='CompressedRowSparseMatrixMat3x3d') + simulation.addObject('GenericConstraintCorrection') + + # Beam + nbEdges = 3 + beam = simulation.addChild('Beam') + beam.addObject('EdgeSetTopologyContainer', edges=[[i, i + 1] for i in range(nbEdges)]) + beam.addObject('MechanicalObject', template='Rigid3', + position=[[100 * i, 0, 0, 0, 0, 0, 1] for i in range(nbEdges + 1)]) + beam.addObject('AdaptiveBeamForceFieldAndMass', massDensity=1e-6) + beam.addObject('BeamInterpolation', straight=False, dofsAndBeamsAligned=False, + defaultYoungModulus=1e5) + + # Particles + particles = simulation.addChild('Particles') + particles.addObject('PointSetTopologyContainer') + particles.addObject('MechanicalObject', template='Rigid3', showObject=True, showObjectScale=30, drawMode=1, + position=[[0, 0, 0, 0, 0, 0, 1], [80, 0, 0, 0, 0, 0, 1], [150, 0, 0, 0, 0, 0, 1]]) + particles.addObject('UniformMass', totalMass=0.005) + particles.addObject('FixedConstraint', indices=[0, 2]) # Fix first and last particles + + # This will constrain the first particle and its projection on the beam to remain attached together + fixing = particles.addChild('FixingConstraintParticle1') + beam.addChild(fixing) + fixing.addObject('MechanicalObject', template='Rigid3', position=[0, 0, 0, 0, 0, 0, 0]) + fixing.addObject('RestShapeSpringsForceField', stiffness=1e6, angularStiffness=1e6) + fixing.addObject('BeamProjectionDifferenceMultiMapping', + directions=[1, 1, 1, 1, 1, 1, 1], # The three positions and rotations + input1=particles.getMechanicalState().linkpath, + indicesInput1=[0], + input2=beam.getMechanicalState().linkpath, + interpolationInput2=beam.BeamInterpolation.linkpath, + output=fixing.getMechanicalState().linkpath) + + # This will constrain the second particle to slide along the beam, and it will also constrain the orientation + # of the beam and the particle to remain the same + sliding = particles.addChild('SlidingConstraintParticle2') + beam.addChild(sliding) + sliding.addObject('MechanicalObject', template='Rigid3', position=[0, 0, 0, 0, 0, 0, 0]) + sliding.addObject('RestShapeSpringsForceField', stiffness=1e6, angularStiffness=0) + sliding.addObject('BeamProjectionDifferenceMultiMapping', + directions=[0, 1, 1, 1, 1, 1, 1], # Only y, z positions to allow the particle to slide on the beam + # but this time we add the three rotations + input1=particles.getMechanicalState().linkpath, + indicesInput1=[1], + input2=beam.getMechanicalState().linkpath, + interpolationInput2=beam.BeamInterpolation.linkpath, + output=sliding.getMechanicalState().linkpath) + + # This will constrain the third particle to slide along the beam + sliding = particles.addChild('SlidingConstraintParticle3') + beam.addChild(sliding) + sliding.addObject('MechanicalObject', template='Rigid3', position=[0, 0, 0, 0, 0, 0, 0]) + sliding.addObject('RestShapeSpringsForceField', stiffness=1e6, angularStiffness=1e6) + sliding.addObject('BeamProjectionDifferenceMultiMapping', + directions=[0, 1, 1, 0, 0, 0, 0], # Only y, z positions to allow the particle to slide on the beam + input1=particles.getMechanicalState().linkpath, + indicesInput1=[2], + input2=beam.getMechanicalState().linkpath, + interpolationInput2=beam.BeamInterpolation.linkpath, + output=sliding.getMechanicalState().linkpath) diff --git a/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.cpp b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.cpp new file mode 100644 index 000000000..ffa54fee6 --- /dev/null +++ b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.cpp @@ -0,0 +1,43 @@ +/****************************************************************************** +* BeamAdapter plugin * +* (c) 2006 Inria, University of Lille, CNRS * +* * +* 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: see Authors.md * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +// +// Author: Eulalie Coevoet +// +// Copyright: See COPYING file that comes with this distribution + +#define BEAMADAPTER_MAPPING_BEAMPROJECTIONDIFFERENCEMULTIMAPPING_CPP + +#include + +#include +#include + +namespace beamadapter::mapping +{ + +using namespace sofa::defaulttype; + +// Register in the Factory +int BeamProjectionDifferenceMultiMappingClass = sofa::core::RegisterObject("Computes the difference between given points and their projection on a beam.") + .add< BeamProjectionDifferenceMultiMapping< Rigid3Types, Rigid3Types, Rigid3Types > >(); + +} // namespace diff --git a/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h new file mode 100644 index 000000000..d79bbd32f --- /dev/null +++ b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h @@ -0,0 +1,206 @@ +/****************************************************************************** +* BeamAdapter plugin * +* (c) 2006 Inria, University of Lille, CNRS * +* * +* 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: see Authors.md * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +// +// Author: Eulalie Coevoet +// +// Copyright: See COPYING file that comes with this distribution + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +namespace beamadapter::mapping +{ +using sofa::defaulttype::SolidTypes ; +using sofa::type::Matrix3; +using sofa::type::Matrix4; +using sofa::type::Vec3; +using sofa::type::Vec6; +using std::get; +using sofa::type::vector; + +/** +* \class BeamProjectionDifferenceMultiMapping +* @brief Computes the difference between a model's points and their projection on a beam +*/ + + +template +class SOFA_BEAMADAPTER_API BeamProjectionDifferenceMultiMapping : public sofa::core::Multi2Mapping +{ +public: + SOFA_CLASS(SOFA_TEMPLATE3(BeamProjectionDifferenceMultiMapping, TIn1,TIn2, TOut), SOFA_TEMPLATE3(sofa::core::Multi2Mapping, TIn1, TIn2, TOut) ); + typedef sofa::core::Multi2Mapping Inherit; + + /// Input Model Type + typedef TIn1 In1; + typedef TIn2 In2; + + /// Output Model Type + typedef TOut Out; + + typedef typename In1::Coord In1Coord; + typedef typename In1::Deriv In1Deriv; + typedef typename In1::VecCoord In1VecCoord; + typedef typename In1::VecDeriv In1VecDeriv; + typedef typename In1::MatrixDeriv In1MatrixDeriv; + typedef sofa::Data In1DataVecCoord; + typedef sofa::Data In1DataVecDeriv; + typedef sofa::Data In1DataMatrixDeriv; + + typedef sofa::defaulttype::Rigid3dTypes::Coord Rigid; + + typedef typename In2::Coord::value_type Real; + typedef typename In2::Coord In2Coord; + typedef typename In2::Deriv In2Deriv; + typedef typename In2::VecCoord In2VecCoord; + typedef typename In2::VecDeriv In2VecDeriv; + typedef typename In2::MatrixDeriv In2MatrixDeriv; + typedef sofa::Data In2DataVecCoord; + typedef sofa::Data In2DataVecDeriv; + typedef sofa::Data In2DataMatrixDeriv; + typedef sofa::type::Mat<4,4,Real> Mat4x4; + + 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 sofa::Data OutDataVecCoord; + typedef sofa::Data OutDataVecDeriv; + typedef sofa::Data OutDataMatrixDeriv; + + typedef typename SolidTypes::Transform Transform; + + typedef sofa::type::vector SeqEdges; + + enum + { + N = Out::spatial_dimensions + }; + enum + { + NIn1 = sofa::defaulttype::DataTypeInfo::Size + }; + enum + { + NIn2 = sofa::defaulttype::DataTypeInfo::Size + }; + enum + { + NOut = sofa::defaulttype::DataTypeInfo::Size + }; + +protected: + BeamProjectionDifferenceMultiMapping() ; + ~BeamProjectionDifferenceMultiMapping() override {} + +public: + void init() override; + void draw(const sofa::core::visual::VisualParams* vparams) override; + + void apply( + const sofa::core::MechanicalParams* mparams, const sofa::type::vector& dataVecOutPos, + const sofa::type::vector& dataVecIn1Pos , + const sofa::type::vector& dataVecIn2Pos) override; + + void applyJ( + const sofa::core::MechanicalParams* mparams, const sofa::type::vector< OutDataVecDeriv*>& dataVecOutVel, + const sofa::type::vector& dataVecIn1Vel, + const sofa::type::vector& dataVecIn2Vel) override; + + void applyJT( + const sofa::core::MechanicalParams* mparams, const sofa::type::vector< In1DataVecDeriv*>& dataVecOut1Force, + const sofa::type::vector< In2DataVecDeriv*>& dataVecOut2RootForce, + const sofa::type::vector& dataVecInForce) override; + + void applyDJT(const sofa::core::MechanicalParams* mparams, + sofa::core::MultiVecDerivId inForce, + sofa::core::ConstMultiVecDerivId outForce) override; + + virtual void applyJT( + const sofa::core::ConstraintParams* cparams , const sofa::type::vector< In1DataMatrixDeriv*>& dataMatOut1Const , + const sofa::type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , + const sofa::type::vector& dataMatInConst) override; + + virtual const sofa::type::vector* getJs() override; + + void computeProjection(const In1VecCoord &xFrom, const In2VecCoord &xTo, const bool &updateOrientation); + +public: + sofa::Data> d_indices; + sofa::Data> d_directions; + sofa::Data d_updateProjectionPosition; + sofa::Data d_updateProjectionOrientation; + sofa::Data d_draw; + sofa::Data d_drawSize; + + sofa::SingleLink, sofa::core::topology::BaseMeshTopology, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> l_in2Topology; + sofa::SingleLink, sofa::component::fem::BeamInterpolation, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> l_interpolation; + + using sofa::core::Multi2Mapping::d_componentState ; + +protected: + sofa::core::State* m_fromModel1; + sofa::core::State* m_fromModel2; + sofa::core::State* m_toModel; + + typedef sofa::linearalgebra::EigenSparseMatrix SparseMatrixEigen1; + SparseMatrixEigen1 m_eigenJacobian1; + typedef sofa::linearalgebra::EigenSparseMatrix SparseMatrixEigen2; + SparseMatrixEigen2 m_eigenJacobian2; ///< Jacobian of the mapping used by getJs + sofa::type::vector m_eigenJacobians; /// used by getJs + + bool m_updateJ; + +private: + + typedef struct { + int edgeIndex; + int pi1, pi2; // edge's points index + Transform interpolatedTransform; + Real alpha; + bool onBeam; + } MappedPoint; + + sofa::type::vector m_mappedPoints; + +}; + +// Declares template as extern to avoid the code generation of the template for +// each compilation unit. see: http://www.stroustrup.com/C++11FAQ.html#extern-templates +#if !defined(BEAMADAPTER_MAPPING_BEAMPROJECTIONDIFFERENCEMULTIMAPPING_CPP) +//extern template class BeamProjectionDifferenceMultiMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Vec3Types >; +extern template class SOFA_BEAMADAPTER_API BeamProjectionDifferenceMultiMapping< sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; +#endif + +} // namespace diff --git a/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl new file mode 100644 index 000000000..e895552bd --- /dev/null +++ b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl @@ -0,0 +1,565 @@ +/****************************************************************************** +* BeamAdapter plugin * +* (c) 2006 Inria, University of Lille, CNRS * +* * +* 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: see Authors.md * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +// +// Author: Eulalie Coevoet +// +// Copyright: See COPYING file that comes with this distribution + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace beamadapter::mapping +{ +using sofa::core::objectmodel::BaseContext ; +using sofa::helper::AdvancedTimer; +using sofa::helper::WriteAccessor; +using sofa::type::RGBAColor ; +using sofa::core::objectmodel::ComponentState; + +template +BeamProjectionDifferenceMultiMapping::BeamProjectionDifferenceMultiMapping() + : d_indices(initData(&d_indices, "indicesInput1", "Indices of model1 to project on model2 (beams)")) + , d_directions(initData(&d_directions, "directions", "Directions to project (in the local frame).")) + , d_updateProjectionPosition(initData(&d_updateProjectionPosition, false, "updateProjectionPosition", "Update the projection on the beam at each time step even when direction[0]=1.")) + , d_updateProjectionOrientation(initData(&d_updateProjectionOrientation, false, "updateProjectionOrientation", "Update the projection on the beam at each time step even when direction[0]=1.")) + , d_draw(initData(&d_draw, "draw", "Draw projection points and directions")) + , d_drawSize(initData(&d_drawSize, Real(3), "drawSize", "")) + , l_in2Topology(initLink("topologyInput2", "link to input2's topology container (beams to project on)")) + , l_interpolation(initLink("interpolationInput2", "link to input2's interpolation component (BeamInterpolation)")) + , m_fromModel1(NULL) + , m_fromModel2(NULL) + , m_toModel(NULL) + , m_updateJ(false) +{ + auto directions = sofa::helper::getWriteAccessor(d_directions); + directions.resize(OutDeriv::total_size, true); +} + + +template +void BeamProjectionDifferenceMultiMapping::init() +{ + d_componentState.setValue(ComponentState::Valid); + + m_eigenJacobians.resize( 2 ); + m_eigenJacobians[0] = &m_eigenJacobian1; + m_eigenJacobians[1] = &m_eigenJacobian2; + + if(this->getFromModels1().empty()) + { + msg_error() << "Error while initializing, input1 not found." ; + d_componentState.setValue(ComponentState::Invalid); + return; + } + m_fromModel1 = this->getFromModels1()[0]; + + if(this->getFromModels2().empty()) + { + msg_error() << "Error while initializing, input2 not found." ; + d_componentState.setValue(ComponentState::Invalid); + return; + } + m_fromModel2 = this->getFromModels2()[0]; + + if(this->getToModels().empty()) + { + msg_error() << "Error while initializing, output not found." ; + d_componentState.setValue(ComponentState::Invalid); + return; + } + m_toModel = this->getToModels()[0]; + + if (l_in2Topology.empty()) + { + msg_info() << "Link to input2's topology container should be set to ensure right behavior. First Topology found in input2's context will be used."; + l_in2Topology.set(m_fromModel2->getContext()->getMeshTopologyLink()); + if (l_in2Topology.empty()){ + msg_error() << "Input2's topology container with edges not found."; + d_componentState.setValue(ComponentState::Invalid); + return; + } + } + + if (l_interpolation.empty()) + { + msg_error() << "Link to input2's interpolation component is empty. The component cannot work."; + d_componentState.setValue(ComponentState::Invalid); + return; + } + + auto directions = sofa::helper::getWriteAccessor(d_directions); + if (directions.size() != OutCoord::total_size) + { + msg_warning() << "Wrong size for directions, [" << directions << "]. The size should be " << OutCoord::total_size << "."; + directions.resize(OutCoord::total_size); + } +} + + +template +void BeamProjectionDifferenceMultiMapping::computeProjection(const In1VecCoord &xFrom, const In2VecCoord &xTo, + const bool &updateOrientation) +{ + auto interpolation = l_interpolation.get(); + auto edges = l_in2Topology.get()->getEdges(); + + // for each point of P of xFrom + const auto& indices = sofa::helper::getReadAccessor(d_indices); + m_mappedPoints.resize(indices.size()); + + for (unsigned int i=0; i::max(); + + // find the min distance between P and its projection on each edge of xTo + for (int e=0; e::epsilon()) // edge is degenerated, continue with next edge + continue; + + dirAxe /= normAxe; + + Real r = In1::getDPos(In1::coordDifference(P, Q1)) * In1::getDPos(dirAxe); + Real alpha = r / normAxe; + alpha = (std::abs(alpha)<1e-2)? std::abs(alpha): alpha; + + if (alpha < 0 || alpha > 1) // not on the edge, continue with next edge + continue; + + In1Coord proj; + for (size_t j=0; jInterpolateTransformUsingSpline(interpolatedTransform, + alpha, + Transform(Q1.getCenter(), Q1.getOrientation()), + Transform(Q2.getCenter(), Q2.getOrientation()), + interpolation->getLength(e)); + mp.interpolatedTransform = interpolatedTransform; + found = true; + } + } + + mp.onBeam = found; + if (!updateOrientation) + { + mp.interpolatedTransform = m_mappedPoints[i].interpolatedTransform; + } + m_mappedPoints[i] = mp; + } +} + + +template +void BeamProjectionDifferenceMultiMapping::apply( const sofa::core::MechanicalParams* mparams, + const sofa::type::vector& dataVecOutPos, + const sofa::type::vector& dataVecIn1Pos , + const sofa::type::vector& dataVecIn2Pos) +{ + SOFA_UNUSED(mparams); + + if(d_componentState.getValue() == ComponentState::Invalid) + return; + + if(dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) + return; + + const In1VecCoord& in1 = dataVecIn1Pos[0]->getValue(); + const In2VecCoord& in2 = dataVecIn2Pos[0]->getValue(); + OutVecCoord& out = *dataVecOutPos[0]->beginEdit(); + const auto& directions = sofa::helper::getReadAccessor(d_directions); + auto interpolation = l_interpolation.get(); + + if (m_mappedPoints.empty() || !directions[0]) + { + computeProjection(in1, in2, true); + } + else if (d_updateProjectionPosition.getValue() || d_updateProjectionOrientation.getValue()) + { + computeProjection(in1, in2, d_updateProjectionOrientation.getValue()); + } + + m_updateJ = true; + + sofa::Size sz = m_mappedPoints.size(); + out.resize(sz); + const auto& indices = sofa::helper::getReadAccessor(d_indices); + + for(sofa::Size i=0; iInterpolateTransformUsingSpline(interpolatedTransform, + mp.alpha, + Transform(in2[mp.pi1].getCenter(), in2[mp.pi1].getOrientation()), + Transform(in2[mp.pi2].getCenter(), in2[mp.pi2].getOrientation()), + interpolation->getLength(mp.edgeIndex)); + + In1Coord projection; + projection.getCenter() = interpolatedTransform.getOrigin(); + projection.getOrientation() = interpolatedTransform.getOrientation(); + + In1Coord p; + for (size_t j=0; jendEdit(); +} + + + +template +void BeamProjectionDifferenceMultiMapping:: applyJ(const sofa::core::MechanicalParams* mparams, + const sofa::type::vector< OutDataVecDeriv*>& dataVecOutVel, + const sofa::type::vector& dataVecIn1Vel, + const sofa::type::vector& dataVecIn2Vel) +{ + SOFA_UNUSED(mparams); + + if(d_componentState.getValue() == ComponentState::Invalid) + return; + + if(dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty() ) + return; + + const In1VecDeriv& in1 = dataVecIn1Vel[0]->getValue(); + const In2VecDeriv& in2 = dataVecIn2Vel[0]->getValue(); + OutVecDeriv& outVel = *dataVecOutVel[0]->beginEdit(); + + const auto& indices = sofa::helper::getReadAccessor(d_indices); + const auto& directions = sofa::helper::getReadAccessor(d_directions); + + size_t sz = m_mappedPoints.size(); + outVel.resize(sz); + for (size_t i = 0 ; i < sz; i++) + { + MappedPoint& mp = m_mappedPoints[i]; + if (mp.onBeam) + { + In1Deriv vel; + for (size_t j=0; jendEdit(); +} + + +template +void BeamProjectionDifferenceMultiMapping::applyJT( const sofa::core::MechanicalParams* mparams, + const sofa::type::vector< In1DataVecDeriv*>& dataVecOut1Force, + const sofa::type::vector< In2DataVecDeriv*>& dataVecOut2Force, + const sofa::type::vector& dataVecInForce) +{ + SOFA_UNUSED(mparams); + + if(d_componentState.getValue() == ComponentState::Invalid) + return; + + if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) + return; + + const OutVecDeriv& in = dataVecInForce[0]->getValue(); + + In1VecDeriv& out1 = *dataVecOut1Force[0]->beginEdit(); + In2VecDeriv& out2 = *dataVecOut2Force[0]->beginEdit(); + + const auto& indices = sofa::helper::getReadAccessor(d_indices); + const auto& directions = sofa::helper::getReadAccessor(d_directions); + + size_t sz = m_mappedPoints.size(); + for (size_t i = 0 ; i < sz; i++) + { + MappedPoint& mp = m_mappedPoints[i]; + if (mp.onBeam) + { + OutDeriv f = in[i]; + + for (size_t j = 0; j < OutDeriv::total_size; j++) + { + f[j] = (directions[j])? f[j] : 0; + } + + In1Deriv v; + v.getVCenter() = mp.interpolatedTransform.getRotationMatrix() * Out::getDPos(f); + v.getVOrientation() = mp.interpolatedTransform.getRotationMatrix() * Out::getDRot(f); + + for (size_t j = 0; j < OutDeriv::total_size; j++){ + out1[indices[i]][j] += v[j]; + out2[mp.pi1][j] -= (1 - mp.alpha) * v[j]; + out2[mp.pi2][j] -= mp.alpha * v[j]; + } + } + } + + dataVecOut1Force[0]->endEdit(); + dataVecOut2Force[0]->endEdit(); +} + + +template +void BeamProjectionDifferenceMultiMapping::applyJT( const sofa::core::ConstraintParams* cparams, + const sofa::type::vector< In1DataMatrixDeriv*>& dataMatOut1Const, + const sofa::type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , + const sofa::type::vector& dataMatInConst) +{ + SOFA_UNUSED(cparams); + + if(d_componentState.getValue() == ComponentState::Invalid) + return; + + if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty()) + return; + + In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); + In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); + const OutMatrixDeriv& in = dataMatInConst[0]->getValue(); + + const auto& indices = sofa::helper::getReadAccessor(d_indices); + const auto& directions = sofa::helper::getReadAccessor(d_directions); + + const auto& rowitEnd = in.end(); + for (auto rowIt = in.begin(); rowIt != rowitEnd; ++rowIt) + { + const auto& colitEnd = rowIt.end(); + for (auto colIt = rowIt.begin(); colIt != colitEnd; ++colIt) + { + typename In1MatrixDeriv::RowIterator o1 = out1.writeLine(rowIt.index()); + typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); + + MappedPoint mp = m_mappedPoints[colIt.index()]; + if (mp.onBeam) + { + OutDeriv h = colIt.val(); + + for (size_t j = 0; j < OutDeriv::total_size; j++) + { + h[j] = (directions[j])? h[j] : 0; + } + + In1Deriv v; + v.getVCenter() = mp.interpolatedTransform.getRotationMatrix() * Out::getDPos(h); + v.getVOrientation() = mp.interpolatedTransform.getRotationMatrix() * Out::getDRot(h); + In1Deriv h1, h2_1, h2_2; + + for (size_t j = 0; j < In1Deriv::total_size; j++) + { + h1[j] = v[j]; + h2_1[j] = - (1 - mp.alpha) * v[j]; + h2_2[j] = - mp.alpha * v[j]; + } + + o1.addCol(indices[colIt.index()], h1); + o2.addCol(mp.pi1, h2_1); + o2.addCol(mp.pi2, h2_2); + } + } + } + dataMatOut1Const[0]->endEdit(); + dataMatOut2Const[0]->endEdit(); +} + +template +void BeamProjectionDifferenceMultiMapping::applyDJT(const sofa::core::MechanicalParams* mparams, + sofa::core::MultiVecDerivId inForce, + sofa::core::ConstMultiVecDerivId outForce) +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(inForce); + SOFA_UNUSED(outForce); +} + +template +const sofa::type::vector* BeamProjectionDifferenceMultiMapping::getJs() +{ + const OutVecCoord& out = m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In1VecCoord& in1 = m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In2VecCoord& in2 = m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); + + typename SparseMatrixEigen1::CompressedMatrix& J1 = m_eigenJacobian1.compressedMatrix; + typename SparseMatrixEigen1::CompressedMatrix& J2 = m_eigenJacobian2.compressedMatrix; + const auto& indices = sofa::helper::getReadAccessor(d_indices); + + if( m_updateJ || J1.size() == 0 || J2.size() == 0 ) + { + J1.resize(out.size() * NOut, in1.size() * NIn1); + J2.resize(out.size() * NOut, in2.size() * NIn2); + J1.setZero(); + J2.setZero(); + + size_t sz = m_mappedPoints.size(); + for (size_t n = 0 ; n < sz; n++) + { + MappedPoint& mp = m_mappedPoints[n]; + sofa::type::Mat3x3d base = mp.interpolatedTransform.getRotationMatrix().transposed(); + + Eigen::Matrix block; + if (mp.onBeam) + { + block.template rightCols<3>() << + base(0,0), base(0,1), base(0,2), + base(1,0), base(1,1), base(1,2), + base(2,0), base(2,1), base(2,2); + block.template leftCols().setIdentity(); + } + + for(unsigned i = 0; i < NOut; ++i) + { + unsigned row = n * NOut + i; + + J1.startVec( row ); + for(unsigned j = 0; j < NIn1; ++j) + { + unsigned col = indices[n] * NIn1 + j; + J1.insertBack(row, col) = block(i, j); + } + + J2.startVec( row ); + for(unsigned j = 0; j < NIn2; ++j) + { + if (mp.onBeam) + { + unsigned col1 = mp.pi1 * NIn2 + j; + unsigned col2 = mp.pi2 * NIn2 + j; + J2.insertBack(row, col1) = - (1 - mp.alpha) * block(i, j); + J2.insertBack(row, col2) = - mp.alpha * block(i, j); + } else + { + J2.insertBack(row, j) = 0; + J2.insertBack(row, j) = 0; + } + } + } + } + J1.finalize(); + J2.finalize(); + } + + m_updateJ = false; + return &m_eigenJacobians; +} + + +template +void BeamProjectionDifferenceMultiMapping::draw(const sofa::core::visual::VisualParams* vparams) +{ + if(d_componentState.getValue() == ComponentState::Invalid) + return; + + auto interpolation = l_interpolation.get(); + Real size = d_drawSize.getValue(); + const auto& x = m_fromModel2->readPositions(); + + if (d_draw.getValue()){ + for (MappedPoint& mp : m_mappedPoints) + { + if (mp.onBeam) + { + Transform interpolatedTransform; + interpolation->InterpolateTransformUsingSpline(interpolatedTransform, + mp.alpha, + Transform(x[mp.pi1].getCenter(), x[mp.pi1].getOrientation()), + Transform(x[mp.pi2].getCenter(), x[mp.pi2].getOrientation()), + interpolation->getLength(mp.edgeIndex)); + In1Coord position; + position.getCenter() = interpolatedTransform.getOrigin(); + sofa::type::Mat3x3 base = interpolatedTransform.getRotationMatrix().transposed(); + vparams->drawTool()->drawArrow(TIn1::getCPos(position), + TIn1::getCPos(position) + sofa::type::Vec3{base(0,0), base(0,1), base(0,2)}*size, size/10, + sofa::type::RGBAColor::red()); + vparams->drawTool()->drawArrow(TIn1::getCPos(position), + TIn1::getCPos(position) + sofa::type::Vec3{base(1,0), base(1,1), base(1,2)}*size, size/10, + sofa::type::RGBAColor::green()); + vparams->drawTool()->drawArrow(TIn1::getCPos(position), + TIn1::getCPos(position) + sofa::type::Vec3{base(2,0), base(2,1), base(2,2)}*size, size/10, + sofa::type::RGBAColor::blue()); + } + } + } +} + +} // namespace