From 28c7a03b95b9a0523bb46df6ccbe761260ffb3f1 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Mon, 24 Feb 2025 18:13:22 +0100 Subject: [PATCH 01/10] Add DistanceToPlaneMapping --- Sofa/Component/Mapping/Linear/CMakeLists.txt | 3 + .../mapping/linear/DistanceToPlaneMapping.cpp | 51 +++++ .../mapping/linear/DistanceToPlaneMapping.h | 72 +++++++ .../mapping/linear/DistanceToPlaneMapping.inl | 178 ++++++++++++++++++ .../sofa/component/mapping/linear/init.cpp | 2 + 5 files changed, 306 insertions(+) create mode 100644 Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.cpp create mode 100644 Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h create mode 100644 Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl diff --git a/Sofa/Component/Mapping/Linear/CMakeLists.txt b/Sofa/Component/Mapping/Linear/CMakeLists.txt index 3743a985ee2..f1c46536f70 100644 --- a/Sofa/Component/Mapping/Linear/CMakeLists.txt +++ b/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 diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.cpp b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.cpp new file mode 100644 index 00000000000..b417e77100b --- /dev/null +++ b/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 . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#define SOFA_COMPONENT_MAPPING_DISTANCETOPLANEMAPPING_CPP +#include + +#include +#include +#include +#include + +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 diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h new file mode 100644 index 00000000000..cd6db132368 --- /dev/null +++ b/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 . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include + +#include +#include +#include +#include +#include +#include + + +namespace sofa::component::mapping::linear +{ + +template +class DistanceToPlaneMapping : public LinearMapping +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(DistanceToPlaneMapping,TIn), SOFA_TEMPLATE2(LinearMapping,TIn, defaulttype::Vec1dTypes)); + typedef LinearMapping Inherit; + typedef defaulttype::Vec1dTypes TOut; + + void init() override; + + void apply(const core::MechanicalParams *mparams, Data>& out, const Data>& in) override; + + void applyJ(const core::MechanicalParams *mparams, Data>& out, const Data>& in) override; + + void applyJT(const core::MechanicalParams *mparams, Data>& out, const Data>& in) override; + + void applyJT(const core::ConstraintParams *cparams, Data>& out, const Data>& in) override; + + const linearalgebra::BaseMatrix* getJ() override; + + void handleTopologyChange() override; + + + Data> d_planeNormal; + Data> d_planePoint; + +protected: + + DistanceToPlaneMapping(); + virtual ~DistanceToPlaneMapping() {}; + + linearalgebra::EigenSparseMatrix J; +}; + + +} diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl new file mode 100644 index 00000000000..0143449b38b --- /dev/null +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl @@ -0,0 +1,178 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ +#pragma once +#include +#include +#include +#include + + +namespace sofa::component::mapping::linear +{ + +template +DistanceToPlaneMapping::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 +void DistanceToPlaneMapping::init() +{ + Inherit::init(); + + //Normalize plane normal + const double normalNorm = d_planeNormal.getValue().norm(); + if (normalNorm::epsilon()) + { + msg_error(this)<<" planeNormal data has null norm."; + this->d_componentState.setValue(core::objectmodel::ComponentState::Invalid); + return; + } + d_planeNormal.setValue(d_planeNormal.getValue()/normalNorm); + + constexpr Size inCoordSize = typename Coord_t::Size(); + Size inSize = this->getFromModel()->getSize(); + this->getToModel()->resize( inSize ); + + const auto planeNormal = d_planeNormal.getValue(); + + J.compressedMatrix.resize( inSize, inSize*inCoordSize ); + + for (Size i = 0; i < inSize; i++) + { + const size_t col = i * inCoordSize; + J.compressedMatrix.startVec(i); + J.compressedMatrix.insertBack( i, col ) = planeNormal[0]; + J.compressedMatrix.insertBack( i, col + 1 ) = planeNormal[1]; + J.compressedMatrix.insertBack( i, col + 2 ) = planeNormal[2]; + J.compressedMatrix.finalize(); + + } + + this->d_componentState.setValue(core::objectmodel::ComponentState::Valid); +} + +template +void DistanceToPlaneMapping::apply(const core::MechanicalParams *mparams, Data>& out, const Data>& 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(); + const auto planePoint = d_planePoint.getValue(); + + for ( unsigned i = 0; i +void DistanceToPlaneMapping::applyJ(const core::MechanicalParams *mparams, Data>& out, const Data>& 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 +void DistanceToPlaneMapping::applyJT(const core::MechanicalParams *mparams, Data>& out, const Data>& 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 +void DistanceToPlaneMapping::applyJT(const core::ConstraintParams *cparams, Data>& out, const Data>& 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) + { + o.addCol(colIt.index(), planeNormal); + + ++colIt; + } + } + } +} + +template +const linearalgebra::BaseMatrix* DistanceToPlaneMapping::getJ() +{ + return &J; +} + +template +void DistanceToPlaneMapping::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(); + } +} + +}; + diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/init.cpp b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/init.cpp index d01af97a4e5..af9ab4f3276 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/init.cpp +++ b/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); From 2e0834ce6be70ffbaa24b00e99003293ef43ceb3 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Mon, 24 Feb 2025 18:14:05 +0100 Subject: [PATCH 02/10] Add tests, still need to test applyJ, applyJT and applyJT for constraints --- .../Mapping/Linear/tests/CMakeLists.txt | 1 + .../tests/DistanceToPlaneMapping_test.cpp | 133 ++++++++++++++++++ 2 files changed, 134 insertions(+) create mode 100644 Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp diff --git a/Sofa/Component/Mapping/Linear/tests/CMakeLists.txt b/Sofa/Component/Mapping/Linear/tests/CMakeLists.txt index 82b1ebaea84..d789c09bdba 100644 --- a/Sofa/Component/Mapping/Linear/tests/CMakeLists.txt +++ b/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}) diff --git a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp new file mode 100644 index 00000000000..95a27063220 --- /dev/null +++ b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp @@ -0,0 +1,133 @@ +/****************************************************************************** +* 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 +#include + +#include + +#include "sofa/core/MechanicalParams.h" +using sofa::testing::BaseTest; + + +#include +using sofa::simulation::graph::DAGSimulation; +using sofa::simulation::Node ; +using sofa::core::objectmodel::New ; +using sofa::core::objectmodel::BaseData ; + +#include +using sofa::component::statecontainer::MechanicalObject ; + +#include +#include +#include + + +using sofa::defaulttype::Vec1Types; +using sofa::defaulttype::Vec3Types; +using sofa::defaulttype::Rigid3Types; + + + +template +sofa::simulation::Simulation* createSimpleScene(typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping) +{ + sofa::simulation::Simulation* simu = sofa::simulation::getSimulation(); + + const Node::SPtr node = simu->createNewGraph("root"); + + typename MechanicalObject::SPtr mechanical = New>(); + mechanical->resize(10); + auto inPos = mechanical->writePositions(); + inPos.resize(10); + auto inRestPos = mechanical->writeRestPositions(); + inRestPos.resize(10); + node->addObject(mechanical); + + + const Node::SPtr nodeMapping = node->createChild("nodeToMap"); + typename MechanicalObject::SPtr targetMechanical = New>(); + nodeMapping->addObject(targetMechanical); + mapping->setFrom(mechanical.get()); + mapping->setTo(targetMechanical.get()); + nodeMapping->addObject(mapping); + + EXPECT_NO_THROW( + sofa::simulation::node::initRoot(node.get()) + ); + return simu; +} + + + + +GTEST_TEST(DistanceToPlaneMapping_Tests_Vec3d, init) +{ + typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + mapping->d_planeNormal.setValue(sofa::type::Vec3(1,2,5)); + auto simu = createSimpleScene(mapping); + EXPECT_DOUBLE_EQ(mapping->d_planeNormal.getValue().norm(),1.0); + EXPECT_EQ(mapping->getFrom()[0]->getSize(),10); + EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); +} + +GTEST_TEST(DistanceToPlaneMapping_Tests_Vec3d, apply) +{ + typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + + sofa::type::Vec3 planePoint{-1, 1, 2}; + sofa::type::Vec3 planeNormal{5, 1, 2}; planeNormal = planeNormal/planeNormal.norm(); + sofa::type::Vec3 planeTangent1{-1, 5, 0}; planeTangent1 = planeTangent1/planeTangent1.norm(); + sofa::type::Vec3 planeTangent2 = cross(planeNormal, planeTangent1); + + sofa::DataVecCoord_t inPos{sofa::DataVecCoord_t::InitData()}; + + std::vector dists{0.2,-0.5,-2.1,0.5}; + + inPos.setValue({planePoint + planeNormal * dists[0], + planePoint + planeNormal * dists[1] + planeTangent1 * 10, + planePoint + planeNormal * dists[2] + planeTangent2 * 10, + planePoint + planeNormal * dists[3] + planeTangent1 * 2 + planeTangent2 * 4}); + sofa::DataVecCoord_t outVec; outVec.beginEdit()->resize(4); + + mapping->d_planeNormal.setValue(planeNormal); + mapping->d_planePoint.setValue(planePoint); + mapping->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + mapping->apply(sofa::core::MechanicalParams::defaultInstance(),outVec,inPos); + + for (unsigned i = 0; i::SPtr mapping = New>(); + mapping->d_planeNormal.setValue(Rigid3Types::Deriv(sofa::type::Vec3(1,2,5),sofa::type::Vec3(1,2,3))); + + auto simu = createSimpleScene(mapping); + EXPECT_EQ(mapping->getFrom()[0]->getSize(),10); + EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); +} + + From 875bc93628583c9f1b492d8ac8120a837fdcf69f Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Mon, 24 Feb 2025 18:14:20 +0100 Subject: [PATCH 03/10] Add example --- .../Mapping/Linear/DistanceToPlaneMapping.scn | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 examples/Component/Mapping/Linear/DistanceToPlaneMapping.scn diff --git a/examples/Component/Mapping/Linear/DistanceToPlaneMapping.scn b/examples/Component/Mapping/Linear/DistanceToPlaneMapping.scn new file mode 100644 index 00000000000..eb693631080 --- /dev/null +++ b/examples/Component/Mapping/Linear/DistanceToPlaneMapping.scn @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From f0cc1c64cd47c9bd3818e6467479910da4f075af Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 25 Feb 2025 16:46:10 +0100 Subject: [PATCH 04/10] Make test work for Rigid + fix API for rigid to take only position into account --- .../mapping/linear/DistanceToPlaneMapping.inl | 40 ++- .../tests/DistanceToPlaneMapping_test.cpp | 299 ++++++++++++++---- 2 files changed, 267 insertions(+), 72 deletions(-) diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl index 0143449b38b..53839ceb372 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl @@ -44,6 +44,14 @@ void DistanceToPlaneMapping::init() { Inherit::init(); + if constexpr (sofa::type::isRigidType) + { + if (d_planeNormal.getValue().getVOrientation() != typename sofa::Deriv_t::Rot()*0) + { + msg_info(this) <<"You've untered a plane normal with a non null rotation. Only position is treated by this component, the rotation part will thus be considered as zero."; + } + } + //Normalize plane normal const double normalNorm = d_planeNormal.getValue().norm(); if (normalNorm::epsilon()) @@ -55,6 +63,7 @@ void DistanceToPlaneMapping::init() d_planeNormal.setValue(d_planeNormal.getValue()/normalNorm); constexpr Size inCoordSize = typename Coord_t::Size(); + constexpr Size inDerivSize = typename Deriv_t::Size(); Size inSize = this->getFromModel()->getSize(); this->getToModel()->resize( inSize ); @@ -66,9 +75,13 @@ void DistanceToPlaneMapping::init() { const size_t col = i * inCoordSize; J.compressedMatrix.startVec(i); - J.compressedMatrix.insertBack( i, col ) = planeNormal[0]; - J.compressedMatrix.insertBack( i, col + 1 ) = planeNormal[1]; - J.compressedMatrix.insertBack( i, col + 2 ) = planeNormal[2]; + + for (Size j = 0; j < inDerivSize; j++ ) + { + if (sofa::type::isRigidType && i>2) + break; + J.compressedMatrix.insertBack( i, col ) = planeNormal[i]; + } J.compressedMatrix.finalize(); } @@ -90,7 +103,10 @@ void DistanceToPlaneMapping::apply(const core::MechanicalParams *mparams, D for ( unsigned i = 0; i) + writeOut[i] = type::dot(readIn[i] - planePoint,planeNormal); + else + writeOut[i] = type::dot(readIn[i].getCenter() - planePoint.getCenter(),planeNormal.getVCenter()); } } @@ -106,7 +122,10 @@ void DistanceToPlaneMapping::applyJ(const core::MechanicalParams *mparams, for ( unsigned i = 0; i) + writeOut[i] = type::dot(readIn[i],planeNormal); + else + writeOut[i] = type::dot(readIn[i].getVCenter(),planeNormal.getVCenter()); } } @@ -123,7 +142,11 @@ void DistanceToPlaneMapping::applyJT(const core::MechanicalParams *mparams, for ( unsigned i = 0; i) + writeOut[i] += planeNormal * readIn[i][0] ; + else + writeOut[i] += sofa::Deriv_t(planeNormal.getVCenter() * readIn[i][0],typename sofa::Deriv_t::Rot()) ; + } } @@ -148,7 +171,10 @@ void DistanceToPlaneMapping::applyJT(const core::ConstraintParams *cparams, auto o = writeMatrixOut->writeLine(rowIt.index()); while (colIt != colItEnd) { - o.addCol(colIt.index(), planeNormal); + if constexpr (!type::isRigidType) + o.addCol(colIt.index(), planeNormal*colIt.val()[0]); + else + o.addCol(colIt.index(), sofa::Deriv_t(planeNormal.getVCenter() * colIt.val()[0],typename sofa::Deriv_t::Rot()) ); ++colIt; } diff --git a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp index 95a27063220..59182815554 100644 --- a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp +++ b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp @@ -24,6 +24,7 @@ #include +#include "sofa/core/ConstraintParams.h" #include "sofa/core/MechanicalParams.h" using sofa::testing::BaseTest; @@ -41,93 +42,261 @@ using sofa::component::statecontainer::MechanicalObject ; #include #include +#define EPSILON 1e-12 using sofa::defaulttype::Vec1Types; -using sofa::defaulttype::Vec3Types; -using sofa::defaulttype::Rigid3Types; +template +class PlaneMappingTest : public testing::Test +{ +public: + sofa::simulation::Simulation* createSimpleScene(typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping) + { -template -sofa::simulation::Simulation* createSimpleScene(typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping) -{ - sofa::simulation::Simulation* simu = sofa::simulation::getSimulation(); - - const Node::SPtr node = simu->createNewGraph("root"); - - typename MechanicalObject::SPtr mechanical = New>(); - mechanical->resize(10); - auto inPos = mechanical->writePositions(); - inPos.resize(10); - auto inRestPos = mechanical->writeRestPositions(); - inRestPos.resize(10); - node->addObject(mechanical); - - - const Node::SPtr nodeMapping = node->createChild("nodeToMap"); - typename MechanicalObject::SPtr targetMechanical = New>(); - nodeMapping->addObject(targetMechanical); - mapping->setFrom(mechanical.get()); - mapping->setTo(targetMechanical.get()); - nodeMapping->addObject(mapping); - - EXPECT_NO_THROW( - sofa::simulation::node::initRoot(node.get()) - ); - return simu; -} + sofa::simulation::Simulation* simu = sofa::simulation::getSimulation(); + const Node::SPtr node = simu->createNewGraph("root"); + typename MechanicalObject::SPtr mechanical = New>(); + mechanical->resize(10); + auto inPos = mechanical->writePositions(); + inPos.resize(10); + auto inRestPos = mechanical->writeRestPositions(); + inRestPos.resize(10); + node->addObject(mechanical); -GTEST_TEST(DistanceToPlaneMapping_Tests_Vec3d, init) -{ - typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); - mapping->d_planeNormal.setValue(sofa::type::Vec3(1,2,5)); - auto simu = createSimpleScene(mapping); - EXPECT_DOUBLE_EQ(mapping->d_planeNormal.getValue().norm(),1.0); - EXPECT_EQ(mapping->getFrom()[0]->getSize(),10); - EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); -} + const Node::SPtr nodeMapping = node->createChild("nodeToMap"); + typename MechanicalObject::SPtr targetMechanical = New>(); + nodeMapping->addObject(targetMechanical); + mapping->setFrom(mechanical.get()); + mapping->setTo(targetMechanical.get()); + nodeMapping->addObject(mapping); -GTEST_TEST(DistanceToPlaneMapping_Tests_Vec3d, apply) -{ - typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + EXPECT_NO_THROW( + sofa::simulation::node::initRoot(node.get()) + ); + return simu; + } + + sofa::Deriv_t getPseudoRandomNormal() + { + constexpr std::array randomValuesForNormal = {1, 5.1, -6, 0, 9.5, 4}; + sofa::Deriv_t returnVec; + for (unsigned i = 0; i< sofa::Deriv_t::size(); ++i) + { + returnVec[i] = randomValuesForNormal[i]; + } + if constexpr (sofa::type::isRigidType) + { + returnVec.getVOrientation() = typename sofa::Deriv_t::Rot()*0; + } + return returnVec; + } + + + sofa::Coord_t getPseudoRandomPoint() + { + constexpr std::array randomValuesForPoint = {-2.5, 1.4, 3, 0, 0.7, 12, 7.07}; + sofa::Coord_t returnVec; + for (unsigned i = 0; i< sofa::Coord_t::size(); ++i) + { + returnVec[i] = randomValuesForPoint[i]; + } + if constexpr (sofa::type::isRigidType) + { + returnVec.getOrientation() = sofa::type::Quatd(0.707,0,0.707,0); + } + return returnVec; + + } + + void testInit(sofa::Deriv_t planeNormal) + { + typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + mapping->d_planeNormal.setValue(planeNormal); + auto simu = this->createSimpleScene(mapping); + EXPECT_LE(fabs(mapping->d_planeNormal.getValue().norm() - 1.0),EPSILON); + EXPECT_EQ(mapping->getFrom()[0]->getSize(),10); + EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); + } + + void testApply(sofa::Deriv_t planeNormal, sofa::Coord_t planePoint) + { + typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + + planeNormal = planeNormal/planeNormal.norm(); + sofa::Deriv_t planeTangent1 = planeNormal; + planeTangent1[0] = planeNormal[1]; + planeTangent1[1] = -planeNormal[0]; + for (unsigned i = 2; i< sofa::Deriv_t::size(); ++i) + { + planeTangent1[i] = 0; + } - sofa::type::Vec3 planePoint{-1, 1, 2}; - sofa::type::Vec3 planeNormal{5, 1, 2}; planeNormal = planeNormal/planeNormal.norm(); - sofa::type::Vec3 planeTangent1{-1, 5, 0}; planeTangent1 = planeTangent1/planeTangent1.norm(); - sofa::type::Vec3 planeTangent2 = cross(planeNormal, planeTangent1); + planeTangent1 = planeTangent1/planeTangent1.norm(); - sofa::DataVecCoord_t inPos{sofa::DataVecCoord_t::InitData()}; + sofa::DataVecCoord_t inPos{typename sofa::DataVecCoord_t::InitData()}; - std::vector dists{0.2,-0.5,-2.1,0.5}; + std::vector dists{0.2,-0.5,-2.1,0.5}; - inPos.setValue({planePoint + planeNormal * dists[0], - planePoint + planeNormal * dists[1] + planeTangent1 * 10, - planePoint + planeNormal * dists[2] + planeTangent2 * 10, - planePoint + planeNormal * dists[3] + planeTangent1 * 2 + planeTangent2 * 4}); - sofa::DataVecCoord_t outVec; outVec.beginEdit()->resize(4); + inPos.setValue({planePoint + planeNormal * dists[0], + planePoint + planeNormal * dists[1] + planeTangent1 * 10, + planePoint + planeNormal * dists[2] , + planePoint + planeNormal * dists[3] + planeTangent1 * 2 }); + sofa::DataVecCoord_t outVec; outVec.beginEdit()->resize(4); - mapping->d_planeNormal.setValue(planeNormal); - mapping->d_planePoint.setValue(planePoint); - mapping->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); - mapping->apply(sofa::core::MechanicalParams::defaultInstance(),outVec,inPos); + mapping->d_planeNormal.setValue(planeNormal); + mapping->d_planePoint.setValue(planePoint); + mapping->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + mapping->apply(sofa::core::MechanicalParams::defaultInstance(),outVec,inPos); - for (unsigned i = 0; i planeNormal) { - EXPECT_DOUBLE_EQ(outVec.getValue()[i][0], dists[i]); + typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + + planeNormal = planeNormal/planeNormal.norm(); + sofa::Deriv_t planeTangent1; + planeTangent1[0] = planeNormal[1]; + planeTangent1[1] = -planeNormal[0]; + for (unsigned i = 2; i< sofa::Deriv_t::size(); ++i) + { + planeTangent1[i] = 0; + } + + planeTangent1 = planeTangent1/planeTangent1.norm(); + + sofa::DataVecDeriv_t inPos{typename sofa::DataVecDeriv_t::InitData()}; + + std::vector dists{0.2,-0.5,-2.1,0.5}; + + inPos.setValue({ planeNormal * dists[0], + planeNormal * dists[1] + planeTangent1 * 10, + planeNormal * dists[2], + planeNormal * dists[3] + planeTangent1 * 2}); + sofa::DataVecCoord_t outVec; outVec.beginEdit()->resize(4); + + mapping->d_planeNormal.setValue(planeNormal); + mapping->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + mapping->applyJ(sofa::core::MechanicalParams::defaultInstance(),outVec,inPos); + + for (unsigned i = 0; i planeNormal) + { + typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + + planeNormal = planeNormal/planeNormal.norm(); + + sofa::DataVecDeriv_t inPos{sofa::DataVecDeriv_t::InitData()}; + + + inPos.setValue({sofa::type::Vec1(0.2),sofa::type::Vec1(-0.5),sofa::type::Vec1(-2.1),sofa::type::Vec1(0.5)}); + + sofa::DataVecDeriv_t outVec; outVec.beginEdit()->resize(4); + + mapping->d_planeNormal.setValue(planeNormal); + mapping->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + mapping->applyJT(sofa::core::MechanicalParams::defaultInstance(),outVec,inPos); + + for (unsigned i = 0; i::size(); ++j) + { + EXPECT_LE(fabs(outVec.getValue()[i][j] - planeNormal[j] * inPos.getValue()[i][0]),EPSILON); + } + } + } + + void testApplyJT_Constraint(sofa::Deriv_t planeNormal) + { + typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); + + planeNormal = planeNormal/planeNormal.norm(); + + sofa::DataMatrixDeriv_t inMat{sofa::DataMatrixDeriv_t::InitData()}; + sofa::DataMatrixDeriv_t outMat{typename sofa::DataMatrixDeriv_t::InitData()}; + + + auto writeMatrixIn = sofa::helper::getWriteAccessor(inMat); + + std::vector dofsIds = {2,4,5,7}; + + unsigned lineId = 0; + for (unsigned i : dofsIds) + { + auto o = writeMatrixIn->writeLine(lineId++); + o.addCol(i,Vec1Types::Deriv( 1 - 2*(lineId%1))); + } + + mapping->d_planeNormal.setValue(planeNormal); + mapping->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + mapping->applyJT(sofa::core::ConstraintParams::defaultInstance(),outMat,inMat); + + const auto readMatrixOut = sofa::helper::getReadAccessor(outMat); + for (auto rowIt = readMatrixOut->begin(); rowIt != readMatrixOut->end(); ++rowIt) + { + auto colIt = rowIt.begin(); + auto colItEnd = rowIt.end(); + + EXPECT_GT(dofsIds.size(),rowIt.index()); + + EXPECT_FALSE(colIt == colItEnd); + + EXPECT_EQ(colIt.index(),dofsIds[rowIt.index()]); + EXPECT_EQ(colIt.val(),planeNormal*( 1 - 2*(rowIt.index()%1))); + + ++colIt; + EXPECT_TRUE(colIt == colItEnd); + } + } +}; + + +using sofa::defaulttype::Vec2Types; +using sofa::defaulttype::Vec3Types; +using sofa::defaulttype::Vec6Types; +using sofa::defaulttype::Rigid3Types; + + +using DataTypes = testing::Types; +TYPED_TEST_SUITE(PlaneMappingTest, DataTypes); + +TYPED_TEST(PlaneMappingTest, init) +{ + + this->testInit(this->getPseudoRandomNormal()); } -GTEST_TEST(DistanceToPlaneMapping_Tests_Rigid3d, init) +TYPED_TEST(PlaneMappingTest, apply) { - typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); - mapping->d_planeNormal.setValue(Rigid3Types::Deriv(sofa::type::Vec3(1,2,5),sofa::type::Vec3(1,2,3))); + this->testApply(this->getPseudoRandomNormal(),this->getPseudoRandomPoint()); +} + +TYPED_TEST(PlaneMappingTest, applyJ) +{ + this->testApplyJ(this->getPseudoRandomNormal()); +} - auto simu = createSimpleScene(mapping); - EXPECT_EQ(mapping->getFrom()[0]->getSize(),10); - EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); +TYPED_TEST(PlaneMappingTest, applyJT_Forces) +{ + this->testApplyJT_Force(this->getPseudoRandomNormal()); +} + +TYPED_TEST(PlaneMappingTest, applyJT_Constraints) +{ + this->testApplyJT_Constraint(this->getPseudoRandomNormal()); } From 58eae9d04ffd77d9278b03acc292813d99a30296 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 25 Feb 2025 16:56:40 +0100 Subject: [PATCH 05/10] Add Rigid2 support --- .../component/mapping/linear/DistanceToPlaneMapping.inl | 7 +++++-- .../Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp | 7 ++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl index 53839ceb372..7191c95a9d3 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl @@ -78,8 +78,11 @@ void DistanceToPlaneMapping::init() for (Size j = 0; j < inDerivSize; j++ ) { - if (sofa::type::isRigidType && i>2) - break; + if constexpr (sofa::type::isRigidType) + { + if (i>=Deriv_t::Pos::size()) + break; + } J.compressedMatrix.insertBack( i, col ) = planeNormal[i]; } J.compressedMatrix.finalize(); diff --git a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp index 59182815554..b532f35c054 100644 --- a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp +++ b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp @@ -104,10 +104,6 @@ class PlaneMappingTest : public testing::Test { returnVec[i] = randomValuesForPoint[i]; } - if constexpr (sofa::type::isRigidType) - { - returnVec.getOrientation() = sofa::type::Quatd(0.707,0,0.707,0); - } return returnVec; } @@ -268,9 +264,10 @@ using sofa::defaulttype::Vec2Types; using sofa::defaulttype::Vec3Types; using sofa::defaulttype::Vec6Types; using sofa::defaulttype::Rigid3Types; +using sofa::defaulttype::Rigid2Types; -using DataTypes = testing::Types; +using DataTypes = testing::Types; TYPED_TEST_SUITE(PlaneMappingTest, DataTypes); TYPED_TEST(PlaneMappingTest, init) From 5c176e575b8b8f65677d5d5d6dcd3081fbc4c994 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 25 Feb 2025 17:06:22 +0100 Subject: [PATCH 06/10] Add little optimization for plane distance computation --- .../mapping/linear/DistanceToPlaneMapping.inl | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl index 7191c95a9d3..b11581921f6 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl @@ -102,14 +102,20 @@ void DistanceToPlaneMapping::apply(const core::MechanicalParams *mparams, D const auto readIn = helper::getReadAccessor(in); const auto planeNormal = d_planeNormal.getValue(); - const auto planePoint = d_planePoint.getValue(); + + double planeDistanceToOrigin; + if constexpr (!type::isRigidType) + planeDistanceToOrigin = dot(d_planePoint.getValue(),planeNormal); + else + planeDistanceToOrigin = dot(d_planePoint.getValue().getCenter(),planeNormal.getVCenter()); + for ( unsigned i = 0; i) - writeOut[i] = type::dot(readIn[i] - planePoint,planeNormal); + writeOut[i] = type::dot(readIn[i],planeNormal) - planeDistanceToOrigin; else - writeOut[i] = type::dot(readIn[i].getCenter() - planePoint.getCenter(),planeNormal.getVCenter()); + writeOut[i] = type::dot(readIn[i].getCenter(),planeNormal.getVCenter()) - planeDistanceToOrigin; } } From 8034f83d00ce5cb6bdd384c92f33ebe41ad5e96b Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Thu, 27 Feb 2025 16:56:38 +0100 Subject: [PATCH 07/10] Update regarding comments suggestions --- .../mapping/linear/DistanceToPlaneMapping.h | 4 +- .../mapping/linear/DistanceToPlaneMapping.inl | 49 +++---------- .../tests/DistanceToPlaneMapping_test.cpp | 68 +++++++++++-------- 3 files changed, 53 insertions(+), 68 deletions(-) diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h index cd6db132368..83d24583d8a 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.h @@ -57,8 +57,8 @@ class DistanceToPlaneMapping : public LinearMapping> d_planeNormal; - Data> d_planePoint; + Data::spatial_dimensions,typename Deriv_t::value_type>> d_planeNormal; + Data::spatial_dimensions,typename Coord_t::value_type>> d_planePoint; protected: diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl index b11581921f6..d3de4ec43a9 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl @@ -44,14 +44,6 @@ void DistanceToPlaneMapping::init() { Inherit::init(); - if constexpr (sofa::type::isRigidType) - { - if (d_planeNormal.getValue().getVOrientation() != typename sofa::Deriv_t::Rot()*0) - { - msg_info(this) <<"You've untered a plane normal with a non null rotation. Only position is treated by this component, the rotation part will thus be considered as zero."; - } - } - //Normalize plane normal const double normalNorm = d_planeNormal.getValue().norm(); if (normalNorm::epsilon()) @@ -62,28 +54,23 @@ void DistanceToPlaneMapping::init() } d_planeNormal.setValue(d_planeNormal.getValue()/normalNorm); - constexpr Size inCoordSize = typename Coord_t::Size(); - constexpr Size inDerivSize = typename Deriv_t::Size(); + constexpr Size inDerivSize = Deriv_t::size(); + constexpr Size inSpatialDimension = Deriv_t::spatial_dimensions; Size inSize = this->getFromModel()->getSize(); this->getToModel()->resize( inSize ); const auto planeNormal = d_planeNormal.getValue(); - J.compressedMatrix.resize( inSize, inSize*inCoordSize ); + J.compressedMatrix.resize( inSize, inSize*inDerivSize ); for (Size i = 0; i < inSize; i++) { - const size_t col = i * inCoordSize; + const size_t col = i * inDerivSize; J.compressedMatrix.startVec(i); - for (Size j = 0; j < inDerivSize; j++ ) + for (Size j = 0; j < inSpatialDimension; j++ ) { - if constexpr (sofa::type::isRigidType) - { - if (i>=Deriv_t::Pos::size()) - break; - } - J.compressedMatrix.insertBack( i, col ) = planeNormal[i]; + J.compressedMatrix.insertBack( i, col + j ) = planeNormal[j]; } J.compressedMatrix.finalize(); @@ -103,19 +90,12 @@ void DistanceToPlaneMapping::apply(const core::MechanicalParams *mparams, D const auto planeNormal = d_planeNormal.getValue(); - double planeDistanceToOrigin; - if constexpr (!type::isRigidType) - planeDistanceToOrigin = dot(d_planePoint.getValue(),planeNormal); - else - planeDistanceToOrigin = dot(d_planePoint.getValue().getCenter(),planeNormal.getVCenter()); + double planeDistanceToOrigin = dot(d_planePoint.getValue(),planeNormal); for ( unsigned i = 0; i) - writeOut[i] = type::dot(readIn[i],planeNormal) - planeDistanceToOrigin; - else - writeOut[i] = type::dot(readIn[i].getCenter(),planeNormal.getVCenter()) - planeDistanceToOrigin; + writeOut[i] = type::dot(TIn::getCPos(readIn[i]),planeNormal) - planeDistanceToOrigin; } } @@ -131,10 +111,7 @@ void DistanceToPlaneMapping::applyJ(const core::MechanicalParams *mparams, for ( unsigned i = 0; i) - writeOut[i] = type::dot(readIn[i],planeNormal); - else - writeOut[i] = type::dot(readIn[i].getVCenter(),planeNormal.getVCenter()); + writeOut[i] = type::dot(TIn::getDPos(readIn[i]),planeNormal); } } @@ -151,11 +128,7 @@ void DistanceToPlaneMapping::applyJT(const core::MechanicalParams *mparams, for ( unsigned i = 0; i) - writeOut[i] += planeNormal * readIn[i][0] ; - else - writeOut[i] += sofa::Deriv_t(planeNormal.getVCenter() * readIn[i][0],typename sofa::Deriv_t::Rot()) ; - + TIn::setDPos(writeOut[i], TIn::getDPos(writeOut[i]) + planeNormal * readIn[i][0]) ; } } @@ -183,7 +156,7 @@ void DistanceToPlaneMapping::applyJT(const core::ConstraintParams *cparams, if constexpr (!type::isRigidType) o.addCol(colIt.index(), planeNormal*colIt.val()[0]); else - o.addCol(colIt.index(), sofa::Deriv_t(planeNormal.getVCenter() * colIt.val()[0],typename sofa::Deriv_t::Rot()) ); + o.addCol(colIt.index(), sofa::Deriv_t(planeNormal * colIt.val()[0],typename sofa::Deriv_t::Rot() * 0) ); ++colIt; } diff --git a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp index b532f35c054..b500f21c357 100644 --- a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp +++ b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp @@ -49,6 +49,9 @@ using sofa::defaulttype::Vec1Types; template class PlaneMappingTest : public testing::Test { + + typedef sofa::type::Vec::spatial_dimensions,typename sofa::Deriv_t::value_type> PlaneNormalType; + typedef sofa::type::Vec::spatial_dimensions,typename sofa::Coord_t::value_type> PlanePointType; public: sofa::simulation::Simulation* createSimpleScene(typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping) { @@ -80,27 +83,23 @@ class PlaneMappingTest : public testing::Test return simu; } - sofa::Deriv_t getPseudoRandomNormal() + PlaneNormalType getPseudoRandomNormal() { constexpr std::array randomValuesForNormal = {1, 5.1, -6, 0, 9.5, 4}; - sofa::Deriv_t returnVec; - for (unsigned i = 0; i< sofa::Deriv_t::size(); ++i) + PlaneNormalType returnVec; + for (unsigned i = 0; i< sofa::Deriv_t::spatial_dimensions; ++i) { returnVec[i] = randomValuesForNormal[i]; } - if constexpr (sofa::type::isRigidType) - { - returnVec.getVOrientation() = typename sofa::Deriv_t::Rot()*0; - } return returnVec; } - sofa::Coord_t getPseudoRandomPoint() + PlanePointType getPseudoRandomPoint() { constexpr std::array randomValuesForPoint = {-2.5, 1.4, 3, 0, 0.7, 12, 7.07}; - sofa::Coord_t returnVec; - for (unsigned i = 0; i< sofa::Coord_t::size(); ++i) + PlanePointType returnVec; + for (unsigned i = 0; i< sofa::Coord_t::spatial_dimensions; ++i) { returnVec[i] = randomValuesForPoint[i]; } @@ -108,7 +107,7 @@ class PlaneMappingTest : public testing::Test } - void testInit(sofa::Deriv_t planeNormal) + void testInit(PlaneNormalType planeNormal) { typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); mapping->d_planeNormal.setValue(planeNormal); @@ -118,12 +117,18 @@ class PlaneMappingTest : public testing::Test EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); } - void testApply(sofa::Deriv_t planeNormal, sofa::Coord_t planePoint) + void testApply(PlaneNormalType planeNormal, PlanePointType planePoint) { typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); planeNormal = planeNormal/planeNormal.norm(); - sofa::Deriv_t planeTangent1 = planeNormal; + sofa::Deriv_t fullNormal; + DataType::setDPos(fullNormal, planeNormal); + + sofa::Coord_t fullPlanePoint; + DataType::setCPos(fullPlanePoint, planePoint); + + sofa::Deriv_t planeTangent1 = fullNormal; planeTangent1[0] = planeNormal[1]; planeTangent1[1] = -planeNormal[0]; for (unsigned i = 2; i< sofa::Deriv_t::size(); ++i) @@ -137,10 +142,10 @@ class PlaneMappingTest : public testing::Test std::vector dists{0.2,-0.5,-2.1,0.5}; - inPos.setValue({planePoint + planeNormal * dists[0], - planePoint + planeNormal * dists[1] + planeTangent1 * 10, - planePoint + planeNormal * dists[2] , - planePoint + planeNormal * dists[3] + planeTangent1 * 2 }); + inPos.setValue({fullPlanePoint + fullNormal * dists[0], + fullPlanePoint + fullNormal * dists[1] + planeTangent1 * 10, + fullPlanePoint + fullNormal * dists[2] , + fullPlanePoint + fullNormal * dists[3] + planeTangent1 * 2 }); sofa::DataVecCoord_t outVec; outVec.beginEdit()->resize(4); mapping->d_planeNormal.setValue(planeNormal); @@ -154,15 +159,17 @@ class PlaneMappingTest : public testing::Test } } - void testApplyJ(sofa::Deriv_t planeNormal) + void testApplyJ(PlaneNormalType planeNormal) { typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); planeNormal = planeNormal/planeNormal.norm(); - sofa::Deriv_t planeTangent1; + sofa::Deriv_t fullNormal; + DataType::setDPos(fullNormal, planeNormal); + sofa::Deriv_t planeTangent1 = fullNormal; planeTangent1[0] = planeNormal[1]; planeTangent1[1] = -planeNormal[0]; - for (unsigned i = 2; i< sofa::Deriv_t::size(); ++i) + for (unsigned i = 2; i< PlaneNormalType::size(); ++i) { planeTangent1[i] = 0; } @@ -173,10 +180,10 @@ class PlaneMappingTest : public testing::Test std::vector dists{0.2,-0.5,-2.1,0.5}; - inPos.setValue({ planeNormal * dists[0], - planeNormal * dists[1] + planeTangent1 * 10, - planeNormal * dists[2], - planeNormal * dists[3] + planeTangent1 * 2}); + inPos.setValue({ fullNormal * dists[0], + fullNormal * dists[1] + planeTangent1 * 10, + fullNormal * dists[2], + fullNormal * dists[3] + planeTangent1 * 2}); sofa::DataVecCoord_t outVec; outVec.beginEdit()->resize(4); mapping->d_planeNormal.setValue(planeNormal); @@ -189,7 +196,7 @@ class PlaneMappingTest : public testing::Test } } - void testApplyJT_Force(sofa::Deriv_t planeNormal) + void testApplyJT_Force(PlaneNormalType planeNormal) { typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); @@ -208,14 +215,14 @@ class PlaneMappingTest : public testing::Test for (unsigned i = 0; i::size(); ++j) + for (unsigned j=0; j< PlaneNormalType::size(); ++j) { EXPECT_LE(fabs(outVec.getValue()[i][j] - planeNormal[j] * inPos.getValue()[i][0]),EPSILON); } } } - void testApplyJT_Constraint(sofa::Deriv_t planeNormal) + void testApplyJT_Constraint(PlaneNormalType planeNormal) { typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); @@ -251,7 +258,12 @@ class PlaneMappingTest : public testing::Test EXPECT_FALSE(colIt == colItEnd); EXPECT_EQ(colIt.index(),dofsIds[rowIt.index()]); - EXPECT_EQ(colIt.val(),planeNormal*( 1 - 2*(rowIt.index()%1))); + EXPECT_EQ(DataType::getDPos(colIt.val()),planeNormal*( 1 - 2*(rowIt.index()%1))); + + if constexpr (sofa::type::isRigidType) + { + EXPECT_EQ(DataType::getDRot(colIt.val()),typename sofa::Deriv_t::Rot() *0 ); + } ++colIt; EXPECT_TRUE(colIt == colItEnd); From 87dbd553a845093a79987c91dcd702de7b2316b7 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Thu, 27 Feb 2025 17:18:16 +0100 Subject: [PATCH 08/10] Apply modification also for constraint matrix --- .../component/mapping/linear/DistanceToPlaneMapping.inl | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl index d3de4ec43a9..55e93a8ff72 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl @@ -153,10 +153,9 @@ void DistanceToPlaneMapping::applyJT(const core::ConstraintParams *cparams, auto o = writeMatrixOut->writeLine(rowIt.index()); while (colIt != colItEnd) { - if constexpr (!type::isRigidType) - o.addCol(colIt.index(), planeNormal*colIt.val()[0]); - else - o.addCol(colIt.index(), sofa::Deriv_t(planeNormal * colIt.val()[0],typename sofa::Deriv_t::Rot() * 0) ); + Deriv_t normal; + TIn::setDPos(normal,planeNormal*colIt.val()[0]); + o.addCol(colIt.index(), normal); ++colIt; } From cd15c4e1445cdaac8da49fca1e5fd8c998b23259 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Wed, 5 Mar 2025 15:57:52 +0100 Subject: [PATCH 09/10] Take comments into account --- .../mapping/linear/DistanceToPlaneMapping.inl | 8 ++++---- .../tests/DistanceToPlaneMapping_test.cpp | 18 +++++++++--------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl index 55e93a8ff72..8c1bcb71f74 100644 --- a/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl +++ b/Sofa/Component/Mapping/Linear/src/sofa/component/mapping/linear/DistanceToPlaneMapping.inl @@ -45,10 +45,10 @@ void DistanceToPlaneMapping::init() Inherit::init(); //Normalize plane normal - const double normalNorm = d_planeNormal.getValue().norm(); - if (normalNorm::epsilon()) + const SReal normalNorm = d_planeNormal.getValue().norm(); + if (normalNorm::epsilon()) { - msg_error(this)<<" planeNormal data has null norm."; + msg_error()<<" planeNormal data has null norm."; this->d_componentState.setValue(core::objectmodel::ComponentState::Invalid); return; } @@ -90,7 +90,7 @@ void DistanceToPlaneMapping::apply(const core::MechanicalParams *mparams, D const auto planeNormal = d_planeNormal.getValue(); - double planeDistanceToOrigin = dot(d_planePoint.getValue(),planeNormal); + SReal planeDistanceToOrigin = dot(d_planePoint.getValue(),planeNormal); for ( unsigned i = 0; i #include -#define EPSILON 1e-12 +static constexpr SReal EPSILON=1e-12; using sofa::defaulttype::Vec1Types; @@ -85,7 +85,7 @@ class PlaneMappingTest : public testing::Test PlaneNormalType getPseudoRandomNormal() { - constexpr std::array randomValuesForNormal = {1, 5.1, -6, 0, 9.5, 4}; + constexpr std::array randomValuesForNormal = {1, 5.1, -6, 0, 9.5, 4}; PlaneNormalType returnVec; for (unsigned i = 0; i< sofa::Deriv_t::spatial_dimensions; ++i) { @@ -97,7 +97,7 @@ class PlaneMappingTest : public testing::Test PlanePointType getPseudoRandomPoint() { - constexpr std::array randomValuesForPoint = {-2.5, 1.4, 3, 0, 0.7, 12, 7.07}; + constexpr std::array randomValuesForPoint = {-2.5, 1.4, 3, 0, 0.7, 12, 7.07}; PlanePointType returnVec; for (unsigned i = 0; i< sofa::Coord_t::spatial_dimensions; ++i) { @@ -112,7 +112,7 @@ class PlaneMappingTest : public testing::Test typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); mapping->d_planeNormal.setValue(planeNormal); auto simu = this->createSimpleScene(mapping); - EXPECT_LE(fabs(mapping->d_planeNormal.getValue().norm() - 1.0),EPSILON); + EXPECT_LE(std::fabs(mapping->d_planeNormal.getValue().norm() - 1.0),EPSILON); EXPECT_EQ(mapping->getFrom()[0]->getSize(),10); EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); } @@ -140,7 +140,7 @@ class PlaneMappingTest : public testing::Test sofa::DataVecCoord_t inPos{typename sofa::DataVecCoord_t::InitData()}; - std::vector dists{0.2,-0.5,-2.1,0.5}; + std::vector dists{0.2,-0.5,-2.1,0.5}; inPos.setValue({fullPlanePoint + fullNormal * dists[0], fullPlanePoint + fullNormal * dists[1] + planeTangent1 * 10, @@ -155,7 +155,7 @@ class PlaneMappingTest : public testing::Test for (unsigned i = 0; i(outVec.getValue()[i][0] - dists[i]), EPSILON); } } @@ -178,7 +178,7 @@ class PlaneMappingTest : public testing::Test sofa::DataVecDeriv_t inPos{typename sofa::DataVecDeriv_t::InitData()}; - std::vector dists{0.2,-0.5,-2.1,0.5}; + std::vector dists{0.2,-0.5,-2.1,0.5}; inPos.setValue({ fullNormal * dists[0], fullNormal * dists[1] + planeTangent1 * 10, @@ -192,7 +192,7 @@ class PlaneMappingTest : public testing::Test for (unsigned i = 0; i(outVec.getValue()[i][0] - dists[i]), EPSILON ); } } @@ -217,7 +217,7 @@ class PlaneMappingTest : public testing::Test { for (unsigned j=0; j< PlaneNormalType::size(); ++j) { - EXPECT_LE(fabs(outVec.getValue()[i][j] - planeNormal[j] * inPos.getValue()[i][0]),EPSILON); + EXPECT_LE(std::fabs(outVec.getValue()[i][j] - planeNormal[j] * inPos.getValue()[i][0]),EPSILON); } } } From 5e61aa6f7017c2892fa32564d67f7ee85147e436 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 6 Mar 2025 10:11:52 +0900 Subject: [PATCH 10/10] fix compilation (std::fabs not templated for floating point) --- .../Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp index 4e15c757d26..56168a685f8 100644 --- a/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp +++ b/Sofa/Component/Mapping/Linear/tests/DistanceToPlaneMapping_test.cpp @@ -112,7 +112,7 @@ class PlaneMappingTest : public testing::Test typename sofa::component::mapping::linear::DistanceToPlaneMapping::SPtr mapping = New>(); mapping->d_planeNormal.setValue(planeNormal); auto simu = this->createSimpleScene(mapping); - EXPECT_LE(std::fabs(mapping->d_planeNormal.getValue().norm() - 1.0),EPSILON); + EXPECT_LE(std::fabs(mapping->d_planeNormal.getValue().norm() - 1.0),EPSILON); EXPECT_EQ(mapping->getFrom()[0]->getSize(),10); EXPECT_EQ(mapping->getTo()[0]->getSize(),mapping->getFrom()[0]->getSize()); } @@ -155,7 +155,7 @@ class PlaneMappingTest : public testing::Test for (unsigned i = 0; i(outVec.getValue()[i][0] - dists[i]), EPSILON); + EXPECT_LE(std::fabs(outVec.getValue()[i][0] - dists[i]), EPSILON); } } @@ -192,7 +192,7 @@ class PlaneMappingTest : public testing::Test for (unsigned i = 0; i(outVec.getValue()[i][0] - dists[i]), EPSILON ); + EXPECT_LE(std::fabs(outVec.getValue()[i][0] - dists[i]), EPSILON ); } } @@ -217,7 +217,7 @@ class PlaneMappingTest : public testing::Test { for (unsigned j=0; j< PlaneNormalType::size(); ++j) { - EXPECT_LE(std::fabs(outVec.getValue()[i][j] - planeNormal[j] * inPos.getValue()[i][0]),EPSILON); + EXPECT_LE(std::fabs(outVec.getValue()[i][j] - planeNormal[j] * inPos.getValue()[i][0]),EPSILON); } } }