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] Split and rename methods sortCurvAbs and interventionalRadiologyComputeSampling for more clarity #105

Merged
merged 3 commits into from
Sep 7, 2023
Merged

[InterventionalRadiologyController] Split and rename methods sortCurvAbs and interventionalRadiologyComputeSampling for more clarity #105

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
69b6a3a
[IRCtrl] rename method IRComputeSampling into ComputeInstrumentsCurvA…
epernod Jul 27, 2023
dcd7ddd
[IRCtrl] Update code comments
epernod Jul 27, 2023
bce4678
[IRCtrl] Update doc and rename method
epernod Jul 28, 2023
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
20 changes: 16 additions & 4 deletions src/BeamAdapter/component/controller/InterventionalRadiologyController.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,11 +130,23 @@ class InterventionalRadiologyController : public MechanicalStateController<DataT

/// Interface for interventionalRadiology instruments:
virtual void applyInterventionalRadiologyController(void);

void processDrop(unsigned int &previousNumControlledNodes, unsigned int &seg_remove);
void interventionalRadiologyComputeSampling(type::vector<Real> &newCurvAbs, type::vector< type::vector<int> > &id_instrument_table, const type::vector<Real> &xBegin, const Real& xEnd);
/// Sort the curv Abs in the ascending order and avoid doubloon
void sortCurvAbs(type::vector<Real> &CurvAbs, type::vector< type::vector<int> >& id_instrument_table);

private:
/** Compute the sambling curv abscisses using each instrument sampling and key points parameters
* Will call @sa sortCurvAbs to sort the curv abs and remove doubloon
* Need each tool starting position to sample only activated nodes and tool total length (combined deployed tool lengths)
**/
void computeInstrumentsCurvAbs(type::vector<Real>& newCurvAbs, const type::vector<Real>& tools_xBegin, const Real& totalLength);

/// Method to sort the curv Abs in the ascending order and remove doubloon that are closer than d_threshold
void sortCurvAbs(type::vector<Real>& curvAbs);

/// Method to fill the id_instrument_table based on curvAbs and each tool begin and end. The table as the same size as the curvAbs buffer and store for each curvAbs[i] a vector with the ids of the instruments that are present at this position.
void fillInstrumentCurvAbsTable(const type::vector<Real>& curvAbs, const type::vector<Real>& tools_xBegin, const type::vector<Real>& tools_xEnd, type::vector< type::vector<int> >& id_instrument_table);

public:
void totalLengthIsChanging(const type::vector<Real>& newNodeCurvAbs, type::vector<Real>& modifiedNodeCurvAbs, const type::vector< type::vector<int> >& newTable);
void fixFirstNodesWithUntil(unsigned int first_simulated_Node);
void activateBeamListForCollision( type::vector<Real> &curv_abs, type::vector< type::vector<int> > &id_instrument_table);
Expand Down
118 changes: 61 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 @@ -520,11 +520,9 @@ void InterventionalRadiologyController<DataTypes>::processDrop(unsigned int &pre


template <class DataTypes>
void InterventionalRadiologyController<DataTypes>::interventionalRadiologyComputeSampling(type::vector<Real> &newCurvAbs,
type::vector< type::vector<int> > &idInstrumentTable,
const type::vector<Real> &xBegin,
const Real& xend)

void InterventionalRadiologyController<DataTypes>::computeInstrumentsCurvAbs(type::vector<Real> &newCurvAbs,
const type::vector<Real>& tools_xBegin,
const Real& totalLength)
{
// Step 1 => put the noticeable Nodes
// Step 2 => add the beams given the sampling parameters
Expand All @@ -534,21 +532,22 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyComput
{
type::vector<Real> xP_noticeable_I;
type::vector< int > density_I;
m_instrumentsList[i]->getSamplingParameters(xP_noticeable_I, density_I);
m_instrumentsList[i]->getSamplingParameters(xP_noticeable_I, density_I); // sampling of the different section of this instrument

// check each interval of noticeable point to see if they go out (>0) and use corresponding density to sample the interval.
for (int j=0; j<(int)(xP_noticeable_I.size()-1); j++)
{
const Real xP = xP_noticeable_I[j];
const Real nxP = xP_noticeable_I[j + 1];

//compute the corresponding abs curv of this "noticeable point" on the combined intrument
const Real curvAbs_xP = xBegin[i] + xP;
const Real curvAbs_nxP = xBegin[i] + nxP;
//compute the corresponding abs curv of this "noticeable point" on the combined intrument deployed.
const Real curvAbs_xP = tools_xBegin[i] + xP; // xBegin = xend - instrument Total Length
const Real curvAbs_nxP = tools_xBegin[i] + nxP;

// In any case, the key points are added as soon as they are deployed
if (curvAbs_xP > 0)
if (curvAbs_xP > 0) {
newCurvAbs.push_back(curvAbs_xP);
}

// compute interval between next point and previous one (0 for the first iter)
const Real curvAbs_interval = (curvAbs_nxP - xSampling);
Expand All @@ -559,7 +558,7 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyComput
Real ratio = Real(density_I[j]) / (nxP - xP);
int numNewNodes = int(floor(curvAbs_interval * ratio)); // if density == 0, no sampling (numNewNodes == 0)

// Add the new points in reverse order
// Add the new points using reverse order iterator as they are computed deduce to next noticeable point
for (int k = numNewNodes; k>0; k--)
{
auto value = curvAbs_nxP - (k / ratio);
Expand All @@ -573,9 +572,10 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyComput
// After the end of the for loop above, we just have to process the
// instrument last key point
const Real lastxP = xP_noticeable_I[xP_noticeable_I.size()-1];
const Real curvAbs_lastxP = xBegin[i] + lastxP;
if (curvAbs_lastxP > 0)
const Real curvAbs_lastxP = tools_xBegin[i] + lastxP;
if (curvAbs_lastxP > 0) {
newCurvAbs.push_back(curvAbs_lastxP);
}
}


Expand All @@ -592,7 +592,7 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyComput
{
Real abs;
abs = *it++;
if (abs<xend) // verify that the rigidified part is not outside the model
if (abs < totalLength) // verify that the rigidified part is not outside the model
{
if(begin)
{
Expand All @@ -609,7 +609,7 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyComput
}
}

sortCurvAbs(newCurvAbs, idInstrumentTable);
sortCurvAbs(newCurvAbs);
}


Expand Down Expand Up @@ -788,31 +788,30 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
if (m_dropCall)
processDrop(previousNumControlledNodes, seg_remove);

/// STEP 1
/// Find the total length of the COMBINED INSTRUMENTS and the one for which xtip > 0 (so the one which are simulated)
Real xend;
// ## STEP 1: Find the total length of the COMBINED INSTRUMENTS and the one for which xtip > 0 (so the one which are simulated)
helper::AdvancedTimer::stepBegin("step1");
Real totalLengthCombined=0.0;
type::vector<Real> xbegin;
type::vector<Real> tools_xBegin;
type::vector<Real> tools_xEnd;
for (unsigned int i=0; i<m_instrumentsList.size(); i++)
{
xend= d_xTip.getValue()[i];
Real xb = xend - m_instrumentsList[i]->getRestTotalLength();
xbegin.push_back(xb);

const Real& xend= d_xTip.getValue()[i];
tools_xEnd.push_back(xend);
tools_xBegin.push_back(xend - m_instrumentsList[i]->getRestTotalLength());
if (xend> totalLengthCombined)
{
totalLengthCombined=xend;
totalLengthCombined = xend;
}

// clear the present interpolation of the beams
m_instrumentsList[i]->clear();

if( xend > 0.0)
{
// create the first node (on x=0)
newCurvAbs.push_back(0.0);
}

// clear the present interpolation of the beams
m_instrumentsList[i]->clear();
}

/// Some verif of step 1
Expand All @@ -827,22 +826,22 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
helper::AdvancedTimer::stepEnd("step1");


/// STEP 2:
/// get the noticeable points that need to be simulated
// Fill=> newCurvAbs which provides a vector with curvilinear abscissa of each simulated node
// => id_instrument_table which provides for each simulated node, the id of all instruments which belong this node
// ## STEP 2: Get the noticeable points that need to be simulated
// => Fill newCurvAbs which provides a vector with curvilinear abscissa of each simulated node
// => xbegin (theoritical curv abs of the beginning point of the instrument (could be negative) xbegin= xtip - intrumentLength)
helper::AdvancedTimer::stepBegin("step2");
helper::AdvancedTimer::stepBegin("step2");
computeInstrumentsCurvAbs(newCurvAbs, tools_xBegin, totalLengthCombined);

// => id_instrument_table which provides for each simulated node, the id of all instruments which belong this node
type::vector<type::vector<int>> idInstrumentTable;
interventionalRadiologyComputeSampling(newCurvAbs, idInstrumentTable, xbegin, totalLengthCombined);
fillInstrumentCurvAbsTable(newCurvAbs, tools_xBegin, tools_xEnd, idInstrumentTable);
helper::AdvancedTimer::stepEnd("step2");


/// STEP 3
/// Re-interpolate the positions and the velocities
// ## STEP 3: Re-interpolate the positions and the velocities
helper::AdvancedTimer::stepBegin("step3");

// Get write access to current nodes/dofs
// => 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();
Expand All @@ -851,7 +850,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
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)
// => 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);

Expand All @@ -860,7 +859,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC

for (sofa::Index xId = 0; xId < nbrCurvAbs; xId++)
{
const sofa::Index globalNodeId = nbrUnactiveNode + xId;
const sofa::Index globalNodeId = nbrUnactiveNode + xId; // fill the end of the dof buffer
const Real xCurvAbs = modifiedCurvAbs[xId];

// 2 cases: TODO : remove first case
Expand Down Expand Up @@ -893,7 +892,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC

if (fabs(prev_xCurvAbs - xCurvAbs) < threshold)
{
x[globalNodeId] = xbuf[prev_globalNodeId];
x[globalNodeId] = xbuf[prev_globalNodeId]; // xBuf all initialised at start with d_startingPos
}
else
{
Expand Down Expand Up @@ -931,8 +930,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
helper::AdvancedTimer::stepEnd("step3");


/// STEP 4
/// Assign the beams
// ## 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;
Expand All @@ -954,8 +952,8 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
Real x1 = newCurvAbs[b+1];
for (unsigned int i=0; i<m_instrumentsList.size(); i++)
{
Real xmax = d_xTip.getValue()[i];
Real xmin = xbegin[i];
const Real& xmax = tools_xEnd[i];
const Real& xmin = tools_xBegin[i];

if (x0>(xmin- threshold) && x0<(xmax+ threshold) && x1>(xmin- threshold) && x1<(xmax+ threshold))
{
Expand All @@ -974,16 +972,15 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
}
helper::AdvancedTimer::stepEnd("step4");

/// STEP 5
/// Fix the not simulated nodes

// ## STEP 5: Fix the not simulated nodes
helper::AdvancedTimer::stepBegin("step5");
unsigned int firstSimulatedNode = m_numControlledNodes - nbrBeam;

//1. Fix the nodes (beginning of the instruments) that are not "out"
// => 1. Fix the nodes (beginning of the instruments) that are not "out"
fixFirstNodesWithUntil(firstSimulatedNode);

//2. Fix the node that are "fixed"
// When there are rigid segments, # of dofs is different than # of edges and beams
// => 2. Fix the node that are "fixed". When there are rigid segments, # of dofs is different than # of edges and beams
const std::vector< Real > *rigidCurvAbs = &d_rigidCurvAbs.getValue();

bool rigid=false;
Expand Down Expand Up @@ -1020,14 +1017,14 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
}

}
helper::AdvancedTimer::stepEnd("step5");

/// STEP 6
/// Activate Beam List for collision of each instrument
// ## STEP 6: Activate Beam List for collision of each instrument
activateBeamListForCollision(newCurvAbs,idInstrumentTable);

// ## STEP 7: Save new computed Data
m_nodeCurvAbs = newCurvAbs;
m_idInstrumentCurvAbsTable = idInstrumentTable;
helper::AdvancedTimer::stepEnd("step5");
}

template <class DataTypes>
Expand Down Expand Up @@ -1062,32 +1059,39 @@ void InterventionalRadiologyController<DataTypes>::totalLengthIsChanging(const t
}
}


template <class DataTypes>
void InterventionalRadiologyController<DataTypes>::sortCurvAbs(type::vector<Real> &curvAbs,
type::vector<type::vector<int> >& idInstrumentTable)
void InterventionalRadiologyController<DataTypes>::sortCurvAbs(type::vector<Real> &curvAbs)
{
// here we sort CurvAbs
std::sort(curvAbs.begin(), curvAbs.end());


// a threshold is used to remove the values that are "too" close...
const auto threshold = d_threshold.getValue();
auto it = std::unique(curvAbs.begin(), curvAbs.end(), [threshold](const Real v1, const Real v2) {
return fabs(v1 - v2) < threshold;
});
curvAbs.erase(it, curvAbs.end());
}


template <class DataTypes>
void InterventionalRadiologyController<DataTypes>::fillInstrumentCurvAbsTable(const type::vector<Real>& curvAbs,
const type::vector<Real>& tools_xBegin,
const type::vector<Real>& tools_xEnd,
type::vector< type::vector<int> >& idInstrumentTable)
{
// here we build a table that provides the list of each instrument for each dof in the list of curvAbs
// dofs can be shared by several instruments
idInstrumentTable.clear();
idInstrumentTable.resize(curvAbs.size());

const auto& xTip = d_xTip.getValue();
const auto threshold = d_threshold.getValue();
for (unsigned int id = 0; id < m_instrumentsList.size(); id++)
{
// Get instrument absciss range
Real xEnd = xTip[id];
Real xBegin = xEnd - m_instrumentsList[id]->getRestTotalLength();
Real xEnd = tools_xEnd[id];
Real xBegin = tools_xBegin[id];

// enlarge range to ensure to considere borders in absisses comparisons
xBegin -= threshold;
Expand Down
Loading