From c3b7f33f56691965eb9bf0c095ce539b38a7f222 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 10 Nov 2025 00:49:58 +0100 Subject: [PATCH 1/6] backup work on center motion --- CMakeLists.txt | 3 + src/InfinyToolkit/CenterLineForceField.cpp | 43 ++++ src/InfinyToolkit/CenterLineForceField.h | 122 ++++++++++++ src/InfinyToolkit/CenterLineForceField.inl | 216 +++++++++++++++++++++ src/InfinyToolkit/initInfinyToolkit.cpp | 2 + 5 files changed, 386 insertions(+) create mode 100644 src/InfinyToolkit/CenterLineForceField.cpp create mode 100644 src/InfinyToolkit/CenterLineForceField.h create mode 100644 src/InfinyToolkit/CenterLineForceField.inl diff --git a/CMakeLists.txt b/CMakeLists.txt index c81c87a..3d3e15c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,8 @@ set(HEADER_FILES ${INFINYTOOLKIT_SRC_DIR}/NeedleTracker.h ${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.h ${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.inl + ${INFINYTOOLKIT_SRC_DIR}/CenterLineForceField.h + ${INFINYTOOLKIT_SRC_DIR}/CenterLineForceField.inl ${INFINYTOOLKIT_SRC_DIR}/Triangle2RefinedTriangleTopologicalMapping.h ## Carving tools sections @@ -67,6 +69,7 @@ set(SOURCE_FILES ${INFINYTOOLKIT_SRC_DIR}/HapticEmulator.cpp ${INFINYTOOLKIT_SRC_DIR}/NeedleTracker.cpp ${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.cpp + ${INFINYTOOLKIT_SRC_DIR}/CenterLineForceField.cpp ${INFINYTOOLKIT_SRC_DIR}/Triangle2RefinedTriangleTopologicalMapping.cpp ## Carving tools sections diff --git a/src/InfinyToolkit/CenterLineForceField.cpp b/src/InfinyToolkit/CenterLineForceField.cpp new file mode 100644 index 0000000..5927011 --- /dev/null +++ b/src/InfinyToolkit/CenterLineForceField.cpp @@ -0,0 +1,43 @@ +/***************************************************************************** + * - Copyright (C) - 2020 - InfinyTech3D - * + * * + * This file is part of the InfinyToolkit plugin for the SOFA framework * + * * + * Commercial License Usage: * + * Licensees holding valid commercial license from InfinyTech3D may use this * + * file in accordance with the commercial license agreement provided with * + * the Software or, alternatively, in accordance with the terms contained in * + * a written agreement between you and InfinyTech3D. For further information * + * on the licensing terms and conditions, contact: contact@infinytech3d.com * + * * + * GNU General Public License Usage: * + * Alternatively, this file may be used under the terms of the GNU General * + * Public License version 3. The licenses are as published by the Free * + * Software Foundation and appearing in the file LICENSE.GPL3 included in * + * the packaging of this file. Please review the following information to * + * ensure the GNU General Public License requirements will be met: * + * https://www.gnu.org/licenses/gpl-3.0.html. * + * * + * Authors: see Authors.txt * + * Further information: https://infinytech3d.com * + ****************************************************************************/ + +#define SOFA_COMPONENT_FORCEFIELD_CenterLineForceField_CPP + +#include +#include + +namespace sofa::infinytoolkit +{ + +using namespace sofa::defaulttype; + +void registerCenterLineForceField(sofa::core::ObjectFactory* factory) +{ + factory->registerObjects(sofa::core::ObjectRegistrationData("Middle interpolated force applied to given degrees of freedom.") + .add< CenterLineForceField >()); +} + +template class SOFA_INFINYTOOLKIT_API CenterLineForceField; + +} // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/CenterLineForceField.h b/src/InfinyToolkit/CenterLineForceField.h new file mode 100644 index 0000000..67f14b3 --- /dev/null +++ b/src/InfinyToolkit/CenterLineForceField.h @@ -0,0 +1,122 @@ +/***************************************************************************** + * - Copyright (C) - 2020 - InfinyTech3D - * + * * + * This file is part of the InfinyToolkit plugin for the SOFA framework * + * * + * Commercial License Usage: * + * Licensees holding valid commercial license from InfinyTech3D may use this * + * file in accordance with the commercial license agreement provided with * + * the Software or, alternatively, in accordance with the terms contained in * + * a written agreement between you and InfinyTech3D. For further information * + * on the licensing terms and conditions, contact: contact@infinytech3d.com * + * * + * GNU General Public License Usage: * + * Alternatively, this file may be used under the terms of the GNU General * + * Public License version 3. The licenses are as published by the Free * + * Software Foundation and appearing in the file LICENSE.GPL3 included in * + * the packaging of this file. Please review the following information to * + * ensure the GNU General Public License requirements will be met: * + * https://www.gnu.org/licenses/gpl-3.0.html. * + * * + * Authors: see Authors.txt * + * Further information: https://infinytech3d.com * + ****************************************************************************/ +#pragma once + +#include +#include + +#include + +namespace sofa::infinytoolkit +{ + using sofa::core::DataEngine; + +/** Apply forces changing to given degres of freedom. Some keyTimes are given +* and the force to be applied is linearly interpolated between keyTimes. */ +template +class CenterLineForceField : public DataEngine +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(CenterLineForceField, DataTypes), core::DataEngine); + + using VecCoord = typename DataTypes::VecCoord; + using VecDeriv = typename DataTypes::VecDeriv; + using Coord = typename DataTypes::Coord; + using Deriv = typename DataTypes::Deriv; + using Real = typename Coord::value_type; + using DataVecCoord = core::objectmodel::Data; + using DataVecDeriv = core::objectmodel::Data; + + CenterLineForceField(); + + void init() override; + + void doUpdate() override; + + + // ForceField methods +// void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + + void draw(const core::visual::VisualParams* vparams) override; + +protected: + /// Will compute the barycenter of the given set of coordinates. + void computeBarycenter(); + + void computeDistribution(); + +public: + /// List of coordinates points + Data d_positions; + Data d_restPositions; + Data d_outputPositions; + + /// List of coordinates points + Data d_centers; + + /// Applied force to all points to simulate maximum compression. + Data d_force; + + /// If true, will apply the same force at each vertex otherwise will apply force proportional to the distance to the barycenter + Data d_uniformForce; + + /// Time to perform a full Pace (deflate + inflate). Same scale as the simulation time. + Data d_pace; + + /// To recompute barycenter every X pace. 0 by default == no refresh + Data d_refreshBaryRate; + + Data d_stiffness; + + + /// Parameter to display the force direction + Data p_showForce; + + // Synchronize with real time (instead of simulation time) + Data d_syncRealTime; + + // Frequency at which a full deflate+inflate is done + Data d_frequency; + +private : + /// Computed barycenter of the given positions @sa d_positions + Coord m_bary; + + /// counter to the last pace the barycenter has been refreshed. To be used with @sa d_refreshBaryRate + unsigned int m_lastBaryRefresh = 0; + + // keep trace of the latest time we measured + std::chrono::time_point m_startTime; + + std::vector m_distribution; + +}; // definition of the CenterLineForceField class + + + +#if !defined(SOFA_COMPONENT_FORCEFIELD_CenterLineForceField_CPP) +extern template class SOFA_INFINYTOOLKIT_API CenterLineForceField; +#endif // !defined(SOFA_COMPONENT_FORCEFIELD_CenterLineForceField_CPP) + +} // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/CenterLineForceField.inl b/src/InfinyToolkit/CenterLineForceField.inl new file mode 100644 index 0000000..bab5a21 --- /dev/null +++ b/src/InfinyToolkit/CenterLineForceField.inl @@ -0,0 +1,216 @@ +/***************************************************************************** + * - Copyright (C) - 2020 - InfinyTech3D - * + * * + * This file is part of the InfinyToolkit plugin for the SOFA framework * + * * + * Commercial License Usage: * + * Licensees holding valid commercial license from InfinyTech3D may use this * + * file in accordance with the commercial license agreement provided with * + * the Software or, alternatively, in accordance with the terms contained in * + * a written agreement between you and InfinyTech3D. For further information * + * on the licensing terms and conditions, contact: contact@infinytech3d.com * + * * + * GNU General Public License Usage: * + * Alternatively, this file may be used under the terms of the GNU General * + * Public License version 3. The licenses are as published by the Free * + * Software Foundation and appearing in the file LICENSE.GPL3 included in * + * the packaging of this file. Please review the following information to * + * ensure the GNU General Public License requirements will be met: * + * https://www.gnu.org/licenses/gpl-3.0.html. * + * * + * Authors: see Authors.txt * + * Further information: https://infinytech3d.com * + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::infinytoolkit +{ + +template +CenterLineForceField::CenterLineForceField() + : d_positions(initData(&d_positions, "position", "List of coordinates points")) + , d_restPositions(initData(&d_restPositions, "restPositions", "List of rest coordinates points")) + , d_outputPositions(initData(&d_outputPositions, "outputPositions", "List of output coordinates points")) + , d_centers(initData(&d_centers, "centers", "List of center coordinates points")) + , d_force(initData(&d_force, 1.0_sreal, "force", "Applied force to all points to simulate maximum compression.")) + , d_uniformForce(initData(&d_uniformForce, bool(false), "uniformForce", "If true, will apply the same force at each vertex otherwise will apply force proportional to the distance to the barycenter")) + , d_pace(initData(&d_pace, 1.0_sreal, "pace", "Time to perform a full Pace (deflate + inflate). Same scale as the simulation time.")) + + , d_stiffness(initData(&d_stiffness, 1000.0_sreal, "stiffness", "Time to perform a full Pace (deflate + inflate). Same scale as the simulation time.")) + + + , d_refreshBaryRate(initData(&d_refreshBaryRate, (unsigned int)(0), "refreshBaryRate", "To recompute barycenter every X pace. 0 by default == no refresh")) + , p_showForce(initData(&p_showForce, bool(false), "showForce", "Parameter to display the force direction")) + , d_syncRealTime(initData(&d_syncRealTime, false, "syncRealTime", "Synchronize with the real time instead of simulation time.")) + , d_frequency(initData(&d_frequency, 1.0_sreal, "frequency", "Frequency at which the full deflate+inflate is done, in Hz, i.e x/sec. Used if pace is not set.")) + , m_startTime() +{ + addInput(&d_restPositions); + addInput(&d_positions); + addInput(&d_centers); + + addOutput(&d_outputPositions); +} + + +template +void CenterLineForceField::init() +{ + size_t nbPoints = d_positions.getValue().size(); + if (nbPoints == 0) + { + msg_error() << "No position set or empty vector given into field: 'position'. Won't be able to compute pace."; + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + + if (!d_pace.isSet() && !d_frequency.isSet()) + { + msg_error() << "Neither Pace nor frequency are set. Won't be able to compute pace."; + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + + if (d_pace.isSet() && d_frequency.isSet()) + { + msg_warning() << "Pace and frequency are both set. Will take into account the frequency value."; + } + + computeDistribution(); + + m_startTime = std::chrono::system_clock::now(); + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); +} + + +template +void CenterLineForceField::computeBarycenter() +{ + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_positions; + + m_bary = Coord(0.0, 0.0, 0.0); + size_t nbPoints = _x.size(); + for (size_t i = 0; i < nbPoints; ++i) + { + m_bary += _x[i]; + } + + m_bary /= nbPoints; +} + + + +template +void CenterLineForceField::computeDistribution() +{ + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_positions; + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; + + m_distribution.resize(_x.size()); + + Coord pt2; + auto dist = [](const Coord& a, const Coord& b) { return (b - a).norm(); }; + auto cmp = [&pt2, &dist](const Coord& a, const Coord& b) { + return dist(a, pt2) < dist(b, pt2); + }; + + for (size_t i = 0; i < _x.size(); ++i) + { + pt2 = _x[i]; + auto it = std::min_element(_centers.begin(), _centers.end(), cmp); + m_distribution[i] = std::distance(_centers.begin(), it); + } +} + + +template +void CenterLineForceField::doUpdate() +{ + if (sofa::core::objectmodel::BaseObject::d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + + sofa::helper::WriteAccessor< core::objectmodel::Data< VecDeriv > > outX = d_outputPositions; + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > inX = d_positions; + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; + + outX.resize(inX.size()); + Real pacePercent = 1.0_sreal; + Real time = 1.0_sreal; + + const Real pace = (!d_frequency.isSet()) ? d_pace.getValue() : Real(1.0 / d_frequency.getValue()); + + if (d_syncRealTime.getValue()) + { + using namespace std::chrono; + + std::chrono::duration duration = system_clock::now() - m_startTime; + time = duration.count(); + } + else + { + time = this->getContext()->getTime(); + } + + // we apply a force proportional to the pace rate. 0 Force at start of pace, 0 at end, F at half pace + pacePercent = std::fmod(time, 2*pace) / (pace); + std::cout << "std::fmod(time, pace): " << std::fmod(time, pace) << std::endl; + std::cout << "std::fmod(time, pace)/2*pace: " << std::fmod(time, pace)/(2*pace) << std::endl; + std::cout << "pacePercent: " << pacePercent << std::endl; + + + + Real factorForce = (pacePercent >= 0.5) ? 1 - pacePercent : pacePercent; + + msg_info() << "Time: " << time << " -> pacePercent: " << pacePercent << " -> " << factorForce; + + + + const Real force = d_force.getValue() * factorForce; + const bool uniformF = d_uniformForce.getValue(); + + // G ---- X -- X0 + + + for (size_t i = 0; i < inX.size(); ++i) + { + const Coord& center = _centers[m_distribution[i]]; + const Coord& p0 = inX[i]; + Coord dir = center - p0; + outX[i] = p0 + dir * factorForce; + } + +} + + +template +void CenterLineForceField::draw(const core::visual::VisualParams* vparams) +{ + //if (!vparams->displayFlags().getShowForceFields() || !p_showForce.getValue()) { + // return; + //} + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + d_positions.getValue(); + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x0 = d_positions; + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_outputPositions; + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; + d_outputPositions.getValue(); + size_t nbPoints = _x.size(); + std::vector vertices; + for (size_t i = 0; i < nbPoints; ++i) + { + vertices.emplace_back(_x[i]); + vertices.emplace_back(_centers[m_distribution[i]]); + } + vparams->drawTool()->drawLines(vertices, 1, sofa::type::RGBAColor(0, 1, 0, 1)); +} + +} // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/initInfinyToolkit.cpp b/src/InfinyToolkit/initInfinyToolkit.cpp index bce9161..bdae6c6 100644 --- a/src/InfinyToolkit/initInfinyToolkit.cpp +++ b/src/InfinyToolkit/initInfinyToolkit.cpp @@ -48,6 +48,7 @@ extern void registerGridBaryCentersPositions(sofa::core::ObjectFactory* factory) // FF extern void registerMiddleForceField(sofa::core::ObjectFactory* factory); +extern void registerCenterLineForceField(sofa::core::ObjectFactory* factory); // Topology extern void registerTriangle2RefinedTriangleTopologicalMapping(sofa::core::ObjectFactory* factory); @@ -135,6 +136,7 @@ void registerObjects(sofa::core::ObjectFactory* factory) // FF registerMiddleForceField(factory); + registerCenterLineForceField(factory); // Topology registerTriangle2RefinedTriangleTopologicalMapping(factory); From d04b18d15137db9e07fe318e8b1c335c001acf80 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 10 Nov 2025 11:55:02 +0100 Subject: [PATCH 2/6] backup work --- src/InfinyToolkit/CenterLineForceField.h | 5 +- src/InfinyToolkit/CenterLineForceField.inl | 78 ++++++++++++++++------ 2 files changed, 61 insertions(+), 22 deletions(-) diff --git a/src/InfinyToolkit/CenterLineForceField.h b/src/InfinyToolkit/CenterLineForceField.h index 67f14b3..b01af59 100644 --- a/src/InfinyToolkit/CenterLineForceField.h +++ b/src/InfinyToolkit/CenterLineForceField.h @@ -53,7 +53,7 @@ class CenterLineForceField : public DataEngine void init() override; void doUpdate() override; - + void handleEvent(sofa::core::objectmodel::Event* event) override; // ForceField methods // void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; @@ -74,6 +74,7 @@ class CenterLineForceField : public DataEngine /// List of coordinates points Data d_centers; + VecCoord m_centersOrdered; /// Applied force to all points to simulate maximum compression. Data d_force; @@ -111,6 +112,8 @@ private : std::vector m_distribution; + Real m_startTimeWave = 1.0; + }; // definition of the CenterLineForceField class diff --git a/src/InfinyToolkit/CenterLineForceField.inl b/src/InfinyToolkit/CenterLineForceField.inl index bab5a21..d1ec3c7 100644 --- a/src/InfinyToolkit/CenterLineForceField.inl +++ b/src/InfinyToolkit/CenterLineForceField.inl @@ -30,6 +30,7 @@ #include #include #include +#include namespace sofa::infinytoolkit { @@ -58,6 +59,7 @@ CenterLineForceField::CenterLineForceField() addInput(&d_centers); addOutput(&d_outputPositions); + this->f_listening.setValue(true); } @@ -113,6 +115,18 @@ void CenterLineForceField::computeDistribution() { sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_positions; sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; + //m_centersOrdered.resize(_centers.size()); + m_centersOrdered = _centers; + std::sort(m_centersOrdered.begin(), m_centersOrdered.end(), + [](const Coord& a, const Coord& b) { + return a[1] > b[1]; + }); + + for (size_t i = 0; i < _centers.size(); ++i) + { + std::cout << i << ": " << _centers[i] << " | " << m_centersOrdered[i] << std::endl; + } + m_distribution.resize(_x.size()); @@ -125,8 +139,8 @@ void CenterLineForceField::computeDistribution() for (size_t i = 0; i < _x.size(); ++i) { pt2 = _x[i]; - auto it = std::min_element(_centers.begin(), _centers.end(), cmp); - m_distribution[i] = std::distance(_centers.begin(), it); + auto it = std::min_element(m_centersOrdered.begin(), m_centersOrdered.end(), cmp); + m_distribution[i] = std::distance(m_centersOrdered.begin(), it); } } @@ -160,55 +174,77 @@ void CenterLineForceField::doUpdate() } // we apply a force proportional to the pace rate. 0 Force at start of pace, 0 at end, F at half pace - pacePercent = std::fmod(time, 2*pace) / (pace); - std::cout << "std::fmod(time, pace): " << std::fmod(time, pace) << std::endl; - std::cout << "std::fmod(time, pace)/2*pace: " << std::fmod(time, pace)/(2*pace) << std::endl; - std::cout << "pacePercent: " << pacePercent << std::endl; + pacePercent = std::fmod(time, pace) / pace; + //std::cout << "std::fmod(time, pace): " << std::fmod(time, pace) << std::endl; + //std::cout << "std::fmod(time, pace)/2*pace: " << std::fmod(time, pace)/(2*pace) << std::endl; + //std::cout << "pacePercent: " << pacePercent << std::endl; - Real factorForce = (pacePercent >= 0.5) ? 1 - pacePercent : pacePercent; + //Real factorForce = (pacePercent >= 0.5) ? 1 - pacePercent : pacePercent; - msg_info() << "Time: " << time << " -> pacePercent: " << pacePercent << " -> " << factorForce; + //msg_info() << "Time: " << time << " -> pacePercent: " << pacePercent << " -> " << factorForce; - const Real force = d_force.getValue() * factorForce; - const bool uniformF = d_uniformForce.getValue(); + // const Real force = d_force.getValue() * factorForce; + //const bool uniformF = d_uniformForce.getValue(); // G ---- X -- X0 + const Real amplitude = 1.0; + const Real frequency = pace; + + + + //std::cout << "oscillation: " << oscillation << std::endl; for (size_t i = 0; i < inX.size(); ++i) { - const Coord& center = _centers[m_distribution[i]]; + int centerId = m_distribution[i]; + + const Coord& center = m_centersOrdered[centerId]; const Coord& p0 = inX[i]; Coord dir = center - p0; - outX[i] = p0 + dir * factorForce; + + Real omega = centerId * i;//2.0 * M_PI * frequency; + omega = omega/ m_distribution.size(); + Real oscillation = amplitude * std::cos(pacePercent * 2.0 * M_PI + omega); + oscillation = 1.0_sreal - (oscillation + 1.0_sreal) / 2.0_sreal; // normalize between 0 and 1 + + outX[i] = p0 + dir * oscillation; + } + +} + +template +void CenterLineForceField::handleEvent(sofa::core::objectmodel::Event* event) +{ + //std::cout << "event" << std::endl; + if (simulation::AnimateBeginEvent::checkEventType(event)) + { + d_positions.setDirtyOutputs(); } - } template void CenterLineForceField::draw(const core::visual::VisualParams* vparams) { - //if (!vparams->displayFlags().getShowForceFields() || !p_showForce.getValue()) { - // return; - //} + if (/*!vparams->displayFlags().getShowForceFields() || */!p_showForce.getValue()) { + return; + } const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - d_positions.getValue(); - sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x0 = d_positions; sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_outputPositions; - sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; - d_outputPositions.getValue(); + //sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; + size_t nbPoints = _x.size(); std::vector vertices; for (size_t i = 0; i < nbPoints; ++i) { vertices.emplace_back(_x[i]); - vertices.emplace_back(_centers[m_distribution[i]]); + vertices.emplace_back(m_centersOrdered[m_distribution[i]]); } vparams->drawTool()->drawLines(vertices, 1, sofa::type::RGBAColor(0, 1, 0, 1)); } From 7d064170bc49cd353e5460ccf0f1704a2fe841e3 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 17 Nov 2025 11:10:25 +0100 Subject: [PATCH 3/6] Fix behavior with totally new mechanism --- src/InfinyToolkit/CenterLineForceField.h | 7 ++ src/InfinyToolkit/CenterLineForceField.inl | 74 ++++++++++++++++------ 2 files changed, 62 insertions(+), 19 deletions(-) diff --git a/src/InfinyToolkit/CenterLineForceField.h b/src/InfinyToolkit/CenterLineForceField.h index b01af59..f993302 100644 --- a/src/InfinyToolkit/CenterLineForceField.h +++ b/src/InfinyToolkit/CenterLineForceField.h @@ -113,6 +113,13 @@ private : std::vector m_distribution; Real m_startTimeWave = 1.0; + int centerDone = -4; + int centerStart = 0; + int centerCurrent = 0; + Real nextStart = 0.0; + + Real length = 4.0_sreal; + //Real current }; // definition of the CenterLineForceField class diff --git a/src/InfinyToolkit/CenterLineForceField.inl b/src/InfinyToolkit/CenterLineForceField.inl index d1ec3c7..fb282c9 100644 --- a/src/InfinyToolkit/CenterLineForceField.inl +++ b/src/InfinyToolkit/CenterLineForceField.inl @@ -90,6 +90,7 @@ void CenterLineForceField::init() m_startTime = std::chrono::system_clock::now(); sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + nextStart = d_pace.getValue() / length; } @@ -177,44 +178,79 @@ void CenterLineForceField::doUpdate() pacePercent = std::fmod(time, pace) / pace; //std::cout << "std::fmod(time, pace): " << std::fmod(time, pace) << std::endl; //std::cout << "std::fmod(time, pace)/2*pace: " << std::fmod(time, pace)/(2*pace) << std::endl; - //std::cout << "pacePercent: " << pacePercent << std::endl; + //std::cout << "time: " << time << " | pace: " << pace << " | pacePercent: " << pacePercent << std::endl; - - - //Real factorForce = (pacePercent >= 0.5) ? 1 - pacePercent : pacePercent; - - //msg_info() << "Time: " << time << " -> pacePercent: " << pacePercent << " -> " << factorForce; - - - - // const Real force = d_force.getValue() * factorForce; - //const bool uniformF = d_uniformForce.getValue(); + if (time >= nextStart) // at every pace/2 we start moving a new center and stop previous one + { + centerStart++; + centerDone++; + + //if (centerCurrent == 4) + { + /* centerDone++; + centerCurrent = 0;*/ + std::cout << "Center " << centerDone << " done at time " << time << std::endl; + } + + //centerDone++; + std::cout << "Center " << centerStart << " started at time " << time << std::endl; + + nextStart += pace / length; + } // G ---- X -- X0 - const Real amplitude = 1.0; + const Real amplitude = 0.8; const Real frequency = pace; - - //std::cout << "oscillation: " << oscillation << std::endl; - + //std::cout << "centerStart: " << centerStart << " | " << centerDone << " / " << _centers.size() << " centers done." << std::endl; for (size_t i = 0; i < inX.size(); ++i) { int centerId = m_distribution[i]; + + if (centerId > centerStart || centerId <= centerDone ) + { + outX[i] = inX[i]; + continue; + } + const Coord& center = m_centersOrdered[centerId]; const Coord& p0 = inX[i]; Coord dir = center - p0; - Real omega = centerId * i;//2.0 * M_PI * frequency; - omega = omega/ m_distribution.size(); - Real oscillation = amplitude * std::cos(pacePercent * 2.0 * M_PI + omega); - oscillation = 1.0_sreal - (oscillation + 1.0_sreal) / 2.0_sreal; // normalize between 0 and 1 + //Real omega = centerId * i;//2.0 * M_PI * frequency; + //Real omega = Real(centerId) / Real(_centers.size()) * M_PI * 12; + Real omega = centerId * 2.0 * M_PI / length; + Real oscillation = amplitude * std::cos(pacePercent * 2.0 * M_PI - omega); + oscillation = amplitude - (oscillation + amplitude) / 2.0_sreal; // normalize between 0 and 0.9 + + // cos: [-1; 1] 1 -1 1 + // [0; 2] 2 0 2 + // [0; 1] 1 0 1 + // [1; 0] 0 1 0 + + // 0.9 -0.9 0.9 + // 1.8 0 1.8 + // 0.9 0 0.9 + // 0 0.9 0 outX[i] = p0 + dir * oscillation; } + + + if (centerDone >= int(m_centersOrdered.size()) && pacePercent >= 0.99) + { + std::cout << "centerDone " << centerDone << " | " << m_centersOrdered.size() << std::endl; + + centerDone = -4; + centerStart = 0; + nextStart = time + pace / length; + } + + } template From 721e11c9f265f37c3f2be942180811d6233e6d04 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 17 Nov 2025 23:27:00 +0100 Subject: [PATCH 4/6] totally change name an clean Data --- CMakeLists.txt | 6 +- ....cpp => ProximityOscillatorConstraint.cpp} | 10 +- ...ield.h => ProximityOscillatorConstraint.h} | 44 ++---- ....inl => ProximityOscillatorConstraint.inl} | 126 ++++-------------- src/InfinyToolkit/initInfinyToolkit.cpp | 4 +- 5 files changed, 45 insertions(+), 145 deletions(-) rename src/InfinyToolkit/{CenterLineForceField.cpp => ProximityOscillatorConstraint.cpp} (85%) rename src/InfinyToolkit/{CenterLineForceField.h => ProximityOscillatorConstraint.h} (71%) rename src/InfinyToolkit/{CenterLineForceField.inl => ProximityOscillatorConstraint.inl} (61%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d3e15c..a121e6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,8 +35,8 @@ set(HEADER_FILES ${INFINYTOOLKIT_SRC_DIR}/NeedleTracker.h ${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.h ${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.inl - ${INFINYTOOLKIT_SRC_DIR}/CenterLineForceField.h - ${INFINYTOOLKIT_SRC_DIR}/CenterLineForceField.inl + ${INFINYTOOLKIT_SRC_DIR}/ProximityOscillatorConstraint.h + ${INFINYTOOLKIT_SRC_DIR}/ProximityOscillatorConstraint.inl ${INFINYTOOLKIT_SRC_DIR}/Triangle2RefinedTriangleTopologicalMapping.h ## Carving tools sections @@ -69,7 +69,7 @@ set(SOURCE_FILES ${INFINYTOOLKIT_SRC_DIR}/HapticEmulator.cpp ${INFINYTOOLKIT_SRC_DIR}/NeedleTracker.cpp ${INFINYTOOLKIT_SRC_DIR}/MiddleForceField.cpp - ${INFINYTOOLKIT_SRC_DIR}/CenterLineForceField.cpp + ${INFINYTOOLKIT_SRC_DIR}/ProximityOscillatorConstraint.cpp ${INFINYTOOLKIT_SRC_DIR}/Triangle2RefinedTriangleTopologicalMapping.cpp ## Carving tools sections diff --git a/src/InfinyToolkit/CenterLineForceField.cpp b/src/InfinyToolkit/ProximityOscillatorConstraint.cpp similarity index 85% rename from src/InfinyToolkit/CenterLineForceField.cpp rename to src/InfinyToolkit/ProximityOscillatorConstraint.cpp index 5927011..6cd21aa 100644 --- a/src/InfinyToolkit/CenterLineForceField.cpp +++ b/src/InfinyToolkit/ProximityOscillatorConstraint.cpp @@ -22,22 +22,22 @@ * Further information: https://infinytech3d.com * ****************************************************************************/ -#define SOFA_COMPONENT_FORCEFIELD_CenterLineForceField_CPP +#define SOFA_COMPONENT_FORCEFIELD_ProximityOscillatorConstraint_CPP #include -#include +#include namespace sofa::infinytoolkit { using namespace sofa::defaulttype; -void registerCenterLineForceField(sofa::core::ObjectFactory* factory) +void registerProximityOscillatorConstraint(sofa::core::ObjectFactory* factory) { factory->registerObjects(sofa::core::ObjectRegistrationData("Middle interpolated force applied to given degrees of freedom.") - .add< CenterLineForceField >()); + .add< ProximityOscillatorConstraint >()); } -template class SOFA_INFINYTOOLKIT_API CenterLineForceField; +template class SOFA_INFINYTOOLKIT_API ProximityOscillatorConstraint; } // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/CenterLineForceField.h b/src/InfinyToolkit/ProximityOscillatorConstraint.h similarity index 71% rename from src/InfinyToolkit/CenterLineForceField.h rename to src/InfinyToolkit/ProximityOscillatorConstraint.h index f993302..b6df693 100644 --- a/src/InfinyToolkit/CenterLineForceField.h +++ b/src/InfinyToolkit/ProximityOscillatorConstraint.h @@ -35,10 +35,10 @@ namespace sofa::infinytoolkit /** Apply forces changing to given degres of freedom. Some keyTimes are given * and the force to be applied is linearly interpolated between keyTimes. */ template -class CenterLineForceField : public DataEngine +class ProximityOscillatorConstraint : public DataEngine { public: - SOFA_CLASS(SOFA_TEMPLATE(CenterLineForceField, DataTypes), core::DataEngine); + SOFA_CLASS(SOFA_TEMPLATE(ProximityOscillatorConstraint, DataTypes), core::DataEngine); using VecCoord = typename DataTypes::VecCoord; using VecDeriv = typename DataTypes::VecDeriv; @@ -48,16 +48,13 @@ class CenterLineForceField : public DataEngine using DataVecCoord = core::objectmodel::Data; using DataVecDeriv = core::objectmodel::Data; - CenterLineForceField(); + ProximityOscillatorConstraint(); void init() override; void doUpdate() override; void handleEvent(sofa::core::objectmodel::Event* event) override; - // ForceField methods -// void addForce(const core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; - void draw(const core::visual::VisualParams* vparams) override; protected: @@ -69,44 +66,21 @@ class CenterLineForceField : public DataEngine public: /// List of coordinates points Data d_positions; - Data d_restPositions; Data d_outputPositions; /// List of coordinates points Data d_centers; VecCoord m_centersOrdered; - /// Applied force to all points to simulate maximum compression. - Data d_force; - - /// If true, will apply the same force at each vertex otherwise will apply force proportional to the distance to the barycenter - Data d_uniformForce; - /// Time to perform a full Pace (deflate + inflate). Same scale as the simulation time. Data d_pace; - /// To recompute barycenter every X pace. 0 by default == no refresh - Data d_refreshBaryRate; - - Data d_stiffness; - + Data d_amplitude; /// Parameter to display the force direction - Data p_showForce; - - // Synchronize with real time (instead of simulation time) - Data d_syncRealTime; - - // Frequency at which a full deflate+inflate is done - Data d_frequency; + Data p_showMotion; private : - /// Computed barycenter of the given positions @sa d_positions - Coord m_bary; - - /// counter to the last pace the barycenter has been refreshed. To be used with @sa d_refreshBaryRate - unsigned int m_lastBaryRefresh = 0; - // keep trace of the latest time we measured std::chrono::time_point m_startTime; @@ -121,12 +95,12 @@ private : Real length = 4.0_sreal; //Real current -}; // definition of the CenterLineForceField class +}; // definition of the ProximityOscillatorConstraint class -#if !defined(SOFA_COMPONENT_FORCEFIELD_CenterLineForceField_CPP) -extern template class SOFA_INFINYTOOLKIT_API CenterLineForceField; -#endif // !defined(SOFA_COMPONENT_FORCEFIELD_CenterLineForceField_CPP) +#if !defined(SOFA_COMPONENT_FORCEFIELD_ProximityOscillatorConstraint_CPP) +extern template class SOFA_INFINYTOOLKIT_API ProximityOscillatorConstraint; +#endif // !defined(SOFA_COMPONENT_FORCEFIELD_ProximityOscillatorConstraint_CPP) } // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/CenterLineForceField.inl b/src/InfinyToolkit/ProximityOscillatorConstraint.inl similarity index 61% rename from src/InfinyToolkit/CenterLineForceField.inl rename to src/InfinyToolkit/ProximityOscillatorConstraint.inl index fb282c9..cbd4002 100644 --- a/src/InfinyToolkit/CenterLineForceField.inl +++ b/src/InfinyToolkit/ProximityOscillatorConstraint.inl @@ -23,7 +23,7 @@ ****************************************************************************/ #pragma once -#include +#include #include #include #include @@ -36,25 +36,15 @@ namespace sofa::infinytoolkit { template -CenterLineForceField::CenterLineForceField() +ProximityOscillatorConstraint::ProximityOscillatorConstraint() : d_positions(initData(&d_positions, "position", "List of coordinates points")) - , d_restPositions(initData(&d_restPositions, "restPositions", "List of rest coordinates points")) - , d_outputPositions(initData(&d_outputPositions, "outputPositions", "List of output coordinates points")) + , d_outputPositions(initData(&d_outputPositions, "outputPositions", "List of output coordinates points")) , d_centers(initData(&d_centers, "centers", "List of center coordinates points")) - , d_force(initData(&d_force, 1.0_sreal, "force", "Applied force to all points to simulate maximum compression.")) - , d_uniformForce(initData(&d_uniformForce, bool(false), "uniformForce", "If true, will apply the same force at each vertex otherwise will apply force proportional to the distance to the barycenter")) , d_pace(initData(&d_pace, 1.0_sreal, "pace", "Time to perform a full Pace (deflate + inflate). Same scale as the simulation time.")) - - , d_stiffness(initData(&d_stiffness, 1000.0_sreal, "stiffness", "Time to perform a full Pace (deflate + inflate). Same scale as the simulation time.")) - - - , d_refreshBaryRate(initData(&d_refreshBaryRate, (unsigned int)(0), "refreshBaryRate", "To recompute barycenter every X pace. 0 by default == no refresh")) - , p_showForce(initData(&p_showForce, bool(false), "showForce", "Parameter to display the force direction")) - , d_syncRealTime(initData(&d_syncRealTime, false, "syncRealTime", "Synchronize with the real time instead of simulation time.")) - , d_frequency(initData(&d_frequency, 1.0_sreal, "frequency", "Frequency at which the full deflate+inflate is done, in Hz, i.e x/sec. Used if pace is not set.")) + , d_amplitude(initData(&d_amplitude, 0.8_sreal, "amplitude", "Amplitude of the oscillation")) + , p_showMotion(initData(&p_showMotion, bool(false), "showMotion", "Parameter to display the force direction")) , m_startTime() { - addInput(&d_restPositions); addInput(&d_positions); addInput(&d_centers); @@ -64,7 +54,7 @@ CenterLineForceField::CenterLineForceField() template -void CenterLineForceField::init() +void ProximityOscillatorConstraint::init() { size_t nbPoints = d_positions.getValue().size(); if (nbPoints == 0) @@ -74,18 +64,13 @@ void CenterLineForceField::init() return; } - if (!d_pace.isSet() && !d_frequency.isSet()) + if (!d_pace.isSet()) { - msg_error() << "Neither Pace nor frequency are set. Won't be able to compute pace."; + msg_error() << "Pace is not set. Won't be able to compute pace."; sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); return; } - if (d_pace.isSet() && d_frequency.isSet()) - { - msg_warning() << "Pace and frequency are both set. Will take into account the frequency value."; - } - computeDistribution(); m_startTime = std::chrono::system_clock::now(); @@ -95,40 +80,25 @@ void CenterLineForceField::init() template -void CenterLineForceField::computeBarycenter() -{ - sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_positions; - - m_bary = Coord(0.0, 0.0, 0.0); - size_t nbPoints = _x.size(); - for (size_t i = 0; i < nbPoints; ++i) - { - m_bary += _x[i]; - } - - m_bary /= nbPoints; -} - - - -template -void CenterLineForceField::computeDistribution() +void ProximityOscillatorConstraint::computeDistribution() { sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_positions; sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; - //m_centersOrdered.resize(_centers.size()); + m_centersOrdered = _centers; std::sort(m_centersOrdered.begin(), m_centersOrdered.end(), [](const Coord& a, const Coord& b) { return a[1] > b[1]; }); - for (size_t i = 0; i < _centers.size(); ++i) + if (f_printLog.getValue()) { - std::cout << i << ": " << _centers[i] << " | " << m_centersOrdered[i] << std::endl; + for (size_t i = 0; i < _centers.size(); ++i) + { + std::cout << i << ": " << _centers[i] << " | " << m_centersOrdered[i] << std::endl; + } } - m_distribution.resize(_x.size()); Coord pt2; @@ -147,7 +117,7 @@ void CenterLineForceField::computeDistribution() template -void CenterLineForceField::doUpdate() +void ProximityOscillatorConstraint::doUpdate() { if (sofa::core::objectmodel::BaseObject::d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) return; @@ -157,59 +127,28 @@ void CenterLineForceField::doUpdate() sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; outX.resize(inX.size()); - Real pacePercent = 1.0_sreal; - Real time = 1.0_sreal; - - const Real pace = (!d_frequency.isSet()) ? d_pace.getValue() : Real(1.0 / d_frequency.getValue()); - - if (d_syncRealTime.getValue()) - { - using namespace std::chrono; + Real time = this->getContext()->getTime(); - std::chrono::duration duration = system_clock::now() - m_startTime; - time = duration.count(); - } - else - { - time = this->getContext()->getTime(); - } + const Real pace = d_pace.getValue(); // we apply a force proportional to the pace rate. 0 Force at start of pace, 0 at end, F at half pace - pacePercent = std::fmod(time, pace) / pace; - //std::cout << "std::fmod(time, pace): " << std::fmod(time, pace) << std::endl; - //std::cout << "std::fmod(time, pace)/2*pace: " << std::fmod(time, pace)/(2*pace) << std::endl; - //std::cout << "time: " << time << " | pace: " << pace << " | pacePercent: " << pacePercent << std::endl; - + const Real pacePercent = std::fmod(time, pace) / pace; + if (time >= nextStart) // at every pace/2 we start moving a new center and stop previous one { centerStart++; centerDone++; - - //if (centerCurrent == 4) - { - /* centerDone++; - centerCurrent = 0;*/ - std::cout << "Center " << centerDone << " done at time " << time << std::endl; - } - - //centerDone++; - std::cout << "Center " << centerStart << " started at time " << time << std::endl; - nextStart += pace / length; } // G ---- X -- X0 - const Real amplitude = 0.8; + const Real amplitude = d_amplitude.getValue(); const Real frequency = pace; - //std::cout << "oscillation: " << oscillation << std::endl; - //std::cout << "centerStart: " << centerStart << " | " << centerDone << " / " << _centers.size() << " centers done." << std::endl; for (size_t i = 0; i < inX.size(); ++i) { int centerId = m_distribution[i]; - - if (centerId > centerStart || centerId <= centerDone ) { outX[i] = inX[i]; @@ -226,35 +165,22 @@ void CenterLineForceField::doUpdate() Real oscillation = amplitude * std::cos(pacePercent * 2.0 * M_PI - omega); oscillation = amplitude - (oscillation + amplitude) / 2.0_sreal; // normalize between 0 and 0.9 - // cos: [-1; 1] 1 -1 1 - // [0; 2] 2 0 2 - // [0; 1] 1 0 1 - // [1; 0] 0 1 0 - - // 0.9 -0.9 0.9 - // 1.8 0 1.8 - // 0.9 0 0.9 - // 0 0.9 0 - outX[i] = p0 + dir * oscillation; } - - if (centerDone >= int(m_centersOrdered.size()) && pacePercent >= 0.99) { - std::cout << "centerDone " << centerDone << " | " << m_centersOrdered.size() << std::endl; + if (f_printLog.getValue()) + std::cout << "centerDone " << centerDone << " | " << m_centersOrdered.size() << std::endl; centerDone = -4; centerStart = 0; nextStart = time + pace / length; } - - } template -void CenterLineForceField::handleEvent(sofa::core::objectmodel::Event* event) +void ProximityOscillatorConstraint::handleEvent(sofa::core::objectmodel::Event* event) { //std::cout << "event" << std::endl; if (simulation::AnimateBeginEvent::checkEventType(event)) @@ -265,9 +191,9 @@ void CenterLineForceField::handleEvent(sofa::core::objectmodel::Event template -void CenterLineForceField::draw(const core::visual::VisualParams* vparams) +void ProximityOscillatorConstraint::draw(const core::visual::VisualParams* vparams) { - if (/*!vparams->displayFlags().getShowForceFields() || */!p_showForce.getValue()) { + if (!p_showMotion.getValue()) { return; } const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); diff --git a/src/InfinyToolkit/initInfinyToolkit.cpp b/src/InfinyToolkit/initInfinyToolkit.cpp index bdae6c6..c4b3a08 100644 --- a/src/InfinyToolkit/initInfinyToolkit.cpp +++ b/src/InfinyToolkit/initInfinyToolkit.cpp @@ -48,7 +48,7 @@ extern void registerGridBaryCentersPositions(sofa::core::ObjectFactory* factory) // FF extern void registerMiddleForceField(sofa::core::ObjectFactory* factory); -extern void registerCenterLineForceField(sofa::core::ObjectFactory* factory); +extern void registerProximityOscillatorConstraint(sofa::core::ObjectFactory* factory); // Topology extern void registerTriangle2RefinedTriangleTopologicalMapping(sofa::core::ObjectFactory* factory); @@ -136,7 +136,7 @@ void registerObjects(sofa::core::ObjectFactory* factory) // FF registerMiddleForceField(factory); - registerCenterLineForceField(factory); + registerProximityOscillatorConstraint(factory); // Topology registerTriangle2RefinedTriangleTopologicalMapping(factory); From 15a233d1e7a99209d491c96894fae83e4cb21c50 Mon Sep 17 00:00:00 2001 From: epernod Date: Wed, 19 Nov 2025 16:29:25 +0100 Subject: [PATCH 5/6] fix typo --- .../ProximityOscillatorConstraint.inl | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/InfinyToolkit/ProximityOscillatorConstraint.inl b/src/InfinyToolkit/ProximityOscillatorConstraint.inl index cbd4002..35a9ca1 100644 --- a/src/InfinyToolkit/ProximityOscillatorConstraint.inl +++ b/src/InfinyToolkit/ProximityOscillatorConstraint.inl @@ -41,15 +41,15 @@ ProximityOscillatorConstraint::ProximityOscillatorConstraint() , d_outputPositions(initData(&d_outputPositions, "outputPositions", "List of output coordinates points")) , d_centers(initData(&d_centers, "centers", "List of center coordinates points")) , d_pace(initData(&d_pace, 1.0_sreal, "pace", "Time to perform a full Pace (deflate + inflate). Same scale as the simulation time.")) - , d_amplitude(initData(&d_amplitude, 0.8_sreal, "amplitude", "Amplitude of the oscillation")) + , d_amplitude(initData(&d_amplitude, 0.8_sreal, "amplitude", "Amplitude of the oscillation")) , p_showMotion(initData(&p_showMotion, bool(false), "showMotion", "Parameter to display the force direction")) , m_startTime() -{ +{ addInput(&d_positions); addInput(&d_centers); addOutput(&d_outputPositions); - this->f_listening.setValue(true); + this->f_listening.setValue(true); } @@ -75,7 +75,7 @@ void ProximityOscillatorConstraint::init() m_startTime = std::chrono::system_clock::now(); sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); - nextStart = d_pace.getValue() / length; + nextStart = d_pace.getValue() / length; } @@ -84,12 +84,12 @@ void ProximityOscillatorConstraint::computeDistribution() { sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_positions; sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; - - m_centersOrdered = _centers; - std::sort(m_centersOrdered.begin(), m_centersOrdered.end(), - [](const Coord& a, const Coord& b) { + + m_centersOrdered = _centers; + std::sort(m_centersOrdered.begin(), m_centersOrdered.end(), + [](const Coord& a, const Coord& b) { return a[1] > b[1]; - }); + }); if (f_printLog.getValue()) { @@ -105,14 +105,14 @@ void ProximityOscillatorConstraint::computeDistribution() auto dist = [](const Coord& a, const Coord& b) { return (b - a).norm(); }; auto cmp = [&pt2, &dist](const Coord& a, const Coord& b) { return dist(a, pt2) < dist(b, pt2); - }; + }; for (size_t i = 0; i < _x.size(); ++i) { pt2 = _x[i]; auto it = std::min_element(m_centersOrdered.begin(), m_centersOrdered.end(), cmp); m_distribution[i] = std::distance(m_centersOrdered.begin(), it); - } + } } @@ -133,36 +133,36 @@ void ProximityOscillatorConstraint::doUpdate() // we apply a force proportional to the pace rate. 0 Force at start of pace, 0 at end, F at half pace const Real pacePercent = std::fmod(time, pace) / pace; - - if (time >= nextStart) // at every pace/2 we start moving a new center and stop previous one + + if (time >= nextStart) // at every pace/2 we start moving a new center and stop previous one { centerStart++; centerDone++; - nextStart += pace / length; + nextStart += pace / length; } // G ---- X -- X0 const Real amplitude = d_amplitude.getValue(); - const Real frequency = pace; + const Real frequency = pace; for (size_t i = 0; i < inX.size(); ++i) { int centerId = m_distribution[i]; - if (centerId > centerStart || centerId <= centerDone ) + if (centerId > centerStart || centerId <= centerDone) { - outX[i] = inX[i]; - continue; + outX[i] = inX[i]; + continue; } - const Coord& center = m_centersOrdered[centerId]; + const Coord& center = m_centersOrdered[centerId]; const Coord& p0 = inX[i]; Coord dir = center - p0; //Real omega = centerId * i;//2.0 * M_PI * frequency; //Real omega = Real(centerId) / Real(_centers.size()) * M_PI * 12; Real omega = centerId * 2.0 * M_PI / length; - Real oscillation = amplitude * std::cos(pacePercent * 2.0 * M_PI - omega); + Real oscillation = amplitude * std::cos(pacePercent * 2.0 * M_PI - omega); oscillation = amplitude - (oscillation + amplitude) / 2.0_sreal; // normalize between 0 and 0.9 outX[i] = p0 + dir * oscillation; @@ -170,9 +170,9 @@ void ProximityOscillatorConstraint::doUpdate() if (centerDone >= int(m_centersOrdered.size()) && pacePercent >= 0.99) { - if (f_printLog.getValue()) + if (f_printLog.getValue()) std::cout << "centerDone " << centerDone << " | " << m_centersOrdered.size() << std::endl; - + centerDone = -4; centerStart = 0; nextStart = time + pace / length; @@ -197,7 +197,7 @@ void ProximityOscillatorConstraint::draw(const core::visual::VisualPa return; } const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - + sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _x = d_outputPositions; //sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > _centers = d_centers; From bedc4fff840b9f38651a777fe5b56543179297c4 Mon Sep 17 00:00:00 2001 From: epernod Date: Tue, 16 Dec 2025 22:42:36 +0100 Subject: [PATCH 6/6] Add basic example --- examples/ProximityOscillatorConstraint.scn | 68 +++++++++++++++++++ .../ProximityOscillatorConstraint.scn.view | 17 +++++ 2 files changed, 85 insertions(+) create mode 100644 examples/ProximityOscillatorConstraint.scn create mode 100644 examples/ProximityOscillatorConstraint.scn.view diff --git a/examples/ProximityOscillatorConstraint.scn b/examples/ProximityOscillatorConstraint.scn new file mode 100644 index 0000000..9bf6957 --- /dev/null +++ b/examples/ProximityOscillatorConstraint.scn @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ProximityOscillatorConstraint.scn.view b/examples/ProximityOscillatorConstraint.scn.view new file mode 100644 index 0000000..1a2bc0c --- /dev/null +++ b/examples/ProximityOscillatorConstraint.scn.view @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + +