From 85e29770988e90ca462cc23328f89ca65e61215a Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 20 Jan 2025 13:30:00 +0100 Subject: [PATCH 1/7] [Tools] backup work on secondary Action on the articulatedTool --- .../ArticulatedToolManager.cpp | 157 ++++++++++++++++-- .../InteractionTools/ArticulatedToolManager.h | 8 + .../InteractionTools/BaseJawModel.h | 13 +- .../InteractionTools/GrasperJawModel.cpp | 56 ++++++- .../InteractionTools/GrasperJawModel.h | 5 + 5 files changed, 219 insertions(+), 20 deletions(-) diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp index 3a903f0..9af9a0b 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -65,6 +66,8 @@ ArticulatedToolManager::ArticulatedToolManager() , l_targetModel(initLink("targetModel", "link to the second jaw model component, if not set will search through graph and take second one encountered.")) , d_handleFactor(initData(&d_handleFactor, SReal(1.0), "handleFactor", "jaw speed factor.")) , d_outputPositions(initData(&d_outputPositions, "outputPositions", "jaw positions.")) + , d_isCutter(initData(&d_isCutter, false, "isCutter", "if true, will draw slices BB, ray and intersected triangles")) + , d_isControlled(initData(&d_isControlled, false, "isControlled", "if true, will draw slices BB, ray and intersected triangles")) , d_drawContacts(initData(&d_drawContacts, false, "drawContacts", "if true, will draw slices BB, ray and intersected triangles")) { this->f_listening.setValue(true); @@ -218,6 +221,93 @@ bool ArticulatedToolManager::stopAction() } +int ArticulatedToolManager::performSecondaryAction() +{ + m_performCut = true; +} + + + void ArticulatedToolManager::performCut() +{ + msg_warning() << "performSecondaryAction()"; + if (!d_isCutter.getValue()) + return; + + sofa::type::vector idGraps1 = l_jawModel1.get()->getRawContactModelIds(); + sofa::type::vector idGraps2 = l_jawModel2.get()->getRawContactModelIds(); + + // Detect all tetra on the cut path + TetrahedronSetTopologyContainer* tetraCon; + l_targetModel->getContext()->get(tetraCon); + if (tetraCon == nullptr) { + msg_info() << "Error: NO tetraCon"; + return; + } + + TetrahedronSetTopologyModifier* tetraModif; + tetraCon->getContext()->get(tetraModif); + + if (tetraModif == nullptr) { + msg_info() << "Error: NO tetraModif"; + return; + } + + // First get all tetra that are on the first side + sofa::type::vector tetraIds; + for (auto id : idGraps1) + { + const BaseMeshTopology::TetrahedraAroundVertex& tetraAV = tetraCon->getTetrahedraAroundVertex(id); + for (int j = 0; j < tetraAV.size(); ++j) + { + int tetraId = tetraAV[j]; + bool found = false; + for (int k = 0; k < tetraIds.size(); ++k) + if (tetraIds[k] == tetraId) + { + found = true; + break; + } + + if (!found) + tetraIds.push_back(tetraId); + } + } + + for (auto id : idGraps2) + { + const BaseMeshTopology::TetrahedraAroundVertex& tetraAV = tetraCon->getTetrahedraAroundVertex(id); + for (int j = 0; j < tetraAV.size(); ++j) + { + int tetraId = tetraAV[j]; + bool found = false; + for (int k = 0; k < tetraIds.size(); ++k) + if (tetraIds[k] == tetraId) + { + found = true; + break; + } + + if (!found) + tetraIds.push_back(tetraId); + } + } + + std::cout << "tetra2Rmove: " << tetraIds << std::endl; + + // remove springs first + stopAction(); + tetraModif->removeTetrahedra(tetraIds); + + + return; +} + +int ArticulatedToolManager::stopSecondaryAction() +{ + msg_warning() << "stopSecondaryAction()"; + return 0; +} + void ArticulatedToolManager::openTool() { @@ -278,8 +368,8 @@ void ArticulatedToolManager::filterCollision() sofa::core::CollisionModel* collMod1 = it->first.first; sofa::core::CollisionModel* collMod2 = it->first.second; - dmsg_info() << "collMod1: " << collMod1->getTypeName() << " -> " << collMod1->getContext()->getName(); - dmsg_info() << "collMod1: " << collMod2->getTypeName() << " -> " << collMod2->getContext()->getName(); + dmsg_warning() << "collMod1: " << collMod1->getTypeName() << " -> " << collMod1->getContext()->getName(); + dmsg_warning() << "collMod2: " << collMod2->getTypeName() << " -> " << collMod2->getContext()->getName(); // Get the number of contacts @@ -295,26 +385,44 @@ void ArticulatedToolManager::filterCollision() { // update the triangle id if a mapping is present GrabContactInfo* info = new GrabContactInfo(); + const ContactVector::value_type& c = (*contacts)[j]; bool firstJaw = false; - const ContactVector::value_type& c = (*contacts)[j]; - if (c.elem.first.getCollisionModel()->getEnumType() == sofa::core::CollisionModel::TRIANGLE_TYPE) // first model is target model + if (c.elem.first.getCollisionModel() == l_jawModel1.get()->l_jawCollision.get() || c.elem.first.getCollisionModel() == l_jawModel2.get()->l_jawCollision.get()) // first model is a jaw + { + info->idTool = c.elem.first.getIndex(); // id of the tool collision model + if (c.elem.second.getCollisionModel()->getEnumType() == sofa::core::CollisionModel::TRIANGLE_TYPE) // first model is triangle model + { + sofa::Index idTri = c.elem.second.getIndex(); + info->idsModel = c.elem.second.getCollisionModel()->getCollisionTopology()->getTriangle(idTri); + } + else + { + info->idvModel = c.elem.second.getIndex(); + } + + if (c.elem.first.getCollisionModel() == l_jawModel1.get()->l_jawCollision.get()) + firstJaw = true; + } + else if (c.elem.second.getCollisionModel() == l_jawModel1.get()->l_jawCollision.get() || c.elem.second.getCollisionModel() == l_jawModel2.get()->l_jawCollision.get()) // second model is a jaw { info->idTool = c.elem.second.getIndex(); - sofa::Index idTri = c.elem.first.getIndex(); - info->idsModel = c.elem.first.getCollisionModel()->getCollisionTopology()->getTriangle(idTri); + if (c.elem.first.getCollisionModel()->getEnumType() == sofa::core::CollisionModel::TRIANGLE_TYPE) // first model is triangle model + { + sofa::Index idTri = c.elem.first.getIndex(); + info->idsModel = c.elem.first.getCollisionModel()->getCollisionTopology()->getTriangle(idTri); + } + else + { + info->idvModel = c.elem.first.getIndex(); + } if (c.elem.second.getCollisionModel() == l_jawModel1.get()->l_jawCollision.get()) firstJaw = true; } - else + else // not related to this tool { - info->idTool = c.elem.first.getIndex(); - sofa::Index idTri = c.elem.second.getIndex(); - info->idsModel = c.elem.second.getCollisionModel()->getCollisionTopology()->getTriangle(idTri); - - if (c.elem.first.getCollisionModel() == l_jawModel1.get()->l_jawCollision.get()) - firstJaw = true; + continue; } info->normal = c.normal; @@ -336,8 +444,18 @@ void ArticulatedToolManager::filterCollision() void ArticulatedToolManager::handleEvent(sofa::core::objectmodel::Event* event) { + if (m_performCut && sofa::simulation::AnimateEndEvent::checkEventType(event)) + { + performCut(); + m_performCut = false; + } + + if (KeypressedEvent::checkEventType(event)) { + if (!d_isControlled.getValue()) + return; + KeypressedEvent *ev = static_cast(event); switch (ev->getKey()) @@ -350,16 +468,25 @@ void ArticulatedToolManager::handleEvent(sofa::core::objectmodel::Event* event) //closeTool(); - filterCollision(); + //filterCollision(); performAction(); + deActivateTool(); break; } case 'G': case 'g': { - openTool(); + stopAction(); + activateTool(); + + break; + } + case 'U': + case 'u': + { + performSecondaryAction(); break; } case '0': diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h index 56586f5..93b4105 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h @@ -67,6 +67,9 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: int performAction(); bool stopAction(); + int performSecondaryAction(); + int stopSecondaryAction(); + void closeTool(); void openTool(); @@ -81,6 +84,8 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: void filterCollision(); + void performCut(); + public: // Path to the different JawModel SingleLink l_jawModel1; @@ -99,6 +104,8 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: Data d_inputPosition; Data > d_outputPositions; + Data d_isCutter; + Data d_isControlled; Data d_drawContacts; ///< if true, draw the collision outputs protected: @@ -110,6 +117,7 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: BaseJawModel::SPtr m_jawModel1 = nullptr; BaseJawModel::SPtr m_jawModel2 = nullptr; + bool m_performCut = false; }; diff --git a/src/InfinyToolkit/InteractionTools/BaseJawModel.h b/src/InfinyToolkit/InteractionTools/BaseJawModel.h index eb64de7..b6fdd4d 100644 --- a/src/InfinyToolkit/InteractionTools/BaseJawModel.h +++ b/src/InfinyToolkit/InteractionTools/BaseJawModel.h @@ -37,7 +37,8 @@ namespace sofa::infinytoolkit class GrabContactInfo { public: - sofa::Index idTool; // in global mesh + sofa::Index idTool = sofa::InvalidID; // in global mesh + sofa::Index idvModel = sofa::InvalidID; sofa::core::topology::BaseMeshTopology::Triangle idsModel; // in global mesh //Vec3 pointA; //Vec3 pointB; @@ -67,6 +68,11 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject /// Main API public method to stop the action of the Jaw virtual void stopAction() {} + /// Main API public method to launch the action of the Jaw + virtual void performSecondaryAction() {} + /// Main API public method to stop the action of the Jaw + virtual void stopSecondaryAction() {} + /// Method to compute tool axis. Will fill @sa m_matP, @sa m_origin, @sa m_xAxis, @sa m_yAxis, @sa m_zAxis void computeAxis(); @@ -78,7 +84,8 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject virtual void addContact(GrabContactInfo* grabInfo); virtual void clearContacts(); - const sofa::type::vector& getContacts() { return m_contactInfos; } + const sofa::type::vector& getContacts() { return m_contactInfos; } const + const sofa::type::vector& getRawContactModelIds() { return m_rawIds; } const void setTargetModel(sofa::core::behavior::BaseMechanicalState* model) { m_target = model; } virtual void drawImpl(const core::visual::VisualParams* vparams); @@ -121,6 +128,8 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject /// List of contacts filter during collision sofa::type::vector m_contactInfos; + + type::vector m_rawIds; }; } // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp b/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp index bd831c7..3f512fa 100644 --- a/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp +++ b/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp @@ -23,6 +23,7 @@ ****************************************************************************/ #include + #include namespace sofa::infinytoolkit @@ -87,10 +88,31 @@ void GrasperJawModel::stopAction() { // clean springs m_forcefield->clear(); + m_rawIds.clear(); //activateImpl(); } +void GrasperJawModel::performSecondaryAction() +{ + /*TetrahedronSetTopologyContainer* tetraCon; + m_target->getContext()->get(tetraCon); + + if (tetraCon == nullptr) { + msg_warning("GrasperJawModel") << "Error: NO tetraCon"; + return; + }*/ + + +} + + +void GrasperJawModel::stopSecondaryAction() +{ + +} + + int GrasperJawModel::createStiffSpringFF() { std::cout << this->getName() << " + createStiffSpringFF()" << std::endl; @@ -114,13 +136,41 @@ void GrasperJawModel::addJawSprings() { Vec3 posTool = Vec3(m_jaw->getPX(cInfo->idTool), m_jaw->getPY(cInfo->idTool), m_jaw->getPZ(cInfo->idTool)); - for (int i = 0; i < 3; ++i) + if (cInfo->idvModel != sofa::InvalidID) // pointModel { - Vec3 posModel = Vec3(m_target->getPX(cInfo->idsModel[i]), m_target->getPY(cInfo->idsModel[i]), m_target->getPZ(cInfo->idsModel[i])); + Vec3 posModel = Vec3(m_target->getPX(cInfo->idvModel), m_target->getPY(cInfo->idvModel), m_target->getPZ(cInfo->idvModel)); SReal dist = (posModel - posTool).norm(); - stiffspringforcefield->addSpring(cInfo->idTool, cInfo->idsModel[i], stiffness, 0.0, dist); + stiffspringforcefield->addSpring(cInfo->idTool, cInfo->idvModel, stiffness, 0.0, dist); + + bool found = false; + for (int id : m_rawIds) + { + if (id == cInfo->idvModel) + { + found = true; + break; + } + } + + if (!found) + m_rawIds.push_back(cInfo->idvModel); + + std::cout << "Add spring: " << cInfo->idTool << " model: " << cInfo->idvModel << std::endl; } + else + { + + } + + //for (int i = 0; i < 3; ++i) + //{ + // Vec3 posModel = Vec3(m_target->getPX(cInfo->idsModel[i]), m_target->getPY(cInfo->idsModel[i]), m_target->getPZ(cInfo->idsModel[i])); + // SReal dist = (posModel - posTool).norm(); + // stiffspringforcefield->addSpring(cInfo->idTool, cInfo->idsModel[i], stiffness, 0.0, dist); + //} } + + std::cout << "m_rawIds: " << m_rawIds << std::endl; } diff --git a/src/InfinyToolkit/InteractionTools/GrasperJawModel.h b/src/InfinyToolkit/InteractionTools/GrasperJawModel.h index 155416b..ee29162 100644 --- a/src/InfinyToolkit/InteractionTools/GrasperJawModel.h +++ b/src/InfinyToolkit/InteractionTools/GrasperJawModel.h @@ -50,6 +50,11 @@ class SOFA_INFINYTOOLKIT_API GrasperJawModel : public BaseJawModel void activateImpl() override; void deActivateImpl() override; + /// Main API public method to launch the action of the Jaw + void performSecondaryAction() override; + /// Main API public method to stop the action of the Jaw + void stopSecondaryAction() override; + Data d_stiffness; protected: From 7d41eaea20c8d3f290490f815d91b1df27863335 Mon Sep 17 00:00:00 2001 From: epernod Date: Fri, 24 Jan 2025 16:50:21 +0100 Subject: [PATCH 2/7] [Backup] PoC work on burning+cutting directly inside the graspers --- .../ArticulatedToolManager.cpp | 93 +++++++++++++------ .../InteractionTools/ArticulatedToolManager.h | 7 ++ 2 files changed, 73 insertions(+), 27 deletions(-) diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp index 9af9a0b..ccea427 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp @@ -69,6 +69,8 @@ ArticulatedToolManager::ArticulatedToolManager() , d_isCutter(initData(&d_isCutter, false, "isCutter", "if true, will draw slices BB, ray and intersected triangles")) , d_isControlled(initData(&d_isControlled, false, "isControlled", "if true, will draw slices BB, ray and intersected triangles")) , d_drawContacts(initData(&d_drawContacts, false, "drawContacts", "if true, will draw slices BB, ray and intersected triangles")) + , d_manageBurning(initData(&d_manageBurning, false, "manageBurning", "if true, will draw slices BB, ray and intersected triangles")) + , m_vtexcoords(initData(&m_vtexcoords, "texcoords", "coordinates of the texture")) { this->f_listening.setValue(true); m_idgrabed.clear(); @@ -141,6 +143,15 @@ void ArticulatedToolManager::init() l_jawModel1->setTargetModel(l_targetModel.get()); l_jawModel2->setTargetModel(l_targetModel.get()); + + if (d_manageBurning.getValue()) + { + TetrahedronSetTopologyContainer* tetraCon; + l_targetModel->getContext()->get(tetraCon); + + m_vtexcoords.createTopologyHandler(tetraCon); + } + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); } @@ -224,6 +235,7 @@ bool ArticulatedToolManager::stopAction() int ArticulatedToolManager::performSecondaryAction() { m_performCut = true; + return 0; } @@ -233,8 +245,8 @@ int ArticulatedToolManager::performSecondaryAction() if (!d_isCutter.getValue()) return; - sofa::type::vector idGraps1 = l_jawModel1.get()->getRawContactModelIds(); - sofa::type::vector idGraps2 = l_jawModel2.get()->getRawContactModelIds(); + sofa::type::vector idVGrab1 = l_jawModel1.get()->getRawContactModelIds(); + sofa::type::vector idVGrab2 = l_jawModel2.get()->getRawContactModelIds(); // Detect all tetra on the cut path TetrahedronSetTopologyContainer* tetraCon; @@ -244,6 +256,43 @@ int ArticulatedToolManager::performSecondaryAction() return; } + std::set idVGrab; + for (auto id : idVGrab1) + { + idVGrab.insert(id); + } + + for (auto id : idVGrab2) + { + idVGrab.insert(id); + } + + if (m_cutCount < 10) + { + + helper::WriteAccessor< Data > texcoords = m_vtexcoords; + if (texcoords.empty()) + { + helper::WriteAccessor< Data > texcoords = m_vtexcoords; + texcoords.resize(tetraCon->getNbPoints()); + } + + float coef = float(m_cutCount + 1) / 10.f; + for (auto id : idVGrab) + { + texcoords[id][0] = coef; + texcoords[id][1] = coef; + } + + m_cutCount++; + return; + } + + + m_cutCount = 0; + + + TetrahedronSetTopologyModifier* tetraModif; tetraCon->getContext()->get(tetraModif); @@ -254,44 +303,34 @@ int ArticulatedToolManager::performSecondaryAction() // First get all tetra that are on the first side sofa::type::vector tetraIds; - for (auto id : idGraps1) + std::map< sofa::core::topology::Topology::TetrahedronID, int> tetraCounter; + for (auto id : idVGrab) { const BaseMeshTopology::TetrahedraAroundVertex& tetraAV = tetraCon->getTetrahedraAroundVertex(id); for (int j = 0; j < tetraAV.size(); ++j) { int tetraId = tetraAV[j]; - bool found = false; - for (int k = 0; k < tetraIds.size(); ++k) - if (tetraIds[k] == tetraId) - { - found = true; - break; - } - if (!found) - tetraIds.push_back(tetraId); + auto elem = tetraCounter.find(tetraId); + if (elem == tetraCounter.end()) // first time + { + tetraCounter[tetraId] = 1; + } + else + { + tetraCounter[tetraId] = elem->second + 1; + } } } - for (auto id : idGraps2) + for (auto elem : tetraCounter) { - const BaseMeshTopology::TetrahedraAroundVertex& tetraAV = tetraCon->getTetrahedraAroundVertex(id); - for (int j = 0; j < tetraAV.size(); ++j) + if (elem.second > 1) { - int tetraId = tetraAV[j]; - bool found = false; - for (int k = 0; k < tetraIds.size(); ++k) - if (tetraIds[k] == tetraId) - { - found = true; - break; - } - - if (!found) - tetraIds.push_back(tetraId); + tetraIds.push_back(elem.first); } } - + std::cout << "tetra2Rmove: " << tetraIds << std::endl; // remove springs first diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h index 93b4105..5d44d03 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h @@ -44,6 +44,9 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: using RigidCoord = sofa::defaulttype::RigidTypes::Coord; using ContactVector = type::vector; + using TexCoord = sofa::type::Vec<2, float>; + using VecTexCoord = type::vector; + protected: ArticulatedToolManager(); @@ -108,6 +111,9 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: Data d_isControlled; Data d_drawContacts; ///< if true, draw the collision outputs + Data d_manageBurning; ///< if true, draw the collision outputs + core::topology::PointData< VecTexCoord > m_vtexcoords; ///< coordinates of the texture + protected: // Buffer of points ids sofa::type::vector m_idgrabed; @@ -118,6 +124,7 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: BaseJawModel::SPtr m_jawModel2 = nullptr; bool m_performCut = false; + int m_cutCount = 0; }; From 52f94bd55411ea6890375ce1595037fc3efb2131 Mon Sep 17 00:00:00 2001 From: epernod Date: Fri, 21 Feb 2025 10:31:31 +0100 Subject: [PATCH 3/7] [backup] save new behavior of grasper with cutter. Need to clean the code as soon as demo has been validated --- .../ArticulatedToolManager.cpp | 75 ++++++++++++------- .../InteractionTools/ArticulatedToolManager.h | 10 ++- 2 files changed, 56 insertions(+), 29 deletions(-) diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp index ccea427..cd797a9 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp @@ -67,11 +67,13 @@ ArticulatedToolManager::ArticulatedToolManager() , d_handleFactor(initData(&d_handleFactor, SReal(1.0), "handleFactor", "jaw speed factor.")) , d_outputPositions(initData(&d_outputPositions, "outputPositions", "jaw positions.")) , d_isCutter(initData(&d_isCutter, false, "isCutter", "if true, will draw slices BB, ray and intersected triangles")) + , d_cutMaxStep(initData(&d_cutMaxStep, int(10), "cutMaxStep", "number of step before really cutting")) + , d_cutMode(initData(&d_cutMode, int(0), "cutMode", "mode of cut (debug)")) , d_isControlled(initData(&d_isControlled, false, "isControlled", "if true, will draw slices BB, ray and intersected triangles")) , d_drawContacts(initData(&d_drawContacts, false, "drawContacts", "if true, will draw slices BB, ray and intersected triangles")) , d_manageBurning(initData(&d_manageBurning, false, "manageBurning", "if true, will draw slices BB, ray and intersected triangles")) , m_vtexcoords(initData(&m_vtexcoords, "texcoords", "coordinates of the texture")) -{ +{ this->f_listening.setValue(true); m_idgrabed.clear(); } @@ -144,15 +146,24 @@ void ArticulatedToolManager::init() l_jawModel2->setTargetModel(l_targetModel.get()); + m_cutCount = 0; + + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); +} + + +void ArticulatedToolManager::bwdInit() +{ if (d_manageBurning.getValue()) { TetrahedronSetTopologyContainer* tetraCon; l_targetModel->getContext()->get(tetraCon); m_vtexcoords.createTopologyHandler(tetraCon); + + helper::WriteAccessor< Data > texcoords = m_vtexcoords; + texcoords.resize(tetraCon->getNbPoints()); } - - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); } @@ -232,16 +243,16 @@ bool ArticulatedToolManager::stopAction() } -int ArticulatedToolManager::performSecondaryAction() +bool ArticulatedToolManager::performSecondaryAction() { m_performCut = true; - return 0; + return true; } void ArticulatedToolManager::performCut() { - msg_warning() << "performSecondaryAction()"; + //msg_warning() << "performSecondaryAction()"; if (!d_isCutter.getValue()) return; @@ -267,28 +278,25 @@ int ArticulatedToolManager::performSecondaryAction() idVGrab.insert(id); } - if (m_cutCount < 10) + const int cutMax = d_cutMaxStep.getValue(); + float invC = 1.0 / float(cutMax); + if (m_cutCount < cutMax) { helper::WriteAccessor< Data > texcoords = m_vtexcoords; - if (texcoords.empty()) - { - helper::WriteAccessor< Data > texcoords = m_vtexcoords; - texcoords.resize(tetraCon->getNbPoints()); - } - - float coef = float(m_cutCount + 1) / 10.f; + float coef = float(m_cutCount) * invC; for (auto id : idVGrab) { texcoords[id][0] = coef; texcoords[id][1] = coef; } + } + if (m_cutCount < cutMax * 2) { m_cutCount++; return; } - m_cutCount = 0; @@ -323,11 +331,24 @@ int ArticulatedToolManager::performSecondaryAction() } } - for (auto elem : tetraCounter) + if (d_cutMode.getValue() == 0) + { + for (auto elem : tetraCounter) + { + if (elem.second > 0) + { + tetraIds.push_back(elem.first); + } + } + } + else { - if (elem.second > 1) + for (auto elem : tetraCounter) { - tetraIds.push_back(elem.first); + if (elem.second > 1) + { + tetraIds.push_back(elem.first); + } } } @@ -341,10 +362,10 @@ int ArticulatedToolManager::performSecondaryAction() return; } -int ArticulatedToolManager::stopSecondaryAction() +bool ArticulatedToolManager::stopSecondaryAction() { - msg_warning() << "stopSecondaryAction()"; - return 0; + //msg_warning() << "stopSecondaryAction()"; + return true; } @@ -407,8 +428,8 @@ void ArticulatedToolManager::filterCollision() sofa::core::CollisionModel* collMod1 = it->first.first; sofa::core::CollisionModel* collMod2 = it->first.second; - dmsg_warning() << "collMod1: " << collMod1->getTypeName() << " -> " << collMod1->getContext()->getName(); - dmsg_warning() << "collMod2: " << collMod2->getTypeName() << " -> " << collMod2->getContext()->getName(); + //dmsg_warning() << "collMod1: " << collMod1->getTypeName() << " -> " << collMod1->getContext()->getName(); + //dmsg_warning() << "collMod2: " << collMod2->getTypeName() << " -> " << collMod2->getContext()->getName(); // Get the number of contacts @@ -467,10 +488,10 @@ void ArticulatedToolManager::filterCollision() info->normal = c.normal; info->dist = c.value; - dmsg_info() << j << " contact: " << c.elem.first.getIndex() << " | " << c.elem.second.getIndex() - << " -> " << " pA: " << c.point[0] << " pB: " << c.point[1] - << " | normal: " << c.normal << " d: " << c.value - << " | cDir: " << (c.point[1] - c.point[0]).normalized() << " d: " << (c.point[1] - c.point[0]).norm(); + //dmsg_info() << j << " contact: " << c.elem.first.getIndex() << " | " << c.elem.second.getIndex() + // << " -> " << " pA: " << c.point[0] << " pB: " << c.point[1] + // << " | normal: " << c.normal << " d: " << c.value + // << " | cDir: " << (c.point[1] - c.point[0]).normalized() << " d: " << (c.point[1] - c.point[0]).norm(); if (firstJaw) l_jawModel1->addContact(info); diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h index 5d44d03..e83231e 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h @@ -54,6 +54,9 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: public: virtual void init() override; + + /// Sofa API init method of the component + void bwdInit() override; int testModels(); const sofa::type::vector< int >& vertexIdsInBroadPhase() { return m_idBroadPhase; } @@ -70,8 +73,8 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: int performAction(); bool stopAction(); - int performSecondaryAction(); - int stopSecondaryAction(); + bool performSecondaryAction(); + bool stopSecondaryAction(); void closeTool(); void openTool(); @@ -108,6 +111,8 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: Data > d_outputPositions; Data d_isCutter; + Data d_cutMaxStep; + Data d_cutMode; Data d_isControlled; Data d_drawContacts; ///< if true, draw the collision outputs @@ -124,6 +129,7 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel:: BaseJawModel::SPtr m_jawModel2 = nullptr; bool m_performCut = false; + int m_cutCount = 0; }; From 7e06b5961a6905033fbcc05e26ef1425b19e9f5c Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 15 Sep 2025 12:01:26 +0200 Subject: [PATCH 4/7] Small doc to trigger github action --- src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h index e83231e..c86306b 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h @@ -33,7 +33,9 @@ namespace sofa::infinytoolkit /** -* +* This class is a SOFA component, it will handle 2 different @sa BaseJawModel, one for the up jaw and another fot the bottom jaw in order +* to simulate grasper or scisors or any other tool defined by the jaws. +* This component will handle the motion, closing opening and activation of the tool. As well as collision detection in some cases. */ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel::BaseObject { From 7a0a1efb8f5ca5909c7930153fe35cc35d4360d8 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 15 Sep 2025 12:13:38 +0200 Subject: [PATCH 5/7] Fix linux compilation and clean some warnings --- .../ArticulatedToolManager.cpp | 20 ++++++++++--------- .../InteractionTools/BaseJawModel.h | 4 ++-- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp index cd797a9..b106f3d 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp @@ -315,7 +315,7 @@ bool ArticulatedToolManager::performSecondaryAction() for (auto id : idVGrab) { const BaseMeshTopology::TetrahedraAroundVertex& tetraAV = tetraCon->getTetrahedraAroundVertex(id); - for (int j = 0; j < tetraAV.size(); ++j) + for (sofa::Index j = 0; j < tetraAV.size(); ++j) { int tetraId = tetraAV[j]; @@ -425,12 +425,14 @@ void ArticulatedToolManager::filterCollision() const ContactVector* contacts = nullptr; for (core::collision::NarrowPhaseDetection::DetectionOutputMap::const_iterator it = detectionOutputs.begin(); it != detectionOutputs.end(); ++it) { - sofa::core::CollisionModel* collMod1 = it->first.first; - sofa::core::CollisionModel* collMod2 = it->first.second; - - //dmsg_warning() << "collMod1: " << collMod1->getTypeName() << " -> " << collMod1->getContext()->getName(); - //dmsg_warning() << "collMod2: " << collMod2->getTypeName() << " -> " << collMod2->getContext()->getName(); + if (this->f_printLog.getValue()) + { + sofa::core::CollisionModel* collMod1 = it->first.first; + sofa::core::CollisionModel* collMod2 = it->first.second; + msg_info() << "collMod1: " << collMod1->getTypeName() << " -> " << collMod1->getContext()->getName(); + msg_info() << "collMod2: " << collMod2->getTypeName() << " -> " << collMod2->getContext()->getName(); + } // Get the number of contacts contacts = dynamic_cast(it->second); @@ -642,9 +644,9 @@ void ArticulatedToolManager::draw(const core::visual::VisualParams* vparams) return; - auto m_model = l_targetModel.get(); - auto m_jaw1 = l_jawModel1.get()->l_jawDofs; - auto m_jaw2 = l_jawModel2.get()->l_jawDofs; + //auto m_model = l_targetModel.get(); + //auto m_jaw1 = l_jawModel1.get()->l_jawDofs; + //auto m_jaw2 = l_jawModel2.get()->l_jawDofs; if (d_drawContacts.getValue()) { diff --git a/src/InfinyToolkit/InteractionTools/BaseJawModel.h b/src/InfinyToolkit/InteractionTools/BaseJawModel.h index b6fdd4d..4b59cee 100644 --- a/src/InfinyToolkit/InteractionTools/BaseJawModel.h +++ b/src/InfinyToolkit/InteractionTools/BaseJawModel.h @@ -84,8 +84,8 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject virtual void addContact(GrabContactInfo* grabInfo); virtual void clearContacts(); - const sofa::type::vector& getContacts() { return m_contactInfos; } const - const sofa::type::vector& getRawContactModelIds() { return m_rawIds; } const + const sofa::type::vector& getContacts() const { return m_contactInfos; } + const sofa::type::vector& getRawContactModelIds() const { return m_rawIds; } void setTargetModel(sofa::core::behavior::BaseMechanicalState* model) { m_target = model; } virtual void drawImpl(const core::visual::VisualParams* vparams); From 1946a72c05cb1c24270b4f9632747839e884a9c4 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 15 Sep 2025 22:31:27 +0200 Subject: [PATCH 6/7] fix some compilation warnings --- .../InteractionTools/BaseJawModel.h | 2 +- .../InteractionTools/GrasperJawModel.cpp | 1 - .../InteractionTools/ScissorJawModel.cpp | 16 ++++++++-------- src/InfinyToolkit/PliersPositionsMapper.h | 2 +- 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/InfinyToolkit/InteractionTools/BaseJawModel.h b/src/InfinyToolkit/InteractionTools/BaseJawModel.h index 4b59cee..890c099 100644 --- a/src/InfinyToolkit/InteractionTools/BaseJawModel.h +++ b/src/InfinyToolkit/InteractionTools/BaseJawModel.h @@ -54,7 +54,7 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject using Vec3 = sofa::type::Vec3; BaseJawModel(); - virtual void init(); + virtual void init() override; virtual ~BaseJawModel() = default; int getModelId() { return m_modelId; } diff --git a/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp b/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp index 3f512fa..9554fa7 100644 --- a/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp +++ b/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp @@ -116,7 +116,6 @@ void GrasperJawModel::stopSecondaryAction() int GrasperJawModel::createStiffSpringFF() { std::cout << this->getName() << " + createStiffSpringFF()" << std::endl; - SReal stiffness = d_stiffness.getValue(); m_forcefield = sofa::core::objectmodel::New(dynamic_cast(m_jaw), dynamic_cast(m_target)); SpringFF* stiffspringforcefield = static_cast(m_forcefield.get()); diff --git a/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp b/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp index aedb5cd..9d1d3da 100644 --- a/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp +++ b/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp @@ -45,7 +45,7 @@ int ScissorJawModel::cutFromTetra(float minX, float maxX, bool cut) // Classify right/left points of the plier sofa::type::vector idsLeft; sofa::type::vector idsRight; - for (int i = 0; i < m_idBroadPhase.size(); i++) + for (sofa::Index i = 0; i < m_idBroadPhase.size(); i++) { Vec3 vert = Vec3(m_jaw->getPX(m_idBroadPhase[i]), m_jaw->getPY(m_idBroadPhase[i]), m_jaw->getPZ(m_idBroadPhase[i])); vert = m_matP * (vert - m_origin); @@ -92,8 +92,8 @@ int ScissorJawModel::cutFromTetra(float minX, float maxX, bool cut) } // First get all tetra that are on the first side - sofa::type::vector tetraIds; - for (int i = 0; i < idsLeft.size(); ++i) + sofa::type::vector tetraIds; + for (sofa::Index i = 0; i < idsLeft.size(); ++i) { const BaseMeshTopology::TetrahedraAroundVertex& tetraAV = tetraCon->getTetrahedraAroundVertex(idsLeft[i]); for (int j = 0; j < tetraAV.size(); ++j) @@ -118,7 +118,7 @@ int ScissorJawModel::cutFromTetra(float minX, float maxX, bool cut) // Then test for each tetra if one of the vertex is on the other side. If yes put on but path tetraIdsOnCut.clear(); std::set< unsigned int > items; - for (int i = 0; i < tetraIds.size(); ++i) + for (sofa::Index i = 0; i < tetraIds.size(); ++i) { const BaseMeshTopology::Tetra& tetra = tetraCon->getTetra(tetraIds[i]); for (unsigned int j = 0; j < 4; ++j) @@ -158,7 +158,7 @@ int ScissorJawModel::cutFromTetra(float minX, float maxX, bool cut) vitems.reserve(items.size()); vitems.insert(vitems.end(), items.rbegin(), items.rend()); - for (int i = 0; i < vitems.size(); i++) + for (sofa::Index i = 0; i < vitems.size(); i++) { sofa::type::vector its; its.push_back(vitems[i]); @@ -195,7 +195,7 @@ int ScissorJawModel::pathCutFromTetra(float minX, float maxX) return -40; } - for (int i = 0; i < tetraIds.size(); i++) + for (sofa::Index i = 0; i < tetraIds.size(); i++) { const BaseMeshTopology::Tetra& tetra = tetraCon->getTetra(tetraIds[i]); for (int j = 0; j < 4; j++) @@ -226,7 +226,7 @@ void ScissorJawModel::cutFromTriangles() // Classify right/left points of the plier sofa::type::vector idsLeft; sofa::type::vector idsRight; - for (int i = 0; i < m_idgrabed.size(); i++) + for (sofa::Index i = 0; i < m_idgrabed.size(); i++) { Vec3 vert = Vec3(m_model->getPX(m_idgrabed[i]), m_model->getPY(m_idgrabed[i]), m_model->getPZ(m_idgrabed[i])); vert = m_matP * (vert - m_origin); @@ -253,7 +253,7 @@ void ScissorJawModel::cutFromTriangles() } const sofa::type::vector& allTri = triCons[1]->getTriangleArray(); - for (int i = 0; i < allTri.size(); ++i) + for (sofa::Index i = 0; i < allTri.size(); ++i) { const BaseMeshTopology::Triangle& tri = allTri[i]; bool foundLeft = false; diff --git a/src/InfinyToolkit/PliersPositionsMapper.h b/src/InfinyToolkit/PliersPositionsMapper.h index 01da45c..9405c89 100644 --- a/src/InfinyToolkit/PliersPositionsMapper.h +++ b/src/InfinyToolkit/PliersPositionsMapper.h @@ -57,7 +57,7 @@ class SOFA_INFINYTOOLKIT_API PliersPositionsMapper: public sofa::core::DataEngin void draw(const core::visual::VisualParams* vparams) override; - void handleTopologyChange(); + void handleTopologyChange() override; /// Pre-construction check method called by ObjectFactory. /// Check that DataTypes matches the MeshTopology. From c594937f6f7769327b947cf92d4e6387212f10c6 Mon Sep 17 00:00:00 2001 From: epernod Date: Mon, 15 Sep 2025 22:42:51 +0200 Subject: [PATCH 7/7] fix mac compilation? --- src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp | 1 + src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp index b106f3d..cc3e78b 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp @@ -33,6 +33,7 @@ #include #include +#include #include diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h index c86306b..0b8ee1c 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h @@ -27,6 +27,7 @@ #include #include #include +#include namespace sofa::infinytoolkit {