diff --git a/src/CollisionAlgorithm/DataDetectionOutput.h b/src/CollisionAlgorithm/DataDetectionOutput.h index 5201fbc..5443209 100644 --- a/src/CollisionAlgorithm/DataDetectionOutput.h +++ b/src/CollisionAlgorithm/DataDetectionOutput.h @@ -13,6 +13,17 @@ class DetectionOutput { public: typedef std::pair PairDetection; + DetectionOutput() = default; + ~DetectionOutput() = default; + + // Copy + DetectionOutput(const DetectionOutput&) = default; + DetectionOutput& operator=(const DetectionOutput&) = default; + + // Move + DetectionOutput(DetectionOutput&&) noexcept = default; + DetectionOutput& operator=(DetectionOutput&&) noexcept = default; + friend std::ostream& operator<<(std::ostream& os, const DetectionOutput& t) { os << t.m_output.size() << ":" ; for (unsigned i=0;iadd(it.first, it.second); + } + inline const PairDetection & operator[](int i) const { return m_output[i]; } diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp index 4847bb6..fbf761f 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp @@ -109,221 +109,269 @@ void InsertionAlgorithm::doDetection() insertionOutput.clear(); collisionOutput.clear(); - if (m_couplingPts.empty() && l_surfGeom) + if (m_couplingPts.empty()) { - // Operations on surface geometry + // Puncture sequence sofa::helper::AdvancedTimer::stepBegin("Puncture detection - " + this->getName()); - auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); - auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); - - // Puncture sequence - if (d_enablePuncture.getValue() && l_tipGeom) - { - auto createTipProximity = - Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); - auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); + if (d_enablePuncture.getValue()) collisionOutput = puncturePhase(); - const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); - for (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++) - { - BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - if (!tipProx) continue; - const BaseProximity::SPtr surfProx = findClosestProxOnSurf( - tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx) - { - surfProx->normalize(); - - // Check whether puncture is happening - if so, create coupling point ... - if (m_constraintSolver) - { - const MechStateTipType::SPtr mstate = - l_tipGeom->getContext()->get(); - const auto& lambda = - m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); - SReal norm{0_sreal}; - for (const auto& l : lambda) - { - norm += l.norm(); - } - if (norm > punctureForceThreshold) - { - m_couplingPts.push_back(surfProx); - continue; - } - } - - // ... if not, create a proximity pair for the tip-surface collision - collisionOutput.add(tipProx, surfProx); - } - } - } sofa::helper::AdvancedTimer::stepEnd("Puncture detection - " + this->getName()); // Shaft collision sequence - Disable if coupling points have been added sofa::helper::AdvancedTimer::stepBegin("Shaft collision - " + this->getName()); - if (d_enableShaftCollision.getValue() && m_couplingPts.empty() && l_shaftGeom) - { - auto createShaftProximity = - Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); - auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) - { - BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); - if (!shaftProx) continue; - const BaseProximity::SPtr surfProx = findClosestProxOnSurf( - shaftProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx) - { - surfProx->normalize(); - - if (d_projective.getValue()) - { - // shaftProx = - // projectOnShaft(surfProx->getPosition(), itShaft->element()).prox; - // if (!shaftProx) continue; - // shaftProx->normalize(); - // Experimental - This enables projection anywhere on the edge - auto findClosestProxOnShaft = - Operations::FindClosestProximity::Operation::get(l_shaftGeom); - shaftProx = findClosestProxOnShaft(surfProx, l_shaftGeom, projectOnShaft, - getFilterFunc()); - if (!shaftProx) continue; - shaftProx->normalize(); - } - collisionOutput.add(shaftProx, surfProx); - } - } - } + + if (d_enableShaftCollision.getValue() && m_couplingPts.empty()) + collisionOutput.add(shaftCollisionPhase()); + sofa::helper::AdvancedTimer::stepEnd("Shaft collision - " + this->getName()); } else { // Insertion sequence sofa::helper::AdvancedTimer::stepBegin("Needle insertion - " + this->getName()); - if (!d_enableInsertion.getValue() || !l_tipGeom || !l_volGeom || !l_shaftGeom) return; - ElementIterator::SPtr itTip = l_tipGeom->begin(); - auto createTipProximity = - Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); - const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - if (!tipProx) return; + if (d_enableInsertion.getValue()) insertionPhase(); - // Remove coupling points that are ahead of the tip in the insertion direction - ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); - auto prunePointsAheadOfTip = - Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo()); - prunePointsAheadOfTip(m_couplingPts, itShaft->element()); + sofa::helper::AdvancedTimer::stepEnd("Needle insertion - " + this->getName()); + } - if (m_couplingPts.empty()) return; + // Reprojection phase + sofa::helper::AdvancedTimer::stepBegin("Reproject coupling points - " + this->getName()); - type::Vec3 lastCP = m_couplingPts.back()->getPosition(); - const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); + if (d_enableInsertion.getValue()) insertionOutput = reprojectCouplingPoints(); - // Vector from tip to last coupling point; used for distance and directional checks - const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); + sofa::helper::AdvancedTimer::stepEnd("Reproject coupling points - " + this->getName()); - // Only add a new coupling point if the needle tip has advanced far enough - if (tipToLastCP.norm() > tipDistThreshold) - { - // Prepare the operations before entering the loop - auto createShaftProximity = - Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); - auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); - auto projectOnVol = Operations::Project::Operation::get(l_volGeom); - auto containsPointInVol = - Operations::ContainsPointInProximity::Operation::get(l_volGeom); - - // Iterate over shaft segments to find which one contains the next candidate CP - for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) - { - BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); - if (!shaftProx) continue; + d_collisionOutput.endEdit(); + d_insertionOutput.endEdit(); +} + +InsertionAlgorithm::AlgorithmOutput InsertionAlgorithm::puncturePhase() +{ + if (!l_tipGeom || !l_surfGeom) return AlgorithmOutput(); + + AlgorithmOutput punctureCollisionOutput; + + // Prepare operations before entering loop + auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); + auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); + auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); - const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); - if (!edgeProx) continue; + const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); + for (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++) + { + BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); - const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); - const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); - const type::Vec3 lastCPToP1 = p1 - lastCP; + if (!tipProx) continue; - // Skip if last CP lies after edge end point - if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue; + const BaseProximity::SPtr surfProx = + findClosestProxOnSurf(tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); + if (surfProx) + { + surfProx->normalize(); - const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold); + // Check whether puncture is happening - if so, create coupling point ... + if (m_constraintSolver) + { + const MechStateTipType::SPtr mstate = + l_tipGeom->getContext()->get(); + const auto& lambda = + m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); + SReal norm{0_sreal}; - for (int idCP = 0; idCP < numCPs; idCP++) + for (const auto& l : lambda) { - // Candidate coupling point along shaft segment - const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; - - // Project candidate CP onto the edge element and compute scalar coordinate - // along segment - const SReal edgeSegmentLength = (p1 - p0).norm(); - const type::Vec3 p0ToCandidateCP = candidateCP - p0; - const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); - - // Skip if candidate CP is outside current edge segment - if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break; - - // Project candidate CP onto shaft geometry ... - shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; - if (!shaftProx) continue; - - // ... then find nearest volume proximity - const BaseProximity::SPtr volProx = findClosestProxOnVol( - shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); - if (!volProx) continue; - - // Proximity can be detected before the tip enters the tetra (e.g. near a - // boundary face) Only accept proximities if the tip is inside the tetra - // during insertion - if (containsPointInVol(shaftProx->getPosition(), volProx)) - { - volProx->normalize(); - m_couplingPts.push_back(volProx); - lastCP = volProx->getPosition(); - } + norm += l.norm(); + } + + if (norm > punctureForceThreshold) + { + m_couplingPts.push_back(surfProx); + continue; } } + + // ... if not, create a proximity pair for the tip-surface collision + punctureCollisionOutput.add(tipProx, surfProx); } - else // Don't bother with removing the point that was just added + } + return punctureCollisionOutput; +} + +InsertionAlgorithm::AlgorithmOutput InsertionAlgorithm::shaftCollisionPhase() +{ + if (!l_shaftGeom || !l_surfGeom) return AlgorithmOutput(); + + AlgorithmOutput shaftCollisionOutput; + + // Prepare operations before entering loop + auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); + auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); + auto createShaftProximity = + Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); + + for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) + { + BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); + + if (!shaftProx) continue; + + const BaseProximity::SPtr surfProx = + findClosestProxOnSurf(shaftProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); + + if (surfProx) { - // Remove coupling points that are ahead of the tip in the insertion direction - ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); - auto prunePointsAheadOfTip = - Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo()); - prunePointsAheadOfTip(m_couplingPts, itShaft->element()); + surfProx->normalize(); + + if (d_projective.getValue()) + { + // shaftProx = + // projectOnShaft(surfProx->getPosition(), itShaft->element()).prox; + // if (!shaftProx) continue; + // shaftProx->normalize(); + + // Experimental - This enables projection anywhere on the edge + auto findClosestProxOnShaft = + Operations::FindClosestProximity::Operation::get(l_shaftGeom); + shaftProx = + findClosestProxOnShaft(surfProx, l_shaftGeom, projectOnShaft, getFilterFunc()); + + if (!shaftProx) continue; + + shaftProx->normalize(); + } + shaftCollisionOutput.add(shaftProx, surfProx); } - sofa::helper::AdvancedTimer::stepEnd("Needle insertion - " + this->getName()); } + return shaftCollisionOutput; +} - sofa::helper::AdvancedTimer::stepBegin("Reproject coupling points - " + this->getName()); - if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom) +void InsertionAlgorithm::insertionPhase() +{ + if (!l_tipGeom || !l_volGeom || !l_shaftGeom) return; + + ElementIterator::SPtr itTip = l_tipGeom->begin(); + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + + if (!tipProx) return; + + // Remove coupling points that are ahead of the tip in the insertion direction + ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); + auto prunePointsAheadOfTip = + Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo()); + prunePointsAheadOfTip(m_couplingPts, itShaft->element()); + + if (m_couplingPts.empty()) return; + + type::Vec3 lastCP = m_couplingPts.back()->getPosition(); + const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); + + // Vector from tip to last coupling point; used for distance and directional checks + const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); + + // Only add a new coupling point if the needle tip has advanced far enough + if (tipToLastCP.norm() < tipDistThreshold) return; + + // Prepare operations before entering loop + auto createShaftProximity = + Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); + auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); + auto projectOnVol = Operations::Project::Operation::get(l_volGeom); + auto containsPointInVol = Operations::ContainsPointInProximity::Operation::get(l_volGeom); + + // Iterate over shaft segments to find which one contains the next candidate CP + for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) { - // Reprojection on shaft geometry sequence - auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); - auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for (const auto& cp : m_couplingPts) + BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); + + if (!shaftProx) continue; + + const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); + + if (!edgeProx) continue; + + const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); + const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); + const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); + const type::Vec3 lastCPToP1 = p1 - lastCP; + + // Skip if last CP lies after edge end point + if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue; + + const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold); + + for (int idCP = 0; idCP < numCPs; idCP++) { - const BaseProximity::SPtr shaftProx = - findClosestProxOnShaft(cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + // Candidate coupling point along shaft segment + const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; + + // Project candidate CP onto the edge element and compute scalar coordinate + // along segment + const SReal edgeSegmentLength = (p1 - p0).norm(); + const type::Vec3 p0ToCandidateCP = candidateCP - p0; + const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); + + // Skip if candidate CP is outside current edge segment + if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break; + + // Project candidate CP onto shaft geometry ... + shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; + if (!shaftProx) continue; - shaftProx->normalize(); - insertionOutput.add(shaftProx, cp); + + // ... then find nearest volume proximity + const BaseProximity::SPtr volProx = + findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + + if (!volProx) continue; + + // Proximity can be detected before the tip enters the tetra (e.g. near a + // boundary face) Only accept proximities if the tip is inside the tetra + // during insertion + if (containsPointInVol(shaftProx->getPosition(), volProx)) + { + volProx->normalize(); + m_couplingPts.push_back(volProx); + lastCP = volProx->getPosition(); + } } - // This is a final-frontier check: If there are coupling points stored, but the - // findClosestProxOnShaf operation yields no proximities on the shaft, it could be - // because the needle has exited abruptly. Thus, we clear the coupling points. - if (insertionOutput.size() == 0) m_couplingPts.clear(); } - sofa::helper::AdvancedTimer::stepEnd("Reproject coupling points - " + this->getName()); +} - d_collisionOutput.endEdit(); - d_insertionOutput.endEdit(); +InsertionAlgorithm::AlgorithmOutput InsertionAlgorithm::reprojectCouplingPoints() +{ + if (m_couplingPts.empty() || !l_shaftGeom) return AlgorithmOutput(); + + AlgorithmOutput reprojectionOutput; + + // Reprojection on shaft geometry sequence + auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); + + for (const auto& cp : m_couplingPts) + { + const BaseProximity::SPtr shaftProx = + findClosestProxOnShaft(cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + + if (!shaftProx) continue; + + shaftProx->normalize(); + reprojectionOutput.add(shaftProx, cp); + } + + // This is a final-frontier check: If there are coupling points stored, but the + // findClosestProxOnShaf operation yields no proximities on the shaft, it could be + // because the needle has exited abruptly. Thus, we clear the coupling points. + if (reprojectionOutput.size() == 0) m_couplingPts.clear(); + + return reprojectionOutput; } } // namespace sofa::collisionalgorithm diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index 13ac70c..a76e413 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -44,6 +44,11 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm /// Detection outputs are used to create collisions and needle-tissue coupling. /// Handles puncture, shaft collisions and insertion into tissue phases. void doDetection() override; + + virtual AlgorithmOutput puncturePhase(); + virtual AlgorithmOutput shaftCollisionPhase(); + virtual void insertionPhase(); + virtual AlgorithmOutput reprojectCouplingPoints(); }; } // namespace sofa::collisionalgorithm diff --git a/src/CollisionAlgorithm/initCollisionAlgorithm.h b/src/CollisionAlgorithm/initCollisionAlgorithm.h index d6a9e84..928abf1 100644 --- a/src/CollisionAlgorithm/initCollisionAlgorithm.h +++ b/src/CollisionAlgorithm/initCollisionAlgorithm.h @@ -1,7 +1,7 @@ #pragma once #include -namespace collisionalgorithm +namespace sofa::collisionalgorithm { void SOFA_COLLISIONALGORITHM_API initCollisionAlgorithm(); } // namespace collisionalgorithm