Skip to content

Commit

Permalink
backup work on new plier version
Browse files Browse the repository at this point in the history
  • Loading branch information
@epernod
epernod committed Jul 30, 2024
1 parent c7cc2e6 commit b031f7b
Show file tree
Hide file tree
Showing 2 changed files with 234 additions and 30 deletions.
230 changes: 208 additions & 22 deletions src/InfinyToolkit/PliersToolManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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();
Expand All @@ -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<core::collision::NarrowPhaseDetection>());
}

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()
Expand Down Expand Up @@ -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<const ContactVector*>(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
Expand Down Expand Up @@ -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':
Expand All @@ -828,31 +993,31 @@ 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;
}
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;
Expand All @@ -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));
Expand Down Expand Up @@ -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<Vec3> 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];
}
Expand Down
34 changes: 26 additions & 8 deletions src/InfinyToolkit/PliersToolManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@

#include <sofa/component/solidmechanics/spring/StiffSpringForceField.h>
#include <sofa/component/constraint/projective/AttachConstraint.h>
#include <sofa/core/collision/NarrowPhaseDetection.h>

namespace sofa::infinytoolkit
{
Expand All @@ -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
};

/**
*
*/
Expand All @@ -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<core::collision::DetectionOutput>;

protected:
PliersToolManager();
Expand Down Expand Up @@ -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<class T>
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<PliersToolManager, core::collision::NarrowPhaseDetection, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_detectionNP;

// Path to the different mechanicalObject
Data<std::string> m_pathMord1;
Data<std::string> m_pathMord2;
Data<std::string> m_pathModel;
Data<bool> d_drawContacts; ///< if true, draw the collision outputs

protected:
// Buffer of points ids
Expand Down Expand Up @@ -135,6 +150,9 @@ class SOFA_INFINYTOOLKIT_API PliersToolManager: public core::objectmodel::BaseOb
// Keep it for debug drawing
sofa::type::vector<sofa::core::topology::Topology::TetrahedronID> tetraIdsOnCut;
sofa::type::vector<sofa::core::topology::Topology::TriangleID> triIdsOnCut;

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


Expand Down

0 comments on commit b031f7b

Please sign in to comment.