Skip to content

Commit

Permalink
[InterventionalRadiologyController] Clean method applyInterventionalR…
Browse files Browse the repository at this point in the history 
…adiologyController (#73)

* backup work on applyInterventionalRadiologyController()

* Backup fix on totalLength changed

* [IRCtrl] replace hardcoded values by epsilon
  • Loading branch information
@epernod
epernod authored Dec 28, 2022
1 parent fec3eac commit 8266631
Showing 1 changed file with 70 additions and 57 deletions.
127 changes: 70 additions & 57 deletions src/BeamAdapter/component/controller/InterventionalRadiologyController.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -696,7 +696,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 @@ -757,6 +758,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 @@ -775,7 +778,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 @@ -796,10 +799,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 @@ -813,84 +816,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 @@ -901,20 +916,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 @@ -923,11 +939,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 @@ -945,7 +959,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 @@ -962,7 +976,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 @@ -1009,7 +1023,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 @@ -1018,12 +1032,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];
}

i--;
Expand Down

0 comments on commit 8266631

Please sign in to comment.