From 0b0af4872d269c90737144c3b70315fbead4deb6 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Wed, 27 Mar 2024 17:50:43 +0100 Subject: [PATCH] test with collision model --- scenes/iGTLinkMouseInteractor.scn | 101 +++++++++++++++ .../igtlink/utils/iGTLinkMouseInteractor.h | 17 ++- .../igtlink/utils/iGTLinkMouseInteractor.inl | 117 ++++++++++++++++-- 3 files changed, 226 insertions(+), 9 deletions(-) create mode 100755 scenes/iGTLinkMouseInteractor.scn diff --git a/scenes/iGTLinkMouseInteractor.scn b/scenes/iGTLinkMouseInteractor.scn new file mode 100755 index 0000000..ea11597 --- /dev/null +++ b/scenes/iGTLinkMouseInteractor.scn @@ -0,0 +1,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sofa/igtlink/utils/iGTLinkMouseInteractor.h b/src/sofa/igtlink/utils/iGTLinkMouseInteractor.h index ea5a4a1..dcf39e9 100644 --- a/src/sofa/igtlink/utils/iGTLinkMouseInteractor.h +++ b/src/sofa/igtlink/utils/iGTLinkMouseInteractor.h @@ -2,14 +2,17 @@ #include #include +#include #include #include #include #include #include +#include #include #include +#include #include @@ -19,23 +22,33 @@ class iGTLinkMouseInteractor : public sofa::gui::component::performer::MouseInte public: SOFA_CLASS(iGTLinkMouseInteractor, SOFA_TEMPLATE(sofa::gui::component::performer::MouseInteractor, defaulttype::Vec3Types)); - Data d_positions; + Data > d_positions; Data< sofa::helper::OptionsGroup > d_pickingType; Data< double > d_springStiffness; Data< unsigned > d_reactionTime; + SingleLink l_destCollisionModel; + + iGTLinkMouseInteractor(); - void updatePosition( SReal dt) override; + virtual void updatePosition( SReal dt) override; void positionChanged(); void attachmentChanged(); + void startPerformer(); + void stopPerformer(); void handleEvent(sofa::core::objectmodel::Event *event); + sofa::Index findCollidingElem(const type::Vec3& pos) const; + private: sofa::core::objectmodel::DataCallback c_positions; sofa::core::objectmodel::DataCallback c_attachmentType; std::chrono::high_resolution_clock::time_point m_lastChange; + std::unique_ptr m_performer; + + bool m_performerStarted; diff --git a/src/sofa/igtlink/utils/iGTLinkMouseInteractor.inl b/src/sofa/igtlink/utils/iGTLinkMouseInteractor.inl index 96b76ae..1429e9b 100644 --- a/src/sofa/igtlink/utils/iGTLinkMouseInteractor.inl +++ b/src/sofa/igtlink/utils/iGTLinkMouseInteractor.inl @@ -2,6 +2,12 @@ #include #include +#include +#include +#include +#include +#include +#include namespace sofa::openigtlink @@ -12,6 +18,9 @@ iGTLinkMouseInteractor::iGTLinkMouseInteractor() , d_positions(initData(&d_positions, "position", "Position")) , d_springStiffness(initData(&d_springStiffness,10.0,"springStiffness","Stiffness of attachment spring used for interaction")) , d_reactionTime(initData(&d_reactionTime,20u, "reactionTime", "Time in milisecond of no change in the position to output a null stiffness")) +, l_destCollisionModel(initLink( "destCollisionModel", "Link to the topology to attach")) +, m_performer(nullptr) +, m_performerStarted(false) { sofa::helper::OptionsGroup m_newoptiongroup{"constraint","spring"}; m_newoptiongroup.setSelectedItem("spring"); @@ -38,31 +47,125 @@ void iGTLinkMouseInteractor::positionChanged() void iGTLinkMouseInteractor::attachmentChanged() { - //Change performer + if(m_performer) + m_performer->clear(); + if(d_pickingType.getValue().getSelectedItem() == std::string("constraint")) + m_performer.reset(new sofa::gui::component::performer::ConstraintAttachBodyPerformer(this)); + else + m_performer.reset(new sofa::gui::component::performer::AttachBodyPerformer(this)); + } void iGTLinkMouseInteractor::updatePosition( SReal dt) { SOFA_UNUSED(dt); - //Do nothing + //Do nothing, see handleEvent +} + +void iGTLinkMouseInteractor::startPerformer() +{ + if(!m_performer || m_performerStarted || !d_positions.getValue().size()) + return; + + sofa::gui::component::performer::BodyPicked bodyPicked; + bodyPicked.indexCollisionElement = findCollidingElem(d_positions.getValue()[0]); + bodyPicked.mstate = l_destCollisionModel->getContext()->get(); + bodyPicked.point = d_positions.getValue()[0]; +// bodyPicked.body = l_destCollisionModel.get(); + this->setBodyPicked(bodyPicked); + this->setMouseAttached(true); + + m_performer->start_partial(bodyPicked); + + m_performerStarted = true; +} + + +void iGTLinkMouseInteractor::stopPerformer() +{ + if(!m_performer || !m_performerStarted) + return; + m_performer->clear(); + this->setMouseAttached(false); + + m_performerStarted = false; +} + +sofa::Index iGTLinkMouseInteractor::findCollidingElem(const type::Vec3& pos) const +{ + sofa::Index closestElem = l_destCollisionModel->begin().getIndex(); + double closestDist = std::numeric_limits::max(); + auto* topo = l_destCollisionModel->getCollisionTopology(); + auto* mstate = l_destCollisionModel->getContext()->get >(); + sofa::helper::ReadAccessor > destPositions = mstate->read(core::VecCoordId::position()); + + if(dynamic_cast* >(l_destCollisionModel.get())) + { + for(unsigned id = 0; id < destPositions.size(); ++id) + { + double dist = (pos - destPositions[id]).norm(); + if(dist* >(l_destCollisionModel.get())) + { + for(unsigned id = 0; id < topo->getLines().size(); ++id) + { + double dist = (pos - (destPositions[topo->getLines()[id][0]] + destPositions[topo->getLines()[id][1]])/2.0).norm(); + if(dist* >(l_destCollisionModel.get())) + { + for(unsigned id = 0; id < topo->getTriangles().size(); ++id) + { + double dist = (pos - (destPositions[topo->getTriangles()[id][0]] + destPositions[topo->getTriangles()[id][1]] + destPositions[topo->getTriangles()[id][2]])/3.0).norm(); + if(dist(event)) + if (dynamic_cast(event)) { + if(!m_performer) + attachmentChanged(); + std::chrono::high_resolution_clock::time_point now = std::chrono::high_resolution_clock::now(); if (std::chrono::duration_cast(now - m_lastChange).count() > d_reactionTime.getValue()) { - //Deactivate the constraint - //Reproduce the start emthod from performer here. + stopPerformer(); } else { - //Activate the constraint - //Clear the performer + if(!d_positions.getValue().size()) + return; + if(!m_performerStarted) + startPerformer(); + + auto* mstate = this->getContext()->get >(); + sofa::helper::WriteAccessor > positions = mstate->write(core::VecCoordId::position()); + + mstate->resize(1); + + positions.resize(1); + positions[0] = d_positions.getValue()[0]; } } }