Skip to content

Commit

Permalink
[Tools] backup work on secondary Action on the articulatedTool
Browse files Browse the repository at this point in the history
  • Loading branch information
@epernod
epernod committed Jan 20, 2025
1 parent 8fa9f56 commit efc82cc
Show file tree
Hide file tree
Showing 5 changed files with 219 additions and 20 deletions.
157 changes: 142 additions & 15 deletions src/InfinyToolkit/InteractionTools/ArticulatedToolManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

#include <sofa/core/objectmodel/KeypressedEvent.h>
#include <sofa/core/objectmodel/KeyreleasedEvent.h>
#include <sofa/simulation/AnimateEndEvent.h>

#include <sofa/component/topology/container/dynamic/TetrahedronSetTopologyContainer.h>
#include <sofa/component/topology/container/dynamic/TetrahedronSetTopologyModifier.h>
Expand Down Expand Up @@ -62,6 +63,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);
Expand Down Expand Up @@ -215,6 +218,93 @@ bool ArticulatedToolManager::stopAction()
}


int ArticulatedToolManager::performSecondaryAction()
{
m_performCut = true;
}


void ArticulatedToolManager::performCut()
{
msg_warning() << "performSecondaryAction()";
if (!d_isCutter.getValue())
return;

sofa::type::vector<int> idGraps1 = l_jawModel1.get()->getRawContactModelIds();
sofa::type::vector<int> 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<sofa::core::topology::Topology::TetrahedronID> 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()
{
Expand Down Expand Up @@ -275,8 +365,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
Expand All @@ -292,26 +382,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;
Expand All @@ -333,8 +441,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<KeypressedEvent *>(event);

switch (ev->getKey())
Expand All @@ -347,16 +465,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':
Expand Down
8 changes: 8 additions & 0 deletions src/InfinyToolkit/InteractionTools/ArticulatedToolManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,9 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel::
int performAction();
bool stopAction();

int performSecondaryAction();
int stopSecondaryAction();

void closeTool();
void openTool();

Expand All @@ -81,6 +84,8 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel::

void filterCollision();

void performCut();

public:
// Path to the different JawModel
SingleLink<ArticulatedToolManager, BaseJawModel, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_jawModel1;
Expand All @@ -99,6 +104,8 @@ class SOFA_INFINYTOOLKIT_API ArticulatedToolManager : public core::objectmodel::
Data <RigidCoord> d_inputPosition;

Data <type::vector<RigidCoord> > d_outputPositions;
Data<bool> d_isCutter;
Data<bool> d_isControlled;
Data<bool> d_drawContacts; ///< if true, draw the collision outputs

protected:
Expand All @@ -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;
};


Expand Down
13 changes: 11 additions & 2 deletions src/InfinyToolkit/InteractionTools/BaseJawModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand All @@ -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<GrabContactInfo*>& getContacts() { return m_contactInfos; }
const sofa::type::vector<GrabContactInfo*>& getContacts() { return m_contactInfos; } const
const sofa::type::vector<int>& getRawContactModelIds() { return m_rawIds; } const

void setTargetModel(sofa::core::behavior::BaseMechanicalState* model) { m_target = model; }
virtual void drawImpl(const core::visual::VisualParams* vparams);
Expand Down Expand Up @@ -121,6 +128,8 @@ class SOFA_INFINYTOOLKIT_API BaseJawModel : public core::objectmodel::BaseObject

/// List of contacts filter during collision
sofa::type::vector<GrabContactInfo*> m_contactInfos;

type::vector<int> m_rawIds;
};

} // namespace sofa::infinytoolkit
Expand Down
56 changes: 53 additions & 3 deletions src/InfinyToolkit/InteractionTools/GrasperJawModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
****************************************************************************/

#include <InfinyToolkit/InteractionTools/GrasperJawModel.h>

#include <sofa/core/ObjectFactory.h>

namespace sofa::infinytoolkit
Expand Down Expand Up @@ -82,10 +83,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;
Expand All @@ -109,13 +131,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;
}


Expand Down
5 changes: 5 additions & 0 deletions src/InfinyToolkit/InteractionTools/GrasperJawModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<SReal> d_stiffness;

protected:
Expand Down

0 comments on commit efc82cc

Please sign in to comment.