[Mapping] Add distance to plane mapping

Sofa/Component/Mapping/Linear/CMakeLists.txt

@@ -43,6 +43,8 @@ set(HEADER_FILES
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/CenterOfMassMultiMapping.inl
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/DeformableOnRigidFrameMapping.h
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/DeformableOnRigidFrameMapping.inl
+    ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/DistanceToPlaneMapping.h
+    ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/DistanceToPlaneMapping.inl
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/IdentityMapping.h
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/IdentityMapping.inl
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/IdentityMultiMapping.h
@@ -93,6 +95,7 @@ set(SOURCE_FILES
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/CenterOfMassMulti2Mapping.cpp
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/CenterOfMassMultiMapping.cpp
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/DeformableOnRigidFrameMapping.cpp
+    ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/DistanceToPlaneMapping.cpp
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/IdentityMapping.cpp
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/IdentityMultiMapping.cpp
     ${SOFACOMPONENTMAPPINGLINEAR_SOURCE_DIR}/LineSetSkinningMapping.cpp

Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.cpp

@@ -0,0 +1,51 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: [email protected]                             *
+******************************************************************************/
+#define SOFA_COMPONENT_MAPPING_DISTANCETOPLANEMAPPING_CPP
+#include <sofa/component/mapping/linear/DistanceToPlaneMapping.inl>
+
+#include <sofa/defaulttype/VecTypes.h>
+#include <sofa/defaulttype/RigidTypes.h>
+#include <sofa/core/ObjectFactory.h>
+#include <sofa/component/mapping/linear/config.h>
+
+namespace sofa::component::mapping::linear
+{
+
+using namespace sofa::defaulttype;
+
+void registerDistanceToPlaneMapping(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(core::ObjectRegistrationData("Mapping that computes the distance to a plane")
+        .add< DistanceToPlaneMapping< Vec3Types > >()
+        .add< DistanceToPlaneMapping< Vec2Types > >()
+        .add< DistanceToPlaneMapping< Vec6Types > >()
+        .add< DistanceToPlaneMapping< Rigid3Types > >()
+        .add< DistanceToPlaneMapping< Rigid2Types > >());
+}
+
+template class SOFA_COMPONENT_MAPPING_LINEAR_API DistanceToPlaneMapping< Vec3Types > ;
+template class SOFA_COMPONENT_MAPPING_LINEAR_API DistanceToPlaneMapping< Vec2Types > ;
+template class SOFA_COMPONENT_MAPPING_LINEAR_API DistanceToPlaneMapping< Vec6Types > ;
+template class SOFA_COMPONENT_MAPPING_LINEAR_API DistanceToPlaneMapping< Rigid3Types >;
+template class SOFA_COMPONENT_MAPPING_LINEAR_API DistanceToPlaneMapping< Rigid2Types >;
+
+} // namespace sofa::component::mapping::linear

Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h

@@ -0,0 +1,72 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: [email protected]                             *
+******************************************************************************/
+#pragma once
+#include <sofa/component/mapping/linear/config.h>
+#include <sofa/component/mapping/linear/LinearMapping.h>
+
+#include <sofa/linearalgebra/EigenSparseMatrix.h>
+#include <sofa/core/Mapping.h>
+#include <sofa/core/Mapping.inl>
+#include <sofa/defaulttype/RigidTypes.h>
+#include <sofa/type/vector.h>
+#include <sofa/core/trait/DataTypes.h>
+
+
+namespace sofa::component::mapping::linear
+{
+
+template <class TIn>
+class DistanceToPlaneMapping : public LinearMapping<TIn, defaulttype::Vec1dTypes>
+{
+public:
+    SOFA_CLASS(SOFA_TEMPLATE(DistanceToPlaneMapping,TIn), SOFA_TEMPLATE2(LinearMapping,TIn, defaulttype::Vec1dTypes));
+    typedef LinearMapping<TIn,  defaulttype::Vec1dTypes> Inherit;
+    typedef  defaulttype::Vec1dTypes TOut;
+
+    void init() override;
+
+    void apply(const core::MechanicalParams *mparams, Data<VecCoord_t<TOut>>& out, const Data<VecCoord_t<TIn>>& in) override;
+
+    void applyJ(const core::MechanicalParams *mparams, Data<VecDeriv_t<TOut>>& out, const Data<VecDeriv_t<TIn>>& in) override;
+
+    void applyJT(const core::MechanicalParams *mparams, Data<VecDeriv_t<TIn>>& out, const Data<VecDeriv_t<TOut>>& in) override;
+
+    void applyJT(const core::ConstraintParams *cparams, Data<MatrixDeriv_t<TIn>>& out, const Data<MatrixDeriv_t<TOut>>& in) override;
+
+    const linearalgebra::BaseMatrix* getJ() override;
+
+    void handleTopologyChange() override;
+
+
+    Data<type::Vec<Deriv_t<TIn>::spatial_dimensions,typename Deriv_t<TIn>::value_type>> d_planeNormal;
+    Data<type::Vec<Coord_t<TIn>::spatial_dimensions,typename Coord_t<TIn>::value_type>> d_planePoint;
+
+protected:
+
+    DistanceToPlaneMapping();
+    virtual ~DistanceToPlaneMapping() {};
+
+    linearalgebra::EigenSparseMatrix<TIn, TOut> J;
+};
+
+
+}

Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl

@@ -0,0 +1,185 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: [email protected]                             *
+******************************************************************************/
+#pragma once
+#include <sofa/component/mapping/linear/DistanceToPlaneMapping.h>
+#include <sofa/defaulttype/VecTypes.h>
+#include <sofa/linearalgebra/EigenSparseMatrix.h>
+#include <sofa/type/Vec.h>
+
+
+namespace sofa::component::mapping::linear
+{
+
+template <class TIn>
+DistanceToPlaneMapping<TIn>::DistanceToPlaneMapping()
+: Inherit()
+, d_planeNormal(initData(&d_planeNormal,"planeNormal","Normal of the plane to compute the distance to"))
+, d_planePoint(initData(&d_planePoint,"planePoint","A point belonging to the plane"))
+{
+
+}
+
+
+template <class TIn>
+void DistanceToPlaneMapping<TIn>::init()
+{
+    Inherit::init();
+
+    //Normalize plane normal
+    const SReal normalNorm = d_planeNormal.getValue().norm();
+    if (normalNorm<std::numeric_limits<SReal>::epsilon())
+    {
+        msg_error()<<" planeNormal data has null norm.";
+        this->d_componentState.setValue(core::objectmodel::ComponentState::Invalid);
+        return;
+    }
+    d_planeNormal.setValue(d_planeNormal.getValue()/normalNorm);
+
+    constexpr Size inDerivSize = Deriv_t<TIn>::size();
+    constexpr Size inSpatialDimension = Deriv_t<TIn>::spatial_dimensions;
+    Size inSize = this->getFromModel()->getSize();
+    this->getToModel()->resize( inSize );
+
+    const auto planeNormal = d_planeNormal.getValue();
+
+    J.compressedMatrix.resize( inSize, inSize*inDerivSize );
+
+    for (Size i = 0; i < inSize; i++)
+    {
+        const size_t col = i * inDerivSize;
+        J.compressedMatrix.startVec(i);
+
+        for (Size j = 0; j < inSpatialDimension; j++ )
+        {
+            J.compressedMatrix.insertBack( i, col + j ) = planeNormal[j];
+        }
+        J.compressedMatrix.finalize();
+
+    }
+
+    this->d_componentState.setValue(core::objectmodel::ComponentState::Valid);
+}
+
+template <class TIn>
+void DistanceToPlaneMapping<TIn>::apply(const core::MechanicalParams *mparams, Data<VecCoord_t<TOut>>& out, const Data<VecCoord_t<TIn>>& in)
+{
+    if (this-> d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
+        return;
+
+    auto writeOut = helper::getWriteAccessor(out);
+    const auto readIn = helper::getReadAccessor(in);
+
+    const auto planeNormal = d_planeNormal.getValue();
+
+    SReal planeDistanceToOrigin = dot(d_planePoint.getValue(),planeNormal);
+
+
+    for ( unsigned i = 0; i<readIn.size(); i++ )
+    {
+        writeOut[i] = type::dot(TIn::getCPos(readIn[i]),planeNormal) - planeDistanceToOrigin;
+    }
+}
+
+template <class TIn>
+void DistanceToPlaneMapping<TIn>::applyJ(const core::MechanicalParams *mparams, Data<VecDeriv_t<TOut>>& out, const Data<VecDeriv_t<TIn>>& in)
+{
+    if (this-> d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
+        return;
+
+    auto writeOut = helper::getWriteAccessor(out);
+    const auto readIn = helper::getReadAccessor(in);
+    const auto planeNormal = d_planeNormal.getValue();
+
+    for ( unsigned i = 0; i<readIn.size(); i++ )
+    {
+        writeOut[i] = type::dot(TIn::getDPos(readIn[i]),planeNormal);
+    }
+}
+
+template <class TIn>
+void DistanceToPlaneMapping<TIn>::applyJT(const core::MechanicalParams *mparams, Data<VecDeriv_t<TIn>>& out, const Data<VecDeriv_t<TOut>>& in)
+{
+    if (this-> d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
+        return;
+
+    auto writeOut = helper::getWriteAccessor(out);
+    const auto readIn = helper::getReadAccessor(in);
+
+    const auto planeNormal = d_planeNormal.getValue();
+
+    for ( unsigned i = 0; i<readIn.size(); i++ )
+    {
+        TIn::setDPos(writeOut[i], TIn::getDPos(writeOut[i]) + planeNormal * readIn[i][0]) ;
+    }
+}
+
+template <class TIn>
+void DistanceToPlaneMapping<TIn>::applyJT(const core::ConstraintParams *cparams, Data<MatrixDeriv_t<TIn>>& out, const Data<MatrixDeriv_t<TOut>>& in)
+{
+    if (this-> d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
+        return;
+
+    auto writeMatrixOut = helper::getWriteAccessor(out);
+    const auto readMatrixIn = helper::getReadAccessor(in);
+
+    const auto planeNormal = d_planeNormal.getValue();
+
+    for (auto rowIt = readMatrixIn->begin(); rowIt != readMatrixIn->end(); ++rowIt)
+    {
+        auto colIt = rowIt.begin();
+        auto colItEnd = rowIt.end();
+        // Creates a constraints if the input constraint is not empty.
+        if (colIt != colItEnd)
+        {
+            auto o = writeMatrixOut->writeLine(rowIt.index());
+            while (colIt != colItEnd)
+            {
+                Deriv_t<TIn> normal;
+                TIn::setDPos(normal,planeNormal*colIt.val()[0]);
+                o.addCol(colIt.index(), normal);
+
+                ++colIt;
+            }
+        }
+    }
+}
+
+template <class TIn>
+const linearalgebra::BaseMatrix* DistanceToPlaneMapping<TIn>::getJ()
+{
+    return &J;
+}
+
+template <class TIn>
+void DistanceToPlaneMapping<TIn>::handleTopologyChange()
+{
+    if (this-> d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
+        return;
+
+    if ( this->toModel && this->fromModel && this->toModel->getSize() != this->fromModel->getSize())
+    {
+        this->init();
+    }
+}
+
+};
+

Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/init.cpp

@@ -33,6 +33,7 @@ extern void registerCenterOfMassMapping(sofa::core::ObjectFactory* factory);
 extern void registerCenterOfMassMulti2Mapping(sofa::core::ObjectFactory* factory);
 extern void registerCenterOfMassMultiMapping(sofa::core::ObjectFactory* factory);
 extern void registerDeformableOnRigidFrameMapping(sofa::core::ObjectFactory* factory);
+extern void registerDistanceToPlaneMapping(sofa::core::ObjectFactory* factory);
 extern void registerIdentityMapping(sofa::core::ObjectFactory* factory);
 extern void registerIdentityMultiMapping(sofa::core::ObjectFactory* factory);
 extern void registerLineSetSkinningMapping(sofa::core::ObjectFactory* factory);
@@ -78,6 +79,7 @@ void registerObjects(sofa::core::ObjectFactory* factory)
     registerCenterOfMassMulti2Mapping(factory);
     registerCenterOfMassMultiMapping(factory);
     registerDeformableOnRigidFrameMapping(factory);
+    registerDistanceToPlaneMapping(factory);
     registerIdentityMapping(factory);
     registerIdentityMultiMapping(factory);
     registerLineSetSkinningMapping(factory);

Sofa/Component/Mapping/Linear/tests/CMakeLists.txt

@@ -6,6 +6,7 @@ set(SOURCE_FILES
     BarycentricMapping_test.cpp
     IdentityMapping_test.cpp
     SubsetMultiMapping_test.cpp
+    DistanceToPlaneMapping_test.cpp
 )
 
 add_executable(${PROJECT_NAME} ${SOURCE_FILES})