diff --git a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp index 5b92e51..339458a 100644 --- a/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp +++ b/src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp @@ -483,44 +483,6 @@ void ArticulatedToolManager::draw(const core::visual::VisualParams* vparams) l_jawModel1->drawImpl(vparams); l_jawModel2->drawImpl(vparams); } - - //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()->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)); - - //if (m_model == nullptr) - // return; - - //for (unsigned int i = 0; i < m_idgrabed.size(); i++) - //{ - // SReal x = m_model->getPX(m_idgrabed[i]); - // SReal y = m_model->getPY(m_idgrabed[i]); - // SReal z = m_model->getPZ(m_idgrabed[i]); - // vparams->drawTool()->drawPoint(Vec3(x, y, z), sofa::type::RGBAColor(255.0, 0.0, 0.0, 1.0)); - //} - - - //TetrahedronSetTopologyContainer* tetraCon; - //m_model->getContext()->get(tetraCon); - //if (tetraCon == nullptr) { - // msg_info() << "Error: NO tetraCon"; - // return; - //} - - //for (unsigned int i = 0; i < tetraIdsOnCut.size(); i++) - //{ - // const BaseMeshTopology::Tetra& tetra = tetraCon->getTetra(tetraIdsOnCut[i]); - // - // Vec3 p0 = Vec3(m_model->getPX(tetra[0]), m_model->getPY(tetra[0]), m_model->getPZ(tetra[0])); - // Vec3 p1 = Vec3(m_model->getPX(tetra[1]), m_model->getPY(tetra[1]), m_model->getPZ(tetra[1])); - // Vec3 p2 = Vec3(m_model->getPX(tetra[2]), m_model->getPY(tetra[2]), m_model->getPZ(tetra[2])); - // Vec3 p3 = Vec3(m_model->getPX(tetra[3]), m_model->getPY(tetra[3]), m_model->getPZ(tetra[3])); - - // vparams->drawTool()->drawTetrahedron(p0, p1, p2, p3, color); - //} } } // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/InteractionTools/BaseJawModel.cpp b/src/InfinyToolkit/InteractionTools/BaseJawModel.cpp index 5c19dac..c3cb6f6 100644 --- a/src/InfinyToolkit/InteractionTools/BaseJawModel.cpp +++ b/src/InfinyToolkit/InteractionTools/BaseJawModel.cpp @@ -45,20 +45,18 @@ void BaseJawModel::init() msg_error() << "Error mechanical state not given"; sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); } + + if (initImpl()) + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + else + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); } bool BaseJawModel::computeBoundingBox() { if (m_jaw == nullptr) - { - msg_info() << "error mechanical state not found"; - const std::string& pathMord1 = l_jawDofs.getPath(); - this->getContext()->get(m_jaw, pathMord1); - - if (m_jaw == nullptr) - return false; - } + return false; for (int i = 0; i < 3; ++i) { @@ -119,36 +117,26 @@ void BaseJawModel::activeTool(bool value) } -void BaseJawModel::performAction() -{ - -} - -void BaseJawModel::stopAction() -{ - -} - void BaseJawModel::computeAxis() { - zero = Vec3(0, 0, 0); - xAxis = Vec3(1, 0, 0); - yAxis = Vec3(0, 1, 0); - zAxis = Vec3(0, 0, 1); + m_origin = Vec3(0, 0, 0); + m_xAxis = Vec3(1, 0, 0); + m_yAxis = Vec3(0, 1, 0); + m_zAxis = Vec3(0, 0, 1); if (m_jaw == nullptr) return; - zero = Vec3(m_jaw->getPX(0), m_jaw->getPY(0), m_jaw->getPZ(0)); - xAxis = Vec3(m_jaw->getPX(1), m_jaw->getPY(1), m_jaw->getPZ(1)); - yAxis = Vec3(m_jaw->getPX(20), m_jaw->getPY(20), m_jaw->getPZ(20)); - zAxis = Vec3(m_jaw->getPX(100), m_jaw->getPY(100), m_jaw->getPZ(100)); + m_origin = Vec3(m_jaw->getPX(0), m_jaw->getPY(0), m_jaw->getPZ(0)); + m_xAxis = Vec3(m_jaw->getPX(1), m_jaw->getPY(1), m_jaw->getPZ(1)); + m_yAxis = Vec3(m_jaw->getPX(20), m_jaw->getPY(20), m_jaw->getPZ(20)); + m_zAxis = Vec3(m_jaw->getPX(100), m_jaw->getPY(100), m_jaw->getPZ(100)); - Vec3 xDir = (xAxis - zero); xDir.normalize(); - Vec3 yDir = (yAxis - zero); yDir.normalize(); - Vec3 zDir = (zAxis - zero); zDir.normalize(); + Vec3 xDir = (m_xAxis - m_origin); xDir.normalize(); + Vec3 yDir = (m_yAxis - m_origin); yDir.normalize(); + Vec3 zDir = (m_zAxis - m_origin); zDir.normalize(); - matP = sofa::type::Mat3x3(xDir, yDir, zDir); + m_matP = sofa::type::Mat3x3(xDir, yDir, zDir); } diff --git a/src/InfinyToolkit/InteractionTools/BaseJawModel.h b/src/InfinyToolkit/InteractionTools/BaseJawModel.h index 298466b..ec0845e 100644 --- a/src/InfinyToolkit/InteractionTools/BaseJawModel.h +++ b/src/InfinyToolkit/InteractionTools/BaseJawModel.h @@ -58,17 +58,23 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject int getModelId() { return m_modelId; } - bool computeBoundingBox(); - + /// Main API public method to activate/deactivate tool. Will call @sa activateImpl or @sa deActivateImpl void activeTool(bool value); bool isToolActivated() { return m_isActivated; } - virtual void performAction(); - virtual void stopAction(); + /// Main API public method to launch the action of the Jaw + virtual void performAction() {} + /// Main API public method to stop the action of the Jaw + virtual void stopAction() {} + + /// Method to compute tool axis. Will fill @sa m_matP, @sa m_origin, @sa m_xAxis, @sa m_yAxis, @sa m_zAxis void computeAxis(); - void setAxis(sofa::type::Mat3x3 _matP) { matP = _matP; } - void setOrigin(Vec3 _zero) { zero = _zero; } + void setAxis(sofa::type::Mat3x3 _matP) { m_matP = _matP; } + void setOrigin(Vec3 _origin) { m_origin = _origin; } + + /// Method to compute BoundingBox, will fill @sa m_min, @sa m_max + bool computeBoundingBox(); virtual void addContact(GrabContactInfo* grabInfo); virtual void clearContacts(); @@ -78,32 +84,38 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject virtual void drawImpl(const core::visual::VisualParams* vparams); protected: + /// Internal API to init the component, will be called by @sa init() + virtual bool initImpl() { return true; } + + /// Internal API to activate/deActivate the jaw virtual void activateImpl() {} virtual void deActivateImpl() {} public: + /// Link to the Jaw controller (mechanicalOjbect linked to the restShapeSpringFF) SingleLink l_jawController; + /// Link to the Jaw current Dofs (mechanicalOjbect under the restShapeSpringFF) SingleLink l_jawDofs; + /// Link to the Jaw collision model (CollisionModel in the same node as the mechanicalObject(linked to the restShapeSpringFF) SingleLink l_jawCollision; protected: - // Buffer of points ids - sofa::type::vector m_idgrabed; - sofa::type::vector m_idBroadPhase; - int m_modelId = sofa::InvalidID; bool m_isActivated = false; - // Projection matrix to move into plier coordinate. X = along the plier, Y -> up, Z -> ortho to plier - sofa::type::Mat3x3 matP; - Vec3 zero; - Vec3 xAxis; - Vec3 yAxis; - Vec3 zAxis; + // Projection matrix to move into plier coordinate. X = along the plier, Y -> up, Z -> ortho to plier. + // Will be computed by computeAxis + sofa::type::Mat3x3 m_matP; + Vec3 m_origin; + Vec3 m_xAxis; + Vec3 m_yAxis; + Vec3 m_zAxis; + // Min Max value of the boundingBox sofa::type::Vec3 m_min, m_max; + sofa::core::behavior::BaseMechanicalState* m_jaw = nullptr; sofa::core::behavior::BaseMechanicalState* m_target = nullptr; diff --git a/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp b/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp index debb2b3..3bbf2aa 100644 --- a/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp +++ b/src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp @@ -40,6 +40,13 @@ GrasperJawModel::GrasperJawModel() } +bool GrasperJawModel::initImpl() +{ + //return (createStiffSpringFF() == 1); + return true; +} + + void GrasperJawModel::activateImpl() { sofa::core::CollisionModel* toolCol = l_jawCollision.get(); @@ -63,44 +70,25 @@ void GrasperJawModel::deActivateImpl() } toolCol->setActive(false); - - - //if (!m_forcefieldUP || !m_forcefieldDOWN) - // return false; - //m_idgrabed.clear(); - //m_idBroadPhase.clear(); - - //// Clear springs created during the grab - //StiffSpringFF* stiffspringforcefield_UP = static_cast(m_forcefieldUP.get()); - //stiffspringforcefield_UP->clear(); - - //StiffSpringFF* stiffspringforcefield_DOWN = static_cast(m_forcefieldDOWN.get()); - //stiffspringforcefield_DOWN->clear(); } void GrasperJawModel::performAction() { std::cout << "GrasperJawModel::performAction()" << std::endl; - deActivateImpl(); - createStiffSpringFF(); - addModelSprings(); - + deActivateImpl(); + addModelSprings(); } void GrasperJawModel::stopAction() { std::cout << "GrasperJawModel::stopAction()" << std::endl; + // clean springs + m_forcefield->clear(); + activateImpl(); } -const sofa::type::vector< int >& GrasperJawModel::grabModel() -{ - - //msg_info() << "Narrow Phase detection: " << m_idgrabed.size(); - return m_idgrabed; -} - void GrasperJawModel::releaseGrab() { //if (!m_forcefieldUP || !m_forcefieldDOWN) @@ -136,9 +124,8 @@ void GrasperJawModel::releaseGrab() int GrasperJawModel::createStiffSpringFF() { + std::cout << this->getName() << " + createStiffSpringFF()" << std::endl; SReal stiffness = d_stiffness.getValue(); - std::cout << "stiffness: " << stiffness << std::endl; - //m_stiffness = _stiffness; m_forcefield = sofa::core::objectmodel::New(dynamic_cast(m_jaw), dynamic_cast(m_target)); StiffSpringFF* stiffspringforcefield = static_cast(m_forcefield.get()); @@ -147,7 +134,7 @@ int GrasperJawModel::createStiffSpringFF() stiffspringforcefield->setStiffness(stiffness); stiffspringforcefield->setDamping(0); stiffspringforcefield->init(); - return -1001; + return 1; } diff --git a/src/InfinyToolkit/InteractionTools/GrasperJawModel.h b/src/InfinyToolkit/InteractionTools/GrasperJawModel.h index 9858b2a..8c7c300 100644 --- a/src/InfinyToolkit/InteractionTools/GrasperJawModel.h +++ b/src/InfinyToolkit/InteractionTools/GrasperJawModel.h @@ -51,11 +51,12 @@ class SOFA_INFINYTOOLKIT_API GrasperJawModel : public BaseJawModel Data d_stiffness; protected: + bool initImpl() override; int createStiffSpringFF(); void addModelSprings(); // API from grabing - const sofa::type::vector< int >& grabModel(); + //const sofa::type::vector< int >& grabModel(); void releaseGrab(); diff --git a/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp b/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp index 0b8e490..aedb5cd 100644 --- a/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp +++ b/src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp @@ -48,7 +48,7 @@ int ScissorJawModel::cutFromTetra(float minX, float maxX, bool cut) for (int 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 = matP * (vert - zero); + vert = m_matP * (vert - m_origin); if (vert[2] < -20.0 || vert[2] > 20.0) // outside on the borders continue; @@ -229,7 +229,7 @@ void ScissorJawModel::cutFromTriangles() for (int 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 = matP * (vert - zero); + vert = m_matP * (vert - m_origin); if (vert[0] < 0.0 || vert[0] > 8.0) continue; @@ -311,4 +311,27 @@ void ScissorJawModel::cutFromTriangles() } +// DrawImpl() +//{ +//TetrahedronSetTopologyContainer* tetraCon; + //m_model->getContext()->get(tetraCon); + //if (tetraCon == nullptr) { + // msg_info() << "Error: NO tetraCon"; + // return; + //} + + //for (unsigned int i = 0; i < tetraIdsOnCut.size(); i++) + //{ + // const BaseMeshTopology::Tetra& tetra = tetraCon->getTetra(tetraIdsOnCut[i]); + // + // Vec3 p0 = Vec3(m_model->getPX(tetra[0]), m_model->getPY(tetra[0]), m_model->getPZ(tetra[0])); + // Vec3 p1 = Vec3(m_model->getPX(tetra[1]), m_model->getPY(tetra[1]), m_model->getPZ(tetra[1])); + // Vec3 p2 = Vec3(m_model->getPX(tetra[2]), m_model->getPY(tetra[2]), m_model->getPZ(tetra[2])); + // Vec3 p3 = Vec3(m_model->getPX(tetra[3]), m_model->getPY(tetra[3]), m_model->getPZ(tetra[3])); + + // vparams->drawTool()->drawTetrahedron(p0, p1, p2, p3, color); + //} +//} + + } // namespace sofa::infinytoolkit diff --git a/src/InfinyToolkit/InteractionTools/ScissorJawModel.h b/src/InfinyToolkit/InteractionTools/ScissorJawModel.h index 7e60425..099c714 100644 --- a/src/InfinyToolkit/InteractionTools/ScissorJawModel.h +++ b/src/InfinyToolkit/InteractionTools/ScissorJawModel.h @@ -50,6 +50,11 @@ class SOFA_INFINYTOOLKIT_API ScissorJawModel : public BaseJawModel void cutFromTriangles(); private: + // Buffer of points ids + sofa::type::vector m_idgrabed; + sofa::type::vector m_idBroadPhase; + + // Keep it for debug drawing sofa::type::vector tetraIdsOnCut; sofa::type::vector triIdsOnCut;