diff --git a/Sofa/Component/SolidMechanics/Spring/CMakeLists.txt b/Sofa/Component/SolidMechanics/Spring/CMakeLists.txt index d9b7983d387..42954ca0934 100644 --- a/Sofa/Component/SolidMechanics/Spring/CMakeLists.txt +++ b/Sofa/Component/SolidMechanics/Spring/CMakeLists.txt @@ -12,6 +12,8 @@ set(HEADER_FILES # ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/BoxStiffSpringForceField.inl ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FastTriangularBendingSprings.h ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FastTriangularBendingSprings.inl + ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FixedWeakConstraint.h + ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FixedWeakConstraint.inl ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FrameSpringForceField.h ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FrameSpringForceField.inl ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/GearSpringForceField.h @@ -58,6 +60,7 @@ set(SOURCE_FILES ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/AngularSpringForceField.cpp # ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/BoxStiffSpringForceField.cpp ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FastTriangularBendingSprings.cpp + ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FixedWeakConstraint.cpp ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/FrameSpringForceField.cpp ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/GearSpringForceField.cpp ${SOFACOMPONENTSOLIDMECHANICSSPRING_SOURCE_DIR}/JointSpring.cpp diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.cpp b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.cpp new file mode 100644 index 00000000000..7a2c57d84ba --- /dev/null +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.cpp @@ -0,0 +1,48 @@ +/****************************************************************************** +* 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_FORCEFIELD_FixedWeakConstraint_CPP + +#include + +#include +#include +#include + + +namespace sofa::component::solidmechanics::spring +{ + +using namespace sofa::defaulttype; + +void registerFixedWeakConstraint(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(core::ObjectRegistrationData("Elastic springs generating forces on degrees of freedom between their current and rest shape position.") + .add< FixedWeakConstraint >() + .add< FixedWeakConstraint >() + .add< FixedWeakConstraint >()); +} + +template class SOFA_COMPONENT_SOLIDMECHANICS_SPRING_API FixedWeakConstraint; +template class SOFA_COMPONENT_SOLIDMECHANICS_SPRING_API FixedWeakConstraint; +template class SOFA_COMPONENT_SOLIDMECHANICS_SPRING_API FixedWeakConstraint; + +} // namespace sofa::component::solidmechanics::spring diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.h b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.h new file mode 100644 index 00000000000..c710d994709 --- /dev/null +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.h @@ -0,0 +1,126 @@ +/****************************************************************************** +* 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::core::behavior +{ + +template< class T > class MechanicalState; + +} // namespace sofa::core::behavior + +namespace sofa::component::solidmechanics::spring +{ + +/** +* @brief This class describes a simple elastic springs ForceField between DOFs positions and rest positions. +* +* Springs are applied to given degrees of freedom between their current positions and their rest shape positions. +* An external MechanicalState reference can also be passed to the ForceField as rest shape position. + */ +template +class FixedWeakConstraint : public core::behavior::ForceField +{ + public: + SOFA_CLASS(SOFA_TEMPLATE(FixedWeakConstraint, DataTypes), SOFA_TEMPLATE(core::behavior::ForceField, DataTypes)); + + typedef core::behavior::ForceField Inherit; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::CPos CPos; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::Real Real; + typedef type::vector< sofa::Index > VecIndex; + typedef sofa::core::topology::TopologySubsetIndices DataSubsetIndex; + typedef type::vector< Real > VecReal; + + static constexpr sofa::Size spatial_dimensions = Coord::spatial_dimensions; + static constexpr sofa::Size coord_total_size = Coord::total_size; + + typedef core::objectmodel::Data DataVecCoord; + typedef core::objectmodel::Data DataVecDeriv; + + DataSubsetIndex d_indices; ///< points controlled by the rest shape springs + core::objectmodel::lifecycle::RemovedData d_points{this,"v24.12","v25.06","points","This data has been replaced by \'indices\'. Please update your scene."}; + + Data d_fixAll; ///< points controlled by the rest shape springs + Data< VecReal > d_stiffness; ///< stiffness values between the actual position and the rest shape position + Data< VecReal > d_angularStiffness; ///< angularStiffness assigned when controlling the rotation of the points + Data< bool > d_drawSpring; ///< draw Spring + Data< sofa::type::RGBAColor > d_springColor; ///< spring color. (default=[0.0,1.0,0.0,1.0]) + + /// Link to be set to the topology container in the component graph. + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; + + + /// BaseObject initialization method. + void bwdInit() override ; + void reinit() override ; + + /// Add the forces. + void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + + void addDForce(const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) override; + SReal getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const override; + + /// Brings ForceField contribution to the global system stiffness matrix. + void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix ) override; + void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; + void buildDampingMatrix(core::behavior::DampingMatrix* matrix) override; + + void draw(const core::visual::VisualParams* vparams) override; + + + protected : + FixedWeakConstraint(); + static constexpr type::fixed_array::coord_total_size> s_defaultActiveDirections = sofa::type::makeHomogeneousArray::coord_total_size>(true); + + virtual const DataVecCoord* getExtPosition() const; + virtual const VecIndex& getIndices() const; + virtual const VecIndex& getExtIndices() const; + virtual const type::fixed_array::coord_total_size>& getActiveDirections() const; + virtual const bool checkState(); + + virtual bool checkOutOfBoundsIndices(); + + bool checkOutOfBoundsIndices(const VecIndex &indices, const sofa::Size dimension); +}; + +#if !defined(SOFA_COMPONENT_FORCEFIELD_FixedWeakConstraint_CPP) +extern template class SOFA_COMPONENT_SOLIDMECHANICS_SPRING_API FixedWeakConstraint; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_SPRING_API FixedWeakConstraint; +extern template class SOFA_COMPONENT_SOLIDMECHANICS_SPRING_API FixedWeakConstraint; +#endif + +} // namespace sofa::component::solidmechanics::spring diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.inl b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.inl new file mode 100644 index 00000000000..5dba0d315f9 --- /dev/null +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/FixedWeakConstraint.inl @@ -0,0 +1,589 @@ +/****************************************************************************** +* 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 +#include + +#include +#include + +namespace sofa::component::solidmechanics::spring +{ + +using helper::WriteAccessor; +using helper::ReadAccessor; +using core::behavior::BaseMechanicalState; +using core::behavior::MultiMatrixAccessor; +using core::behavior::ForceField; +using linearalgebra::BaseMatrix; +using core::VecCoordId; +using core::MechanicalParams; +using type::Vec3; +using type::Vec4f; +using type::vector; +using core::visual::VisualParams; + + +template +FixedWeakConstraint::FixedWeakConstraint() + : d_indices(initData(&d_indices, "indices", "points controlled by the rest shape springs")) + , d_fixAll(initData(&d_fixAll, false, "fixAll", "Force to fix all points")) + , d_stiffness(initData(&d_stiffness, "stiffness", "stiffness values between the actual position and the rest shape position")) + , d_angularStiffness(initData(&d_angularStiffness, "angularStiffness", "angularStiffness assigned when controlling the rotation of the points")) + , d_drawSpring(initData(&d_drawSpring,false,"drawSpring","draw Spring")) + , d_springColor(initData(&d_springColor, sofa::type::RGBAColor::green(), "springColor","spring color. (default=[0.0,1.0,0.0,1.0])")) + , l_topology(initLink("topology", "Link to be set to the topology container in the component graph")) +{ + this->addUpdateCallback("updateIndices", {&d_indices}, [this](const core::DataTracker& t) + { + SOFA_UNUSED(t); + if (!checkOutOfBoundsIndices()) + { + msg_error(this) << "Some input indices are out of bound"; + return sofa::core::objectmodel::ComponentState::Invalid; + } + else + { + return sofa::core::objectmodel::ComponentState::Valid; + } + }, {}); +} + + +template +const bool FixedWeakConstraint::checkState() +{ + if (d_stiffness.getValue().empty()) + { + msg_info(this) << "No stiffness is defined, assuming equal stiffness on each node, k = 100.0 "; + d_stiffness.setValue({static_cast(100)}); + } + + if (l_topology.empty()) + { + msg_info(this) << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; + l_topology.set(this->getContext()->getMeshTopologyLink()); + } + + if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) + { + msg_info(this) << "Topology path used: '" << l_topology.getLinkedPath() << "'"; + + // Initialize topological changes support + d_indices.createTopologyHandler(_topology); + } + else + { + msg_info(this) << "Cannot find the topology: topological changes will not be supported"; + } + + + + /// Compile time condition to check if we are working with a Rigid3Types or a type that does not + /// need the Angular Stiffness parameters. + //if constexpr (isRigid()) + if constexpr (sofa::type::isRigidType) + { + sofa::helper::ReadAccessor> s = d_stiffness; + sofa::helper::WriteOnlyAccessor> as = d_angularStiffness; + + if (as.size() < s.size()) + { + msg_info(this) << "'stiffness' is larger than 'angularStiffness', add the default value (100.0) to the missing entries."; + + for(size_t i = as.size();i s.size()) + { + msg_info(this) << "'stiffness' is smaller than 'angularStiffness', clamp the extra values in angularStiffness."; + as.resize(s.size()); + } + } + + if (!checkOutOfBoundsIndices()) + { + return false; + } + else + { + return true; + } +} + +template +void FixedWeakConstraint::bwdInit() +{ + ForceField::init(); + + if (checkState()) + { + this->d_componentState.setValue(core::objectmodel::ComponentState::Valid); + } + else + { + this->d_componentState.setValue(core::objectmodel::ComponentState::Invalid); + + } +} + + +template +void FixedWeakConstraint::reinit() +{ + ForceField::reinit(); + + if (checkState()) + { + this->d_componentState.setValue(core::objectmodel::ComponentState::Valid); + } + else + { + this->d_componentState.setValue(core::objectmodel::ComponentState::Invalid); + + } + +} + + + +template +bool FixedWeakConstraint::checkOutOfBoundsIndices() +{ + if (!checkOutOfBoundsIndices(getIndices(), this->mstate->getSize())) + { + return false; + } + return true; +} + +template +bool FixedWeakConstraint::checkOutOfBoundsIndices(const VecIndex &indices, const sofa::Size dimension) +{ + for (sofa::Index i = 0; i < indices.size(); i++) + { + if (indices[i] >= dimension) + { + return false; + } + } + return true; +} + +template +const typename FixedWeakConstraint::DataVecCoord* FixedWeakConstraint::getExtPosition() const +{ + if (this->mstate) + { + return this->mstate->read(core::vec_id::write_access::restPosition); + } + return nullptr; +} + +template +const typename FixedWeakConstraint::VecIndex& FixedWeakConstraint::getIndices() const +{ + + return d_indices.getValue(); +} + +template +const typename FixedWeakConstraint::VecIndex& FixedWeakConstraint::getExtIndices() const +{ + return d_indices.getValue(); +} + +template +const type::fixed_array::coord_total_size>& FixedWeakConstraint::getActiveDirections() const +{ + return FixedWeakConstraint::s_defaultActiveDirections; +} + +template +void FixedWeakConstraint::addForce(const MechanicalParams* mparams , DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v ) +{ + if (this->d_componentState.getValue() != core::objectmodel::ComponentState::Valid) + return; + + SOFA_UNUSED(mparams); + SOFA_UNUSED(v); + + WriteAccessor< DataVecDeriv > f1 = f; + ReadAccessor< DataVecCoord > p1 = x; + + const DataVecCoord* extPosition = getExtPosition(); + const auto & indices = getIndices(); + const auto & extIndices = getExtIndices(); + if (!extPosition) + { + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + + ReadAccessor< DataVecCoord > p0 = *extPosition; + + const VecReal& k = d_stiffness.getValue(); + const VecReal& k_a = d_angularStiffness.getValue(); + + + f1.resize(p1.size()); + + const bool fixedAll = d_fixAll.getValue(); + const unsigned maxIt = fixedAll ? this->mstate->getSize() : indices.size(); + + for (sofa::Index i = 0; i < maxIt; i++) + { + sofa::Index ext_index = i; + sofa::Index index = i; + + if (!fixedAll) + { + index = indices[i]; + ext_index = extIndices[i]; + } + + const auto stiffness = k[static_cast(i < k.size()) * i]; + + const auto & activeDirections = getActiveDirections(); + + // rigid case + if constexpr (sofa::type::isRigidType) + { + + CPos dx = p1[index].getCenter() - p0[ext_index].getCenter(); + // We filter the difference dx by setting to 0 the entries corresponding + // to 0 values in d_activeDirections + for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) + { + if (!activeDirections[entryId]) + dx[entryId] = 0; + } + getVCenter(f1[index]) -= dx * stiffness; + + + + // rotation + type::Quat dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse(); + dq.normalize(); + + type::Vec<3, Real> dir{type::NOINIT}; + Real angle = 0.; + + if (dq[3] < 0.) + { + dq = dq * -1.0; + } + + if (dq[3] < 1.0) + dq.quatToAxis(dir, angle); + + // We change the direction of the axis of rotation based on + // the 0 values in d_activeDirections. This is equivalent + // to senting to 0 the rotation axis components along x, y + // and/or z, depending on the rotations we want to take into + // account. + for (sofa::Size entryId = spatial_dimensions; entryId < coord_total_size; ++entryId) + { + if (!activeDirections[entryId]) + dir[entryId-spatial_dimensions] = 0; + } + + const auto angularStiffness = k_a[static_cast(i < k_a.size()) * i]; + getVOrientation(f1[index]) -= dir * angle * angularStiffness; + } + else // non-rigid implementation + { + Deriv dx = p1[index] - p0[ext_index]; + // We filter the difference dx by setting to 0 the entries corresponding + // to 0 values in d_activeDirections + for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) + { + if (!activeDirections[entryId]) + dx[entryId] = 0; + } + f1[index] -= dx * stiffness; + } + } +} + +template +void FixedWeakConstraint::addDForce(const MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) +{ + if (this->d_componentState.getValue() != core::objectmodel::ComponentState::Valid) + return; + + WriteAccessor< DataVecDeriv > df1 = df; + ReadAccessor< DataVecDeriv > dx1 = dx; + Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); + const VecReal& k = d_stiffness.getValue(); + const VecReal& k_a = d_angularStiffness.getValue(); + const auto & activeDirections = getActiveDirections(); + + const auto & indices = getIndices(); + + const bool fixedAll = d_fixAll.getValue(); + const unsigned maxIt = fixedAll ? this->mstate->getSize() : indices.size(); + + for (sofa::Index i = 0; i < maxIt; i++) + { + sofa::Index curIndex = i; + + if (!fixedAll) + { + curIndex = indices[i]; + } + + const auto stiffness = k[static_cast(i < k.size()) * i]; + + if constexpr (sofa::type::isRigidType) + { + const auto angularStiffness = k_a[static_cast(i < k_a.size()) * i]; + + // We filter the difference in translation by setting to 0 the entries corresponding + // to 0 values in d_activeDirections + auto currentSpringDx = getVCenter(dx1[curIndex]); + for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) + { + if (!activeDirections[entryId]) + currentSpringDx[entryId] = 0; + } + getVCenter(df1[curIndex]) -= currentSpringDx * stiffness * kFactor; + + auto currentSpringRotationalDx = getVOrientation(dx1[curIndex]); + // We change the direction of the axis of rotation based on + // the 0 values in d_activeDirections. This is equivalent + // to senting to 0 the rotation axis components along x, y + // and/or z, depending on the rotations we want to take into + // account. + for (sofa::Size entryId = spatial_dimensions; entryId < coord_total_size; ++entryId) + { + if (!activeDirections[entryId]) + currentSpringRotationalDx[entryId-spatial_dimensions] = 0; + } + getVOrientation(df1[curIndex]) -= currentSpringRotationalDx * angularStiffness * kFactor; + } + else + { + // We filter the difference in translation by setting to 0 the entries corresponding + // to 0 values in d_activeDirections + auto currentSpringDx = dx1[curIndex]; + for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) + { + if (!activeDirections[entryId]) + currentSpringDx[entryId] = 0; + } + df1[curIndex] -= currentSpringDx * stiffness * kFactor; + } + } + + +} + +template +SReal FixedWeakConstraint::getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(x); + + msg_warning() << "Method getPotentialEnergy not implemented yet."; + return 0.0; +} + +template +void FixedWeakConstraint::addKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix ) +{ + if (this->d_componentState.getValue() != core::objectmodel::ComponentState::Valid) + return; + + const MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); + BaseMatrix* mat = mref.matrix; + const unsigned int offset = mref.offset; + Real kFact = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); + + const VecReal& k = d_stiffness.getValue(); + const VecReal& k_a = d_angularStiffness.getValue(); + const auto & activeDirections = getActiveDirections(); + const auto & indices = getIndices(); + + constexpr sofa::Size space_size = Deriv::spatial_dimensions; // == total_size if DataTypes = VecTypes + constexpr sofa::Size total_size = Deriv::total_size; + + const bool fixedAll = d_fixAll.getValue(); + const unsigned maxIt = fixedAll ? this->mstate->getSize() : indices.size(); + + for (sofa::Index index = 0; index < maxIt; index++) + { + sofa::Index curIndex = index; + + if (!fixedAll) + { + curIndex = indices[index]; + } + + // translation + const auto vt = -kFact * k[(index < k.size()) * index]; + for (sofa::Size i = 0; i < space_size; i++) + { + // Contribution to the stiffness matrix are only taken into + // account for 1 values in d_activeDirections + if (activeDirections[i]) + mat->add(offset + total_size * curIndex + i, offset + total_size * curIndex + i, vt); + } + + // rotation (if applicable) + if constexpr (sofa::type::isRigidType) + { + const auto vr = -kFact * k_a[(index < k_a.size()) * index]; + for (sofa::Size i = space_size; i < total_size; i++) + { + // Contribution to the stiffness matrix are only taken into + // account for 1 values in d_activeDirections + if (activeDirections[i]) + mat->add(offset + total_size * curIndex + i, offset + total_size * curIndex + i, vr); + } + } + } +} + +template +void FixedWeakConstraint::buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) +{ + if (this->d_componentState.getValue() != core::objectmodel::ComponentState::Valid) + return; + + const VecReal& k = d_stiffness.getValue(); + const VecReal& k_a = d_angularStiffness.getValue(); + const auto & activeDirections = getActiveDirections(); + const auto & indices = getIndices(); + + constexpr sofa::Size space_size = Deriv::spatial_dimensions; // == total_size if DataTypes = VecTypes + constexpr sofa::Size total_size = Deriv::total_size; + + auto dfdx = matrix->getForceDerivativeIn(this->mstate) + .withRespectToPositionsIn(this->mstate); + const bool fixedAll = d_fixAll.getValue(); + const unsigned maxIt = fixedAll ? this->mstate->getSize() : indices.size(); + + for (sofa::Index i = 0; i < maxIt; i++) + { + sofa::Index index = i; + + if (!fixedAll) + { + index = indices[i]; + } + // translation + const auto vt = -k[(index < k.size()) * index]; + for(sofa::Index i = 0; i < space_size; i++) + { + dfdx(total_size * index + i, total_size * index + i) += vt; + } + + // rotation (if applicable) + if constexpr (sofa::type::isRigidType) + { + const auto vr = -k_a[(index < k_a.size()) * index]; + for (sofa::Size i = space_size; i < total_size; ++i) + { + // Contribution to the stiffness matrix are only taken into + // account for 1 values in d_activeDirections + if (activeDirections[i]) + { + dfdx(total_size * index + i, total_size * index + i) += vr; + } + } + } + } +} + +template +void FixedWeakConstraint::buildDampingMatrix( + core::behavior::DampingMatrix* matrix) +{ + SOFA_UNUSED(matrix); +} + +template +void FixedWeakConstraint::draw(const VisualParams *vparams) +{ + if (this->d_componentState.getValue() != core::objectmodel::ComponentState::Valid) + return; + + if (!vparams->displayFlags().getShowForceFields() || !d_drawSpring.getValue()) + return; /// \todo put this in the parent class + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + vparams->drawTool()->setLightingEnabled(false); + + const DataVecCoord* extPosition = getExtPosition(); + if (!extPosition) + { + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + + ReadAccessor< DataVecCoord > p0 = *extPosition; + ReadAccessor< DataVecCoord > p = this->mstate->read(sofa::core::vec_id::write_access::position); + std::vector vertices; + + + const auto & indices = getIndices(); + const auto & extIndices = getExtIndices(); + const bool fixedAll = d_fixAll.getValue(); + const unsigned maxIt = fixedAll ? this->mstate->getSize() : indices.size(); + + for (sofa::Index i = 0; i < maxIt; i++) + { + sofa::Index ext_index = i; + sofa::Index index = i; + + if (!fixedAll) + { + index = indices[i]; + ext_index = extIndices[i]; + } + + Vec3 v0(0.0, 0.0, 0.0); + Vec3 v1(0.0, 0.0, 0.0); + for(sofa::Index j=0 ; j< std::min(DataTypes::spatial_dimensions, static_cast(3)) ; j++) + { + v0[j] = (DataTypes::getCPos(p[index]))[j]; + v1[j] = (DataTypes::getCPos(p0[ext_index]))[j]; + } + + vertices.push_back(v0); + vertices.push_back(v1); + } + + //todo(dmarchal) because of https://github.com/sofa-framework/sofa/issues/64 + vparams->drawTool()->drawLines(vertices,5, d_springColor.getValue()); + +} +} // namespace sofa::component::solidmechanics::spring diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.h b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.h index bba03aad7c0..a073c3f1671 100644 --- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.h +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include namespace sofa::core::behavior @@ -48,12 +50,12 @@ namespace sofa::component::solidmechanics::spring * An external MechanicalState reference can also be passed to the ForceField as rest shape position. */ template -class RestShapeSpringsForceField : public core::behavior::ForceField +class RestShapeSpringsForceField : public FixedWeakConstraint { public: - SOFA_CLASS(SOFA_TEMPLATE(RestShapeSpringsForceField, DataTypes), SOFA_TEMPLATE(core::behavior::ForceField, DataTypes)); + SOFA_CLASS(SOFA_TEMPLATE(RestShapeSpringsForceField, DataTypes), SOFA_TEMPLATE(FixedWeakConstraint, DataTypes)); - typedef core::behavior::ForceField Inherit; + typedef FixedWeakConstraint Inherit; typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::VecDeriv VecDeriv; typedef typename DataTypes::Coord Coord; @@ -70,73 +72,30 @@ class RestShapeSpringsForceField : public core::behavior::ForceField typedef core::objectmodel::Data DataVecCoord; typedef core::objectmodel::Data DataVecDeriv; - DataSubsetIndex d_points; ///< points controlled by the rest shape springs - Data< VecReal > d_stiffness; ///< stiffness values between the actual position and the rest shape position - Data< VecReal > d_angularStiffness; ///< angularStiffness assigned when controlling the rotation of the points - Data< type::vector< CPos > > d_pivotPoints; ///< global pivot points used when translations instead of the rigid mass centers - Data< VecIndex > d_external_points; ///< points from the external Mechanical State that define the rest shape springs - Data< bool > d_recompute_indices; ///< Recompute indices (should be false for BBOX) - Data< bool > d_drawSpring; ///< draw Spring - Data< sofa::type::RGBAColor > d_springColor; ///< spring color. (default=[0.0,1.0,0.0,1.0]) Data< type::fixed_array > d_activeDirections; ///< directions (translation, and rotation in case of Rigids) in which the spring is active + Data< VecIndex > d_externalIndices; ///< points from the external Mechanical State that define the rest shape springs + core::objectmodel::lifecycle::RemovedData d_external_points{this,"v24.12","v25.06","external_points","This data has been replaced by \'externalIndices\'. Please update your scene."}; + core::objectmodel::DataCallback c_fixAllCallback; SingleLink, sofa::core::behavior::MechanicalState< DataTypes >, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_restMState; - linearalgebra::EigenBaseSparseMatrix matS; -protected: - RestShapeSpringsForceField(); - - static constexpr type::fixed_array s_defaultActiveDirections = sofa::type::makeHomogeneousArray(true); - -public: /// BaseObject initialization method. void bwdInit() override ; - void parse(core::objectmodel::BaseObjectDescription *arg) override ; - void reinit() override ; - - /// Add the forces. - void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; - /// Link to be set to the topology container in the component graph. - SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - - void addDForce(const core::MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) override; - SReal getPotentialEnergy(const core::MechanicalParams* mparams, const DataVecCoord& x) const override - { - SOFA_UNUSED(mparams); - SOFA_UNUSED(x); - msg_warning() << "Method getPotentialEnergy not implemented yet."; - return 0.0; - } - - /// Brings ForceField contribution to the global system stiffness matrix. - void addKToMatrix(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix ) override; - void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; - void buildDampingMatrix(core::behavior::DampingMatrix* matrix) override; - - void draw(const core::visual::VisualParams* vparams) override; - - - const DataVecCoord* getExtPosition() const; - const VecIndex& getIndices() const { return m_indices; } - const VecIndex& getExtIndices() const { return (useRestMState ? m_ext_indices : m_indices); } protected : + RestShapeSpringsForceField(); - void recomputeIndices(); - bool checkOutOfBoundsIndices(); - bool checkOutOfBoundsIndices(const VecIndex &indices, const sofa::Size dimension); - - VecIndex m_indices; - VecIndex m_ext_indices; - type::vector m_pivots; + virtual const DataVecCoord* getExtPosition() const override; + virtual const VecIndex& getExtIndices() const override; + virtual const type::fixed_array& getActiveDirections() const override; - SReal lastUpdatedStep{}; + virtual bool checkOutOfBoundsIndices(); private : - bool useRestMState{}; /// An external MechanicalState is used as rest reference. + bool m_useRestMState; /// An external MechanicalState is used as rest reference. }; #if !defined(SOFA_COMPONENT_FORCEFIELD_RESTSHAPESPRINGSFORCEFIELD_CPP) diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.inl b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.inl index 9f6d088de0c..14e67f2cf63 100644 --- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.inl +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.inl @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -51,54 +52,36 @@ using core::visual::VisualParams; template RestShapeSpringsForceField::RestShapeSpringsForceField() - : d_points(initData(&d_points, "points", "points controlled by the rest shape springs")) - , d_stiffness(initData(&d_stiffness, "stiffness", "stiffness values between the actual position and the rest shape position")) - , d_angularStiffness(initData(&d_angularStiffness, "angularStiffness", "angularStiffness assigned when controlling the rotation of the points")) - , d_pivotPoints(initData(&d_pivotPoints, "pivot_points", "global pivot points used when translations instead of the rigid mass centers")) - , d_external_points(initData(&d_external_points, "external_points", "points from the external Mechanical State that define the rest shape springs")) - , d_recompute_indices(initData(&d_recompute_indices, true, "recompute_indices", "Recompute indices (should be false for BBOX)")) - , d_drawSpring(initData(&d_drawSpring,false,"drawSpring","draw Spring")) - , d_springColor(initData(&d_springColor, sofa::type::RGBAColor::green(), "springColor","spring color. (default=[0.0,1.0,0.0,1.0])")) - , d_activeDirections(initData(&d_activeDirections, s_defaultActiveDirections, - "activeDirections", std::string("Directions in which the spring is active (default=[" + sofa::helper::join(s_defaultActiveDirections, ',') + "])").c_str())) + : d_activeDirections(initData(&d_activeDirections, FixedWeakConstraint::s_defaultActiveDirections, + "activeDirections", std::string("Directions in which the spring is active (default=[" + sofa::helper::join(FixedWeakConstraint::s_defaultActiveDirections, ',') + "])").c_str())) + , d_externalIndices(initData(&d_externalIndices, "externalIndices","Indices from the external Mechanical State that define the rest shape springs")) , l_restMState(initLink("external_rest_shape", "rest_shape can be defined by the position of an external Mechanical State")) - , l_topology(initLink("topology", "Link to be set to the topology container in the component graph")) { - this->addUpdateCallback("updateIndices", {&d_points}, [this](const core::DataTracker& t) + c_fixAllCallback.addInput(&this->d_fixAll); + c_fixAllCallback.addCallback([this]() { - SOFA_UNUSED(t); - this->recomputeIndices(); - return sofa::core::objectmodel::ComponentState::Valid; - }, {}); -} - -template -void RestShapeSpringsForceField::parse(core::objectmodel::BaseObjectDescription *arg) -{ - const char* attr = arg->getAttribute("external_rest_shape") ; - if( attr != nullptr && attr[0] != '@') - { - msg_error() << "RestShapeSpringsForceField have changed since 17.06. The parameter 'external_rest_shape' is now a Link. To fix your scene you need to add and '@' in front of the provided path. See PR#315" ; - } - Inherit::parse(arg) ; + if (this->getMState()->getSize() != l_restMState->getSize()) + { + msg_error(this) <<"the fixAll option only works with either one mstate or when the two mstate have the same size."; + this->d_componentState.setValue(core::objectmodel::ComponentState::Invalid); + } + else + { + this->d_componentState.setValue(core::objectmodel::ComponentState::Valid); + } + }); } template void RestShapeSpringsForceField::bwdInit() { - ForceField::init(); - - if (d_stiffness.getValue().empty()) - { - msg_info() << "No stiffness is defined, assuming equal stiffness on each node, k = 100.0 "; - d_stiffness.setValue({static_cast(100)}); - } + FixedWeakConstraint::bwdInit(); if (l_restMState.get() == nullptr) { - useRestMState = false; - msg_info() << "no external rest shape used"; + m_useRestMState = false; + msg_info(this) << "no external rest shape used"; if(!l_restMState.empty()) { @@ -107,178 +90,23 @@ void RestShapeSpringsForceField::bwdInit() } else { - msg_info() << "external rest shape used"; - useRestMState = true; - } - - if (l_topology.empty()) - { - msg_info() << "link to Topology container should be set to ensure right behavior. First Topology found in current context will be used."; - l_topology.set(this->getContext()->getMeshTopologyLink()); - } - - if (sofa::core::topology::BaseMeshTopology* _topology = l_topology.get()) - { - msg_info() << "Topology path used: '" << l_topology.getLinkedPath() << "'"; - - // Initialize topological changes support - d_points.createTopologyHandler(_topology); - } - else - { - msg_info() << "Cannot find the topology: topological changes will not be supported"; - } - - recomputeIndices(); - if (this->d_componentState.getValue() == sofa::core::objectmodel::ComponentState::Invalid) - return; - - const BaseMechanicalState* state = this->getContext()->getMechanicalState(); - if(!state) - { - msg_warning() << "MechanicalState of the current context returns null pointer"; - } - else - { - assert(state); - matS.resize(state->getMatrixSize(),state->getMatrixSize()); - } - - /// Compile time condition to check if we are working with a Rigid3Types or a type that does not - /// need the Angular Stiffness parameters. - //if constexpr (isRigid()) - if constexpr (sofa::type::isRigidType) - { - sofa::helper::ReadAccessor> s = d_stiffness; - sofa::helper::WriteOnlyAccessor> as = d_angularStiffness; - - if (as.size() < s.size()) - { - msg_info() << "'stiffness' is larger than 'angularStiffness', add the default value (100.0) to the missing entries."; - - for(size_t i = as.size();i s.size()) - { - msg_info() << "'stiffness' is smaller than 'angularStiffness', clamp the extra values in angularStiffness."; - as.resize(s.size()); - } - } - - lastUpdatedStep = -1.0; - - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); -} - - -template -void RestShapeSpringsForceField::reinit() -{ - if (!checkOutOfBoundsIndices()) - { - m_indices.clear(); - } - else - { - msg_info() << "Indices successfully checked"; - } - - if (d_stiffness.getValue().empty()) - { - msg_info() << "No stiffness is defined, assuming equal stiffness on each node, k = 100.0 " ; - - VecReal stiffs; - stiffs.push_back(100.0); - d_stiffness.setValue(stiffs); - } - else - { - const VecReal &k = d_stiffness.getValue(); - if ( k.size() != m_indices.size() ) - { - msg_warning() << "Size of stiffness vector is not correct (" << k.size() << "), should be either 1 or " << m_indices.size() << msgendl - << "First value of stiffness will be used"; - } + msg_info(this) << "external rest shape used"; + m_useRestMState = true; } - } -template -void RestShapeSpringsForceField::recomputeIndices() -{ - m_indices.clear(); - m_ext_indices.clear(); - - for (const sofa::Index i : d_points.getValue()) - { - m_indices.push_back(i); - } - - for (const sofa::Index i : d_external_points.getValue()) - { - m_ext_indices.push_back(i); - } - - if (m_indices.empty()) - { - // no point are defined, default case: points = all points - msg_info() << "No point are defined. Change to default case: points = all points"; - for (sofa::Index i = 0; i < this->mstate->getSize(); i++) - { - m_indices.push_back(i); - } - } - - if (m_ext_indices.empty()) - { - if (useRestMState) - { - if (const DataVecCoord* extPosition = getExtPosition()) - { - const auto& extPositionValue = extPosition->getValue(); - for (sofa::Index i = 0; i < extPositionValue.size(); i++) - { - m_ext_indices.push_back(i); - } - } - else - { - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - } - } - else - { - for (const sofa::Index i : m_indices) - { - m_ext_indices.push_back(i); - } - } - } - - if (!checkOutOfBoundsIndices()) - { - msg_error() << "The dimension of the source and the targeted points are different "; - m_indices.clear(); - } - else - { - msg_info() << "Indices successfully checked"; - } -} template bool RestShapeSpringsForceField::checkOutOfBoundsIndices() { - if (!checkOutOfBoundsIndices(m_indices, this->mstate->getSize())) + if (!FixedWeakConstraint::checkOutOfBoundsIndices(this->getIndices(), this->mstate->getSize())) { msg_error() << "Out of Bounds d_indices detected. ForceField is not activated."; return false; } if (const DataVecCoord* extPosition = getExtPosition()) { - if (!checkOutOfBoundsIndices(m_ext_indices, sofa::Size(extPosition->getValue().size()))) + if (!FixedWeakConstraint::checkOutOfBoundsIndices(getExtIndices(), sofa::Size(extPosition->getValue().size()))) { msg_error() << "Out of Bounds m_ext_indices detected. ForceField is not activated."; return false; @@ -288,7 +116,7 @@ bool RestShapeSpringsForceField::checkOutOfBoundsIndices() { this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); } - if (m_indices.size() != m_ext_indices.size()) + if (this->getIndices().size() != getExtIndices().size()) { msg_error() << "Dimensions of the source and the targeted points are different. ForceField is not activated."; return false; @@ -296,28 +124,19 @@ bool RestShapeSpringsForceField::checkOutOfBoundsIndices() return true; } -template -bool RestShapeSpringsForceField::checkOutOfBoundsIndices(const VecIndex &indices, const sofa::Size dimension) -{ - for (sofa::Index i = 0; i < indices.size(); i++) - { - if (indices[i] >= dimension) - { - return false; - } - } - return true; -} - template const typename RestShapeSpringsForceField::DataVecCoord* RestShapeSpringsForceField::getExtPosition() const { - if(useRestMState) + if(m_useRestMState) { if (l_restMState) { return l_restMState->read(core::vec_id::write_access::position); } + else + { + msg_error(this)<<"The external rest shape is not set correctly."; + } } else { @@ -329,308 +148,24 @@ const typename RestShapeSpringsForceField::DataVecCoord* RestShapeSpr return nullptr; } -template -void RestShapeSpringsForceField::addForce(const MechanicalParams* mparams , DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v ) -{ - SOFA_UNUSED(mparams); - SOFA_UNUSED(v); - - WriteAccessor< DataVecDeriv > f1 = f; - ReadAccessor< DataVecCoord > p1 = x; - - const DataVecCoord* extPosition = getExtPosition(); - if (!extPosition) - { - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - - ReadAccessor< DataVecCoord > p0 = *extPosition; - - const VecReal& k = d_stiffness.getValue(); - const VecReal& k_a = d_angularStiffness.getValue(); - - f1.resize(p1.size()); - - if (d_recompute_indices.getValue()) - { - recomputeIndices(); - } - - for (sofa::Index i = 0; i < m_indices.size(); i++) - { - const sofa::Index index = m_indices[i]; - sofa::Index ext_index = m_indices[i]; - if (useRestMState) - ext_index = m_ext_indices[i]; - - const auto stiffness = k[static_cast(i < k.size()) * i]; - - const auto activeDirections = d_activeDirections.getValue(); - - // rigid case - if constexpr (sofa::type::isRigidType) - { - // translation - if (i >= m_pivots.size()) - { - CPos dx = p1[index].getCenter() - p0[ext_index].getCenter(); - // We filter the difference dx by setting to 0 the entries corresponding - // to 0 values in d_activeDirections - for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) - { - if (!activeDirections[entryId]) - dx[entryId] = 0; - } - getVCenter(f1[index]) -= dx * stiffness; - } - else - { - CPos localPivot = p0[ext_index].getOrientation().inverseRotate(m_pivots[i] - p0[ext_index].getCenter()); - CPos rotatedPivot = p1[index].getOrientation().rotate(localPivot); - CPos pivot2 = p1[index].getCenter() + rotatedPivot; - CPos dx = pivot2 - m_pivots[i]; - getVCenter(f1[index]) -= dx * stiffness; - } - - // rotation - type::Quat dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse(); - dq.normalize(); - - type::Vec<3, Real> dir{type::NOINIT}; - Real angle = 0.; - - if (dq[3] < 0.) - { - dq = dq * -1.0; - } - - if (dq[3] < 1.0) - dq.quatToAxis(dir, angle); - - // We change the direction of the axis of rotation based on - // the 0 values in d_activeDirections. This is equivalent - // to senting to 0 the rotation axis components along x, y - // and/or z, depending on the rotations we want to take into - // account. - for (sofa::Size entryId = spatial_dimensions; entryId < coord_total_size; ++entryId) - { - if (!activeDirections[entryId]) - dir[entryId-spatial_dimensions] = 0; - } - - const auto angularStiffness = k_a[static_cast(i < k_a.size()) * i]; - getVOrientation(f1[index]) -= dir * angle * angularStiffness; - } - else // non-rigid implementation - { - Deriv dx = p1[index] - p0[ext_index]; - // We filter the difference dx by setting to 0 the entries corresponding - // to 0 values in d_activeDirections - for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) - { - if (!activeDirections[entryId]) - dx[entryId] = 0; - } - f1[index] -= dx * stiffness; - } - } -} template -void RestShapeSpringsForceField::addDForce(const MechanicalParams* mparams, DataVecDeriv& df, const DataVecDeriv& dx) +const typename RestShapeSpringsForceField::VecIndex& RestShapeSpringsForceField::getExtIndices() const { - WriteAccessor< DataVecDeriv > df1 = df; - ReadAccessor< DataVecDeriv > dx1 = dx; - Real kFactor = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); - const VecReal& k = d_stiffness.getValue(); - const VecReal& k_a = d_angularStiffness.getValue(); - const auto activeDirections = d_activeDirections.getValue(); - - for (unsigned int i = 0; i < m_indices.size(); i++) + if (m_useRestMState ) { - const sofa::Index curIndex = m_indices[i]; - const auto stiffness = k[static_cast(i < k.size()) * i]; - - if constexpr (sofa::type::isRigidType) - { - const auto angularStiffness = k_a[static_cast(i < k_a.size()) * i]; - - // We filter the difference in translation by setting to 0 the entries corresponding - // to 0 values in d_activeDirections - auto currentSpringDx = getVCenter(dx1[curIndex]); - for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) - { - if (!activeDirections[entryId]) - currentSpringDx[entryId] = 0; - } - getVCenter(df1[curIndex]) -= currentSpringDx * stiffness * kFactor; - - auto currentSpringRotationalDx = getVOrientation(dx1[curIndex]); - // We change the direction of the axis of rotation based on - // the 0 values in d_activeDirections. This is equivalent - // to senting to 0 the rotation axis components along x, y - // and/or z, depending on the rotations we want to take into - // account. - for (sofa::Size entryId = spatial_dimensions; entryId < coord_total_size; ++entryId) - { - if (!activeDirections[entryId]) - currentSpringRotationalDx[entryId-spatial_dimensions] = 0; - } - getVOrientation(df1[curIndex]) -= currentSpringRotationalDx * angularStiffness * kFactor; - } - else - { - // We filter the difference in translation by setting to 0 the entries corresponding - // to 0 values in d_activeDirections - auto currentSpringDx = dx1[m_indices[i]]; - for (sofa::Size entryId = 0; entryId < spatial_dimensions; ++entryId) - { - if (!activeDirections[entryId]) - currentSpringDx[entryId] = 0; - } - df1[m_indices[i]] -= currentSpringDx * stiffness * kFactor; - } + return d_externalIndices.getValue(); } - - -} - -template -void RestShapeSpringsForceField::draw(const VisualParams *vparams) -{ - if (!vparams->displayFlags().getShowForceFields() || !d_drawSpring.getValue()) - return; /// \todo put this in the parent class - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - vparams->drawTool()->setLightingEnabled(false); - - const DataVecCoord* extPosition = getExtPosition(); - if (!extPosition) - { - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - - ReadAccessor< DataVecCoord > p0 = *extPosition; - ReadAccessor< DataVecCoord > p = this->mstate->read(sofa::core::vec_id::write_access::position); - - const VecIndex& indices = m_indices; - const VecIndex& ext_indices = (useRestMState ? m_ext_indices : m_indices); - - std::vector vertices; - - for (sofa::Index i=0; i(3)) ; j++) - { - v0[j] = (DataTypes::getCPos(p[index]))[j]; - v1[j] = (DataTypes::getCPos(p0[ext_index]))[j]; - } - - vertices.push_back(v0); - vertices.push_back(v1); - } - - //todo(dmarchal) because of https://github.com/sofa-framework/sofa/issues/64 - vparams->drawTool()->drawLines(vertices,5, d_springColor.getValue()); - -} - -template -void RestShapeSpringsForceField::addKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix ) -{ - const MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); - BaseMatrix* mat = mref.matrix; - const unsigned int offset = mref.offset; - Real kFact = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()); - - const VecReal& k = d_stiffness.getValue(); - const VecReal& k_a = d_angularStiffness.getValue(); - const auto activeDirections = d_activeDirections.getValue(); - - constexpr sofa::Size space_size = Deriv::spatial_dimensions; // == total_size if DataTypes = VecTypes - constexpr sofa::Size total_size = Deriv::total_size; - - sofa::Index curIndex = 0; - - for (sofa::Index index = 0; index < m_indices.size(); index++) + else { - curIndex = m_indices[index]; - - // translation - const auto vt = -kFact * k[(index < k.size()) * index]; - for (sofa::Size i = 0; i < space_size; i++) - { - // Contribution to the stiffness matrix are only taken into - // account for 1 values in d_activeDirections - if (activeDirections[i]) - mat->add(offset + total_size * curIndex + i, offset + total_size * curIndex + i, vt); - } - - // rotation (if applicable) - if constexpr (sofa::type::isRigidType) - { - const auto vr = -kFact * k_a[(index < k_a.size()) * index]; - for (sofa::Size i = space_size; i < total_size; i++) - { - // Contribution to the stiffness matrix are only taken into - // account for 1 values in d_activeDirections - if (activeDirections[i]) - mat->add(offset + total_size * curIndex + i, offset + total_size * curIndex + i, vr); - } - } + return this->d_indices.getValue(); } } template -void RestShapeSpringsForceField::buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) +const type::fixed_array& RestShapeSpringsForceField::getActiveDirections() const { - const VecReal& k = d_stiffness.getValue(); - const VecReal& k_a = d_angularStiffness.getValue(); - const auto activeDirections = d_activeDirections.getValue(); - - constexpr sofa::Size space_size = Deriv::spatial_dimensions; // == total_size if DataTypes = VecTypes - constexpr sofa::Size total_size = Deriv::total_size; - - auto dfdx = matrix->getForceDerivativeIn(this->mstate) - .withRespectToPositionsIn(this->mstate); - - for (const auto index : m_indices) - { - // translation - const auto vt = -k[(index < k.size()) * index]; - for(sofa::Index i = 0; i < space_size; i++) - { - dfdx(total_size * index + i, total_size * index + i) += vt; - } - - // rotation (if applicable) - if constexpr (sofa::type::isRigidType) - { - const auto vr = -k_a[(index < k_a.size()) * index]; - for (sofa::Size i = space_size; i < total_size; ++i) - { - // Contribution to the stiffness matrix are only taken into - // account for 1 values in d_activeDirections - if (activeDirections[i]) - { - dfdx(total_size * index + i, total_size * index + i) += vr; - } - } - } - } + return d_activeDirections.getValue(); } -template -void RestShapeSpringsForceField::buildDampingMatrix( - core::behavior::DampingMatrix* matrix) -{ - SOFA_UNUSED(matrix); -} } // namespace sofa::component::solidmechanics::spring diff --git a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/init.cpp b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/init.cpp index 329e648e6b7..3d415081f78 100644 --- a/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/init.cpp +++ b/Sofa/Component/SolidMechanics/Spring/src/sofa/component/solidmechanics/spring/init.cpp @@ -28,6 +28,7 @@ namespace sofa::component::solidmechanics::spring extern void registerAngularSpringForceField(sofa::core::ObjectFactory* factory); extern void registerFastTriangularBendingSprings(sofa::core::ObjectFactory* factory); +extern void registerFixedWeakConstraint(sofa::core::ObjectFactory* factory); extern void registerFrameSpringForceField(sofa::core::ObjectFactory* factory); extern void registerGearSpringForceField(sofa::core::ObjectFactory* factory); extern void registerNonUniformHexahedronFEMForceFieldAndMass(sofa::core::ObjectFactory* factory); @@ -72,6 +73,7 @@ void registerObjects(sofa::core::ObjectFactory* factory) { registerAngularSpringForceField(factory); registerFastTriangularBendingSprings(factory); + registerFixedWeakConstraint(factory); registerFrameSpringForceField(factory); registerGearSpringForceField(factory); registerNonUniformHexahedronFEMForceFieldAndMass(factory); diff --git a/Sofa/Component/SolidMechanics/Spring/tests/CMakeLists.txt b/Sofa/Component/SolidMechanics/Spring/tests/CMakeLists.txt index 80a5f3143f8..efb46546b5b 100644 --- a/Sofa/Component/SolidMechanics/Spring/tests/CMakeLists.txt +++ b/Sofa/Component/SolidMechanics/Spring/tests/CMakeLists.txt @@ -7,6 +7,7 @@ set(SOURCE_FILES RestShapeSpringsForceField_test.cpp SpringForceField_test.cpp TriangularBendingSprings_test.cpp + FixedWeakConstraint_test.cpp ) add_executable(${PROJECT_NAME} ${SOURCE_FILES}) diff --git a/Sofa/Component/SolidMechanics/Spring/tests/FixedWeakConstraint_test.cpp b/Sofa/Component/SolidMechanics/Spring/tests/FixedWeakConstraint_test.cpp new file mode 100644 index 00000000000..6362a67d214 --- /dev/null +++ b/Sofa/Component/SolidMechanics/Spring/tests/FixedWeakConstraint_test.cpp @@ -0,0 +1,131 @@ +/****************************************************************************** +* 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::BaseTest; + +#include +using namespace sofa::simpleapi; + +#include +#include + +#include +using sofa::component::statecontainer::MechanicalObject; + +using sofa::defaulttype::Vec3Types; +using sofa::defaulttype::Rigid3dTypes; +using sofa::helper::ReadAccessor; +using sofa::Data; + +/// Test suite for RestShapeSpringsForceField +class FixedWeakConstraint_test : public BaseTest +{ +public: + ~FixedWeakConstraint_test() override; + sofa::simulation::Node::SPtr createScene(const std::string& type); + + template + void testDefaultBehavior(sofa::simulation::Node::SPtr root); + + template + void checkDifference(MechanicalObject& mo); +}; + +FixedWeakConstraint_test::~FixedWeakConstraint_test() +{ +} + +sofa::simulation::Node::SPtr FixedWeakConstraint_test::createScene(const std::string& type) +{ + const auto theSimulation = createSimulation(); + auto theRoot = createRootNode(theSimulation, "root"); + sofa::simpleapi::importPlugin("Sofa.Component.ODESolver.Backward"); + sofa::simpleapi::importPlugin("Sofa.Component.LinearSolver.Iterative"); + sofa::simpleapi::importPlugin("Sofa.Component.StateContainer"); + sofa::simpleapi::importPlugin("Sofa.Component.Mass"); + + createObject(theRoot, "DefaultAnimationLoop"); + createObject(theRoot, "EulerImplicitSolver"); + createObject(theRoot, "CGLinearSolver", {{ "iterations", "25" }, { "tolerance", "1e-5" }, {"threshold", "1e-5"}}); + + /// Create an object with a mass and use a rest shape spring ff so it stays + /// at the initial position + const auto fixedObject = createChild(theRoot, "fixedObject"); + auto fixedObject_dofs = createObject(fixedObject, "MechanicalObject", {{"name","dofs"}, + {"size","10"}, + {"template",type}}); + createObject(fixedObject, "UniformMass", {{"totalMass", "1"}}); + + createObject(fixedObject, "FixedWeakConstraint", {{"stiffness","1000"},{"fixAll","true"}}); + + const auto movingObject = createChild(theRoot, "movingObject"); + auto movingObject_dofs = createObject(movingObject, "MechanicalObject", {{"name","dofs"}, + {"size","10"}, + {"template",type}}); + createObject(movingObject, "UniformMass", {{"totalMass", "1"}}); + + sofa::simulation::node::initRoot(theRoot.get()); + for(unsigned int i=0;i<20;i++) + { + sofa::simulation::node::animate(theRoot.get(), 0.01_sreal); + } + return theRoot; +} + +template +void FixedWeakConstraint_test::checkDifference(MechanicalObject& mo) +{ + ReadAccessor< Data > positions = mo.x; + ReadAccessor< Data > rest_positions = mo.x0; + for(size_t i=0;i +void FixedWeakConstraint_test::testDefaultBehavior(sofa::simulation::Node::SPtr root) +{ + auto fixedDofs = dynamic_cast*>(root->getChild("fixedObject")->getObject("dofs")); + ASSERT_TRUE( fixedDofs != nullptr ); + + auto movingDofs = dynamic_cast*>(root->getChild("movingObject")->getObject("dofs")); + ASSERT_TRUE( movingDofs != nullptr ); + + checkDifference(*fixedDofs); +} + + +TEST_F(FixedWeakConstraint_test, defaultBehaviorVec3) +{ + this->testDefaultBehavior(this->createScene("Vec3")); +} + +TEST_F(FixedWeakConstraint_test, defaultBehaviorRigid3) +{ + this->testDefaultBehavior(this->createScene("Rigid3")); +} diff --git a/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/robot.py b/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/robot.py index a8e7179a80f..8b72a19be18 100644 --- a/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/robot.py +++ b/applications/plugins/ArticulatedSystemPlugin/examples/ArticulatedArm/robot.py @@ -88,7 +88,7 @@ def addRobot(self, name='Robot', translation=[0,0,0]): articulations.addObject('MechanicalObject', name='dofs', template='Vec1', rest_position=robot.getData('angles').getLinkPath(), position=initAngles) articulations.addObject('ArticulatedHierarchyContainer') articulations.addObject('UniformMass', totalMass=1) - articulations.addObject('RestShapeSpringsForceField', stiffness=1e10, points=list(range(6))) + articulations.addObject('RestShapeSpringsForceField', stiffness=1e10, indices=list(range(6))) # Rigid rigid = articulations.addChild('Rigid') diff --git a/applications/plugins/Geomagic/scenes/Geomagic-DeformableCubes.scn b/applications/plugins/Geomagic/scenes/Geomagic-DeformableCubes.scn index 8447dc4d862..3295ff9ddcd 100644 --- a/applications/plugins/Geomagic/scenes/Geomagic-DeformableCubes.scn +++ b/applications/plugins/Geomagic/scenes/Geomagic-DeformableCubes.scn @@ -126,7 +126,7 @@ - + diff --git a/applications/plugins/Geomagic/scenes/Geomagic-DeformableSphere.scn b/applications/plugins/Geomagic/scenes/Geomagic-DeformableSphere.scn index d3f88c411bb..a969ae31d7c 100644 --- a/applications/plugins/Geomagic/scenes/Geomagic-DeformableSphere.scn +++ b/applications/plugins/Geomagic/scenes/Geomagic-DeformableSphere.scn @@ -105,7 +105,7 @@ - + diff --git a/applications/plugins/Geomagic/scenes/Geomagic-RigidCubes.scn b/applications/plugins/Geomagic/scenes/Geomagic-RigidCubes.scn index b49b7dd575d..c667058822b 100644 --- a/applications/plugins/Geomagic/scenes/Geomagic-RigidCubes.scn +++ b/applications/plugins/Geomagic/scenes/Geomagic-RigidCubes.scn @@ -88,7 +88,7 @@ - + diff --git a/applications/plugins/Geomagic/scenes/Geomagic-RigidSkull-restShapeSpringForceField.scn b/applications/plugins/Geomagic/scenes/Geomagic-RigidSkull-restShapeSpringForceField.scn index 37ccac55ceb..7c956d6f764 100644 --- a/applications/plugins/Geomagic/scenes/Geomagic-RigidSkull-restShapeSpringForceField.scn +++ b/applications/plugins/Geomagic/scenes/Geomagic-RigidSkull-restShapeSpringForceField.scn @@ -59,7 +59,7 @@ - + diff --git a/applications/plugins/Geomagic/scenes/Geomagic-RigidSphere.scn b/applications/plugins/Geomagic/scenes/Geomagic-RigidSphere.scn index 880396565d4..fb730dd5bbc 100644 --- a/applications/plugins/Geomagic/scenes/Geomagic-RigidSphere.scn +++ b/applications/plugins/Geomagic/scenes/Geomagic-RigidSphere.scn @@ -76,7 +76,7 @@ - + diff --git a/applications/plugins/Geomagic/scenes/GeomagicEmulator-RigidCubes.scn b/applications/plugins/Geomagic/scenes/GeomagicEmulator-RigidCubes.scn index f950b7b60e1..f4d7be61acc 100644 --- a/applications/plugins/Geomagic/scenes/GeomagicEmulator-RigidCubes.scn +++ b/applications/plugins/Geomagic/scenes/GeomagicEmulator-RigidCubes.scn @@ -88,7 +88,7 @@ - + diff --git a/applications/plugins/PersistentContact/examples/instrument.xml b/applications/plugins/PersistentContact/examples/instrument.xml index fd67eb58411..551bf782cde 100644 --- a/applications/plugins/PersistentContact/examples/instrument.xml +++ b/applications/plugins/PersistentContact/examples/instrument.xml @@ -5,12 +5,12 @@ - + - + diff --git a/applications/plugins/PersistentContact/examples/instrument2.xml b/applications/plugins/PersistentContact/examples/instrument2.xml index f338f657865..f6a6e04273c 100644 --- a/applications/plugins/PersistentContact/examples/instrument2.xml +++ b/applications/plugins/PersistentContact/examples/instrument2.xml @@ -5,12 +5,12 @@ - + - + diff --git a/applications/plugins/Sensable/examples/Old Examples/omni1.scn b/applications/plugins/Sensable/examples/Old Examples/omni1.scn index b3978a75c3e..b635978b532 100644 --- a/applications/plugins/Sensable/examples/Old Examples/omni1.scn +++ b/applications/plugins/Sensable/examples/Old Examples/omni1.scn @@ -20,7 +20,7 @@ - + diff --git a/applications/plugins/Sensable/examples/Old Examples/omni2.scn b/applications/plugins/Sensable/examples/Old Examples/omni2.scn index 9d38b20cd1e..4e19ad2b2a7 100644 --- a/applications/plugins/Sensable/examples/Old Examples/omni2.scn +++ b/applications/plugins/Sensable/examples/Old Examples/omni2.scn @@ -15,7 +15,7 @@ - + diff --git a/applications/plugins/Xitact/examples/1xitact.scn b/applications/plugins/Xitact/examples/1xitact.scn index 66de05934b7..338be5d68d8 100644 --- a/applications/plugins/Xitact/examples/1xitact.scn +++ b/applications/plugins/Xitact/examples/1xitact.scn @@ -14,7 +14,7 @@ - + diff --git a/applications/plugins/Xitact/examples/2xitact2.scn b/applications/plugins/Xitact/examples/2xitact2.scn index e717de5cb74..4f62edbbb67 100644 --- a/applications/plugins/Xitact/examples/2xitact2.scn +++ b/applications/plugins/Xitact/examples/2xitact2.scn @@ -15,7 +15,7 @@ - + diff --git a/applications/plugins/Xitact/examples/XitactGrasping.scn b/applications/plugins/Xitact/examples/XitactGrasping.scn index a2b37a124ca..228bf45a0c6 100644 --- a/applications/plugins/Xitact/examples/XitactGrasping.scn +++ b/applications/plugins/Xitact/examples/XitactGrasping.scn @@ -85,7 +85,7 @@ - + @@ -104,7 +104,7 @@ - + diff --git a/applications/plugins/Xitact/examples/XitactGraspingWithForceFeedBack.scn b/applications/plugins/Xitact/examples/XitactGraspingWithForceFeedBack.scn index 3725346d2fe..7a15d6ce5ff 100644 --- a/applications/plugins/Xitact/examples/XitactGraspingWithForceFeedBack.scn +++ b/applications/plugins/Xitact/examples/XitactGraspingWithForceFeedBack.scn @@ -85,7 +85,7 @@ - + @@ -104,7 +104,7 @@ - + diff --git a/applications/plugins/Xitact/examples/XitactGrasping_coupledModel.scn b/applications/plugins/Xitact/examples/XitactGrasping_coupledModel.scn index 6431ef2d7d9..a08c6f561d8 100644 --- a/applications/plugins/Xitact/examples/XitactGrasping_coupledModel.scn +++ b/applications/plugins/Xitact/examples/XitactGrasping_coupledModel.scn @@ -85,7 +85,7 @@ - + @@ -104,7 +104,7 @@ - + @@ -202,7 +202,7 @@ - + @@ -221,7 +221,7 @@ - + diff --git a/applications/plugins/Xitact/examples/XitactGrasping_coupledModel_withITP.scn b/applications/plugins/Xitact/examples/XitactGrasping_coupledModel_withITP.scn index c9c2a905e1f..bf57c1fb18c 100644 --- a/applications/plugins/Xitact/examples/XitactGrasping_coupledModel_withITP.scn +++ b/applications/plugins/Xitact/examples/XitactGrasping_coupledModel_withITP.scn @@ -85,7 +85,7 @@ - + @@ -104,7 +104,7 @@ - + @@ -202,7 +202,7 @@ - + @@ -221,7 +221,7 @@ - + @@ -319,7 +319,7 @@ - + @@ -338,7 +338,7 @@ - + diff --git a/applications/plugins/Xitact/examples/xitactTest.scn b/applications/plugins/Xitact/examples/xitactTest.scn index 8b9ce1ecf9a..88bad4d4613 100644 --- a/applications/plugins/Xitact/examples/xitactTest.scn +++ b/applications/plugins/Xitact/examples/xitactTest.scn @@ -17,7 +17,7 @@ - + @@ -34,7 +34,7 @@ - + diff --git a/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn b/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn index 8e4fe5d1f71..2b1705c4c29 100644 --- a/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn +++ b/examples/Component/Constraint/Lagrangian/FixedLagrangianConstaint_Vec3.scn @@ -46,7 +46,7 @@ - + diff --git a/examples/Component/SolidMechanics/FEM/RestShapeSpringsForceField3.scn b/examples/Component/SolidMechanics/FEM/RestShapeSpringsForceField3.scn index e2d8f2ac81f..bce21c1d1ff 100644 --- a/examples/Component/SolidMechanics/FEM/RestShapeSpringsForceField3.scn +++ b/examples/Component/SolidMechanics/FEM/RestShapeSpringsForceField3.scn @@ -21,7 +21,7 @@ diff --git a/examples/Component/SolidMechanics/Spring/FixedWeakConstaint_Rigid3.scn b/examples/Component/SolidMechanics/Spring/FixedWeakConstaint_Rigid3.scn new file mode 100644 index 00000000000..af8009855fd --- /dev/null +++ b/examples/Component/SolidMechanics/Spring/FixedWeakConstaint_Rigid3.scn @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Component/SolidMechanics/Spring/FixedWeakConstaint_Vec3.scn b/examples/Component/SolidMechanics/Spring/FixedWeakConstaint_Vec3.scn new file mode 100644 index 00000000000..6787469b811 --- /dev/null +++ b/examples/Component/SolidMechanics/Spring/FixedWeakConstaint_Vec3.scn @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Component/SolidMechanics/Spring/RestShapeSpringsForceField2.scn b/examples/Component/SolidMechanics/Spring/RestShapeSpringsForceField2.scn index 1bc4f821d8c..07d1c58f388 100644 --- a/examples/Component/SolidMechanics/Spring/RestShapeSpringsForceField2.scn +++ b/examples/Component/SolidMechanics/Spring/RestShapeSpringsForceField2.scn @@ -26,7 +26,7 @@ - + diff --git a/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn b/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn index 6befcf8641f..8567ed4afb9 100644 --- a/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn +++ b/examples/Component/SolidMechanics/Spring/angularSpringForceField.scn @@ -49,7 +49,7 @@ - +