Skip to content

Commit

Permalink
[tools] Some code cleaning
Browse files Browse the repository at this point in the history
  • Loading branch information
@epernod
epernod committed Jul 31, 2024
1 parent 52a7063 commit d45fde6
Show file tree
Hide file tree
Showing 7 changed files with 92 additions and 114 deletions.
38 changes: 0 additions & 38 deletions src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
48 changes: 18 additions & 30 deletions src/InfinyToolkit/InteractionTools/BaseJawModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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);
}


Expand Down
44 changes: 28 additions & 16 deletions src/InfinyToolkit/InteractionTools/BaseJawModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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<BaseJawModel, sofa::core::behavior::BaseMechanicalState, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_jawController;
/// Link to the Jaw current Dofs (mechanicalOjbect under the restShapeSpringFF)
SingleLink<BaseJawModel, sofa::core::behavior::BaseMechanicalState, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_jawDofs;
/// Link to the Jaw collision model (CollisionModel in the same node as the mechanicalObject(linked to the restShapeSpringFF)
SingleLink<BaseJawModel, sofa::core::CollisionModel, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_jawCollision;


protected:
// Buffer of points ids
sofa::type::vector <int> m_idgrabed;
sofa::type::vector <int> 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;

Expand Down
41 changes: 14 additions & 27 deletions src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,13 @@ GrasperJawModel::GrasperJawModel()
}


bool GrasperJawModel::initImpl()
{
//return (createStiffSpringFF() == 1);
return true;
}


void GrasperJawModel::activateImpl()
{
sofa::core::CollisionModel* toolCol = l_jawCollision.get();
Expand All @@ -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<StiffSpringFF*>(m_forcefieldUP.get());
//stiffspringforcefield_UP->clear();

//StiffSpringFF* stiffspringforcefield_DOWN = static_cast<StiffSpringFF*>(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)
Expand Down Expand Up @@ -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<StiffSpringFF>(dynamic_cast<mechaState*>(m_jaw), dynamic_cast<mechaState*>(m_target));
StiffSpringFF* stiffspringforcefield = static_cast<StiffSpringFF*>(m_forcefield.get());
Expand All @@ -147,7 +134,7 @@ int GrasperJawModel::createStiffSpringFF()
stiffspringforcefield->setStiffness(stiffness);
stiffspringforcefield->setDamping(0);
stiffspringforcefield->init();
return -1001;
return 1;
}


Expand Down
3 changes: 2 additions & 1 deletion src/InfinyToolkit/InteractionTools/GrasperJawModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,11 +51,12 @@ class SOFA_INFINYTOOLKIT_API GrasperJawModel : public BaseJawModel
Data<SReal> 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();

Expand Down
27 changes: 25 additions & 2 deletions src/InfinyToolkit/InteractionTools/ScissorJawModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
5 changes: 5 additions & 0 deletions src/InfinyToolkit/InteractionTools/ScissorJawModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,11 @@ class SOFA_INFINYTOOLKIT_API ScissorJawModel : public BaseJawModel
void cutFromTriangles();

private:
// Buffer of points ids
sofa::type::vector <int> m_idgrabed;
sofa::type::vector <int> m_idBroadPhase;


// Keep it for debug drawing
sofa::type::vector<TetrahedronID> tetraIdsOnCut;
sofa::type::vector<TriangleID> triIdsOnCut;
Expand Down

0 comments on commit d45fde6

Please sign in to comment.