diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index a38a3c144..838733375 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -696,7 +696,8 @@ void InterventionalRadiologyController::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::epsilon() * xMaxInstrument + || fabs(absCurvPoint[i] - absCurvPoint[i - 1]) < std::numeric_limits::epsilon() * xMaxInstrument) m_activatedPointsBuf.push_back(false); else m_activatedPointsBuf.push_back(true); @@ -757,6 +758,8 @@ void InterventionalRadiologyController::activateBeamListForCollision( template void InterventionalRadiologyController::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 newCurvAbs; @@ -775,7 +778,7 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC type::vector xbegin; for (unsigned int i=0; igetRestTotalLength(); xbegin.push_back(xb); @@ -796,10 +799,10 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC /// Some verif of step 1 // if the totalLength is 0, move the first instrument - if(totalLengthCombined<0.0001) + if (totalLengthCombined < std::numeric_limits::epsilon()) { auto x = sofa::helper::getWriteOnlyAccessor(d_xTip); - x[0]=0.0001; + x[0] = std::numeric_limits::epsilon(); applyInterventionalRadiologyController(); return; } @@ -813,42 +816,45 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC // => xbegin (theoritical curv abs of the beginning point of the instrument (could be negative) xbegin= xtip - intrumentLength) helper::AdvancedTimer::stepBegin("step2"); type::vector> 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* datax = this->getMechanicalState()->write(core::VecCoordId::position()); + // Get write access to current nodes/dofs + Data* 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 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 xmax_prev + d_threshold.getValue()) + if ((xCurvAbs - std::numeric_limits::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 @@ -856,41 +862,50 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC else { // case 2 (the node position can be interpolated straightfully using previous step positions) - unsigned int p0=0; - while(p0xabs) + 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)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::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(); } } } @@ -901,20 +916,21 @@ void InterventionalRadiologyController::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::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; @@ -945,7 +959,7 @@ void InterventionalRadiologyController::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); @@ -962,7 +976,7 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC for (unsigned int i=0; i ((*it)-0.001)) // node= border of the rigid segment + if (newCurvAbs[i] < ((*it)+ std::numeric_limits::epsilon()) && newCurvAbs[i] > ((*it)- std::numeric_limits::epsilon())) // node= border of the rigid segment { if (!rigid) { @@ -1009,7 +1023,7 @@ void InterventionalRadiologyController::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 @@ -1018,12 +1032,11 @@ void InterventionalRadiologyController::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]