diff --git a/CMakeLists.txt b/CMakeLists.txt index 37ba1ed..881390d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,7 +24,6 @@ set(HEADER_FILES ${COLLISIONALGORITHM_SRC}/InternalData.h ${COLLISIONALGORITHM_SRC}/algorithm/Find2DClosestProximityAlgorithm.h - ${COLLISIONALGORITHM_SRC}/algorithm/FindClosestProximityAlgorithm.h ${COLLISIONALGORITHM_SRC}/algorithm/InsertionAlgorithm.h ${COLLISIONALGORITHM_SRC}/broadphase/AABBBroadPhase.h @@ -69,7 +68,6 @@ set(SOURCE_FILES ${COLLISIONALGORITHM_SRC}/CollisionPipeline.cpp ${COLLISIONALGORITHM_SRC}/algorithm/Find2DClosestProximityAlgorithm.cpp - ${COLLISIONALGORITHM_SRC}/algorithm/FindClosestProximityAlgorithm.cpp ${COLLISIONALGORITHM_SRC}/algorithm/InsertionAlgorithm.cpp ${COLLISIONALGORITHM_SRC}/broadphase/AABBBroadPhase.cpp diff --git a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp b/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp deleted file mode 100644 index 1761699..0000000 --- a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include -#include - -namespace sofa::collisionalgorithm -{ -void registerFindClosestProximityAlgorithm(sofa::core::ObjectFactory* factory) -{ - factory->registerObjects( - sofa::core::ObjectRegistrationData( - "An algorithm to find the closest proximity between two BaseGeometry types") - .add()); -} -} // namespace sofa::collisionalgorithm diff --git a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h b/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h deleted file mode 100644 index fe830bf..0000000 --- a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h +++ /dev/null @@ -1,87 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace sofa::collisionalgorithm { - -class SOFA_COLLISIONALGORITHM_API FindClosestProximityAlgorithm : public BaseAlgorithm { -public: - SOFA_CLASS(FindClosestProximityAlgorithm, BaseAlgorithm); - - core::objectmodel::SingleLink l_from; - core::objectmodel::SingleLink l_dest; - Data d_drawCollision ; - Data > d_output; - Data d_projective ; -// Data > d_outputDist; - - FindClosestProximityAlgorithm() - : l_from(initLink("fromGeom", "link to from geometry")) - , l_dest(initLink("destGeom", "link to dest geometry")) - , d_drawCollision (initData(&d_drawCollision, true, "drawcollision", "draw collision")) - , d_output(initData(&d_output,"output", "output of the collision detection")) - , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) -// , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) - {} - - void draw(const core::visual::VisualParams* vparams) { - if (! vparams->displayFlags().getShowCollisionModels() && ! d_drawCollision.getValue()) return; - glDisable(GL_LIGHTING); - glColor4f(0,1,0,1); - - glBegin(GL_LINES); - DetectionOutput output = d_output.getValue() ; - for (unsigned i=0;igetPosition().data()); - glVertex3dv(output[i].second->getPosition().data()); - } - glEnd(); - } - - void doDetection() { - if (l_from == NULL) return; - if (l_dest == NULL) return; - - auto & output = *d_output.beginEdit(); - output.clear(); - - auto itfrom = l_from->begin(); - - auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); - auto findClosestProxOp = Operations::FindClosestProximity::Operation::get(l_dest); - auto projectOp = Operations::Project::Operation::get(l_dest); - auto projectFromOp = Operations::Project::Operation::get(l_from); - - for (;itfrom!=l_from->end();itfrom++) { - auto pfrom = createProximityOp(itfrom->element()); - if (pfrom == nullptr) continue; - - auto pdest = findClosestProxOp(pfrom, l_dest.get(), projectOp, getFilterFunc()); - if (pdest == nullptr) continue; - pdest->normalize(); - - - if (d_projective.getValue()) { - auto pfromProj = projectFromOp(pdest->getPosition(),itfrom->element()).prox; - if (pfromProj == nullptr) continue; - pfromProj->normalize(); - - output.add(pfromProj,pdest); - } else { - output.add(pfrom,pdest); - } - - } - - d_output.endEdit(); - } - -}; - -} - diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index d089360..29ed4b3 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -3,11 +3,11 @@ #include #include #include +#include #include #include -#include -#include #include +#include #include #include #include @@ -29,7 +29,7 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm GeomLink l_tipGeom, l_surfGeom, l_shaftGeom, l_volGeom; Data d_collisionOutput, d_insertionOutput; - Data d_projective; + Data d_projective, d_enablePuncture, d_enableInsertion, d_enableShaftCollision; Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver::SPtr m_constraintSolver; std::vector m_couplingPts; @@ -50,6 +50,12 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm d_projective(initData( &d_projective, false, "projective", "Projection of closest detected proximity back onto the needle tip element.")), + d_enablePuncture( + initData(&d_enablePuncture, true, "enablePuncture", "Enable puncture algorithm.")), + d_enableInsertion( + initData(&d_enableInsertion, true, "enableInsertion", "Enable insertion algorithm.")), + d_enableShaftCollision(initData(&d_enableShaftCollision, true, "enableShaftCollision", + "Enable shaft-surface collision.")), d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1_sreal, "punctureForceThreshold", "Threshold for the force applied to the needle tip. " @@ -70,24 +76,30 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm void init() override { BaseAlgorithm::init(); - this->getContext()->get(m_constraintSolver); - msg_warning_when(!m_constraintSolver) - << "No constraint solver found in context. Insertion algorithm is disabled."; - if (d_punctureForceThreshold.getValue() < 0) + if (d_enablePuncture.getValue()) { - msg_warning() << d_punctureForceThreshold.getName() + - " parameter not defined or set to negative value." msgendl - << "Puncture will not function properly; provide a positive value"; - d_punctureForceThreshold.setValue(std::numeric_limits::max()); + this->getContext()->get(m_constraintSolver); + msg_warning_when(!m_constraintSolver) + << "No constraint solver found in context. Insertion algorithm is disabled."; + if (d_punctureForceThreshold.getValue() < 0) + { + msg_warning() << d_punctureForceThreshold.getName() + + " parameter not defined or set to negative value." msgendl + << "Puncture will not function properly; provide a positive value"; + d_punctureForceThreshold.setValue(std::numeric_limits::max()); + } } - if (d_tipDistThreshold.getValue() < 0) + if (d_enableInsertion.getValue()) { - msg_warning() << d_tipDistThreshold.getName() + - " parameter not defined or set to negative value." msgendl - << "Needle-volume coupling is disabled; provide a positive value"; - d_tipDistThreshold.setValue(std::numeric_limits::max()); + if (d_tipDistThreshold.getValue() < 0) + { + msg_warning() << d_tipDistThreshold.getName() + + " parameter not defined or set to negative value." msgendl + << "Needle-volume coupling is disabled; provide a positive value"; + d_tipDistThreshold.setValue(std::numeric_limits::max()); + } } } @@ -118,61 +130,64 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm void doDetection() { - if (!l_tipGeom || !l_surfGeom || !l_shaftGeom || !l_volGeom) return; - auto& collisionOutput = *d_collisionOutput.beginEdit(); auto& insertionOutput = *d_insertionOutput.beginEdit(); insertionOutput.clear(); collisionOutput.clear(); - if (m_couplingPts.empty()) + if (m_couplingPts.empty() && l_surfGeom) { - // 1. Puncture algorithm - auto createTipProximity = - Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); + // Operations on surface geometry auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); - auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); - const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); - for (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++) + // Puncture sequence + if (d_enablePuncture.getValue() && l_tipGeom) { - BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - if (!tipProx) continue; - const BaseProximity::SPtr surfProx = findClosestProxOnSurf( - tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx) - { - surfProx->normalize(); + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); + auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); - // 1.1 Check whether puncture is happening - if so, create coupling point - if (m_constraintSolver) + 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) { - 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) + surfProx->normalize(); + + // Check whether puncture is happening - if so, create coupling point ... + if (m_constraintSolver) { - m_couplingPts.push_back(surfProx); - continue; + 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); + // ... if not, create a proximity pair for the tip-surface collision + collisionOutput.add(tipProx, surfProx); + } } } - // 1.3 Collision with the shaft geometry - if (m_couplingPts.empty()) + // Shaft collision sequence - Disable if coupling points have been added + if (d_enableShaftCollision.getValue() && m_couplingPts.empty() && l_shaftGeom) { auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); @@ -187,11 +202,17 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm { surfProx->normalize(); - // 1.2 If not, create a proximity pair for the tip-surface collision if (d_projective.getValue()) { - shaftProx = - projectOnShaft(surfProx->getPosition(), itShaft->element()).prox; + //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(); } @@ -202,7 +223,9 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm } else { - // 2. Needle insertion algorithm + // Insertion sequence + 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()); @@ -256,7 +279,7 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold); - for(int idCP = 0 ; idCP < numCPs ; idCP++) + for (int idCP = 0; idCP < numCPs; idCP++) { // Candidate coupling point along shaft segment const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; @@ -270,7 +293,7 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm // Skip if candidate CP is outside current edge segment if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break; - // Project candidate CP onto shaft geometry ... + // Project candidate CP onto shaft geometry ... shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; if (!shaftProx) continue; @@ -279,8 +302,8 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm 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 + // 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)) { @@ -291,11 +314,19 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm } } } + else // Don't bother with removing the point that was just added + { + // 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()) + if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom) { - // 3. Re-project proximities on the shaft geometry + // Reprojection on shaft geometry sequence auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); diff --git a/src/CollisionAlgorithm/initCollisionAlgorithm.cpp b/src/CollisionAlgorithm/initCollisionAlgorithm.cpp index ba5b084..539882a 100644 --- a/src/CollisionAlgorithm/initCollisionAlgorithm.cpp +++ b/src/CollisionAlgorithm/initCollisionAlgorithm.cpp @@ -20,7 +20,6 @@ extern void registerCollisionLoop(sofa::core::ObjectFactory* factory); // Algorithms extern void registerFind2DClosestProximityAlgorithm(sofa::core::ObjectFactory* factory); -extern void registerFindClosestProximityAlgorithm(sofa::core::ObjectFactory* factory); extern void registerInsertionAlgorithm(sofa::core::ObjectFactory* factory); // BroadPhase @@ -90,7 +89,6 @@ void registerObjects(sofa::core::ObjectFactory* factory) registerCollisionLoop(factory); // Register Algorithms registerFind2DClosestProximityAlgorithm(factory); - registerFindClosestProximityAlgorithm(factory); registerInsertionAlgorithm(factory); // Register BroadPhase registerAABBBroadPhase(factory);