Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[InterventionalRadiologyController] Clean method applyInterventionalRadiologyController #73

Merged
merged 4 commits into from
Dec 28, 2022
Merged

[InterventionalRadiologyController] Clean method applyInterventionalRadiologyController #73

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0119797
backup work on applyInterventionalRadiologyController()
epernod Oct 5, 2022
e382b91
Backup fix on totalLength changed
epernod Oct 13, 2022
4555db4
Merge branch 'sofa-framework:master' into IRCtrl_applyController
epernod Oct 19, 2022
5646088
[IRCtrl] replace hardcoded values by epsilon
epernod Dec 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
127 changes: 70 additions & 57 deletions src/BeamAdapter/component/controller/InterventionalRadiologyController.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -637,7 +637,8 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyCollis
{
Real xMaxInstrument = m_instrumentsList[idInstrumentList[i]]->getRestTotalLength();

if (absCurvPoint[i] < 0.00000001*xMaxInstrument || fabs(absCurvPoint[i] - absCurvPoint[i-1])<0.00000001*xMaxInstrument)
if (absCurvPoint[i] < std::numeric_limits<float>::epsilon() * xMaxInstrument
|| fabs(absCurvPoint[i] - absCurvPoint[i - 1]) < std::numeric_limits<float>::epsilon() * xMaxInstrument)
m_activatedPointsBuf.push_back(false);
else
m_activatedPointsBuf.push_back(true);
Expand Down Expand Up @@ -698,6 +699,8 @@ void InterventionalRadiologyController<DataTypes>::activateBeamListForCollision(
template <class DataTypes>
void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyController()
{
const Real& threshold = d_threshold.getValue();

/// Create vectors with the CurvAbs of the noticiable points and the id of the corresponding instrument
type::vector<Real> newCurvAbs;

Expand All @@ -716,7 +719,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
type::vector<Real> xbegin;
for (unsigned int i=0; i<m_instrumentsList.size(); i++)
{
xend= d_xTip.getValue()[i];
xend= d_xTip.getValue()[i];
Real xb = xend - m_instrumentsList[i]->getRestTotalLength();
xbegin.push_back(xb);

Expand All @@ -737,10 +740,10 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC

/// Some verif of step 1
// if the totalLength is 0, move the first instrument
if(totalLengthCombined<0.0001)
if (totalLengthCombined < std::numeric_limits<float>::epsilon())
{
auto x = sofa::helper::getWriteOnlyAccessor(d_xTip);
x[0]=0.0001;
x[0] = std::numeric_limits<float>::epsilon();
applyInterventionalRadiologyController();
return;
}
Expand All @@ -754,84 +757,96 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
// => xbegin (theoritical curv abs of the beginning point of the instrument (could be negative) xbegin= xtip - intrumentLength)
helper::AdvancedTimer::stepBegin("step2");
type::vector<type::vector<int>> idInstrumentTable;
interventionalRadiologyComputeSampling(newCurvAbs,idInstrumentTable, xbegin, totalLengthCombined);
interventionalRadiologyComputeSampling(newCurvAbs, idInstrumentTable, xbegin, totalLengthCombined);
helper::AdvancedTimer::stepEnd("step2");


/// STEP 3
/// Re-interpolate the positions and the velocities
helper::AdvancedTimer::stepBegin("step3");
unsigned int nbeam=newCurvAbs.size()-1; // number of simulated beams
unsigned int nnode=newCurvAbs.size(); // number of simulated nodes

unsigned int nnode_old= m_nodeCurvAbs.size(); // previous number of simulated nodes;
Data<VecCoord>* datax = this->getMechanicalState()->write(core::VecCoordId::position());
// Get write access to current nodes/dofs
Data<VecCoord>* datax = this->getMechanicalState()->write(core::VecCoordId::position());
auto x = sofa::helper::getWriteOnlyAccessor(*datax);

VecCoord xbuf = x.ref();

const sofa::Size nbrCurvAbs = newCurvAbs.size(); // number of simulated nodes
const sofa::Size prev_nbrCurvAbs = m_nodeCurvAbs.size(); // previous number of simulated nodes;
const Real prev_maxCurvAbs = m_nodeCurvAbs.back();

// Change curv if totalLength has changed: modifiedCurvAbs = newCurvAbs - current motion (Length between new and old tip curvAbs)
type::vector<Real> modifiedCurvAbs;

totalLengthIsChanging(newCurvAbs, modifiedCurvAbs, idInstrumentTable);

Real xmax_prev = m_nodeCurvAbs[m_nodeCurvAbs.size()-1];
sofa::Size nbrUnactiveNode = m_numControlledNodes - nbrCurvAbs; // m_numControlledNodes == nbr Dof | nbr of CurvAbs > 0
sofa::Size prev_nbrUnactiveNode = previousNumControlledNodes - prev_nbrCurvAbs;

for (unsigned int p=0; p<nbeam+1; p++)
for (sofa::Index xId = 0; xId < nbrCurvAbs; xId++)
{
int idP = m_numControlledNodes-nnode + p;
Real xabs = modifiedCurvAbs[p];

const sofa::Index globalNodeId = nbrUnactiveNode + xId;
const Real xCurvAbs = modifiedCurvAbs[xId];
// 2 cases: TODO : remove first case
//1. the abs curv is further than the previous state of the instrument
//2. this is not the case and the node position can be interpolated using previous step positions
if(xabs > xmax_prev + d_threshold.getValue())
if ((xCurvAbs - std::numeric_limits<float>::epsilon()) > prev_maxCurvAbs + threshold)
{
msg_error_when(f_printLog.getValue())
<< "case 1 should never happen ==> avoid using totalLengthIsChanging ! xabs = " << xabs << " - xmax_prev = " << xmax_prev
<< "newCurvAbs = " << newCurvAbs << " previous nodeCurvAbs" << m_nodeCurvAbs
<< "modifiedCurvAbs =" << modifiedCurvAbs;
msg_error() << "Case 1 should never happen ==> avoid using totalLengthIsChanging! xCurvAbs = " << xCurvAbs
<< " > prev_maxCurvAbs = " << prev_maxCurvAbs << " + threshold: " << threshold << "\n"
<< "\n | newCurvAbs: " << newCurvAbs
<< "\n | modifiedCurvAbs: " << modifiedCurvAbs
<< "\n | previous nodeCurvAbs: " << m_nodeCurvAbs;
// case 1 (the abs curv is further than the previous state of the instrument)
// verifier qu'il s'agit bien d'un instrument qu'on est en train de controller
// interpoler toutes les positions "sorties" de l'instrument en supprimant l'ajout de dx qu'on vient de faire
}
else
{
// case 2 (the node position can be interpolated straightfully using previous step positions)
unsigned int p0=0;
while(p0<m_nodeCurvAbs.size())
sofa::Index prev_xId = 0;
while (prev_xId < m_nodeCurvAbs.size()) // check which prev_curvAbs is above current curvAbs using threshold value
{
if((m_nodeCurvAbs[p0]+d_threshold.getValue())>xabs)
if ((m_nodeCurvAbs[prev_xId] + threshold) > xCurvAbs)
break;
p0++;
prev_xId++;
}

int idP0 = previousNumControlledNodes + seg_remove - nnode_old + p0 ;
sofa::Index prev_globalNodeId = prev_nbrUnactiveNode + seg_remove + prev_xId;
const Real prev_xCurvAbs = m_nodeCurvAbs[prev_xId];

if(fabs(m_nodeCurvAbs[p0]-xabs)<d_threshold.getValue())
x[idP] = xbuf[idP0];
if (fabs(prev_xCurvAbs - xCurvAbs) < threshold)
{
x[globalNodeId] = xbuf[prev_globalNodeId];
}
else
{
// the node must be interpolated using beam interpolation
//find the instrument
int id = m_idInstrumentCurvAbsTable[p0][0];
//find the instrument
int id = m_idInstrumentCurvAbsTable[prev_xId][0];
//find the good beam (TODO: do not work if xbegin of one instrument >0)
int b = p0-1;
int b = prev_xId - 1;
// test to avoid wrong indices
if (b<0)
x[p]=d_startingPos.getValue();
if (b < 0)
x[globalNodeId] = d_startingPos.getValue();
else
{
Transform global_H_interpol;
Real ratio = (xabs - m_nodeCurvAbs[b])/ (m_nodeCurvAbs[p0]-m_nodeCurvAbs[b]);
Transform Global_H_local0(xbuf[idP0-1].getCenter(),xbuf[idP0-1].getOrientation() ), Global_H_local1(xbuf[idP0].getCenter(),xbuf[idP0].getOrientation() );

Real L = m_nodeCurvAbs[p0] - m_nodeCurvAbs[b];

m_instrumentsList[id]->InterpolateTransformUsingSpline(global_H_interpol, ratio, Global_H_local0, Global_H_local1 ,L);

x[idP].getCenter() = global_H_interpol.getOrigin();
x[idP].getOrientation() = global_H_interpol.getOrientation();

const Real L = prev_xCurvAbs - m_nodeCurvAbs[b];
Real baryCoef = 1.0;
if (L < std::numeric_limits<float>::epsilon()) {
msg_error() << "Two consecutives curvAbs with the same position. Length is null. Using barycenter coefficient: baryCoef = 1";
}
else {
baryCoef = (xCurvAbs - m_nodeCurvAbs[b]) / L;
}

Transform Global_H_local0(xbuf[prev_globalNodeId - 1].getCenter(), xbuf[prev_globalNodeId - 1].getOrientation());
Transform Global_H_local1(xbuf[prev_globalNodeId].getCenter(), xbuf[prev_globalNodeId].getOrientation());

m_instrumentsList[id]->InterpolateTransformUsingSpline(global_H_interpol, baryCoef, Global_H_local0, Global_H_local1, L);

x[globalNodeId].getCenter() = global_H_interpol.getOrigin();
x[globalNodeId].getOrientation() = global_H_interpol.getOrientation();
}
}
}
Expand All @@ -842,20 +857,21 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
/// STEP 4
/// Assign the beams
helper::AdvancedTimer::stepBegin("step4");
sofa::Size nbrBeam = newCurvAbs.size() - 1; // number of simulated beams
unsigned int numEdges= m_numControlledNodes-1;

// verify that there is a sufficient number of Edge in the topology : TODO if not, modify topo !
if(numEdges<nbeam)
if (numEdges<nbrBeam)
{
if (f_printLog.getValue())
{
msg_error()<<"Not enough edges in the topology.";
}
nbeam=numEdges;
nbrBeam = numEdges;
}


for (unsigned int b=0; b<nbeam; b++)
for (unsigned int b=0; b< nbrBeam; b++)
{
Real x0 = newCurvAbs[b];
Real x1 = newCurvAbs[b+1];
Expand All @@ -864,11 +880,9 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
Real xmax = d_xTip.getValue()[i];
Real xmin = xbegin[i];

Real eps= d_threshold.getValue();

if (x0>(xmin-eps) && x0<(xmax+eps) && x1>(xmin-eps) && x1<(xmax+eps))
if (x0>(xmin- threshold) && x0<(xmax+ threshold) && x1>(xmin- threshold) && x1<(xmax+ threshold))
{
BaseMeshTopology::EdgeID eID = (BaseMeshTopology::EdgeID)(numEdges-nbeam + b );
BaseMeshTopology::EdgeID eID = (BaseMeshTopology::EdgeID)(numEdges- nbrBeam + b );

Real length = x1 - x0;
Real x0_local = x0-xmin;
Expand All @@ -886,7 +900,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
/// STEP 5
/// Fix the not simulated nodes
helper::AdvancedTimer::stepBegin("step5");
unsigned int firstSimulatedNode = m_numControlledNodes - nbeam;
unsigned int firstSimulatedNode = m_numControlledNodes - nbrBeam;

//1. Fix the nodes (beginning of the instruments) that are not "out"
fixFirstNodesWithUntil(firstSimulatedNode);
Expand All @@ -903,7 +917,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC

for (unsigned int i=0; i<newCurvAbs.size(); i++)
{
if (newCurvAbs[i] < ((*it)+0.001) && newCurvAbs[i] > ((*it)-0.001)) // node= border of the rigid segment
if (newCurvAbs[i] < ((*it)+ std::numeric_limits<float>::epsilon()) && newCurvAbs[i] > ((*it)- std::numeric_limits<float>::epsilon())) // node= border of the rigid segment
{
if (!rigid)
{
Expand Down Expand Up @@ -950,7 +964,7 @@ void InterventionalRadiologyController<DataTypes>::totalLengthIsChanging(const t
// we initialize some points at a x_curv ref pos without the motion (computed by DLength)
// due to the elasticity of the beam, the point will then naturally go the position that reespects the newNodeCurvAbs

Real dLength = newNodeCurvAbs[ newNodeCurvAbs.size()-1] - m_nodeCurvAbs[m_nodeCurvAbs.size() - 1];
Real dLength = newNodeCurvAbs.back() - m_nodeCurvAbs.back();
modifiedNodeCurvAbs = newNodeCurvAbs;

// we look for the last value in the CurvAbs
Expand All @@ -959,12 +973,11 @@ void InterventionalRadiologyController<DataTypes>::totalLengthIsChanging(const t
unsigned int i=newTable.size()-1;
while (i>0 && newTable[i].size()==1)
{
modifiedNodeCurvAbs[i]-=dLength;
modifiedNodeCurvAbs[i] -= dLength;

// force modifiedNode to be "locally" sorted
if(modifiedNodeCurvAbs[i]<modifiedNodeCurvAbs[i-1])
if (modifiedNodeCurvAbs[i] < modifiedNodeCurvAbs[i - 1])
{
modifiedNodeCurvAbs[i] = modifiedNodeCurvAbs[i-1]+ d_threshold.getValue();
modifiedNodeCurvAbs[i] = modifiedNodeCurvAbs[i - 1];
hugtalbot marked this conversation as resolved.
Show resolved Hide resolved
}

i--;
Expand Down