From 085990b79de22d64b616a672ab275f0cd2bc6b3a Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 15:44:37 +0200 Subject: [PATCH 01/20] [src] Added the .clang-format file from main SOFA repository --- .clang-format | 168 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 168 insertions(+) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..346d2074 --- /dev/null +++ b/.clang-format @@ -0,0 +1,168 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: WithoutElse +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Allman +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 100 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + - Regex: '.*' + Priority: 3 + SortPriority: 0 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: true +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 4 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + CanonicalDelimiter: '' + BasedOnStyle: google +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Auto +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +DerivePointerAlignment: false +PointerAlignment: Left +... + From 712878ee4c0953e620c7cd4cf6304550e89ae7d9 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 13:34:52 +0200 Subject: [PATCH 02/20] [algorithm] Removed commented outputDist variable --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 4670aea7..75dc4670 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -29,7 +29,6 @@ class InsertionAlgorithm : public BaseAlgorithm { Data d_projective ; Data d_punctureThreshold ; Data d_slideDistance ; -// Data > d_outputDist; sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; std::vector m_needlePts; std::vector m_couplingPts; @@ -47,7 +46,6 @@ class InsertionAlgorithm : public BaseAlgorithm { , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) , d_punctureThreshold(initData(&d_punctureThreshold, std::numeric_limits::max(), "punctureThreshold", "Threshold for puncture detection")) , d_slideDistance(initData(&d_slideDistance, std::numeric_limits::min(), "slideDistance", "Distance along the insertion trajectory after which the proximities slide backwards along the needle shaft")) -// , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) , m_constraintSolver(nullptr) , m_needlePts() , m_couplingPts() From 9f1370ddafffe4d3973d18d42b1640513b2a5371 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 28 Jul 2025 16:42:09 +0200 Subject: [PATCH 03/20] [algorithm] Added transparency to RGBAColor for the coupling points drawn --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 75dc4670..f32e7836 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -68,8 +68,8 @@ class InsertionAlgorithm : public BaseAlgorithm { DetectionOutput outputList = d_outputList.getValue() ; for (const auto& it : outputList) { - vparams->drawTool()->drawSphere(it.first->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(1, 0, 0, 1)); - vparams->drawTool()->drawSphere(it.second->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(0, 0, 1, 1)); + vparams->drawTool()->drawSphere(it.first->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(1, 0, 1, 0.9)); + vparams->drawTool()->drawSphere(it.second->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(0, 0, 1, 0.9)); vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), sofa::type::RGBAColor(1, 1, 0, 1)); } } From e757de15115be18dc8e2bac1c61ebc79a07294c6 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 16:09:56 +0200 Subject: [PATCH 04/20] [algorithm] Bring back the mstate that was deleted --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index f32e7836..c0bbdf13 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -87,10 +87,6 @@ class InsertionAlgorithm : public BaseAlgorithm { { const sofa::core::behavior::MechanicalState* mstate = l_from->getContext()->get>(); - if (mstate->getSize() > 1) { - msg_warning() << "Requested MechanicalObject, corresponding to the tip of the needle in the InsertionAlgorithm, has a size greater than 1. " - << "The algorithm is designed to work with a single point. Only the first element will be used."; - } if (m_constraintSolver) { defaulttype::Vec3Types::VecCoord lambda = From fcfc68c838eb51101c7e46c95d04a80c312a1998 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 13:14:46 +0200 Subject: [PATCH 05/20] [algorithm] Set default boolean values for drawing to false --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index c0bbdf13..3bd5bc24 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -38,8 +38,8 @@ class InsertionAlgorithm : public BaseAlgorithm { , l_dest(initLink("destGeom", "link to dest geometry")) , l_fromVol(initLink("fromVol", "link to from geometry (volume)")) , l_destVol(initLink("destVol", "link to dest geometry (volume)")) - , d_drawCollision (initData(&d_drawCollision, true, "drawcollision", "draw collision")) - , d_drawPoints(initData(&d_drawPoints, true, "drawPoints", "draw detection outputs")) + , d_drawCollision (initData(&d_drawCollision, false, "drawcollision", "draw collision")) + , d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "draw detection outputs")) , d_sphereRadius(initData(&d_sphereRadius, 0.0005, "sphereRadius", "radius for drawing detection outputs")) , d_output(initData(&d_output,"output", "output of the collision detection")) , d_outputList(initData(&d_outputList,"outputList", "output of the detection inside the volume")) From b8b3de01099d5b365629e785c0436cc5042dd203 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 13:19:28 +0200 Subject: [PATCH 06/20] [algorithm] Change name of sphereRadius to drawPointsScale [scene] Adjust scene files --- scenes/InsertionGeomagic.py | 2 +- scenes/NeedleInsertion.py | 2 +- scenes/NeedleInsertionCycles.py | 2 +- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 8 ++++---- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/scenes/InsertionGeomagic.py b/scenes/InsertionGeomagic.py index 3c7a7413..f50b2e58 100644 --- a/scenes/InsertionGeomagic.py +++ b/scenes/InsertionGeomagic.py @@ -187,7 +187,7 @@ def createScene(root): punctureThreshold=2., slideDistance=0.003, drawcollision=True, - sphereRadius=0.0001 + drawPointsScale=0.0001 ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 4ac96001..52e135a6 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -186,7 +186,7 @@ def createScene(root): punctureThreshold=2., slideDistance=0.003, drawcollision=True, - sphereRadius=0.0001 + drawPointsScale=0.0001 ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") diff --git a/scenes/NeedleInsertionCycles.py b/scenes/NeedleInsertionCycles.py index 158af515..699139b7 100644 --- a/scenes/NeedleInsertionCycles.py +++ b/scenes/NeedleInsertionCycles.py @@ -202,7 +202,7 @@ def createScene(root): punctureThreshold=2., slideDistance=0.003, drawcollision=True, - sphereRadius=0.0001 + drawPointsScale=0.0001 ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 3bd5bc24..64eb74e2 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -23,7 +23,7 @@ class InsertionAlgorithm : public BaseAlgorithm { core::objectmodel::SingleLink l_destVol; Data d_drawCollision ; Data d_drawPoints ; - Data d_sphereRadius ; + Data d_drawPointsScale ; Data > d_output; Data > d_outputList; Data d_projective ; @@ -40,7 +40,7 @@ class InsertionAlgorithm : public BaseAlgorithm { , l_destVol(initLink("destVol", "link to dest geometry (volume)")) , d_drawCollision (initData(&d_drawCollision, false, "drawcollision", "draw collision")) , d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "draw detection outputs")) - , d_sphereRadius(initData(&d_sphereRadius, 0.0005, "sphereRadius", "radius for drawing detection outputs")) + , d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", "scale the drawing of detection output points")) , d_output(initData(&d_output,"output", "output of the collision detection")) , d_outputList(initData(&d_outputList,"outputList", "output of the detection inside the volume")) , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) @@ -68,8 +68,8 @@ class InsertionAlgorithm : public BaseAlgorithm { DetectionOutput outputList = d_outputList.getValue() ; for (const auto& it : outputList) { - vparams->drawTool()->drawSphere(it.first->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(1, 0, 1, 0.9)); - vparams->drawTool()->drawSphere(it.second->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(0, 0, 1, 0.9)); + vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), sofa::type::RGBAColor(1, 0, 1, 0.9)); + vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), sofa::type::RGBAColor(0, 0, 1, 0.9)); vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), sofa::type::RGBAColor(1, 1, 0, 1)); } } From a82662338845618e30b7b545635931873cb0eba1 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 16:26:16 +0200 Subject: [PATCH 07/20] [algorithm] Simplified syntax using typedefs for code brevity --- .../algorithm/InsertionAlgorithm.h | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 64eb74e2..f9745dea 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -17,19 +17,24 @@ class InsertionAlgorithm : public BaseAlgorithm { public: SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm); - core::objectmodel::SingleLink l_from; - core::objectmodel::SingleLink l_dest; - core::objectmodel::SingleLink l_fromVol; - core::objectmodel::SingleLink l_destVol; + typedef sofa::core::behavior::MechanicalState MechStateTipType; + typedef core::objectmodel::SingleLink GeomLink; + typedef DetectionOutput AlgorithmOutput; + typedef sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl ConstraintSolver; + + GeomLink l_from; + GeomLink l_dest; + GeomLink l_fromVol; + GeomLink l_destVol; Data d_drawCollision ; Data d_drawPoints ; Data d_drawPointsScale ; - Data > d_output; - Data > d_outputList; + Data d_output; + Data d_outputList; Data d_projective ; Data d_punctureThreshold ; Data d_slideDistance ; - sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; + ConstraintSolver* m_constraintSolver; std::vector m_needlePts; std::vector m_couplingPts; @@ -54,7 +59,7 @@ class InsertionAlgorithm : public BaseAlgorithm { void init() override { BaseAlgorithm::init(); m_constraintSolver - = this->getContext()->get(); + = this->getContext()->get(); } void draw(const core::visual::VisualParams* vparams) { @@ -85,12 +90,10 @@ class InsertionAlgorithm : public BaseAlgorithm { if (outputList.size() == 0) { - const sofa::core::behavior::MechanicalState* mstate - = l_from->getContext()->get>(); + const MechStateTipType* mstate = l_from->getContext()->get(); if (m_constraintSolver) { - defaulttype::Vec3Types::VecCoord lambda = - m_constraintSolver->getLambda()[mstate].read()->getValue(); + const auto lambda = m_constraintSolver->getLambda()[mstate].read()->getValue(); if (lambda[0].norm() > d_punctureThreshold.getValue()) { auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_fromVol); From 9babc45c084a249cbbe12e3776d8565bd5f05c3d Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 16:28:02 +0200 Subject: [PATCH 08/20] [algorithm] Removed unnecessary sofa:: namespace qualifier --- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index f9745dea..fa7dd566 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -58,8 +58,7 @@ class InsertionAlgorithm : public BaseAlgorithm { void init() override { BaseAlgorithm::init(); - m_constraintSolver - = this->getContext()->get(); + m_constraintSolver = this->getContext()->get(); } void draw(const core::visual::VisualParams* vparams) { @@ -68,14 +67,14 @@ class InsertionAlgorithm : public BaseAlgorithm { DetectionOutput output = d_output.getValue() ; for (const auto& it : output) { - vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), sofa::type::RGBAColor(0, 1, 0, 1)); + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), type::RGBAColor(0, 1, 0, 1)); } DetectionOutput outputList = d_outputList.getValue() ; for (const auto& it : outputList) { - vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), sofa::type::RGBAColor(1, 0, 1, 0.9)); - vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), sofa::type::RGBAColor(0, 0, 1, 0.9)); - vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), sofa::type::RGBAColor(1, 1, 0, 1)); + vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), type::RGBAColor(1, 0, 1, 0.9)); + vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), type::RGBAColor(0, 0, 1, 0.9)); + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), type::RGBAColor(1, 1, 0, 1)); } } From 59a3e99e9851b1ce1dc0ba0fa9cce2132224bd1b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 16:38:18 +0200 Subject: [PATCH 09/20] [algorithm] Re-organized variables' definition --- .../algorithm/InsertionAlgorithm.h | 31 +++++++------------ 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index fa7dd566..71e4a7ec 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -17,35 +17,25 @@ class InsertionAlgorithm : public BaseAlgorithm { public: SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm); - typedef sofa::core::behavior::MechanicalState MechStateTipType; + typedef core::behavior::MechanicalState MechStateTipType; typedef core::objectmodel::SingleLink GeomLink; typedef DetectionOutput AlgorithmOutput; - typedef sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl ConstraintSolver; - - GeomLink l_from; - GeomLink l_dest; - GeomLink l_fromVol; - GeomLink l_destVol; - Data d_drawCollision ; - Data d_drawPoints ; - Data d_drawPointsScale ; - Data d_output; - Data d_outputList; + typedef component::constraint::lagrangian::solver::ConstraintSolverImpl ConstraintSolver; + + GeomLink l_from, l_dest, l_fromVol, l_destVol; + Data d_output, d_outputList; Data d_projective ; - Data d_punctureThreshold ; - Data d_slideDistance ; + Data d_punctureThreshold, d_slideDistance; ConstraintSolver* m_constraintSolver; - std::vector m_needlePts; - std::vector m_couplingPts; + std::vector m_needlePts, m_couplingPts; + Data d_drawCollision, d_drawPoints ; + Data d_drawPointsScale ; InsertionAlgorithm() : l_from(initLink("fromGeom", "link to from geometry")) , l_dest(initLink("destGeom", "link to dest geometry")) , l_fromVol(initLink("fromVol", "link to from geometry (volume)")) , l_destVol(initLink("destVol", "link to dest geometry (volume)")) - , d_drawCollision (initData(&d_drawCollision, false, "drawcollision", "draw collision")) - , d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "draw detection outputs")) - , d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", "scale the drawing of detection output points")) , d_output(initData(&d_output,"output", "output of the collision detection")) , d_outputList(initData(&d_outputList,"outputList", "output of the detection inside the volume")) , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) @@ -54,6 +44,9 @@ class InsertionAlgorithm : public BaseAlgorithm { , m_constraintSolver(nullptr) , m_needlePts() , m_couplingPts() + , d_drawCollision (initData(&d_drawCollision, false, "drawcollision", "draw collision")) + , d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "draw detection outputs")) + , d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", "scale the drawing of detection output points")) {} void init() override { From 401a162db7d527108cbf482038aae50cd7d17a47 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 16:51:24 +0200 Subject: [PATCH 10/20] [algorithm] Renamed detection output variables to distinguish their use for either puncture or insertion --- scenes/InsertionGeomagic.py | 4 +- scenes/NeedleInsertion.py | 4 +- scenes/NeedleInsertionCycles.py | 4 +- .../algorithm/InsertionAlgorithm.h | 40 +++++++++---------- 4 files changed, 26 insertions(+), 26 deletions(-) diff --git a/scenes/InsertionGeomagic.py b/scenes/InsertionGeomagic.py index f50b2e58..69b8593e 100644 --- a/scenes/InsertionGeomagic.py +++ b/scenes/InsertionGeomagic.py @@ -191,7 +191,7 @@ def createScene(root): ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") - root.addObject("ConstraintUnilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale=0.001, mu=0.001) + root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001, mu=0.001) root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams") - root.addObject("ConstraintInsertion",input="@InsertionAlgo.outputList", directions="@bindDirection",draw_scale="0.01", frictionCoeff=0.000) + root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale="0.01", frictionCoeff=0.000) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 52e135a6..f7c0d0c3 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -190,7 +190,7 @@ def createScene(root): ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") - root.addObject("ConstraintUnilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale=0.001) + root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001) root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams") - root.addObject("ConstraintInsertion",input="@InsertionAlgo.outputList", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.05) + root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.05) diff --git a/scenes/NeedleInsertionCycles.py b/scenes/NeedleInsertionCycles.py index 699139b7..2e099475 100644 --- a/scenes/NeedleInsertionCycles.py +++ b/scenes/NeedleInsertionCycles.py @@ -206,7 +206,7 @@ def createScene(root): ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") - root.addObject("ConstraintUnilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale=0.001, mu=0.001) + root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001, mu=0.001) root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams") - root.addObject("ConstraintInsertion",input="@InsertionAlgo.outputList", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.0023) + root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.0023) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 71e4a7ec..e7eef7f6 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -23,7 +23,7 @@ class InsertionAlgorithm : public BaseAlgorithm { typedef component::constraint::lagrangian::solver::ConstraintSolverImpl ConstraintSolver; GeomLink l_from, l_dest, l_fromVol, l_destVol; - Data d_output, d_outputList; + Data d_collisionOutput, d_insertionOutput; Data d_projective ; Data d_punctureThreshold, d_slideDistance; ConstraintSolver* m_constraintSolver; @@ -36,8 +36,8 @@ class InsertionAlgorithm : public BaseAlgorithm { , l_dest(initLink("destGeom", "link to dest geometry")) , l_fromVol(initLink("fromVol", "link to from geometry (volume)")) , l_destVol(initLink("destVol", "link to dest geometry (volume)")) - , d_output(initData(&d_output,"output", "output of the collision detection")) - , d_outputList(initData(&d_outputList,"outputList", "output of the detection inside the volume")) + , d_collisionOutput(initData(&d_collisionOutput,"collisionOutput", "detected proximities during puncture")) + , d_insertionOutput(initData(&d_insertionOutput,"insertionOutput", "detected proximities during insertion")) , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) , d_punctureThreshold(initData(&d_punctureThreshold, std::numeric_limits::max(), "punctureThreshold", "Threshold for puncture detection")) , d_slideDistance(initData(&d_slideDistance, std::numeric_limits::min(), "slideDistance", "Distance along the insertion trajectory after which the proximities slide backwards along the needle shaft")) @@ -58,13 +58,13 @@ class InsertionAlgorithm : public BaseAlgorithm { if (! vparams->displayFlags().getShowCollisionModels() && ! d_drawCollision.getValue()) return; vparams->drawTool()->disableLighting(); - DetectionOutput output = d_output.getValue() ; - for (const auto& it : output) { + DetectionOutput collisionOutput = d_collisionOutput.getValue() ; + for (const auto& it : collisionOutput) { vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), type::RGBAColor(0, 1, 0, 1)); } - DetectionOutput outputList = d_outputList.getValue() ; - for (const auto& it : outputList) { + DetectionOutput insertionOutput = d_insertionOutput.getValue() ; + for (const auto& it : insertionOutput) { vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), type::RGBAColor(1, 0, 1, 0.9)); vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), type::RGBAColor(0, 0, 1, 0.9)); vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), type::RGBAColor(1, 1, 0, 1)); @@ -77,10 +77,10 @@ class InsertionAlgorithm : public BaseAlgorithm { if (l_fromVol == NULL) return; if (l_destVol == NULL) return; - auto& output = *d_output.beginEdit(); - auto& outputList = *d_outputList.beginEdit(); + auto& collisionOutput = *d_collisionOutput.beginEdit(); + auto& insertionOutput = *d_insertionOutput.beginEdit(); - if (outputList.size() == 0) + if (insertionOutput.size() == 0) { const MechStateTipType* mstate = l_from->getContext()->get(); if (m_constraintSolver) @@ -90,20 +90,20 @@ class InsertionAlgorithm : public BaseAlgorithm { { auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_fromVol); auto projectOp_needle = Operations::Project::Operation::get(l_fromVol); - for (const auto& dpair : output) + for (const auto& dpair : collisionOutput) { // Reproject onto the needle to create an EdgeProximity - The EdgeHandler requires this auto pfromVol = findClosestProxOp_needle(dpair.second, l_fromVol.get(), projectOp_needle, getFilterFunc()); m_needlePts.push_back(pfromVol); m_couplingPts.push_back(dpair.second->copy()); - outputList.add(pfromVol, dpair.second->copy()); + insertionOutput.add(pfromVol, dpair.second->copy()); } - output.clear(); + collisionOutput.clear(); return; } } - output.clear(); + collisionOutput.clear(); auto itfrom = l_from->begin(); @@ -125,17 +125,17 @@ class InsertionAlgorithm : public BaseAlgorithm { if (pfromProj == nullptr) continue; pfromProj->normalize(); - output.add(pfromProj, pdest); + collisionOutput.add(pfromProj, pdest); } else { - output.add(pfrom, pdest); + collisionOutput.add(pfrom, pdest); } } } } else { - outputList.clear(); + insertionOutput.clear(); auto itfrom = l_from->begin(); auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); @@ -179,11 +179,11 @@ class InsertionAlgorithm : public BaseAlgorithm { } for(int i = 0 ; i < m_couplingPts.size(); i++) - outputList.add(m_needlePts[i], m_couplingPts[i]); + insertionOutput.add(m_needlePts[i], m_couplingPts[i]); } - d_output.endEdit(); - d_outputList.endEdit(); + d_collisionOutput.endEdit(); + d_insertionOutput.endEdit(); } }; From 44d80f4f10e367be0353163a3ab0875658fe49bf Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 29 Jul 2025 17:03:34 +0200 Subject: [PATCH 11/20] [algorithm] Renamed link variables to BaseGeometry components [scene] Changed scenes accordingly * Signify that the link is directed at a Geometry object * For the needle, the name signifies whether the Geometry refers to the tip or the shaft * For the punctured volume, the name signifies whether the Geometry will be used for: 1. the puncture and, thus, only the surface is of interest or 2. the insertion and, thus, the volume is now of interest * The user should not care whether puncture/insertion happens "from" the needle to the "destination" surface/volume. The user should be concerned with appropriately defining the tip and shaft geometry as well as the geometries of the punctured volumes and surfaces. --- scenes/InsertionGeomagic.py | 8 +-- scenes/NeedleInsertion.py | 8 +-- scenes/NeedleInsertionCycles.py | 8 +-- .../algorithm/InsertionAlgorithm.h | 56 +++++++++---------- 4 files changed, 40 insertions(+), 40 deletions(-) diff --git a/scenes/InsertionGeomagic.py b/scenes/InsertionGeomagic.py index 69b8593e..d62a6780 100644 --- a/scenes/InsertionGeomagic.py +++ b/scenes/InsertionGeomagic.py @@ -180,10 +180,10 @@ def createScene(root): root.addObject("InsertionAlgorithm", name="InsertionAlgo", - fromGeom="@Needle/tipCollision/geom_tip", - destGeom="@Volume/collision/geom_tri", - fromVol="@Needle/bodyCollision/geom_body", - destVol="@Volume/geom_tetra", + tipGeom="@Needle/tipCollision/geom_tip", + surfGeom="@Volume/collision/geom_tri", + shaftGeom="@Needle/bodyCollision/geom_body", + volGeom="@Volume/geom_tetra", punctureThreshold=2., slideDistance=0.003, drawcollision=True, diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index f7c0d0c3..9aaf6df4 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -179,10 +179,10 @@ def createScene(root): root.addObject("InsertionAlgorithm", name="InsertionAlgo", - fromGeom="@Needle/tipCollision/geom_tip", - destGeom="@Volume/collision/geom_tri", - fromVol="@Needle/bodyCollision/geom_body", - destVol="@Volume/geom_tetra", + tipGeom="@Needle/tipCollision/geom_tip", + surfGeom="@Volume/collision/geom_tri", + shaftGeom="@Needle/bodyCollision/geom_body", + volGeom="@Volume/geom_tetra", punctureThreshold=2., slideDistance=0.003, drawcollision=True, diff --git a/scenes/NeedleInsertionCycles.py b/scenes/NeedleInsertionCycles.py index 2e099475..4b5b5a2c 100644 --- a/scenes/NeedleInsertionCycles.py +++ b/scenes/NeedleInsertionCycles.py @@ -195,10 +195,10 @@ def createScene(root): root.addObject("InsertionAlgorithm", name="InsertionAlgo", - fromGeom="@Needle/tipCollision/geom_tip", - destGeom="@Volume/collision/geom_tri", - fromVol="@Needle/bodyCollision/geom_body", - destVol="@Volume/geom_tetra", + tipGeom="@Needle/tipCollision/geom_tip", + surfGeom="@Volume/collision/geom_tri", + shaftGeom="@Needle/bodyCollision/geom_body", + volGeom="@Volume/geom_tetra", punctureThreshold=2., slideDistance=0.003, drawcollision=True, diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index e7eef7f6..31dbf987 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -22,7 +22,7 @@ class InsertionAlgorithm : public BaseAlgorithm { typedef DetectionOutput AlgorithmOutput; typedef component::constraint::lagrangian::solver::ConstraintSolverImpl ConstraintSolver; - GeomLink l_from, l_dest, l_fromVol, l_destVol; + GeomLink l_tipGeom, l_surfGeom, l_shaftGeom, l_volGeom; Data d_collisionOutput, d_insertionOutput; Data d_projective ; Data d_punctureThreshold, d_slideDistance; @@ -32,10 +32,10 @@ class InsertionAlgorithm : public BaseAlgorithm { Data d_drawPointsScale ; InsertionAlgorithm() - : l_from(initLink("fromGeom", "link to from geometry")) - , l_dest(initLink("destGeom", "link to dest geometry")) - , l_fromVol(initLink("fromVol", "link to from geometry (volume)")) - , l_destVol(initLink("destVol", "link to dest geometry (volume)")) + : l_tipGeom(initLink("tipGeom", "link to the geometry structure of the needle tip")) + , l_surfGeom(initLink("surfGeom", "link to the geometry of the surface punctured by the needle")) + , l_shaftGeom(initLink("shaftGeom", "link to the geometry structure of the needle shaft")) + , l_volGeom(initLink("volGeom", "link to the geometry of volume wherein the needle is inserted")) , d_collisionOutput(initData(&d_collisionOutput,"collisionOutput", "detected proximities during puncture")) , d_insertionOutput(initData(&d_insertionOutput,"insertionOutput", "detected proximities during insertion")) , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) @@ -72,28 +72,28 @@ class InsertionAlgorithm : public BaseAlgorithm { } void doDetection() { - if (l_from == NULL) return; - if (l_dest == NULL) return; - if (l_fromVol == NULL) return; - if (l_destVol == NULL) return; + if (l_tipGeom == NULL) return; + if (l_surfGeom == NULL) return; + if (l_shaftGeom == NULL) return; + if (l_volGeom == NULL) return; auto& collisionOutput = *d_collisionOutput.beginEdit(); auto& insertionOutput = *d_insertionOutput.beginEdit(); if (insertionOutput.size() == 0) { - const MechStateTipType* mstate = l_from->getContext()->get(); + const MechStateTipType* mstate = l_tipGeom->getContext()->get(); if (m_constraintSolver) { const auto lambda = m_constraintSolver->getLambda()[mstate].read()->getValue(); if (lambda[0].norm() > d_punctureThreshold.getValue()) { - auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_fromVol); - auto projectOp_needle = Operations::Project::Operation::get(l_fromVol); + auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto projectOp_needle = Operations::Project::Operation::get(l_shaftGeom); for (const auto& dpair : collisionOutput) { // Reproject onto the needle to create an EdgeProximity - The EdgeHandler requires this - auto pfromVol = findClosestProxOp_needle(dpair.second, l_fromVol.get(), projectOp_needle, getFilterFunc()); + auto pfromVol = findClosestProxOp_needle(dpair.second, l_shaftGeom.get(), projectOp_needle, getFilterFunc()); m_needlePts.push_back(pfromVol); m_couplingPts.push_back(dpair.second->copy()); insertionOutput.add(pfromVol, dpair.second->copy()); @@ -105,18 +105,18 @@ class InsertionAlgorithm : public BaseAlgorithm { collisionOutput.clear(); - auto itfrom = l_from->begin(); + auto itfrom = l_tipGeom->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); + auto findClosestProxOp = Operations::FindClosestProximity::Operation::get(l_surfGeom); + auto projectOp = Operations::Project::Operation::get(l_surfGeom); + auto projectFromOp = Operations::Project::Operation::get(l_tipGeom); - for (; itfrom != l_from->end(); itfrom++) + for (; itfrom != l_tipGeom->end(); itfrom++) { auto pfrom = createProximityOp(itfrom->element()); if (pfrom == nullptr) continue; - auto pdest = findClosestProxOp(pfrom, l_dest.get(), projectOp, getFilterFunc()); + auto pdest = findClosestProxOp(pfrom, l_surfGeom.get(), projectOp, getFilterFunc()); if (pdest != nullptr) { pdest->normalize(); @@ -137,11 +137,11 @@ class InsertionAlgorithm : public BaseAlgorithm { { insertionOutput.clear(); - auto itfrom = l_from->begin(); + auto itfrom = l_tipGeom->begin(); auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); auto pfrom = createProximityOp(itfrom->element()); - auto itfromVol = l_fromVol->begin(l_fromVol->getSize() - 2); + auto itfromVol = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); auto createProximityOpVol = Operations::CreateCenterProximity::Operation::get(itfromVol->getTypeInfo()); auto pfromVol = createProximityOpVol(itfromVol->element()); const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(pfromVol); @@ -157,10 +157,10 @@ class InsertionAlgorithm : public BaseAlgorithm { const SReal dist = ab.norm(); if(dist > d_slideDistance.getValue()) { - auto findClosestProxOp_vol = Operations::FindClosestProximity::Operation::get(l_destVol); - auto projectOp_vol = Operations::Project::Operation::get(l_destVol); - auto projectFromOp_vol = Operations::Project::Operation::get(l_fromVol); - auto pdestVol = findClosestProxOp_vol(pfrom, l_destVol.get(), projectOp_vol, getFilterFunc()); + auto findClosestProxOp_vol = Operations::FindClosestProximity::Operation::get(l_volGeom); + auto projectOp_vol = Operations::Project::Operation::get(l_volGeom); + auto projectFromOp_vol = Operations::Project::Operation::get(l_shaftGeom); + auto pdestVol = findClosestProxOp_vol(pfrom, l_volGeom.get(), projectOp_vol, getFilterFunc()); if (pdestVol) { pdestVol->normalize(); @@ -169,12 +169,12 @@ class InsertionAlgorithm : public BaseAlgorithm { } } - auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_fromVol); - auto projectOp_needle = Operations::Project::Operation::get(l_fromVol); + auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto projectOp_needle = Operations::Project::Operation::get(l_shaftGeom); for(int i = 0 ; i < m_couplingPts.size(); i++) { - auto pfromVol = findClosestProxOp_needle(m_couplingPts[i], l_fromVol.get(), projectOp_needle, getFilterFunc()); + auto pfromVol = findClosestProxOp_needle(m_couplingPts[i], l_shaftGeom.get(), projectOp_needle, getFilterFunc()); m_needlePts[i] = pfromVol; } From c9554a774b9d631687e570a5308057cf2b48fd65 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 13:48:53 +0200 Subject: [PATCH 12/20] [algorithm] Renamed Geometry iterators, Operations specializations and returned proximities --- .../algorithm/InsertionAlgorithm.h | 77 +++++++++---------- 1 file changed, 38 insertions(+), 39 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 31dbf987..2150fa54 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -88,15 +88,15 @@ class InsertionAlgorithm : public BaseAlgorithm { const auto lambda = m_constraintSolver->getLambda()[mstate].read()->getValue(); if (lambda[0].norm() > d_punctureThreshold.getValue()) { - auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_shaftGeom); - auto projectOp_needle = Operations::Project::Operation::get(l_shaftGeom); + auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); for (const auto& dpair : collisionOutput) { - // Reproject onto the needle to create an EdgeProximity - The EdgeHandler requires this - auto pfromVol = findClosestProxOp_needle(dpair.second, l_shaftGeom.get(), projectOp_needle, getFilterFunc()); - m_needlePts.push_back(pfromVol); + // Reproject onto the needle to create an EdgeProximity - the EdgeNormalHandler in the Constraint classes will need this + auto shaftProx = findClosestProxOnShaft(dpair.second, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + m_needlePts.push_back(shaftProx); m_couplingPts.push_back(dpair.second->copy()); - insertionOutput.add(pfromVol, dpair.second->copy()); + insertionOutput.add(shaftProx, dpair.second->copy()); } collisionOutput.clear(); return; @@ -105,30 +105,30 @@ class InsertionAlgorithm : public BaseAlgorithm { collisionOutput.clear(); - auto itfrom = l_tipGeom->begin(); + auto itTip = l_tipGeom->begin(); - auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); - auto findClosestProxOp = Operations::FindClosestProximity::Operation::get(l_surfGeom); - auto projectOp = Operations::Project::Operation::get(l_surfGeom); - auto projectFromOp = Operations::Project::Operation::get(l_tipGeom); + auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); + auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); + auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); - for (; itfrom != l_tipGeom->end(); itfrom++) + for (; itTip != l_tipGeom->end(); itTip++) { - auto pfrom = createProximityOp(itfrom->element()); - if (pfrom == nullptr) continue; - auto pdest = findClosestProxOp(pfrom, l_surfGeom.get(), projectOp, getFilterFunc()); - if (pdest != nullptr) { - pdest->normalize(); + auto tipProx = createTipProximity(itTip->element()); + if (tipProx == nullptr) continue; + auto surfProx = findClosestProxOnSurf(tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); + if (surfProx != nullptr) { + surfProx->normalize(); if (d_projective.getValue()) { - auto pfromProj = projectFromOp(pdest->getPosition(), itfrom->element()).prox; + auto pfromProj = projectOnTip(surfProx->getPosition(), itTip->element()).prox; if (pfromProj == nullptr) continue; pfromProj->normalize(); - collisionOutput.add(pfromProj, pdest); + collisionOutput.add(pfromProj, surfProx); } else { - collisionOutput.add(pfrom, pdest); + collisionOutput.add(tipProx, surfProx); } } } @@ -137,16 +137,16 @@ class InsertionAlgorithm : public BaseAlgorithm { { insertionOutput.clear(); - auto itfrom = l_tipGeom->begin(); - auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); - auto pfrom = createProximityOp(itfrom->element()); + auto itTip = l_tipGeom->begin(); + auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + auto tipProx = createTipProximity(itTip->element()); - auto itfromVol = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); - auto createProximityOpVol = Operations::CreateCenterProximity::Operation::get(itfromVol->getTypeInfo()); - auto pfromVol = createProximityOpVol(itfromVol->element()); - const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(pfromVol); + auto itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); + auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); + auto shaftProx = createShaftProximity(itShaft->element()); + const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()).normalized(); - type::Vec3 ab = m_couplingPts.back()->getPosition() - pfrom->getPosition(); + type::Vec3 ab = m_couplingPts.back()->getPosition() - tipProx->getPosition(); const SReal dotProd = dot(ab, normal); if (dotProd > 0.0) { @@ -157,25 +157,24 @@ class InsertionAlgorithm : public BaseAlgorithm { const SReal dist = ab.norm(); if(dist > d_slideDistance.getValue()) { - auto findClosestProxOp_vol = Operations::FindClosestProximity::Operation::get(l_volGeom); - auto projectOp_vol = Operations::Project::Operation::get(l_volGeom); - auto projectFromOp_vol = Operations::Project::Operation::get(l_shaftGeom); - auto pdestVol = findClosestProxOp_vol(pfrom, l_volGeom.get(), projectOp_vol, getFilterFunc()); - if (pdestVol) + auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); + auto projectOnVol = Operations::Project::Operation::get(l_volGeom); + auto volProx = findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + if (volProx) { - pdestVol->normalize(); - m_couplingPts.push_back(pdestVol); + volProx->normalize(); + m_couplingPts.push_back(volProx); m_needlePts.push_back(m_needlePts.back()); } } - auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_shaftGeom); - auto projectOp_needle = Operations::Project::Operation::get(l_shaftGeom); + auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); for(int i = 0 ; i < m_couplingPts.size(); i++) { - auto pfromVol = findClosestProxOp_needle(m_couplingPts[i], l_shaftGeom.get(), projectOp_needle, getFilterFunc()); - m_needlePts[i] = pfromVol; + auto shaftProx = findClosestProxOnShaft(m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + m_needlePts[i] = shaftProx; } for(int i = 0 ; i < m_couplingPts.size(); i++) From 000c3db11859d9c8a1568d796e10bfde122796b9 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 11:16:31 +0200 Subject: [PATCH 13/20] [algorithm] Renamed threshold variables and re-wrote descriptions [scene] Adjusted scenes accordingly --- scenes/InsertionGeomagic.py | 4 +-- scenes/NeedleInsertion.py | 4 +-- scenes/NeedleInsertionCycles.py | 4 +-- .../algorithm/InsertionAlgorithm.h | 30 +++++++++---------- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/scenes/InsertionGeomagic.py b/scenes/InsertionGeomagic.py index d62a6780..4eaacc77 100644 --- a/scenes/InsertionGeomagic.py +++ b/scenes/InsertionGeomagic.py @@ -184,8 +184,8 @@ def createScene(root): surfGeom="@Volume/collision/geom_tri", shaftGeom="@Needle/bodyCollision/geom_body", volGeom="@Volume/geom_tetra", - punctureThreshold=2., - slideDistance=0.003, + punctureForceThreshold=2., + tipDistThreshold=0.003, drawcollision=True, drawPointsScale=0.0001 ) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 9aaf6df4..7213c843 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -183,8 +183,8 @@ def createScene(root): surfGeom="@Volume/collision/geom_tri", shaftGeom="@Needle/bodyCollision/geom_body", volGeom="@Volume/geom_tetra", - punctureThreshold=2., - slideDistance=0.003, + punctureForceThreshold=2., + tipDistThreshold=0.003, drawcollision=True, drawPointsScale=0.0001 ) diff --git a/scenes/NeedleInsertionCycles.py b/scenes/NeedleInsertionCycles.py index 4b5b5a2c..522b9c3f 100644 --- a/scenes/NeedleInsertionCycles.py +++ b/scenes/NeedleInsertionCycles.py @@ -199,8 +199,8 @@ def createScene(root): surfGeom="@Volume/collision/geom_tri", shaftGeom="@Needle/bodyCollision/geom_body", volGeom="@Volume/geom_tetra", - punctureThreshold=2., - slideDistance=0.003, + punctureForceThreshold=2., + tipDistThreshold=0.003, drawcollision=True, drawPointsScale=0.0001 ) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 2150fa54..192ee48f 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -25,28 +25,28 @@ class InsertionAlgorithm : public BaseAlgorithm { GeomLink l_tipGeom, l_surfGeom, l_shaftGeom, l_volGeom; Data d_collisionOutput, d_insertionOutput; Data d_projective ; - Data d_punctureThreshold, d_slideDistance; + Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver* m_constraintSolver; std::vector m_needlePts, m_couplingPts; Data d_drawCollision, d_drawPoints ; Data d_drawPointsScale ; InsertionAlgorithm() - : l_tipGeom(initLink("tipGeom", "link to the geometry structure of the needle tip")) - , l_surfGeom(initLink("surfGeom", "link to the geometry of the surface punctured by the needle")) - , l_shaftGeom(initLink("shaftGeom", "link to the geometry structure of the needle shaft")) - , l_volGeom(initLink("volGeom", "link to the geometry of volume wherein the needle is inserted")) - , d_collisionOutput(initData(&d_collisionOutput,"collisionOutput", "detected proximities during puncture")) - , d_insertionOutput(initData(&d_insertionOutput,"insertionOutput", "detected proximities during insertion")) - , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) - , d_punctureThreshold(initData(&d_punctureThreshold, std::numeric_limits::max(), "punctureThreshold", "Threshold for puncture detection")) - , d_slideDistance(initData(&d_slideDistance, std::numeric_limits::min(), "slideDistance", "Distance along the insertion trajectory after which the proximities slide backwards along the needle shaft")) + : l_tipGeom(initLink("tipGeom", "Link to the geometry structure of the needle tip.")) + , l_surfGeom(initLink("surfGeom", "Link to the geometry of the surface punctured by the needle.")) + , l_shaftGeom(initLink("shaftGeom", "Link to the geometry structure of the needle shaft.")) + , l_volGeom(initLink("volGeom", "Link to the geometry of volume wherein the needle is inserted.")) + , d_collisionOutput(initData(&d_collisionOutput,"collisionOutput", "Detected proximities during puncture.")) + , d_insertionOutput(initData(&d_insertionOutput,"insertionOutput", "Detected proximities during insertion.")) + , d_projective(initData(&d_projective, false,"projective", "Projection of closest detected proximity back onto the needle tip element.")) + , d_punctureForceThreshold(initData(&d_punctureForceThreshold, std::numeric_limits::max(), "punctureForceThreshold", "Threshold for the force applied to the needle tip. Once exceeded, puncture is initiated.")) + , d_tipDistThreshold(initData(&d_tipDistThreshold, std::numeric_limits::min(), "tipDistThreshold", "Threshold for the distance advanced by the needle tip since the last proximity detection. Once exceeded, a new proximity pair is added for the needle-volume coupling.")) , m_constraintSolver(nullptr) , m_needlePts() , m_couplingPts() - , d_drawCollision (initData(&d_drawCollision, false, "drawcollision", "draw collision")) - , d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "draw detection outputs")) - , d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", "scale the drawing of detection output points")) + , d_drawCollision (initData(&d_drawCollision, false, "drawcollision", "Draw collision.")) + , d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")) + , d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", "Scale the drawing of detection output points.")) {} void init() override { @@ -86,7 +86,7 @@ class InsertionAlgorithm : public BaseAlgorithm { if (m_constraintSolver) { const auto lambda = m_constraintSolver->getLambda()[mstate].read()->getValue(); - if (lambda[0].norm() > d_punctureThreshold.getValue()) + if (lambda[0].norm() > d_punctureForceThreshold.getValue()) { auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); @@ -155,7 +155,7 @@ class InsertionAlgorithm : public BaseAlgorithm { } const SReal dist = ab.norm(); - if(dist > d_slideDistance.getValue()) + if(dist > d_tipDistThreshold.getValue()) { auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); auto projectOnVol = Operations::Project::Operation::get(l_volGeom); From 5ac43802c4e1a2696bcadbd48eadbc3e62a692e6 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 11:54:11 +0200 Subject: [PATCH 14/20] [algorithm] Simplified link check in doDetection --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 192ee48f..0eb7b3f3 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -72,10 +72,7 @@ class InsertionAlgorithm : public BaseAlgorithm { } void doDetection() { - if (l_tipGeom == NULL) return; - if (l_surfGeom == NULL) return; - if (l_shaftGeom == NULL) return; - if (l_volGeom == NULL) return; + if (!l_tipGeom || !l_surfGeom || !l_shaftGeom || !l_volGeom) return; auto& collisionOutput = *d_collisionOutput.beginEdit(); auto& insertionOutput = *d_insertionOutput.beginEdit(); From b84ffb456960a6576c61a799fcef4136929c4c7e Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 14:31:10 +0200 Subject: [PATCH 15/20] [algorithm] Simplified pointer checks against nullptr --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 0eb7b3f3..28547ef0 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -112,14 +112,14 @@ class InsertionAlgorithm : public BaseAlgorithm { for (; itTip != l_tipGeom->end(); itTip++) { auto tipProx = createTipProximity(itTip->element()); - if (tipProx == nullptr) continue; + if (!tipProx) continue; auto surfProx = findClosestProxOnSurf(tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx != nullptr) { + if (surfProx) { surfProx->normalize(); if (d_projective.getValue()) { auto pfromProj = projectOnTip(surfProx->getPosition(), itTip->element()).prox; - if (pfromProj == nullptr) continue; + if (!pfromProj) continue; pfromProj->normalize(); collisionOutput.add(pfromProj, surfProx); From d3a7817587b7a839f596ec95b63903503f149c4b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 14:36:44 +0200 Subject: [PATCH 16/20] [algorithm] Format code using .clang-format shipped with main SOFA distro --- .../algorithm/InsertionAlgorithm.h | 181 +++++++++++------- 1 file changed, 112 insertions(+), 69 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 28547ef0..46fc4675 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -4,93 +4,125 @@ #include #include #include -#include #include -#include -#include - +#include #include +#include +#include -namespace sofa::collisionAlgorithm { +namespace sofa::collisionAlgorithm +{ -class InsertionAlgorithm : public BaseAlgorithm { -public: +class InsertionAlgorithm : public BaseAlgorithm +{ + public: SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm); typedef core::behavior::MechanicalState MechStateTipType; - typedef core::objectmodel::SingleLink GeomLink; - typedef DetectionOutput AlgorithmOutput; + typedef core::objectmodel::SingleLink + GeomLink; + typedef DetectionOutput AlgorithmOutput; typedef component::constraint::lagrangian::solver::ConstraintSolverImpl ConstraintSolver; GeomLink l_tipGeom, l_surfGeom, l_shaftGeom, l_volGeom; Data d_collisionOutput, d_insertionOutput; - Data d_projective ; + Data d_projective; Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver* m_constraintSolver; std::vector m_needlePts, m_couplingPts; - Data d_drawCollision, d_drawPoints ; - Data d_drawPointsScale ; + Data d_drawCollision, d_drawPoints; + Data d_drawPointsScale; InsertionAlgorithm() - : l_tipGeom(initLink("tipGeom", "Link to the geometry structure of the needle tip.")) - , l_surfGeom(initLink("surfGeom", "Link to the geometry of the surface punctured by the needle.")) - , l_shaftGeom(initLink("shaftGeom", "Link to the geometry structure of the needle shaft.")) - , l_volGeom(initLink("volGeom", "Link to the geometry of volume wherein the needle is inserted.")) - , d_collisionOutput(initData(&d_collisionOutput,"collisionOutput", "Detected proximities during puncture.")) - , d_insertionOutput(initData(&d_insertionOutput,"insertionOutput", "Detected proximities during insertion.")) - , d_projective(initData(&d_projective, false,"projective", "Projection of closest detected proximity back onto the needle tip element.")) - , d_punctureForceThreshold(initData(&d_punctureForceThreshold, std::numeric_limits::max(), "punctureForceThreshold", "Threshold for the force applied to the needle tip. Once exceeded, puncture is initiated.")) - , d_tipDistThreshold(initData(&d_tipDistThreshold, std::numeric_limits::min(), "tipDistThreshold", "Threshold for the distance advanced by the needle tip since the last proximity detection. Once exceeded, a new proximity pair is added for the needle-volume coupling.")) - , m_constraintSolver(nullptr) - , m_needlePts() - , m_couplingPts() - , d_drawCollision (initData(&d_drawCollision, false, "drawcollision", "Draw collision.")) - , d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")) - , d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", "Scale the drawing of detection output points.")) - {} - - void init() override { + : l_tipGeom(initLink("tipGeom", "Link to the geometry structure of the needle tip.")), + l_surfGeom( + initLink("surfGeom", "Link to the geometry of the surface punctured by the needle.")), + l_shaftGeom(initLink("shaftGeom", "Link to the geometry structure of the needle shaft.")), + l_volGeom(initLink("volGeom", + "Link to the geometry of volume wherein the needle is inserted.")), + d_collisionOutput(initData(&d_collisionOutput, "collisionOutput", + "Detected proximities during puncture.")), + d_insertionOutput(initData(&d_insertionOutput, "insertionOutput", + "Detected proximities during insertion.")), + d_projective(initData( + &d_projective, false, "projective", + "Projection of closest detected proximity back onto the needle tip element.")), + d_punctureForceThreshold(initData(&d_punctureForceThreshold, + std::numeric_limits::max(), + "punctureForceThreshold", + "Threshold for the force applied to the needle tip. " + "Once exceeded, puncture is initiated.")), + d_tipDistThreshold(initData(&d_tipDistThreshold, std::numeric_limits::min(), + "tipDistThreshold", + "Threshold for the distance advanced by the needle tip since " + "the last proximity detection. Once exceeded, a new " + "proximity pair is added for the needle-volume coupling.")), + m_constraintSolver(nullptr), + m_needlePts(), + m_couplingPts(), + d_drawCollision(initData(&d_drawCollision, false, "drawcollision", "Draw collision.")), + d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")), + d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", + "Scale the drawing of detection output points.")) + { + } + + void init() override + { BaseAlgorithm::init(); m_constraintSolver = this->getContext()->get(); } - void draw(const core::visual::VisualParams* vparams) { - if (! vparams->displayFlags().getShowCollisionModels() && ! d_drawCollision.getValue()) return; + void draw(const core::visual::VisualParams* vparams) + { + if (!vparams->displayFlags().getShowCollisionModels() && !d_drawCollision.getValue()) + return; vparams->drawTool()->disableLighting(); - DetectionOutput collisionOutput = d_collisionOutput.getValue() ; - for (const auto& it : collisionOutput) { - vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), type::RGBAColor(0, 1, 0, 1)); + DetectionOutput collisionOutput = d_collisionOutput.getValue(); + for (const auto& it : collisionOutput) + { + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), + type::RGBAColor(0, 1, 0, 1)); } - DetectionOutput insertionOutput = d_insertionOutput.getValue() ; - for (const auto& it : insertionOutput) { - vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), type::RGBAColor(1, 0, 1, 0.9)); - vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), type::RGBAColor(0, 0, 1, 0.9)); - vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), type::RGBAColor(1, 1, 0, 1)); + DetectionOutput insertionOutput = d_insertionOutput.getValue(); + for (const auto& it : insertionOutput) + { + vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), + type::RGBAColor(1, 0, 1, 0.9)); + vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), + type::RGBAColor(0, 0, 1, 0.9)); + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), + type::RGBAColor(1, 1, 0, 1)); } } - void doDetection() { + void doDetection() + { if (!l_tipGeom || !l_surfGeom || !l_shaftGeom || !l_volGeom) return; auto& collisionOutput = *d_collisionOutput.beginEdit(); auto& insertionOutput = *d_insertionOutput.beginEdit(); - if (insertionOutput.size() == 0) + if (insertionOutput.size() == 0) { - const MechStateTipType* mstate = l_tipGeom->getContext()->get(); + const MechStateTipType* mstate = l_tipGeom->getContext()->get(); if (m_constraintSolver) { const auto lambda = m_constraintSolver->getLambda()[mstate].read()->getValue(); if (lambda[0].norm() > d_punctureForceThreshold.getValue()) { - auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto findClosestProxOnShaft = + Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); for (const auto& dpair : collisionOutput) { - // Reproject onto the needle to create an EdgeProximity - the EdgeNormalHandler in the Constraint classes will need this - auto shaftProx = findClosestProxOnShaft(dpair.second, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + // Reproject onto the needle to create an EdgeProximity - the + // EdgeNormalHandler in the Constraint classes will need this + auto shaftProx = findClosestProxOnShaft(dpair.second, l_shaftGeom.get(), + projectOnShaft, getFilterFunc()); m_needlePts.push_back(shaftProx); m_couplingPts.push_back(dpair.second->copy()); insertionOutput.add(shaftProx, dpair.second->copy()); @@ -103,28 +135,33 @@ class InsertionAlgorithm : public BaseAlgorithm { collisionOutput.clear(); auto itTip = l_tipGeom->begin(); - - auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); - auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + auto findClosestProxOnSurf = + Operations::FindClosestProximity::Operation::get(l_surfGeom); auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); - for (; itTip != l_tipGeom->end(); itTip++) + for (; itTip != l_tipGeom->end(); itTip++) { auto tipProx = createTipProximity(itTip->element()); if (!tipProx) continue; - auto surfProx = findClosestProxOnSurf(tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx) { + auto surfProx = findClosestProxOnSurf(tipProx, l_surfGeom.get(), projectOnSurf, + getFilterFunc()); + if (surfProx) + { surfProx->normalize(); - - if (d_projective.getValue()) { - auto pfromProj = projectOnTip(surfProx->getPosition(), itTip->element()).prox; + if (d_projective.getValue()) + { + auto pfromProj = + projectOnTip(surfProx->getPosition(), itTip->element()).prox; if (!pfromProj) continue; pfromProj->normalize(); collisionOutput.add(pfromProj, surfProx); } - else { + else + { collisionOutput.add(tipProx, surfProx); } } @@ -135,14 +172,18 @@ class InsertionAlgorithm : public BaseAlgorithm { insertionOutput.clear(); auto itTip = l_tipGeom->begin(); - auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); auto tipProx = createTipProximity(itTip->element()); auto itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); - auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); + auto createShaftProximity = + Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); auto shaftProx = createShaftProximity(itShaft->element()); const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); - const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()).normalized(); + const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - + edgeProx->element()->getP0()->getPosition()) + .normalized(); type::Vec3 ab = m_couplingPts.back()->getPosition() - tipProx->getPosition(); const SReal dotProd = dot(ab, normal); if (dotProd > 0.0) @@ -152,11 +193,13 @@ class InsertionAlgorithm : public BaseAlgorithm { } const SReal dist = ab.norm(); - if(dist > d_tipDistThreshold.getValue()) + if (dist > d_tipDistThreshold.getValue()) { - auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); + auto findClosestProxOnVol = + Operations::FindClosestProximity::Operation::get(l_volGeom); auto projectOnVol = Operations::Project::Operation::get(l_volGeom); - auto volProx = findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + auto volProx = + findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc()); if (volProx) { volProx->normalize(); @@ -165,24 +208,24 @@ class InsertionAlgorithm : public BaseAlgorithm { } } - auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto findClosestProxOnShaft = + Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for(int i = 0 ; i < m_couplingPts.size(); i++) + for (int i = 0; i < m_couplingPts.size(); i++) { - auto shaftProx = findClosestProxOnShaft(m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + auto shaftProx = findClosestProxOnShaft(m_couplingPts[i], l_shaftGeom.get(), + projectOnShaft, getFilterFunc()); m_needlePts[i] = shaftProx; } - for(int i = 0 ; i < m_couplingPts.size(); i++) + for (int i = 0; i < m_couplingPts.size(); i++) insertionOutput.add(m_needlePts[i], m_couplingPts[i]); } d_collisionOutput.endEdit(); d_insertionOutput.endEdit(); } - }; -} - +} // namespace sofa::collisionAlgorithm From 2f63446223fc0c060fdd340d404d10ed088e428e Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 16:28:27 +0200 Subject: [PATCH 17/20] [algorithm] Replaced auto with ElementIterator::SPtr for clarity for iterators --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 46fc4675..3bd3a611 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -134,7 +134,7 @@ class InsertionAlgorithm : public BaseAlgorithm collisionOutput.clear(); - auto itTip = l_tipGeom->begin(); + ElementIterator::SPtr itTip = l_tipGeom->begin(); auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); auto findClosestProxOnSurf = @@ -171,12 +171,12 @@ class InsertionAlgorithm : public BaseAlgorithm { insertionOutput.clear(); - auto itTip = l_tipGeom->begin(); + ElementIterator::SPtr itTip = l_tipGeom->begin(); auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); auto tipProx = createTipProximity(itTip->element()); - auto itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); + ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); auto shaftProx = createShaftProximity(itShaft->element()); From 0649256c8a4284ce3c983fd57b88f5b87c5ae944 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 16:32:20 +0200 Subject: [PATCH 18/20] [algorithm] Replaced auto with BaseProximity::SPtr when returned from Operations functions --- .../algorithm/InsertionAlgorithm.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 3bd3a611..1624d29f 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -121,8 +121,8 @@ class InsertionAlgorithm : public BaseAlgorithm { // Reproject onto the needle to create an EdgeProximity - the // EdgeNormalHandler in the Constraint classes will need this - auto shaftProx = findClosestProxOnShaft(dpair.second, l_shaftGeom.get(), - projectOnShaft, getFilterFunc()); + BaseProximity::SPtr shaftProx = findClosestProxOnShaft( + dpair.second, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); m_needlePts.push_back(shaftProx); m_couplingPts.push_back(dpair.second->copy()); insertionOutput.add(shaftProx, dpair.second->copy()); @@ -144,10 +144,10 @@ class InsertionAlgorithm : public BaseAlgorithm for (; itTip != l_tipGeom->end(); itTip++) { - auto tipProx = createTipProximity(itTip->element()); + BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); if (!tipProx) continue; - auto surfProx = findClosestProxOnSurf(tipProx, l_surfGeom.get(), projectOnSurf, - getFilterFunc()); + BaseProximity::SPtr surfProx = findClosestProxOnSurf( + tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); if (surfProx) { surfProx->normalize(); @@ -174,12 +174,12 @@ class InsertionAlgorithm : public BaseAlgorithm ElementIterator::SPtr itTip = l_tipGeom->begin(); auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); - auto tipProx = createTipProximity(itTip->element()); + BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); - auto shaftProx = createShaftProximity(itShaft->element()); + BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()) @@ -198,7 +198,7 @@ class InsertionAlgorithm : public BaseAlgorithm auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); auto projectOnVol = Operations::Project::Operation::get(l_volGeom); - auto volProx = + BaseProximity::SPtr volProx = findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc()); if (volProx) { @@ -214,7 +214,7 @@ class InsertionAlgorithm : public BaseAlgorithm for (int i = 0; i < m_couplingPts.size(); i++) { - auto shaftProx = findClosestProxOnShaft(m_couplingPts[i], l_shaftGeom.get(), + BaseProximity::SPtr shaftProx = findClosestProxOnShaft(m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); m_needlePts[i] = shaftProx; } From 296b50dc17ad121bc570884318cb0e51572853e3 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 16:36:36 +0200 Subject: [PATCH 19/20] [algorithm] Re-use tipProx pointer instead of re-creating one when projective is set to true --- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 1624d29f..41058e8f 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -153,12 +153,11 @@ class InsertionAlgorithm : public BaseAlgorithm surfProx->normalize(); if (d_projective.getValue()) { - auto pfromProj = - projectOnTip(surfProx->getPosition(), itTip->element()).prox; - if (!pfromProj) continue; - pfromProj->normalize(); + tipProx = projectOnTip(surfProx->getPosition(), itTip->element()).prox; + if (!tipProx) continue; + tipProx->normalize(); - collisionOutput.add(pfromProj, surfProx); + collisionOutput.add(tipProx, surfProx); } else { From 155a6d34f156a2e96949ddb47cadd92f99390534 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Thu, 31 Jul 2025 09:11:58 +0200 Subject: [PATCH 20/20] [src] Delete .clang-format Not really needed --- .clang-format | 168 -------------------------------------------------- 1 file changed, 168 deletions(-) delete mode 100644 .clang-format diff --git a/.clang-format b/.clang-format deleted file mode 100644 index 346d2074..00000000 --- a/.clang-format +++ /dev/null @@ -1,168 +0,0 @@ ---- -Language: Cpp -# BasedOnStyle: Google -AccessModifierOffset: -1 -AlignAfterOpenBracket: Align -AlignConsecutiveMacros: false -AlignConsecutiveAssignments: false -AlignConsecutiveDeclarations: false -AlignEscapedNewlines: Left -AlignOperands: true -AlignTrailingComments: true -AllowAllArgumentsOnNextLine: true -AllowAllConstructorInitializersOnNextLine: true -AllowAllParametersOfDeclarationOnNextLine: true -AllowShortBlocksOnASingleLine: Never -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All -AllowShortLambdasOnASingleLine: All -AllowShortIfStatementsOnASingleLine: WithoutElse -AllowShortLoopsOnASingleLine: true -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: true -AlwaysBreakTemplateDeclarations: Yes -BinPackArguments: true -BinPackParameters: true -BraceWrapping: - AfterCaseLabel: false - AfterClass: false - AfterControlStatement: false - AfterEnum: false - AfterFunction: false - AfterNamespace: false - AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false - AfterExternBlock: false - BeforeCatch: false - BeforeElse: false - IndentBraces: false - SplitEmptyFunction: true - SplitEmptyRecord: true - SplitEmptyNamespace: true -BreakBeforeBinaryOperators: None -BreakBeforeBraces: Allman -BreakBeforeInheritanceComma: false -BreakInheritanceList: BeforeColon -BreakBeforeTernaryOperators: true -BreakConstructorInitializersBeforeComma: false -BreakConstructorInitializers: BeforeColon -BreakAfterJavaFieldAnnotations: false -BreakStringLiterals: true -ColumnLimit: 100 -CommentPragmas: '^ IWYU pragma:' -CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: true -ConstructorInitializerIndentWidth: 4 -ContinuationIndentWidth: 4 -Cpp11BracedListStyle: true -DeriveLineEnding: true -DisableFormat: false -ExperimentalAutoDetectBinPacking: false -FixNamespaceComments: true -ForEachMacros: - - foreach - - Q_FOREACH - - BOOST_FOREACH -IncludeBlocks: Regroup -IncludeCategories: - - Regex: '^' - Priority: 2 - SortPriority: 0 - - Regex: '^<.*\.h>' - Priority: 1 - SortPriority: 0 - - Regex: '^<.*' - Priority: 2 - SortPriority: 0 - - Regex: '.*' - Priority: 3 - SortPriority: 0 -IncludeIsMainRegex: '([-_](test|unittest))?$' -IncludeIsMainSourceRegex: '' -IndentCaseLabels: true -IndentGotoLabels: true -IndentPPDirectives: None -IndentWidth: 4 -IndentWrappedFunctionNames: false -JavaScriptQuotes: Leave -JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: false -MacroBlockBegin: '' -MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCBinPackProtocolList: Never -ObjCBlockIndentWidth: 4 -ObjCSpaceAfterProperty: false -ObjCSpaceBeforeProtocolList: true -PenaltyBreakAssignment: 4 -PenaltyBreakBeforeFirstCallParameter: 1 -PenaltyBreakComment: 300 -PenaltyBreakFirstLessLess: 120 -PenaltyBreakString: 1000 -PenaltyBreakTemplateDeclaration: 10 -PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 200 -RawStringFormats: - - Language: Cpp - Delimiters: - - cc - - CC - - cpp - - Cpp - - CPP - - 'c++' - - 'C++' - CanonicalDelimiter: '' - BasedOnStyle: google - - Language: TextProto - Delimiters: - - pb - - PB - - proto - - PROTO - EnclosingFunctions: - - EqualsProto - - EquivToProto - - PARSE_PARTIAL_TEXT_PROTO - - PARSE_TEST_PROTO - - PARSE_TEXT_PROTO - - ParseTextOrDie - - ParseTextProtoOrDie - CanonicalDelimiter: '' - BasedOnStyle: google -ReflowComments: true -SortIncludes: true -SortUsingDeclarations: true -SpaceAfterCStyleCast: false -SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: true -SpaceBeforeAssignmentOperators: true -SpaceBeforeCpp11BracedList: false -SpaceBeforeCtorInitializerColon: true -SpaceBeforeInheritanceColon: true -SpaceBeforeParens: ControlStatements -SpaceBeforeRangeBasedForLoopColon: true -SpaceInEmptyBlock: false -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 2 -SpacesInAngles: false -SpacesInConditionalStatement: false -SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -SpaceBeforeSquareBrackets: false -Standard: Auto -StatementMacros: - - Q_UNUSED - - QT_REQUIRE_VERSION -TabWidth: 8 -UseCRLF: false -UseTab: Never -DerivePointerAlignment: false -PointerAlignment: Left -... -