Skip to content

Commit

Permalink
test with collision model
Browse files Browse the repository at this point in the history
  • Loading branch information
@bakpaul
bakpaul committed Mar 27, 2024
1 parent a263da6 commit 0b0af48
Show file tree
Hide file tree
Showing 3 changed files with 226 additions and 9 deletions.
101 changes: 101 additions & 0 deletions scenes/iGTLinkMouseInteractor.scn
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@
<?xml version="1.0"?>

<Node name="root" dt="0.01" gravity="0 0 0">

<RequiredPlugin pluginName='SofaPreconditioner'/> <!-- Needed to use components [ShewchukPCGLinearSolver, ]-->
<RequiredPlugin pluginName='SofaSparseSolver'/> <!-- Needed to use components [SparseLUSolver, ]-->
<RequiredPlugin pluginName='SofaOpenglVisual'/> <!-- Needed to use components [OglModel, ]-->
<RequiredPlugin name='SofaConstraint'/> <!-- Needed to use components [LinearSolverConstraintCorrection, ]-->
<RequiredPlugin name='SofaGeneralSimpleFem'/> <!-- Needed to use components [BeamFEMForceField, ]-->
<RequiredPlugin name='SofaImplicitOdeSolver'/> <!-- Needed to use components [EulerImplicitSolver, ]-->
<RequiredPlugin name='SofaMiscMapping'/> <!-- Needed to use components [BeamLinearMapping, TubularMapping, ]-->
<RequiredPlugin name='SofaTopologyMapping'/> <!-- Needed to use components [Edge2QuadTopologicalMapping, ]-->
<RequiredPlugin name="Sofa.Component.Constraint.Projective"/> <!-- Needed to use components [FixedConstraint] -->
<RequiredPlugin name="Sofa.Component.Engine.Select"/> <!-- Needed to use components [BoxROI] -->
<RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [SparseLDLSolver] -->
<RequiredPlugin name="Sofa.Component.Mapping.Linear"/> <!-- Needed to use components [IdentityMapping] -->
<RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [UniformMass] -->
<RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
<RequiredPlugin name="Sofa.Component.SolidMechanics.FEM.Elastic"/> <!-- Needed to use components [TetrahedronFEMForceField] -->
<RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
<RequiredPlugin name="Sofa.Component.Topology.Container.Dynamic"/> <!-- Needed to use components [TetrahedronSetTopologyContainer,TetrahedronSetTopologyModifier,TriangleSetTopologyContainer,TriangleSetTopologyModifier] -->
<RequiredPlugin name="Sofa.Component.Topology.Container.Grid"/> <!-- Needed to use components [RegularGridTopology] -->
<RequiredPlugin name="Sofa.Component.Topology.Mapping"/> <!-- Needed to use components [Hexa2TetraTopologicalMapping,Tetra2TriangleTopologicalMapping] -->
<RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualStyle] -->
<RequiredPlugin name="Sofa.GL.Component.Rendering3D"/> <!-- Needed to use components [OglModel] -->
<RequiredPlugin name="SofaIGTLink"/> <!-- Needed to use components [OglModel] -->


<VisualStyle displayFlags=" showForceFields "/>
<DefaultAnimationLoop />




<!-- $$$$$$$$$$$$$$$$$$$$$$ TOPOLOGY 1 $$$$$$$$$$$$$$$$$$$$$$ -->
<Node name="BEAMVOLUME">
<RegularGrid name="HexaTop" n="25 5 5" min="0 0 0" max="0.5 0.1 0.1"/>
<TetrahedronSetTopologyContainer name="Container" position="@HexaTop.position"/>
<TetrahedronSetTopologyModifier name="Modifier"/>
<Hexa2TetraTopologicalMapping input="@HexaTop" output="@Container" swapping="true"/>
</Node>



<Node name="FEM">
<EulerImplicitSolver firstOrder="false" rayleighMass="0.1" rayleighStiffness="0.1"/>
<PCGLinearSolver preconditioner="@precond"/>
<SparseLDLSolver name="precond" template="CompressedRowSparseMatrixMat3x3" parallelInverseProduct="true" />

<TetrahedronSetTopologyContainer name="Container" position="@../BEAMVOLUME/HexaTop.position"
tetrahedra="@../BEAMVOLUME/Container.tetrahedra"/>
<TetrahedronSetTopologyModifier name="Modifier"/>

<MechanicalObject name="mstate" template="Vec3d" src="@Container"/>
<TetrahedronFEMForceField name="forceField" listening="true" youngModulus="6e4" poissonRatio="0.40" />


<BoxROI name="box" box="-0.01 -0.01 -0.01 0.01 0.11 0.11"/>
<FixedConstraint indices="@box.indices"/>

<UniformMass totalMass="1"/>

<Node name="Surface">
<TriangleSetTopologyContainer name="Container"/>
<TriangleSetTopologyModifier name="Modifier"/>
<Tetra2TriangleTopologicalMapping input="@../Container" output="@Container" flipNormals="false"/>
<MechanicalObject name="dofs" rest_position="@../mstate.rest_position"/>
<TriangleCollisionModel name="Torus8CMT" proximity="0.001" contactStiffness="20" color="0.94117647058824 0.93725490196078 0.89411764705882" />
<Node name="Visual" activated="1">
<TriangleSetTopologyContainer name="Container" src="@../Container"/>
<OglModel color="1.0 0.1 0.2 1.0" name="visualModel"/>
<IdentityMapping name="VisualMapping"/>
</Node>
<IdentityMapping name="SurfaceMapping"/>
</Node>

<Node name="AttachPoint">
<PointSetTopologyContainer name="Container" />
<PointSetTopologyModifier name="Modifier"/>
<MechanicalObject name="mstate" template="Vec3d" position="0.5 0.05 0.05" drawMode="1" showObjectScale="0.01" showColor="0 1 0" showObject="false"/>

<UserInteractionController name="UIC" inStiffness="10000" reactionTime="50" position="@../../AttachPointMessage.points" />
<RestShapeSpringsForceField stiffness='@UIC.outStiffness' external_rest_shape='@../../AttachPoint/mstate' points='0' external_points='0'/>

<BarycentricMapping />
</Node>


</Node>

<Node name="AttachPoint">
<PointSetTopologyContainer name="Container" />
<PointSetTopologyModifier name="Modifier"/>
<MechanicalObject name="mstate" template="Vec3d" position="0.5 0.05 0.05" drawMode="2" showObjectScale="0.01" showObject="false"/>
<iGTLinkMouseInteractor pickingType="spring" position="0 0 0" reactionTime="2000" destCollisionModel="@../FEM/Surface/Torus8CMT"/>

</Node>

</Node>


17 changes: 15 additions & 2 deletions src/sofa/igtlink/utils/iGTLinkMouseInteractor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,17 @@
#include <sofa/gui/component/config.h>

#include <sofa/gui/component/performer/InteractionPerformer.h>
#include <sofa/gui/component/performer/BaseAttachBodyPerformer.h>
#include <sofa/component/collision/geometry/RayModel.h>
#include <sofa/component/statecontainer/MechanicalObject.h>

#include <sofa/core/collision/DetectionOutput.h>
#include <sofa/core/BehaviorModel.h>
#include <sofa/gui/component/performer/MouseInteractor.h>
#include <sofa/gui/component/AttachBodyButtonSetting.h>
#include <sofa/helper/OptionsGroup.h>
#include <sofa/core/objectmodel/DataCallback.h>
#include <sofa/core/CollisionModel.h>
#include <chrono>


Expand All @@ -19,23 +22,33 @@ class iGTLinkMouseInteractor : public sofa::gui::component::performer::MouseInte
public:
SOFA_CLASS(iGTLinkMouseInteractor, SOFA_TEMPLATE(sofa::gui::component::performer::MouseInteractor, defaulttype::Vec3Types));

Data<type::Vec3> d_positions;
Data<sofa::type::vector<type::Vec3> > d_positions;
Data< sofa::helper::OptionsGroup > d_pickingType;
Data< double > d_springStiffness;
Data< unsigned > d_reactionTime;

SingleLink<iGTLinkMouseInteractor, core::CollisionModel, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_destCollisionModel;


iGTLinkMouseInteractor();

void updatePosition( SReal dt) override;
virtual void updatePosition( SReal dt) override;

void positionChanged();
void attachmentChanged();
void startPerformer();
void stopPerformer();
void handleEvent(sofa::core::objectmodel::Event *event);
sofa::Index findCollidingElem(const type::Vec3& pos) const;


private:
sofa::core::objectmodel::DataCallback c_positions;
sofa::core::objectmodel::DataCallback c_attachmentType;
std::chrono::high_resolution_clock::time_point m_lastChange;
std::unique_ptr<sofa::gui::component::performer::BaseAttachBodyPerformer> m_performer;

bool m_performerStarted;



Expand Down
117 changes: 110 additions & 7 deletions src/sofa/igtlink/utils/iGTLinkMouseInteractor.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,12 @@

#include <sofa/igtlink/utils/iGTLinkMouseInteractor.h>
#include <sofa/simulation/AnimateBeginEvent.h>
#include <sofa/simulation/AnimateEndEvent.h>
#include <sofa/gui/component/performer/ConstraintAttachBodyPerformer.h>
#include <sofa/gui/component/performer/AttachBodyPerformer.h>
#include <sofa/component/collision/geometry/LineModel.h>
#include <sofa/component/collision/geometry/TriangleModel.h>
#include <sofa/component/collision/geometry/PointModel.h>


namespace sofa::openigtlink
Expand All @@ -12,6 +18,9 @@ iGTLinkMouseInteractor::iGTLinkMouseInteractor()
, d_positions(initData(&d_positions, "position", "Position"))
, d_springStiffness(initData(&d_springStiffness,10.0,"springStiffness","Stiffness of attachment spring used for interaction"))
, d_reactionTime(initData(&d_reactionTime,20u, "reactionTime", "Time in milisecond of no change in the position to output a null stiffness"))
, l_destCollisionModel(initLink( "destCollisionModel", "Link to the topology to attach"))
, m_performer(nullptr)
, m_performerStarted(false)
{
sofa::helper::OptionsGroup m_newoptiongroup{"constraint","spring"};
m_newoptiongroup.setSelectedItem("spring");
Expand All @@ -38,31 +47,125 @@ void iGTLinkMouseInteractor::positionChanged()

void iGTLinkMouseInteractor::attachmentChanged()
{
//Change performer
if(m_performer)
m_performer->clear();
if(d_pickingType.getValue().getSelectedItem() == std::string("constraint"))
m_performer.reset(new sofa::gui::component::performer::ConstraintAttachBodyPerformer<defaulttype::Vec3Types>(this));
else
m_performer.reset(new sofa::gui::component::performer::AttachBodyPerformer<defaulttype::Vec3Types>(this));

}


void iGTLinkMouseInteractor::updatePosition( SReal dt)
{
SOFA_UNUSED(dt);
//Do nothing
//Do nothing, see handleEvent
}

void iGTLinkMouseInteractor::startPerformer()
{
if(!m_performer || m_performerStarted || !d_positions.getValue().size())
return;

sofa::gui::component::performer::BodyPicked bodyPicked;
bodyPicked.indexCollisionElement = findCollidingElem(d_positions.getValue()[0]);
bodyPicked.mstate = l_destCollisionModel->getContext()->get<sofa::core::behavior::BaseMechanicalState>();
bodyPicked.point = d_positions.getValue()[0];
// bodyPicked.body = l_destCollisionModel.get();
this->setBodyPicked(bodyPicked);
this->setMouseAttached(true);

m_performer->start_partial(bodyPicked);

m_performerStarted = true;
}


void iGTLinkMouseInteractor::stopPerformer()
{
if(!m_performer || !m_performerStarted)
return;
m_performer->clear();
this->setMouseAttached(false);

m_performerStarted = false;
}

sofa::Index iGTLinkMouseInteractor::findCollidingElem(const type::Vec3& pos) const
{
sofa::Index closestElem = l_destCollisionModel->begin().getIndex();
double closestDist = std::numeric_limits<double>::max();
auto* topo = l_destCollisionModel->getCollisionTopology();
auto* mstate = l_destCollisionModel->getContext()->get<sofa::core::behavior::MechanicalState<defaulttype::Vec3Types> >();
sofa::helper::ReadAccessor<Data <defaulttype::Vec3Types::VecCoord> > destPositions = mstate->read(core::VecCoordId::position());

if(dynamic_cast<sofa::component::collision::geometry::PointCollisionModel<defaulttype::Vec3Types>* >(l_destCollisionModel.get()))
{
for(unsigned id = 0; id < destPositions.size(); ++id)
{
double dist = (pos - destPositions[id]).norm();
if(dist<closestDist)
{
closestDist = dist;
closestElem = id;
}
}
}
else if(dynamic_cast<sofa::component::collision::geometry::LineCollisionModel<defaulttype::Vec3Types>* >(l_destCollisionModel.get()))
{
for(unsigned id = 0; id < topo->getLines().size(); ++id)
{
double dist = (pos - (destPositions[topo->getLines()[id][0]] + destPositions[topo->getLines()[id][1]])/2.0).norm();
if(dist<closestDist)
{
closestDist = dist;
closestElem = id;
}
}
}
else if(dynamic_cast<sofa::component::collision::geometry::TriangleCollisionModel<defaulttype::Vec3Types>* >(l_destCollisionModel.get()))
{
for(unsigned id = 0; id < topo->getTriangles().size(); ++id)
{
double dist = (pos - (destPositions[topo->getTriangles()[id][0]] + destPositions[topo->getTriangles()[id][1]] + destPositions[topo->getTriangles()[id][2]])/3.0).norm();
if(dist<closestDist)
{
closestDist = dist;
closestElem = id;
}
}
}
return closestElem;
}


void iGTLinkMouseInteractor::handleEvent(sofa::core::objectmodel::Event *event)
{
if (dynamic_cast<sofa::simulation::AnimateBeginEvent*>(event))
if (dynamic_cast<sofa::simulation::AnimateEndEvent*>(event))
{
if(!m_performer)
attachmentChanged();

std::chrono::high_resolution_clock::time_point now = std::chrono::high_resolution_clock::now();
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - m_lastChange).count() > d_reactionTime.getValue())
{
//Deactivate the constraint
//Reproduce the start emthod from performer here.
stopPerformer();
}
else
{
//Activate the constraint
//Clear the performer
if(!d_positions.getValue().size())
return;
if(!m_performerStarted)
startPerformer();

auto* mstate = this->getContext()->get<sofa::core::behavior::MechanicalState<defaulttype::Vec3Types> >();
sofa::helper::WriteAccessor<Data <defaulttype::Vec3Types::VecCoord> > positions = mstate->write(core::VecCoordId::position());

mstate->resize(1);

positions.resize(1);
positions[0] = d_positions.getValue()[0];
}
}
}
Expand Down

0 comments on commit 0b0af48

Please sign in to comment.