From b031f7bb49cdabfcddb5af41aef500a6c64a9ac2 Mon Sep 17 00:00:00 2001 From: epernod Date: Tue, 30 Jul 2024 15:43:27 +0200 Subject: [PATCH] backup work on new plier version --- src/InfinyToolkit/PliersToolManager.cpp | 230 +++++++++++++++++++++--- src/InfinyToolkit/PliersToolManager.h | 34 +++- 2 files changed, 234 insertions(+), 30 deletions(-) diff --git a/src/InfinyToolkit/PliersToolManager.cpp b/src/InfinyToolkit/PliersToolManager.cpp index 01ad6b3..34354a9 100644 --- a/src/InfinyToolkit/PliersToolManager.cpp +++ b/src/InfinyToolkit/PliersToolManager.cpp @@ -67,6 +67,7 @@ PliersToolManager::PliersToolManager() , m_forcefieldDOWN(NULL) , m_oldCollisionStiffness(5000) , m_stiffness(500) + , d_drawContacts(initData(&d_drawContacts, false, "drawContacts", "if true, will draw slices BB, ray and intersected triangles")) { this->f_listening.setValue(true); m_idgrabed.clear(); @@ -81,6 +82,8 @@ PliersToolManager::~PliersToolManager() void PliersToolManager::init() { + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Loading); + const std::string& pathMord1 = m_pathMord1.getValue(); const std::string& pathMord2 = m_pathMord2.getValue(); const std::string& pathModel = m_pathModel.getValue(); @@ -105,7 +108,23 @@ void PliersToolManager::init() msg_info() << "m_mord2: " << m_mord2->getName(); msg_info() << "m_mord2: " << m_model->getName(); + + computeBoundingBox(); + + + // If no NarrowPhaseDetection is set using the link try to find the component + if (l_detectionNP.get() == nullptr) + { + l_detectionNP.set(getContext()->get()); + } + + if (l_detectionNP.get() == nullptr) { + msg_error() << "NarrowPhaseDetection not found"; + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + } + + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); } int PliersToolManager::testModels() @@ -195,6 +214,91 @@ void PliersToolManager::reinit() */ } + +void PliersToolManager::filterCollision() +{ + if (!this->isComponentStateValid()) + return; + + clearContacts(); + + // get the collision output + const core::collision::NarrowPhaseDetection::DetectionOutputMap& detectionOutputs = l_detectionNP.get()->getDetectionOutputs(); + if (detectionOutputs.size() == 0) // exit if no collision + { + return; + } + + // loop on the contact to get the one between the CarvingSurface and the CarvingTool collision model + 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_info() << "collMod1: " << collMod1->getTypeName() << " -> " << collMod1->getContext()->getName(); + dmsg_info() << "collMod1: " << collMod2->getTypeName() << " -> " << collMod2->getContext()->getName(); + + + // Get the number of contacts + contacts = dynamic_cast(it->second); + if (contacts == nullptr) + continue; + + size_t ncontacts = contacts->size(); + if (contacts->size() == 0) + continue; + + for (size_t j = 0; j < ncontacts; ++j) + { + // update the triangle id if a mapping is present + GrabContactInfo* info = new GrabContactInfo(); + + const ContactVector::value_type& c = (*contacts)[j]; + + if (c.elem.first.getCollisionModel()->getEnumType() == sofa::core::CollisionModel::TRIANGLE_TYPE) // first model is target model + { + info->idTool = c.elem.second.getIndex(); + sofa::Index idTri = c.elem.first.getIndex(); + info->idsModel = c.elem.first.getCollisionModel()->getCollisionTopology()->getTriangle(idTri); + } + else + { + + info->idTool = c.elem.first.getIndex(); + sofa::Index idTri = c.elem.second.getIndex(); + info->idsModel = c.elem.second.getCollisionModel()->getCollisionTopology()->getTriangle(idTri); + } + + info->normal = c.normal; + info->dist = c.value; + std::cout << "Type first: " << c.elem.first.getCollisionModel()->getEnumType() << std::endl; + std::cout << "Type second: " << c.elem.second.getCollisionModel()->getEnumType() << std::endl; + + + 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(); + + + m_contactInfos.push_back(info); + } + } +} + + +void PliersToolManager::clearContacts() +{ + for (unsigned int i = 0; i < m_contactInfos.size(); i++) + { + delete m_contactInfos[i]; + m_contactInfos[i] = nullptr; + } + m_contactInfos.clear(); +} + + void PliersToolManager::computeVertexIdsInBroadPhase(float margin) { // First compute boundingbox @@ -791,17 +895,78 @@ void PliersToolManager::handleEvent(sofa::core::objectmodel::Event* event) case 'T': case 't': { - releaseGrab(); + filterCollision(); + //releaseGrab(); - computeVertexIdsInBroadPhase(); - grabModel(); + //computeVertexIdsInBroadPhase(); + //grabModel(); break; } - case 'G': - case 'g': + case '0': + { + //releaseGrab(); + computeBoundingBox(); + break; + } + case '8': // Up + { + m_mord1->applyTranslation(0, 0.1, 0); + m_mord2->applyTranslation(0, 0.1, 0); + break; + } + case '5': // Down + { + m_mord1->applyTranslation(0, -0.1, 0); + m_mord2->applyTranslation(0, -0.1, 0); + break; + } + case '4': // Left + { + m_mord1->applyTranslation(-0.1, 0, 0); + m_mord2->applyTranslation(-0.1, 0, 0); + break; + } + case '6': // Right + { + m_mord1->applyTranslation(0.1, 0, 0); + m_mord2->applyTranslation(0.1, 0, 0); + break; + } + case '+': // forward + { + m_mord1->applyTranslation(0.0, 0, -0.1); + m_mord2->applyTranslation(0.0, 0, -0.1); + break; + } + case '-': // backward { - releaseGrab(); + m_mord1->applyTranslation(0.0, 0, 0.1); + m_mord2->applyTranslation(0.0, 0, 0.1); + break; + } + case '9': // turn right + { + m_mord1->applyRotation(0.0, 1.0, 0.0); + m_mord2->applyRotation(0.0, 1.0, 0.0); + break; + } + case '7': // turn left + { + m_mord1->applyRotation(0.0, -1.0, 0.0); + m_mord2->applyRotation(0.0, -1.0, 0.0); + break; + } + case '1': // turn right + { + m_mord1->applyRotation(1.0, 0.0, 0.0); + m_mord2->applyRotation(1.0, 0.0, 0.0); + break; + } + case '3': // turn left + { + m_mord1->applyRotation(-1.0, 0.0, 0.0); + m_mord2->applyRotation(-1.0, 0.0, 0.0); break; } case 'Y': @@ -828,15 +993,15 @@ void PliersToolManager::handleEvent(sofa::core::objectmodel::Event* event) case 'F': case 'f': { - releaseGrab(); + //releaseGrab(); - computeVertexIdsInBroadPhase(); - computePlierAxis(); - //cutFromTriangles(); - //for (int i=0; i<7; i++) - // cutFromTetra(i*2, i*2+2); + //computeVertexIdsInBroadPhase(); + //computePlierAxis(); + ////cutFromTriangles(); + ////for (int i=0; i<7; i++) + //// cutFromTetra(i*2, i*2+2); - cutFromTetra(0, 14, false); + //cutFromTetra(0, 14, false); break; @@ -844,15 +1009,15 @@ void PliersToolManager::handleEvent(sofa::core::objectmodel::Event* event) case 'D': case 'd': { - releaseGrab(); + //releaseGrab(); - computeVertexIdsInBroadPhase(); - computePlierAxis(); - //cutFromTriangles(); - /*for (int i=0; i<7; i++) - cutFromTetra(i*2, i*2+2);*/ + //computeVertexIdsInBroadPhase(); + //computePlierAxis(); + ////cutFromTriangles(); + ///*for (int i=0; i<7; i++) + // cutFromTetra(i*2, i*2+2);*/ - cutFromTetra(0, 14, true); + //cutFromTetra(0, 14, true); break; @@ -863,12 +1028,17 @@ void PliersToolManager::handleEvent(sofa::core::objectmodel::Event* event) void PliersToolManager::draw(const core::visual::VisualParams* vparams) { + if (!this->isComponentStateValid()) + return; + + if (!vparams->displayFlags().getShowBehaviorModels()) return; + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + sofa::type::RGBAColor color(0.2f, 1.0f, 1.0f, 1.0f); - vparams->drawTool()->drawLine(m_min, m_max, sofa::type::RGBAColor(1.0, 0.0, 1.0, 1.0)); - + vparams->drawTool()->drawBoundingBox(m_min, m_max); vparams->drawTool()->drawLine(zero, xAxis, sofa::type::RGBAColor(1.0, 0.0, 0.0, 0.0)); vparams->drawTool()->drawLine(zero, yAxis, sofa::type::RGBAColor(0.0, 1.0, 0.0, 0.0)); vparams->drawTool()->drawLine(zero, zAxis, sofa::type::RGBAColor(0.0, 0.0, 1.0, 0.0)); @@ -904,6 +1074,22 @@ void PliersToolManager::draw(const core::visual::VisualParams* vparams) vparams->drawTool()->drawTetrahedron(p0, p1, p2, p3, color); } + if (d_drawContacts.getValue()) + { + for (GrabContactInfo* cInfo : m_contactInfos) + { + std::vector vertices; + + vertices.push_back(Vec3(m_model->getPX(cInfo->idsModel[0]), m_model->getPY(cInfo->idsModel[0]), m_model->getPZ(cInfo->idsModel[0]))); + vertices.push_back(Vec3(m_model->getPX(cInfo->idsModel[1]), m_model->getPY(cInfo->idsModel[1]), m_model->getPZ(cInfo->idsModel[1]))); + vertices.push_back(Vec3(m_model->getPX(cInfo->idsModel[2]), m_model->getPY(cInfo->idsModel[2]), m_model->getPZ(cInfo->idsModel[2]))); + + sofa::type::RGBAColor color4(1.0f, 1.0, 0.0f, 1.0); + + vparams->drawTool()->drawLines(vertices, 1, color4); + } + } + // msg_info() << "drawLine: " << m_min[0] << " " << m_min[1] << " " << m_min[2]; } diff --git a/src/InfinyToolkit/PliersToolManager.h b/src/InfinyToolkit/PliersToolManager.h index b444bf0..f28f138 100644 --- a/src/InfinyToolkit/PliersToolManager.h +++ b/src/InfinyToolkit/PliersToolManager.h @@ -27,6 +27,7 @@ #include #include +#include namespace sofa::infinytoolkit { @@ -35,6 +36,20 @@ typedef sofa::component::solidmechanics::spring::StiffSpringForceField< sofa::de typedef sofa::component::solidmechanics::spring::StiffSpringForceField< sofa::defaulttype::Vec3Types > StiffSpringFF; typedef sofa::component::constraint::projective::AttachConstraint< sofa::defaulttype::Vec3Types > AttachConstraint; +using namespace sofa::defaulttype; +using namespace sofa::type; + +class GrabContactInfo +{ +public: + sofa::Index idTool; // in global mesh + sofa::core::topology::BaseMeshTopology::Triangle idsModel; // in global mesh + //Vec3 pointA; + //Vec3 pointB; + Vec3 normal; // equal to ||pB - pA|| + double dist; // equalt to (pB - pA).norm - contactDistance +}; + /** * */ @@ -44,6 +59,7 @@ class SOFA_INFINYTOOLKIT_API PliersToolManager: public core::objectmodel::BaseOb SOFA_CLASS(PliersToolManager,core::objectmodel::BaseObject); using Vec3 = sofa::type::Vec3; + using ContactVector = type::vector; protected: PliersToolManager(); @@ -91,22 +107,21 @@ class SOFA_INFINYTOOLKIT_API PliersToolManager: public core::objectmodel::BaseOb void draw(const core::visual::VisualParams* vparams) override; - /// Pre-construction check method called by ObjectFactory. - /// Check that DataTypes matches the MeshTopology. - template - static bool canCreate(T*& obj, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) - { - return BaseObject::canCreate(obj, context, arg); - } - protected: + void clearContacts(); + + void filterCollision(); public: + // link to the scene detection Method component (Narrow phase only) + SingleLink l_detectionNP; + // Path to the different mechanicalObject Data m_pathMord1; Data m_pathMord2; Data m_pathModel; + Data d_drawContacts; ///< if true, draw the collision outputs protected: // Buffer of points ids @@ -135,6 +150,9 @@ class SOFA_INFINYTOOLKIT_API PliersToolManager: public core::objectmodel::BaseOb // Keep it for debug drawing sofa::type::vector tetraIdsOnCut; sofa::type::vector triIdsOnCut; + + /// List of contacts filter during collision + sofa::type::vector m_contactInfos; };