From bd57b3a9418a1218e10a6b757dc47172f148c63b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 23 Jun 2025 11:21:40 +0200 Subject: [PATCH 1/3] [CollisionAlgorithm] Applied patch for new SOFA version and added example scene --- scenes/NeedleInsertion.py | 176 ++++++++++++++++++ .../collisionAlgorithm/BaseAABBBroadPhase.h | 4 +- .../algorithm/FindClosestProximityAlgorithm.h | 4 +- 3 files changed, 180 insertions(+), 4 deletions(-) create mode 100644 scenes/NeedleInsertion.py diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py new file mode 100644 index 00000000..34cc851e --- /dev/null +++ b/scenes/NeedleInsertion.py @@ -0,0 +1,176 @@ +import Sofa + +g_needleLength=0.100 #(m) +g_needleNumberOfElems=20 #(# of edges) +g_needleBaseOffset=[0.04,0.04,0] +g_needleRadius = 0.001 #(m) +g_needleMechanicalParameters = { + "radius":g_needleRadius, + "youngModulus":1e11, + "poissonRatio":0.3 +} +g_needleTotalMass=0.01 + +g_gelRegularGridParameters = { + "n":[8, 8, 8], + "min":[-0.125, -0.125, -0.350], + "max":[0.125, 0.125, -0.100] +} #Again all in mm +g_gelMechanicalParameters = { + "youngModulus":8000, + "poissonRatio":0.45, + "method":"large" +} +g_gelTotalMass = 1 +g_cubeColor=[0.8, 0.34, 0.34, 0.3] +g_gelFixedBoxROI=[-0.130, -0.130, -0.360, 0.130, 0.130, -0.300 ] + +# Function called when the scene graph is being created +def createScene(root): + root.gravity=[0,0,-9.81] + root.dt = 0.01 + + root.addObject("RequiredPlugin",pluginName=['Sofa.Component.AnimationLoop', + 'Sofa.Component.Constraint.Lagrangian.Solver', + 'Sofa.Component.ODESolver.Backward', + 'Sofa.Component.Visual', + 'Sofa.Component.Constraint.Lagrangian.Correction', + 'Sofa.Component.Constraint.Lagrangian.Model', + 'Sofa.Component.LinearSolver.Direct', + 'Sofa.Component.Mapping.Linear', + 'Sofa.Component.Mass', + 'Sofa.Component.SolidMechanics.FEM.Elastic', + 'Sofa.Component.StateContainer', + 'Sofa.Component.Topology.Container.Dynamic', + 'Sofa.Component.Topology.Mapping', + 'Sofa.Component.Mapping.NonLinear', + 'Sofa.Component.Topology.Container.Grid', + 'Sofa.Component.Constraint.Projective', + 'Sofa.Component.SolidMechanics.Spring', + 'Sofa.GL.Component.Rendering3D', + 'Sofa.GUI.Component', + 'Sofa.Component.Engine.Select', + 'MultiThreading', + 'CollisionAlgorithm', + 'ConstraintGeometry']) + + + root.addObject("ConstraintAttachButtonSetting") + root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields hideWireframe showInteractionForceFields" ) + root.addObject("FreeMotionAnimationLoop") + root.addObject("GenericConstraintSolver", tolerance=0.01, maxIt=5000, printLog=False) + root.addObject("CollisionLoop") + + needleBaseMaster = root.addChild("NeedleBaseMaster") + needleBaseMaster.addObject("MechanicalObject", name="mstate", position=[0.04, 0.04, 0, 0, 0, 0, 1], template="Rigid3d", showObjectScale=0.002, showObject="true", drawMode=1) + needleBaseMaster.addObject("LinearMovementProjectiveConstraint",indices=[0], keyTimes=[0,1,7,9],movements=[[0.04, 0.04,0,0,0,0],[0.04, 0.04,0.05,0,3.14/2,0],[0.04, 0.04,-0.07,0,3.14/2,0],[0.05, 0.04,-0.07,0,3.14/2 + 3.14/16,0]],relativeMovements=False) + + + + needle = root.addChild("Needle") + needle.addObject("EulerImplicitSolver", firstOrder=True) + needle.addObject("EigenSparseLU", name="LinearSolver", template="CompressedRowSparseMatrixd") + needle.addObject("EdgeSetTopologyContainer", name="Container", position=[[i * g_needleLength/(g_needleNumberOfElems) + g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]] for i in range(g_needleNumberOfElems + 1)] + , edges=[[i, i+1] for i in range(g_needleNumberOfElems)]) + + needle.addObject("EdgeSetTopologyModifier", name="modifier") + needle.addObject("PointSetTopologyModifier", name="modifier2") + + needle.addObject("MechanicalObject", name="mstate", template="Rigid3d", showObjectScale=0.0002, showObject="true", drawMode=1) + + needle.addObject("UniformMass", totalMass=g_needleTotalMass) + needle.addObject("BeamFEMForceField", name="FEM", **g_needleMechanicalParameters) + # needle.addObject("FixedLagrangianConstraint", indices="0" ) + needle.addObject("LinearSolverConstraintCorrection", printLog="false", linearSolver="@LinearSolver") + + needleBase = needle.addChild("needleBase") + needleBase.addObject("PointSetTopologyContainer", name="Container", position=[0, 0, 0]) + needleBase.addObject("MechanicalObject",name="mstate", template="Rigid3d",) + needleBase.addObject("RestShapeSpringsForceField",points=[0],stiffness=1e8, angularStiffness=1e8,external_points=[0],external_rest_shape="@/NeedleBaseMaster/mstate") + + needleBase.addObject("SubsetMapping", indices="0") + + needleBodyCollision = needle.addChild("bodyCollision") + needleBodyCollision.addObject("EdgeSetTopologyContainer", name="Container", src="@../Container") + needleBodyCollision.addObject("MechanicalObject",name="mstate", template="Vec3d",) + needleBodyCollision.addObject("EdgeGeometry",name="geom",mstate="@mstate", topology="@Container") + + needleBodyCollision.addObject("IdentityMapping") + + + needleTipCollision = needle.addChild("tipCollision") + needleTipCollision.addObject("MechanicalObject",name="mstate",position=[g_needleLength+g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]],template="Vec3d",) + needleTipCollision.addObject("PointGeometry",name="geom",mstate="@mstate") + needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True) + + + needleVisual = needle.addChild("visual") + needleVisual.addObject("QuadSetTopologyContainer", name="ContainerCath") + needleVisual.addObject("QuadSetTopologyModifier", name="Modifier") + needleVisual.addObject("Edge2QuadTopologicalMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../Container", output="@ContainerCath") + + needleVisual.addObject("MechanicalObject", name="VisualCatheter", showObjectScale="0.0002", showObject="true", drawMode="1") + + needleVisual.addObject("TubularMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../mstate", output="@VisualCatheter") + + needleOGL = needleVisual.addChild("OGL") + needleOGL.addObject("OglModel", position="@../ContainerCath.position", + vertices="@../ContainerCath.position", + quads="@../ContainerCath.quads", + color="0.4 0.34 0.34", + material="texture Ambient 1 0.4 0.34 0.34 1.0 Diffuse 0 0.4 0.34 0.34 1.0 Specular 1 0.4 0.34 0.34 0.1 Emissive 1 0.5 0.54 0.54 .01 Shininess 1 20", + name="VisualCatheter") + needleOGL.addObject("IdentityMapping") + + + + gelTopo = root.addChild("GelGridTopo") + gelTopo.addObject("RegularGridTopology", name="HexaTop", **g_gelRegularGridParameters) + + + volume = root.addChild("Volume") + volume.addObject("EulerImplicitSolver") + volume.addObject("EigenSimplicialLDLT", name="LinearSolver", template='CompressedRowSparseMatrixMat3x3d') + volume.addObject("TetrahedronSetTopologyContainer", name="Container", position="@../GelGridTopo/HexaTop.position") + volume.addObject("TetrahedronSetTopologyModifier", name="Modifier") + volume.addObject("Hexa2TetraTopologicalMapping", input="@../GelGridTopo/HexaTop", output="@Container", swapping="false") + + volume.addObject("MechanicalObject", name="mstate", template="Vec3d") + volume.addObject("TetrahedronGeometry", name="geom",draw=False) + volume.addObject("AABBBroadPhase",name="AABBTetra",geometry="@geom",nbox=[3,3,3],thread=1) + volume.addObject("ParallelTetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters) + volume.addObject("MeshMatrixMass", name="Mass",totalMass=g_gelTotalMass) + + volume.addObject("BoxROI",name="BoxROI",box=g_gelFixedBoxROI) + volume.addObject("RestShapeSpringsForceField", stiffness='1e6',points="@BoxROI.indices" ) + + volume.addObject("LinearSolverConstraintCorrection", printLog="false", linearSolver="@LinearSolver") + + volumeCollision = volume.addChild("collision") + volumeCollision.addObject("TriangleSetTopologyContainer", name="TriContainer") + volumeCollision.addObject("TriangleSetTopologyModifier", name="Modifier") + volumeCollision.addObject("Tetra2TriangleTopologicalMapping", name="mapping", input="@../Container", output="@TriContainer", flipNormals=False) + volumeCollision.addObject("MechanicalObject", name="mstate",position="@../Container.position") + volumeCollision.addObject("TriangleGeometry", name="geom", mstate="@mstate", topology="@TriContainer",draw=False) + volumeCollision.addObject("PhongTriangleNormalHandler", name="SurfaceTriangles", geometry="@geom") + volumeCollision.addObject("AABBBroadPhase",name="AABBTriangles",thread=1,nbox=[2,2,3]) + + volumeCollision.addObject("IdentityMapping", name="identityMappingToCollision", input="@../mstate", output="@mstate", isMechanical=True) + + volumeVisu = volumeCollision.addChild("visu") + volumeVisu.addObject("OglModel", position="@../TriContainer.position", + vertices="@../TriContainer.position", + triangles="@../TriContainer.triangles", + color=g_cubeColor,name="VisualCatheter",template="Vec3d") + volumeVisu.addObject("IdentityMapping") + + + root.addObject("FindClosestProximityAlgorithm",name="PunctureAlgo",fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/collision/geom") + root.addObject("DistanceFilter",algo="@PunctureAlgo",distance=0.01) + root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") + root.addObject("ConstraintUnilateral",input="@PunctureAlgo.output",directions="@punctureDirection",draw_scale="0.001") + + + root.addObject("FindClosestProximityAlgorithm",name="InsertionAlgo",fromGeom="@Needle/bodyCollision/geom", destGeom="@Volume/geom") + root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.005) + diff --git a/src/sofa/collisionAlgorithm/BaseAABBBroadPhase.h b/src/sofa/collisionAlgorithm/BaseAABBBroadPhase.h index faa84fab..cee5c088 100644 --- a/src/sofa/collisionAlgorithm/BaseAABBBroadPhase.h +++ b/src/sofa/collisionAlgorithm/BaseAABBBroadPhase.h @@ -26,8 +26,8 @@ class BaseAABBBroadPhase : public BaseGeometry::BroadPhase { , d_static(initData(&d_static, false,"isStatic", "Optimization: object is not moving in the scene")) , d_method(initData(&d_method, 0,"method", "chosen method to determine the boxes containing the elements")) , d_thread(initData(&d_thread, 8,"thread","Number of threads")){ - c_nbox.addInputs({&d_nbox}); - c_nbox.addCallback(std::bind(&BaseAABBBroadPhase::updateBroadPhase,this)); + //c_nbox.addInputs({&d_nbox}); + //c_nbox.addCallback(std::bind(&BaseAABBBroadPhase::updateBroadPhase,this)); } virtual void newContainer() = 0; diff --git a/src/sofa/collisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h index 4c73e704..2a3888f7 100644 --- a/src/sofa/collisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h @@ -21,8 +21,8 @@ class FindClosestProximityAlgorithm : public BaseAlgorithm { // Data > d_outputDist; FindClosestProximityAlgorithm() - : l_from(initLink("from", "link to from geometry")) - , l_dest(initLink("dest", "link to dest geometry")) + : 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")) From ed7ead3d60919586418d8f542cf8813c958062ac Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 23 Jun 2025 11:23:43 +0200 Subject: [PATCH 2/3] [algorithm] Added a puncture algorithm class --- CMakeLists.txt | 4 + scenes/NeedleInsertion.py | 7 +- .../algorithm/PunctureAlgorithm.cpp | 13 +++ .../algorithm/PunctureAlgorithm.h | 109 ++++++++++++++++++ 4 files changed, 130 insertions(+), 3 deletions(-) create mode 100644 src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp create mode 100644 src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h diff --git a/CMakeLists.txt b/CMakeLists.txt index de9c45b8..fe18d332 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,8 @@ file(GLOB_RECURSE DEPRECATED_FILES ) find_package(Sofa.Simulation.Core REQUIRED) +find_package(Sofa.Component.StateContainer REQUIRED) +find_package(Sofa.Component.Constraint.Lagrangian.Solver REQUIRED) find_package(Sofa.GL REQUIRED) @@ -42,6 +44,8 @@ add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${HEADER_FILES} ${SCENES_FILE set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DPLUGIN_DATA_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\"") target_link_libraries(${PROJECT_NAME} Sofa.Simulation.Core + Sofa.Component.StateContainer + Sofa.Component.Constraint.Lagrangian.Solver Sofa.GL ) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 34cc851e..6cf30eb2 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -58,7 +58,7 @@ def createScene(root): root.addObject("ConstraintAttachButtonSetting") root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields hideWireframe showInteractionForceFields" ) root.addObject("FreeMotionAnimationLoop") - root.addObject("GenericConstraintSolver", tolerance=0.01, maxIt=5000, printLog=False) + root.addObject("GenericConstraintSolver", tolerance=0.01, maxIt=5000, printLog=False, computeConstraintForces=True) root.addObject("CollisionLoop") needleBaseMaster = root.addChild("NeedleBaseMaster") @@ -138,7 +138,8 @@ def createScene(root): volume.addObject("MechanicalObject", name="mstate", template="Vec3d") volume.addObject("TetrahedronGeometry", name="geom",draw=False) volume.addObject("AABBBroadPhase",name="AABBTetra",geometry="@geom",nbox=[3,3,3],thread=1) - volume.addObject("ParallelTetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters) + #volume.addObject("ParallelTetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters) + volume.addObject("TetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters) volume.addObject("MeshMatrixMass", name="Mass",totalMass=g_gelTotalMass) volume.addObject("BoxROI",name="BoxROI",box=g_gelFixedBoxROI) @@ -165,7 +166,7 @@ def createScene(root): volumeVisu.addObject("IdentityMapping") - root.addObject("FindClosestProximityAlgorithm",name="PunctureAlgo",fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/collision/geom") + root.addObject("PunctureAlgorithm",name="PunctureAlgo",fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/collision/geom", punctureThreshold=0.1) root.addObject("DistanceFilter",algo="@PunctureAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") root.addObject("ConstraintUnilateral",input="@PunctureAlgo.output",directions="@punctureDirection",draw_scale="0.001") diff --git a/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp b/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp new file mode 100644 index 00000000..d51c5037 --- /dev/null +++ b/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp @@ -0,0 +1,13 @@ +#include +#include + +namespace sofa::collisionAlgorithm { + +int PunctureAlgorithmClass = core::RegisterObject("PunctureAlgorithm") +.add< PunctureAlgorithm >(); + + +} + + + diff --git a/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h new file mode 100644 index 00000000..c444e77e --- /dev/null +++ b/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h @@ -0,0 +1,109 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::collisionAlgorithm { + +class PunctureAlgorithm : public BaseAlgorithm { +public: + SOFA_CLASS(PunctureAlgorithm, BaseAlgorithm); + + core::objectmodel::SingleLink l_from; + core::objectmodel::SingleLink l_dest; + Data d_drawCollision ; + Data > d_output; + Data d_projective ; + Data d_punctureThreshold ; +// Data > d_outputDist; + sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; + + PunctureAlgorithm() + : 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_punctureThreshold(initData(&d_punctureThreshold, std::numeric_limits::max(), "punctureThreshold", "Threshold for puncture detection")) +// , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) + , m_constraintSolver(nullptr) + {} + + 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; + vparams->drawTool()->disableLighting(); + + DetectionOutput output = d_output.getValue() ; + for (unsigned i=0;idrawTool()->drawLine( + output[i].first->getPosition(), + output[i].second->getPosition(), + sofa::type::RGBAColor(0, 1, 0, 1) + ); + } + + } + + void doDetection() { + if (l_from == NULL) return; + if (l_dest == NULL) return; + + auto& output = *d_output.beginEdit(); + output.clear(); + + const sofa::component::statecontainer::MechanicalObject* mstate + = l_from->getContext()->get>(); + if (m_constraintSolver) + { + defaulttype::RigidTypes::Vec3 lambda = + m_constraintSolver->getLambda()[mstate].read()->getValue()[0].getVCenter(); + if (lambda.norm() > d_punctureThreshold.getValue()) return;// Exit and leave output empty + } + + 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(); + } + +}; + +} + From 64fc19babb306d9a30ec59bea91781abda42def0 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 8 Jul 2025 17:15:21 +0200 Subject: [PATCH 3/3] [algorithm] Enable the detection of tetrahedron proximities while insertion progresses inside the volume WIP: The detected tetrahedra are pushed back into a std::vector continuously. This is unnecessary/undesirable; however, there is no need to spend time fixing it, because in practice, proximity detection will only be activated when the needle points slide a predetermined distance along the needle shaft. --- scenes/NeedleInsertion.py | 18 +- .../algorithm/InsertionAlgorithm.cpp | 13 ++ .../algorithm/InsertionAlgorithm.h | 182 ++++++++++++++++++ .../algorithm/PunctureAlgorithm.cpp | 13 -- .../algorithm/PunctureAlgorithm.h | 109 ----------- 5 files changed, 206 insertions(+), 129 deletions(-) create mode 100644 src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.cpp create mode 100644 src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h delete mode 100644 src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp delete mode 100644 src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 6cf30eb2..7d889eba 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -56,7 +56,7 @@ def createScene(root): root.addObject("ConstraintAttachButtonSetting") - root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields hideWireframe showInteractionForceFields" ) + root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields" ) root.addObject("FreeMotionAnimationLoop") root.addObject("GenericConstraintSolver", tolerance=0.01, maxIt=5000, printLog=False, computeConstraintForces=True) root.addObject("CollisionLoop") @@ -136,7 +136,9 @@ def createScene(root): volume.addObject("Hexa2TetraTopologicalMapping", input="@../GelGridTopo/HexaTop", output="@Container", swapping="false") volume.addObject("MechanicalObject", name="mstate", template="Vec3d") - volume.addObject("TetrahedronGeometry", name="geom",draw=False) + volume.addObject("TetrahedronGeometry", name="geom", mstate="@mstate", topology="@Container", draw=False) + volume.addObject("TriangleGeometry", name="tri_geom", mstate="@mstate", topology="@Container",draw=True) + volume.addObject("PhongTriangleNormalHandler", name="InternalTriangles", geometry="@geom") volume.addObject("AABBBroadPhase",name="AABBTetra",geometry="@geom",nbox=[3,3,3],thread=1) #volume.addObject("ParallelTetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters) volume.addObject("TetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters) @@ -166,12 +168,14 @@ def createScene(root): volumeVisu.addObject("IdentityMapping") - root.addObject("PunctureAlgorithm",name="PunctureAlgo",fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/collision/geom", punctureThreshold=0.1) - root.addObject("DistanceFilter",algo="@PunctureAlgo",distance=0.01) + root.addObject("InsertionAlgorithm", name="InsertionAlgo", fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/collision/geom", destVol="@Volume/geom", punctureThreshold=0.1) + root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") - root.addObject("ConstraintUnilateral",input="@PunctureAlgo.output",directions="@punctureDirection",draw_scale="0.001") + root.addObject("ConstraintUnilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale="0.001")#, mu="0.001") - root.addObject("FindClosestProximityAlgorithm",name="InsertionAlgo",fromGeom="@Needle/bodyCollision/geom", destGeom="@Volume/geom") - root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.005) + #root.addObject("InsertionAlgorithm",name="InsertionAlgo",fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/tri_geom") + #root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.005) + #root.addObject("BindDirection",name="insertionDirection")#,handler="@Volume/InternalTriangles") + #root.addObject("ConstraintBilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale="0.001") diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.cpp b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.cpp new file mode 100644 index 00000000..b9b1e602 --- /dev/null +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.cpp @@ -0,0 +1,13 @@ +#include +#include + +namespace sofa::collisionAlgorithm { + +int InsertionAlgorithmClass = core::RegisterObject("InsertionAlgorithm") +.add< InsertionAlgorithm >(); + + +} + + + diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h new file mode 100644 index 00000000..cf290454 --- /dev/null +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -0,0 +1,182 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sofa::collisionAlgorithm { + +class InsertionAlgorithm : public BaseAlgorithm { +public: + SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm); + + core::objectmodel::SingleLink l_from; + core::objectmodel::SingleLink l_dest; + core::objectmodel::SingleLink l_destVol; + Data d_drawCollision ; + Data > d_output; + Data > d_outputInside; + Data d_projective ; + Data d_punctureThreshold ; +// Data > d_outputDist; + sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; + std::vector m_proximities; + std::vector m_tetras; + + InsertionAlgorithm() + : l_from(initLink("fromGeom", "link to from geometry")) + , l_dest(initLink("destGeom", "link to dest geometry")) + , l_destVol(initLink("destVol", "link to dest geometry (volume)")) + , d_drawCollision (initData(&d_drawCollision, true, "drawcollision", "draw collision")) + , d_output(initData(&d_output,"output", "output of the collision detection")) + , d_outputInside(initData(&d_outputInside,"outputInside", "output of the detection inside the volume")) + , 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_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) + , m_constraintSolver(nullptr) + {} + + 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; + vparams->drawTool()->disableLighting(); + + DetectionOutput output = d_output.getValue() ; + for (unsigned i=0;idrawTool()->drawLine( + output[i].first->getPosition(), + output[i].second->getPosition(), + sofa::type::RGBAColor(0, 1, 0, 1) + ); + } + + DetectionOutput outputInside = d_outputInside.getValue() ; + for (unsigned i=0;idrawTool()->drawLine( + outputInside[i].first->getPosition(), + outputInside[i].second->getPosition(), + sofa::type::RGBAColor(1, 1, 0, 1) + ); + } + + for(unsigned i = 0; i < m_tetras.size(); ++i) { + vparams->drawTool()->drawTetrahedron( + m_tetras[i]->getP0()->getPosition(), + m_tetras[i]->getP1()->getPosition(), + m_tetras[i]->getP2()->getPosition(), + m_tetras[i]->getP3()->getPosition(), + sofa::type::RGBAColor(1, 1, 0, 1) + ); + } + + } + + void doDetection() { + if (l_from == NULL) return; + if (l_dest == NULL) return; + + auto& output = *d_output.beginEdit(); + auto& outputInside = *d_outputInside.beginEdit(); + + const sofa::component::statecontainer::MechanicalObject* mstate + = l_from->getContext()->get>(); + if (m_constraintSolver) + { + defaulttype::RigidTypes::Vec3 lambda = + m_constraintSolver->getLambda()[mstate].read()->getValue()[0].getVCenter(); + if (lambda.norm() > d_punctureThreshold.getValue()) + { + for (const auto& itOutputPair : output) { + m_proximities.push_back(itOutputPair.second->copy()); + } + output.clear(); + return; + } + } + + output.clear(); + outputInside.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 projectVolOp = Operations::Project::Operation::get(l_destVol); + 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) + { + 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); + } + } + + auto findClosestProxOpVol = Operations::FindClosestProximity::Operation::get(l_destVol); + auto pdestVol = findClosestProxOpVol(pfrom, l_destVol.get(), projectVolOp, getFilterFunc()); + if (pdestVol != nullptr) + { + TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); + double* baryCoords = tetraProx->getBaryCoord(); + if (toolbox::TetrahedronToolBox::isInTetra( + pfrom->getPosition(), + tetraProx->element()->getTetrahedronInfo(), + baryCoords[0], + baryCoords[1], + baryCoords[2], + baryCoords[3] + ) + ) + { + m_tetras.push_back(tetraProx->element()); + } + + pdestVol->normalize(); + + if (d_projective.getValue()) { + auto pfromProj = projectFromOp(pdestVol->getPosition(), itfrom->element()).prox; + if (pfromProj == nullptr) continue; + pfromProj->normalize(); + + outputInside.add(pfromProj, pdestVol); + } + else { + outputInside.add(pfrom, pdestVol); + } + } + } + + d_output.endEdit(); + d_outputInside.endEdit(); + } + +}; + +} + diff --git a/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp b/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp deleted file mode 100644 index d51c5037..00000000 --- a/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include -#include - -namespace sofa::collisionAlgorithm { - -int PunctureAlgorithmClass = core::RegisterObject("PunctureAlgorithm") -.add< PunctureAlgorithm >(); - - -} - - - diff --git a/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h deleted file mode 100644 index c444e77e..00000000 --- a/src/sofa/collisionAlgorithm/algorithm/PunctureAlgorithm.h +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sofa::collisionAlgorithm { - -class PunctureAlgorithm : public BaseAlgorithm { -public: - SOFA_CLASS(PunctureAlgorithm, BaseAlgorithm); - - core::objectmodel::SingleLink l_from; - core::objectmodel::SingleLink l_dest; - Data d_drawCollision ; - Data > d_output; - Data d_projective ; - Data d_punctureThreshold ; -// Data > d_outputDist; - sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; - - PunctureAlgorithm() - : 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_punctureThreshold(initData(&d_punctureThreshold, std::numeric_limits::max(), "punctureThreshold", "Threshold for puncture detection")) -// , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) - , m_constraintSolver(nullptr) - {} - - 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; - vparams->drawTool()->disableLighting(); - - DetectionOutput output = d_output.getValue() ; - for (unsigned i=0;idrawTool()->drawLine( - output[i].first->getPosition(), - output[i].second->getPosition(), - sofa::type::RGBAColor(0, 1, 0, 1) - ); - } - - } - - void doDetection() { - if (l_from == NULL) return; - if (l_dest == NULL) return; - - auto& output = *d_output.beginEdit(); - output.clear(); - - const sofa::component::statecontainer::MechanicalObject* mstate - = l_from->getContext()->get>(); - if (m_constraintSolver) - { - defaulttype::RigidTypes::Vec3 lambda = - m_constraintSolver->getLambda()[mstate].read()->getValue()[0].getVCenter(); - if (lambda.norm() > d_punctureThreshold.getValue()) return;// Exit and leave output empty - } - - 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(); - } - -}; - -} -