diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 78f330233..466164369 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,6 +29,7 @@ jobs: path: ${{ env.WORKSPACE_SRC_PATH }} - name: Build and install + id: build-install shell: bash run: | if [[ "$RUNNER_OS" == "Windows" ]]; then @@ -70,12 +71,14 @@ jobs: echo "artifact_name=$artifact_name" >> $env:GITHUB_OUTPUT - name: Create artifact + id: create-artifact uses: actions/upload-artifact@v4.4.0 with: name: ${{ steps.sanitize.outputs.artifact_name }} path: ${{ env.WORKSPACE_INSTALL_PATH }} - name: Install artifact + id: install-artifact uses: actions/download-artifact@v4.1.7 with: name: ${{ steps.sanitize.outputs.artifact_name }} @@ -116,7 +119,9 @@ jobs: # echo '----------------------' # echo "SOFA_ROOT = $SOFA_ROOT" + - name: Run BeamAdapter_test + id: unit-test if: always() shell: bash run: | @@ -125,6 +130,7 @@ jobs: ./bin/BeamAdapter_test${{ steps.sofa.outputs.exe }} - name: Fetch, install and run Regression_test + id: regression-test if: always() shell: bash run: | @@ -139,12 +145,43 @@ jobs: # Setup mandatory env vars export REGRESSION_SCENES_DIR="${WORKSPACE_SRC_PATH}/examples" export REGRESSION_REFERENCES_DIR="${WORKSPACE_SRC_PATH}/regression/references" + export PYTHONPATH=$SOFA_ROOT/plugins/SofaPython3/lib/python3/site-packages # Run regression test bench ${SOFA_ROOT}/bin/Regression_test${{ steps.sofa.outputs.exe }} else echo "Regression tests are not supported on the CI for macOS yet (TODO)" fi + - name: Notify dashboard + if: always() && startsWith(github.repository, 'sofa-framework') && startsWith(github.ref, 'refs/heads/master') # we are not on a fork and on master + env: + DASH_AUTH: ${{ secrets.PLUGIN_DASH }} + shell: bash + run: | + + test_status=$([ '${{ steps.unit-test.outcome }}' == 'success' ] && \ + [ '${{ steps.regression-test.outcome }}' == 'success' ] && \ + echo 'true' || echo 'false') + + build_status=$([ '${{ steps.build-install.outcome }}' == 'success' ] && \ + echo 'true' || echo 'false') + + binary_status=$([ '${{ steps.sanitize.outcome }}' == 'success' ] && \ + [ '${{ steps.create-artifact.outcome }}' == 'success' ] && \ + [ '${{ steps.install-artifact.outcome }}' == 'success' ] && \ + echo 'true' || echo 'false') + + + curl -X POST -H "X-API-KEY: $DASH_AUTH" -H "Content-Type: application/json" -d \ + "{\"id\":\"$(echo "${{ github.repository }}" | awk -F/ '{ print $2 }')\",\ + \"github_ref\":\"${{ github.sha }}\",\ + \"url\":\"https://github.com/${{ github.repository }}/actions/runs/${{ github.run_id }}\",\ + \"build\":$build_status,\ + \"tests\":$test_status,\ + \"binary\":$binary_status}"\ + https://sofa-framework.org:5000/api/v1/plugins + + deploy: name: Deploy artifacts if: always() && startsWith(github.repository, 'sofa-framework') && (startsWith(github.ref, 'refs/heads/') || startsWith(github.ref, 'refs/tags/')) # we are not on a fork and on a branch or a tag (not a PR) needs: [build-and-test] diff --git a/BeamAdapter_test/component/model/WireRestShape_test.cpp b/BeamAdapter_test/component/model/WireRestShape_test.cpp index 118895742..05c5f1d8a 100644 --- a/BeamAdapter_test/component/model/WireRestShape_test.cpp +++ b/BeamAdapter_test/component/model/WireRestShape_test.cpp @@ -163,10 +163,10 @@ void WireRestShape_test::testParameterInit() Real straightLength = 95.0; EXPECT_EQ(fullLength, 100.0); - int nbrE0 = 50; - int nbrE1 = 10; + sofa::Size nbrE0 = 50; + sofa::Size nbrE1 = 10; vector keysPoints, keysPoints_ref = { 0, straightLength, fullLength }; - vector nbP_density, nbP_density_ref = { nbrE0, nbrE1 }; + vector nbP_density, nbP_density_ref = { nbrE0, nbrE1 }; wire->getSamplingParameters(keysPoints, nbP_density); EXPECT_EQ(keysPoints.size(), 3); diff --git a/CMakeLists.txt b/CMakeLists.txt index 85302c55c..d136f5563 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.12) -project(BeamAdapter VERSION 1.0) +project(BeamAdapter VERSION 1.0 LANGUAGES CXX) include(cmake/environment.cmake) diff --git a/examples/3instruments.scn b/examples/3instruments.scn index 755c085ec..e1030a6e9 100644 --- a/examples/3instruments.scn +++ b/examples/3instruments.scn @@ -19,11 +19,11 @@ - + - - + + @@ -33,8 +33,8 @@ - - + + @@ -44,8 +44,8 @@ - - + + @@ -60,7 +60,7 @@ - + - - + + @@ -49,8 +49,8 @@ - - + + @@ -60,8 +60,8 @@ - - + + @@ -76,7 +76,7 @@ - - + + @@ -142,14 +142,21 @@ - - - - - - - - + + + + + + + + + + + + + + + diff --git a/examples/RegressionStateScenes.regression-tests b/examples/RegressionStateScenes.regression-tests index 178dbe192..2b2e861cf 100644 --- a/examples/RegressionStateScenes.regression-tests +++ b/examples/RegressionStateScenes.regression-tests @@ -7,6 +7,8 @@ ### Demo scenes ### SingleBeam.scn 1000 1e-4 1 1 -SingleBeamDeployment.scn 1000 1e-4 1 1 -SingleBeamDeploymentCollision.scn 1000 1e-4 1 1 -3instruments_collis.scn 2000 1e-4 1 1 +SingleBeamDeployment.scn 10000 1e-4 1 1 +SingleBeamDeploymentCollision.scn 7000 1e-4 1 1 +3instruments.scn 2000 1e-4 1 1 +3instruments_collis.scn 3000 1e-4 1 1 +Tool_from_loader.scn 1000 1e-4 1 1 \ No newline at end of file diff --git a/examples/SingleBeamDeployment.scn b/examples/SingleBeamDeployment.scn index d84ea4732..8feca7e10 100644 --- a/examples/SingleBeamDeployment.scn +++ b/examples/SingleBeamDeployment.scn @@ -15,8 +15,8 @@ - - + + @@ -28,7 +28,7 @@ - @@ -36,8 +36,8 @@ diff --git a/examples/SingleBeamDeploymentCollision.scn b/examples/SingleBeamDeploymentCollision.scn index f8afde1da..6c6cd5f53 100644 --- a/examples/SingleBeamDeploymentCollision.scn +++ b/examples/SingleBeamDeploymentCollision.scn @@ -32,8 +32,8 @@ - - + + @@ -46,7 +46,7 @@ - @@ -54,8 +54,8 @@ @@ -66,8 +66,8 @@ - - + + diff --git a/examples/Tool_from_loader.scn b/examples/Tool_from_loader.scn index 1b09e0984..86775126e 100644 --- a/examples/Tool_from_loader.scn +++ b/examples/Tool_from_loader.scn @@ -25,8 +25,8 @@ - - + + @@ -39,7 +39,7 @@ - @@ -51,7 +51,7 @@ diff --git a/examples/python3/RegressionStateScenes.regression-tests b/examples/python3/RegressionStateScenes.regression-tests new file mode 100644 index 000000000..25ae3ab84 --- /dev/null +++ b/examples/python3/RegressionStateScenes.regression-tests @@ -0,0 +1,11 @@ +# WARNING: +# REGRESSION_TEST DOES NOT SUPPORT DASHES ("-") IN SCENE NAMES. +# USE UNDERSCORES ("_") INSTEAD. + +### References relative path ### +../../regression/references + +### Demo scenes ### +SingleBeam.py 1000 1e-4 1 1 +SingleBeamDeployment.py 10000 1e-4 1 1 +SingleBeamDeploymentCollision.py 7000 1e-4 1 1 \ No newline at end of file diff --git a/examples/python3/SingleBeamDeployment.py b/examples/python3/SingleBeamDeployment.py index daf321da4..df8059a49 100644 --- a/examples/python3/SingleBeamDeployment.py +++ b/examples/python3/SingleBeamDeployment.py @@ -12,14 +12,14 @@ def createScene(rootNode): topoLines = rootNode.addChild('EdgeTopology') topoLines.addObject('RodStraightSection', name='StraightSection', length=980.0, radius=0.9, - nbEdgesCollis=30, nbEdgesVisu=196, - youngModulus=20000) + nbBeams=30, nbEdgesCollis=30, nbEdgesVisu=196, + youngModulus=20000, massDensity=0.00000155) topoLines.addObject('RodSpireSection', name='SpireSection', length=20.0, radius=0.9, - nbEdgesCollis=5, nbEdgesVisu=4, + nbBeams=5, nbEdgesCollis=5, nbEdgesVisu=4, spireDiameter=25, spireHeight=0, - youngModulus=20000) + youngModulus=20000, massDensity=0.00000155) topoLines.addObject('WireRestShape', name='BeamRestShape', template="Rigid3d", wireMaterials="@StraightSection @SpireSection") @@ -34,15 +34,15 @@ def createScene(rootNode): BeamMechanics.addObject('EulerImplicitSolver', rayleighStiffness=0.2, printLog=False, rayleighMass=0.1) BeamMechanics.addObject('BTDLinearSolver', verification=False, subpartSolve=False, verbose=False) BeamMechanics.addObject('RegularGridTopology', name='MeshLines', drawEdges=True, - nx=60, ny=1, nz=1, + nx=61, ny=1, nz=1, xmax=0.0, xmin=0.0, ymin=0, ymax=0, zmax=0, zmin=0, p0=[0,0,0]) BeamMechanics.addObject('MechanicalObject', showIndices=False, name='DOFs Container', template='Rigid3d', ry=-90) BeamMechanics.addObject('WireBeamInterpolation', name='BeamInterpolation', WireRestShape='@../EdgeTopology/BeamRestShape', printLog=False) BeamMechanics.addObject('AdaptiveBeamForceFieldAndMass', name='BeamForceField', massDensity=0.00000155, interpolation='@BeamInterpolation') BeamMechanics.addObject('InterventionalRadiologyController', name='DeployController', template='Rigid3d', instruments='BeamInterpolation', - startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0, 0, 0], printLog=True, - rotationInstrument=[0, 0, 0], step=0.5, speed=0.5, + topology="@MeshLines", startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0], printLog=True, + rotationInstrument=[0], step=0.5, speed=0.5, listening=True, controlledInstrument=0) BeamMechanics.addObject('FixedProjectiveConstraint', indices=0, name='FixedConstraint') BeamMechanics.addObject('RestShapeSpringsForceField', name="RestSPForceField", points='@DeployController.indexFirstNode', angularStiffness=1e8, stiffness=1e8) diff --git a/examples/python3/SingleBeamDeploymentCollision.py b/examples/python3/SingleBeamDeploymentCollision.py index 79c532aea..f56869dde 100644 --- a/examples/python3/SingleBeamDeploymentCollision.py +++ b/examples/python3/SingleBeamDeploymentCollision.py @@ -20,14 +20,14 @@ def createScene(rootNode): topoLines = rootNode.addChild('EdgeTopology') topoLines.addObject('RodStraightSection', name='StraightSection', length=980.0, radius=0.9, - nbEdgesCollis=50, nbEdgesVisu=200, - youngModulus=20000, massDensity=0.1, poissonRatio=0.3) + nbBeams=50, nbEdgesCollis=50, nbEdgesVisu=200, + youngModulus=20000, massDensity=0.00000155, poissonRatio=0.3) topoLines.addObject('RodSpireSection', name='SpireSection', length=20.0, radius=0.9, - nbEdgesCollis=10, nbEdgesVisu=200, + nbBeams=10, nbEdgesCollis=10, nbEdgesVisu=200, spireDiameter=25, spireHeight=0, - youngModulus=20000, massDensity=0.1, poissonRatio=0.3) + youngModulus=20000, massDensity=0.00000155, poissonRatio=0.3) topoLines.addObject('WireRestShape', name='BeamRestShape', template="Rigid3d", wireMaterials="@StraightSection @SpireSection") @@ -42,15 +42,15 @@ def createScene(rootNode): BeamMechanics.addObject('EulerImplicitSolver', rayleighStiffness=0.2, rayleighMass=0.1) BeamMechanics.addObject('BTDLinearSolver', verification=False, subpartSolve=False, verbose=False) BeamMechanics.addObject('RegularGridTopology', name='MeshLines', - nx=60, ny=1, nz=1, + nx=61, ny=1, nz=1, xmax=0.0, xmin=0.0, ymin=0, ymax=0, zmax=0, zmin=0, p0=[0,0,0]) BeamMechanics.addObject('MechanicalObject', showIndices=False, name='DOFs', template='Rigid3d', ry=-90) BeamMechanics.addObject('WireBeamInterpolation', name='BeamInterpolation', WireRestShape='@../EdgeTopology/BeamRestShape', printLog=False) BeamMechanics.addObject('AdaptiveBeamForceFieldAndMass', name='BeamForceField', massDensity=0.00000155, interpolation='@BeamInterpolation') BeamMechanics.addObject('InterventionalRadiologyController', name='DeployController', template='Rigid3d', instruments='BeamInterpolation', - startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0, 0, 0], printLog=True, - rotationInstrument=[0, 0, 0], step=5., speed=5., + topology="@MeshLines", startingPos=[0, 0, 0, 0, 0, 0, 1], xtip=[0], printLog=True, + rotationInstrument=[0], step=5., speed=5., listening=True, controlledInstrument=0) BeamMechanics.addObject('LinearSolverConstraintCorrection', wire_optimization='true', printLog=False) BeamMechanics.addObject('FixedProjectiveConstraint', indices=0, name='FixedConstraint') @@ -63,8 +63,8 @@ def createScene(rootNode): BeamCollis.addObject('EdgeSetTopologyModifier', name='colliseEdgeModifier') BeamCollis.addObject('MechanicalObject', name='CollisionDOFs') BeamCollis.addObject('MultiAdaptiveBeamMapping', controller='../DeployController', useCurvAbs=True, printLog=False, name='collisMap') - BeamCollis.addObject('LineCollisionModel', proximity=0.0) - BeamCollis.addObject('PointCollisionModel', proximity=0.0) + BeamCollis.addObject('LineCollisionModel', contactDistance=0.0) + BeamCollis.addObject('PointCollisionModel', contactDistance=0.0) Carotids = rootNode.addChild('Carotids') Carotids.addObject('MeshSTLLoader', filename='../mesh/carotids.stl', flipNormals=False, triangulate=True, name='meshLoader', rotation=[10.0, 0.0, -90.0]) diff --git a/examples/python3/beamadapter/parts/instrument.py b/examples/python3/beamadapter/parts/instrument.py index 2fab3fed0..aa2249a6b 100644 --- a/examples/python3/beamadapter/parts/instrument.py +++ b/examples/python3/beamadapter/parts/instrument.py @@ -101,8 +101,8 @@ def createInstrumentsCombined(node, xtip=[1, 0, 0], instruments=['guide'], Collis.addObject('MechanicalObject', name='CollisionDOFs') Collis.addObject('MultiAdaptiveBeamMapping', controller='../m_ircontroller', useCurvAbs=True, printLog=False, name='collisMap') - Collis.addObject('LineCollisionModel', proximity=0.0, group=1) - Collis.addObject('PointCollisionModel', proximity=0.0, group=1) + Collis.addObject('LineCollisionModel', contactDistance=0.0, group=1) + Collis.addObject('PointCollisionModel', contactDistance=0.0, group=1) # Visualization Guide VisuGuide = InstrumentCombined.addChild('VisuGuide') @@ -142,8 +142,8 @@ def createInstrumentsCombinedXRay(node, xtip=[1, 0, 0], instruments=['guide'], Collis.addObject('MechanicalObject', name='CollisionDOFs') Collis.addObject('MultiAdaptiveBeamMapping', controller='../m_ircontroller', useCurvAbs=True, printLog=False, name='collisMap') - Collis.addObject('LineCollisionModel', proximity=0.0, group=1) - Collis.addObject('PointCollisionModel', proximity=0.0, group=1) + Collis.addObject('LineCollisionModel', contactDistance=0.0, group=1) + Collis.addObject('PointCollisionModel', contactDistance=0.0, group=1) # Visualization Guide VisuGuide = InstrumentCombined.addChild('VisuGuide') diff --git a/extensions/CUDA/CMakeLists.txt b/extensions/CUDA/CMakeLists.txt index 51ffef0dc..edcfdfcde 100644 --- a/extensions/CUDA/CMakeLists.txt +++ b/extensions/CUDA/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.12) -project(BeamAdapter.CUDA) +project(BeamAdapter.CUDA LANGUAGES CXX CUDA) set(HEADER_FILES src/BeamAdapter/CUDA/init.h diff --git a/extensions/CUDA/src/BeamAdapter/CUDA/CudaInstantiations.cpp b/extensions/CUDA/src/BeamAdapter/CUDA/CudaInstantiations.cpp index 3c56f451b..bc5f3437f 100644 --- a/extensions/CUDA/src/BeamAdapter/CUDA/CudaInstantiations.cpp +++ b/extensions/CUDA/src/BeamAdapter/CUDA/CudaInstantiations.cpp @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #include -#include +#include #include #include @@ -36,149 +36,117 @@ #include #include #include -#include // for InterventionalRadiologyController +#include // for InterventionalRadiologyController #include using namespace sofa::gpu::cuda; -namespace sofa::component::fem::_beaminterpolation_ +namespace beamadapter { // template class SOFA_BEAMADAPTER_CUDA_API BeamInterpolation; -#ifdef SOFA_GPU_CUDA_DOUBLE - template class SOFA_BEAMADAPTER_CUDA_API BeamInterpolation; -#endif -} // namespace sofa::component::fem::_beaminterpolation_ - -namespace sofa::component::fem::_wirebeaminterpolation_ -{ // template class SOFA_BEAMADAPTER_CUDA_API WireBeamInterpolation; -#ifdef SOFA_GPU_CUDA_DOUBLE - template class SOFA_BEAMADAPTER_CUDA_API WireBeamInterpolation; -#endif -} // namespace sofa::component::fem::_beaminterpolation_ - -namespace sofa::component::engine::_wirerestshape_ -{ template class SOFA_BEAMADAPTER_CUDA_API WireRestShape; -#ifdef SOFA_GPU_CUDA_DOUBLE - template class SOFA_BEAMADAPTER_CUDA_API WireRestShape; -#endif -} // namespace sofa::component::engine::_wirerestshape_ - -namespace sofa::component::controller::_interventionalradiologycontroller_ -{ template class SOFA_BEAMADAPTER_CUDA_API InterventionalRadiologyController; -#ifdef SOFA_GPU_CUDA_DOUBLE - template class SOFA_BEAMADAPTER_CUDA_API InterventionalRadiologyController; -#endif -} // namespace sofa::component::controller::_interventionalradiologycontroller_ - -namespace sofa::component::mapping::_adaptivebeammapping_ -{ template class SOFA_BEAMADAPTER_CUDA_API AdaptiveBeamMapping; -#ifdef SOFA_GPU_CUDA_DOUBLE - template class SOFA_BEAMADAPTER_CUDA_API AdaptiveBeamMapping; -#endif -} // namespace sofa::component::mapping::_adaptivebeammapping_ - -namespace sofa::component::mapping -{ template class SOFA_BEAMADAPTER_CUDA_API MultiAdaptiveBeamMapping; -#ifdef SOFA_GPU_CUDA_DOUBLE - template class SOFA_BEAMADAPTER_CUDA_API MultiAdaptiveBeamMapping; -#endif -} // namespace sofa::component::mapping - -namespace sofa::beamadapter -{ template class SOFA_BEAMADAPTER_CUDA_API RodMeshSection; template class SOFA_BEAMADAPTER_CUDA_API RodSpireSection; template class SOFA_BEAMADAPTER_CUDA_API RodStraightSection; #ifdef SOFA_GPU_CUDA_DOUBLE + template class SOFA_BEAMADAPTER_CUDA_API BeamInterpolation; + template class SOFA_BEAMADAPTER_CUDA_API WireBeamInterpolation; + template class SOFA_BEAMADAPTER_CUDA_API WireRestShape; + template class SOFA_BEAMADAPTER_CUDA_API InterventionalRadiologyController; + template class SOFA_BEAMADAPTER_CUDA_API AdaptiveBeamMapping; + template class SOFA_BEAMADAPTER_CUDA_API MultiAdaptiveBeamMapping; template class SOFA_BEAMADAPTER_CUDA_API RodMeshSection; template class SOFA_BEAMADAPTER_CUDA_API RodSpireSection; template class SOFA_BEAMADAPTER_CUDA_API RodStraightSection; #endif -} // namespace sofa::beamadapter - -namespace sofa::gpu::cuda +using namespace sofa::gpu::cuda; +namespace cuda +{ +void registerBeamAdapterCUDAComponents(sofa::core::ObjectFactory* factory) { - #ifdef SOFA_GPU_CUDA_DOUBLE -int CudaBeamInterpolationClass = core::RegisterObject("Adaptive Beam Interpolation - Supports GPU-side computations using CUDA") - // .add< sofa::component::fem::BeamInterpolation >() - .add< sofa::component::fem::BeamInterpolation >() - ; + factory->registerObjects(sofa::core::ObjectRegistrationData("Adaptive Beam Interpolation - Supports GPU-side computations using CUDA") + // .add< BeamInterpolation >() + .add< BeamInterpolation >()); #endif #ifdef SOFA_GPU_CUDA_DOUBLE -int CudaWireBeamInterpolationClass = core::RegisterObject("Adaptive Wire Beam Interpolation - Supports GPU-side computations using CUDA") - // .add< sofa::component::fem::WireBeamInterpolation >() - .add< sofa::component::fem::WireBeamInterpolation >() - ; + factory->registerObjects(sofa::core::ObjectRegistrationData("Adaptive Wire Beam Interpolation - Supports GPU-side computations using CUDA") + // .add< WireBeamInterpolation >() + .add< WireBeamInterpolation >()); #endif -int CudaWireRestShapenClass = core::RegisterObject("Wire Shape - Supports GPU-side computations using CUDA") - .add< sofa::component::engine::WireRestShape >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Wire Shape - Supports GPU-side computations using CUDA") + .add< WireRestShape >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::component::engine::WireRestShape >() + .add< WireRestShape >() #endif - ; + ); -int CudaAdaptiveBeamForceFieldAndMassClass = core::RegisterObject("Adaptive Beam finite elements - Supports GPU-side computations using CUDA") - .add< sofa::component::forcefield::AdaptiveBeamForceFieldAndMass >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Adaptive Beam finite elements - Supports GPU-side computations using CUDA") + .add< AdaptiveBeamForceFieldAndMass >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::component::forcefield::AdaptiveBeamForceFieldAndMass >() + .add< AdaptiveBeamForceFieldAndMass >() #endif - ; + ); -int CudaInterventionalRadiologyControllerClass = core::RegisterObject("Provides a Mouse & Keyboard user control on an EdgeSet Topology - Supports GPU-side computations using CUDA") - .add< sofa::component::controller::InterventionalRadiologyController >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Provides a Mouse & Keyboard user control on an EdgeSet Topology - Supports GPU-side computations using CUDA") + .add< InterventionalRadiologyController >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::component::controller::InterventionalRadiologyController >() + .add< InterventionalRadiologyController >() #endif - ; + ); -int CudaAdaptiveBeamMappingClass = core::RegisterObject("Set the positions and velocities of points attached to a beam using linear interpolation between DOFs - Supports GPU-side computations using CUDA") - .add< sofa::component::mapping::AdaptiveBeamMapping >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Provides a Mouse & Keyboard user control on an EdgeSet Topology - Supports GPU-side computations using CUDA") + .add< InterventionalRadiologyController >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::component::mapping::AdaptiveBeamMapping >() + .add< InterventionalRadiologyController >() #endif - ; + ); -int CudaMultiAdaptiveBeamMappingClass = core::RegisterObject("Set the positions and velocities of points attached to a beam using linear interpolation between DOFs - Supports GPU-side computations using CUDA") - .add< sofa::component::mapping::MultiAdaptiveBeamMapping >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Set the positions and velocities of points attached to a beam using linear interpolation between DOFs - Supports GPU-side computations using CUDA") + .add< AdaptiveBeamMapping >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::component::mapping::MultiAdaptiveBeamMapping >() + .add< AdaptiveBeamMapping >() #endif - ; + ); -const int CudaRodMeshSectionClass = core::RegisterObject("Class defining a Rod Section using a MeshLoader and material parameters using CUDA.") - .add< sofa::beamadapter::RodMeshSection >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Set the positions and velocities of points attached to a beam using linear interpolation between DOFs - Supports GPU-side computations using CUDA") + .add< MultiAdaptiveBeamMapping >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::beamadapter::RodMeshSection >() + .add< MultiAdaptiveBeamMapping >() #endif - ; + ); -const int CudaRodSpireSectionClass = core::RegisterObject("Class defining a rod spire section, defining material and geometry parameters using CUDA.") - .add< sofa::beamadapter::RodSpireSection >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Class defining a Rod Section using a MeshLoader and material parameters using CUDA.") + .add< RodMeshSection >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::beamadapter::RodSpireSection >() + .add< RodMeshSection >() #endif -; + ); -const int CudaRodStraightSectionClass = core::RegisterObject("Class defining a rod straight section Material, defining material and geometry parameters using CUDA.") - .add< sofa::beamadapter::RodStraightSection >() + factory->registerObjects(sofa::core::ObjectRegistrationData("Class defining a rod spire section, defining material and geometry parameters using CUDA.") + .add< RodSpireSection >() #ifdef SOFA_GPU_CUDA_DOUBLE - .add< sofa::beamadapter::RodStraightSection >() + .add< RodSpireSection >() #endif -; - - + ); + factory->registerObjects(sofa::core::ObjectRegistrationData("Class defining a rod straight section Material, defining material and geometry parameters using CUDA.") + .add< RodStraightSection >() +#ifdef SOFA_GPU_CUDA_DOUBLE + .add< RodStraightSection >() +#endif + ); +} -} // namespace sofa::gpu::cuda +} // namespace cuda +} // namespace beamadapter diff --git a/extensions/CUDA/src/BeamAdapter/CUDA/init.cpp b/extensions/CUDA/src/BeamAdapter/CUDA/init.cpp index 585cdc745..b29d0a635 100644 --- a/extensions/CUDA/src/BeamAdapter/CUDA/init.cpp +++ b/extensions/CUDA/src/BeamAdapter/CUDA/init.cpp @@ -22,15 +22,20 @@ #include #include #include + #include +#include + namespace beamadapter::cuda { +extern void registerBeamAdapterCUDAComponents(sofa::core::ObjectFactory* factory); + extern "C" { SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule(); SOFA_EXPORT_DYNAMIC_LIBRARY const char* getModuleName(); SOFA_EXPORT_DYNAMIC_LIBRARY const char* getModuleVersion(); - SOFA_EXPORT_DYNAMIC_LIBRARY const char* getModuleComponentList(); + SOFA_BEAMADAPTER_API void registerObjects(sofa::core::ObjectFactory* factory); } void initExternalModule() @@ -53,17 +58,18 @@ void init() static bool first = true; if (first) { - sofa::component::initBeamAdapter(); + // make sure that this plugin is registered into the PluginManager + sofa::helper::system::PluginManager::getInstance().registerPlugin(MODULE_NAME); + + beamadapter::initBeamAdapter(); sofa::gpu::cuda::init(); first = false; } } -const char* getModuleComponentList() +void registerObjects(sofa::core::ObjectFactory* factory) { - /// string containing the names of the classes provided by the plugin - static std::string classes = sofa::core::ObjectFactory::getInstance()->listClassesFromTarget(MODULE_NAME); - return classes.c_str(); + registerBeamAdapterCUDAComponents(factory); } } // namespace beamadapter::cuda diff --git a/regression/references/3instruments.scn.reference b/regression/references/3instruments.scn.reference new file mode 100644 index 000000000..e69de29bb diff --git a/regression/references/3instruments.scn.reference_0_DOFs_mstate.txt.gz b/regression/references/3instruments.scn.reference_0_DOFs_mstate.txt.gz new file mode 100644 index 000000000..433d55d98 Binary files /dev/null and b/regression/references/3instruments.scn.reference_0_DOFs_mstate.txt.gz differ diff --git a/regression/references/3instruments.scn.reference_1_Quads_mstate.txt.gz b/regression/references/3instruments.scn.reference_1_Quads_mstate.txt.gz new file mode 100644 index 000000000..7e98af92b Binary files /dev/null and b/regression/references/3instruments.scn.reference_1_Quads_mstate.txt.gz differ diff --git a/regression/references/3instruments.scn.reference_2_Quads_mstate.txt.gz b/regression/references/3instruments.scn.reference_2_Quads_mstate.txt.gz new file mode 100644 index 000000000..85b718bb5 Binary files /dev/null and b/regression/references/3instruments.scn.reference_2_Quads_mstate.txt.gz differ diff --git a/regression/references/3instruments.scn.reference_3_Quads_mstate.txt.gz b/regression/references/3instruments.scn.reference_3_Quads_mstate.txt.gz new file mode 100644 index 000000000..19a2760ea Binary files /dev/null and b/regression/references/3instruments.scn.reference_3_Quads_mstate.txt.gz differ diff --git a/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz index 25287bcb6..9916ddd1f 100644 Binary files a/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz and b/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz differ diff --git a/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz index 84512cf85..a648774dc 100644 Binary files a/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz and b/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz differ diff --git a/regression/references/3instruments_collis.scn.reference_2_Quads_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_2_Quads_mstate.txt.gz deleted file mode 100644 index acd07db10..000000000 Binary files a/regression/references/3instruments_collis.scn.reference_2_Quads_mstate.txt.gz and /dev/null differ diff --git a/regression/references/3instruments_collis.scn.reference_3_Quads_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_3_Quads_mstate.txt.gz deleted file mode 100644 index 10e5de405..000000000 Binary files a/regression/references/3instruments_collis.scn.reference_3_Quads_mstate.txt.gz and /dev/null differ diff --git a/regression/references/3instruments_collis.scn.reference_4_Quads_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_4_Quads_mstate.txt.gz deleted file mode 100644 index e18d1a110..000000000 Binary files a/regression/references/3instruments_collis.scn.reference_4_Quads_mstate.txt.gz and /dev/null differ diff --git a/regression/references/SingleBeam.py.reference b/regression/references/SingleBeam.py.reference new file mode 100644 index 000000000..e69de29bb diff --git a/regression/references/SingleBeam.py.reference_0_DOFs_mstate.txt.gz b/regression/references/SingleBeam.py.reference_0_DOFs_mstate.txt.gz new file mode 100644 index 000000000..a28afb8b0 Binary files /dev/null and b/regression/references/SingleBeam.py.reference_0_DOFs_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeployment.py.reference b/regression/references/SingleBeamDeployment.py.reference new file mode 100644 index 000000000..e69de29bb diff --git a/regression/references/SingleBeamDeployment.py.reference_0_DOFs Container_mstate.txt.gz b/regression/references/SingleBeamDeployment.py.reference_0_DOFs Container_mstate.txt.gz new file mode 100644 index 000000000..619cd91d1 Binary files /dev/null and b/regression/references/SingleBeamDeployment.py.reference_0_DOFs Container_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeployment.scn.reference_0_DOFs Container_mstate.txt.gz b/regression/references/SingleBeamDeployment.scn.reference_0_DOFs Container_mstate.txt.gz index f714fac74..547f03bf1 100644 Binary files a/regression/references/SingleBeamDeployment.scn.reference_0_DOFs Container_mstate.txt.gz and b/regression/references/SingleBeamDeployment.scn.reference_0_DOFs Container_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeploymentCollision.py.reference b/regression/references/SingleBeamDeploymentCollision.py.reference new file mode 100644 index 000000000..e69de29bb diff --git a/regression/references/SingleBeamDeploymentCollision.py.reference_0_DOFs_mstate.txt.gz b/regression/references/SingleBeamDeploymentCollision.py.reference_0_DOFs_mstate.txt.gz new file mode 100644 index 000000000..298ad728f Binary files /dev/null and b/regression/references/SingleBeamDeploymentCollision.py.reference_0_DOFs_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeploymentCollision.py.reference_1_CollisionDOFs_mstate.txt.gz b/regression/references/SingleBeamDeploymentCollision.py.reference_1_CollisionDOFs_mstate.txt.gz new file mode 100644 index 000000000..ec5a97a49 Binary files /dev/null and b/regression/references/SingleBeamDeploymentCollision.py.reference_1_CollisionDOFs_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz b/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz index 397f4ad15..1c27efad9 100644 Binary files a/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz and b/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz b/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz index 2886309b2..581b69d59 100644 Binary files a/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz and b/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz differ diff --git a/regression/references/Tool_from_loader.scn.reference b/regression/references/Tool_from_loader.scn.reference new file mode 100644 index 000000000..e69de29bb diff --git a/regression/references/Tool_from_loader.scn.reference_0_Instrument_DOFs_mstate.txt.gz b/regression/references/Tool_from_loader.scn.reference_0_Instrument_DOFs_mstate.txt.gz new file mode 100644 index 000000000..b31a80699 Binary files /dev/null and b/regression/references/Tool_from_loader.scn.reference_0_Instrument_DOFs_mstate.txt.gz differ diff --git a/regression/references/Tool_from_loader.scn.reference_1_dof_visual_GC_mstate.txt.gz b/regression/references/Tool_from_loader.scn.reference_1_dof_visual_GC_mstate.txt.gz new file mode 100644 index 000000000..91dace6e9 Binary files /dev/null and b/regression/references/Tool_from_loader.scn.reference_1_dof_visual_GC_mstate.txt.gz differ diff --git a/src/BeamAdapter/component/BaseBeamInterpolation.h b/src/BeamAdapter/component/BaseBeamInterpolation.h index f8c75e0fd..7bd71e861 100644 --- a/src/BeamAdapter/component/BaseBeamInterpolation.h +++ b/src/BeamAdapter/component/BaseBeamInterpolation.h @@ -28,14 +28,16 @@ #include #include #include -#include #include +#include #include #include #include +#include +#include + -#include #include @@ -55,12 +57,14 @@ using sofa::component::statecontainer::MechanicalObject; * */ template -class BaseBeamInterpolation : public virtual sofa::core::objectmodel::BaseObject +class BaseBeamInterpolation : public sofa::core::behavior::SingleStateAccessor { public: SOFA_CLASS(SOFA_TEMPLATE(BaseBeamInterpolation, DataTypes) , - sofa::core::objectmodel::BaseObject); + SOFA_TEMPLATE(sofa::core::behavior::SingleStateAccessor, DataTypes)); + using Inherit = sofa::core::behavior::SingleStateAccessor; + using Coord = typename DataTypes::Coord; using VecCoord = typename DataTypes::VecCoord; using Real = typename Coord::value_type; @@ -68,8 +72,8 @@ class BaseBeamInterpolation : public virtual sofa::core::objectmodel::BaseObject using Deriv = typename DataTypes::Deriv; using VecDeriv = typename DataTypes::VecDeriv; - using Transform = typename sofa::defaulttype::SolidTypes::Transform; - using SpatialVector = typename sofa::defaulttype::SolidTypes::SpatialVector; + using Transform = sofa::type::Transform; + using SpatialVector = sofa::type::SpatialVector; using Vec2 = sofa::type::Vec<2, Real>; using Vec3 = sofa::type::Vec<3, Real>; @@ -82,15 +86,15 @@ class BaseBeamInterpolation : public virtual sofa::core::objectmodel::BaseObject using VecEdgeID = type::vector; using VecEdges = type::vector; - BaseBeamInterpolation(/*sofa::component::engine::WireRestShape *_restShape = nullptr*/); + BaseBeamInterpolation(); virtual ~BaseBeamInterpolation() = default; - void bwdInit() override; + void init() override; static void getControlPointsFromFrame( const Transform& global_H_local0, const Transform& global_H_local1, - const Real& L, + const Real L, Vec3& P0, Vec3& P1, Vec3& P2, Vec3& P3); @@ -103,99 +107,100 @@ class BaseBeamInterpolation : public virtual sofa::core::objectmodel::BaseObject virtual void clear(); public: - virtual void addBeam(const BaseMeshTopology::EdgeID& eID, const Real& length, const Real& x0, const Real& x1, const Real& angle); - unsigned int getNumBeams() { return this->d_edgeList.getValue().size(); } + virtual void addBeam(const EdgeID eID, const Real length, const Real x0, const Real x1, const Real angle); + sofa::Size getNumBeams() const { return static_cast(this->d_edgeList.getValue().size()); } - void getAbsCurvXFromBeam(int beam, Real& x_curv); - void getAbsCurvXFromBeam(int beam, Real& x_curv_start, Real& x_curv_end); + void getAbsCurvXFromBeam(const sofa::Index beam, Real& x_curv); + void getAbsCurvXFromBeam(const sofa::Index beam, Real& x_curv_start, Real& x_curv_end); /// getLength / setLength => provides the rest length of each spline using @sa d_lengthList virtual Real getRestTotalLength() = 0; - Real getLength(unsigned int edgeInList); - void setLength(unsigned int edgeInList, Real& length); + Real getLength(const EdgeID edgeInList); + void setLength(const EdgeID edgeInList, Real& length); + + virtual void getMechanicalSampling(Real& dx, const Real x_localcurv_abs) = 0; /// Collision information using @sa d_beamCollision - virtual void getCollisionSampling(Real& dx, const Real& x_localcurv_abs) = 0; - void addCollisionOnBeam(unsigned int b); + virtual void getCollisionSampling(Real& dx, const Real x_localcurv_abs) = 0; + void addCollisionOnBeam(const sofa::Index beam); void clearCollisionOnBeam(); virtual void getSamplingParameters(type::vector& xP_noticeable, - type::vector& nbP_density) = 0; - virtual void getNumberOfCollisionSegment(Real& dx, unsigned int& numLines) = 0; - + type::vector& nbP_density) = 0; + virtual void getNumberOfCollisionSegment(Real& dx, sofa::Size& numLines) = 0; - virtual void getCurvAbsAtBeam(const unsigned int& edgeInList_input, const Real& baryCoord_input, Real& x_output) = 0; - virtual void getSplineRestTransform(unsigned int edgeInList, Transform& local_H_local0_rest, Transform& local_H_local1_rest) = 0; + virtual void getCurvAbsAtBeam(const EdgeID edgeInList_input, const Real baryCoord_input, Real& x_output) = 0; + virtual void getSplineRestTransform(const EdgeID edgeInList, Transform& local_H_local0_rest, Transform& local_H_local1_rest) = 0; /// Returns the BeamSection @sa m_beamSection corresponding to the given beam - virtual const BeamSection& getBeamSection(sofa::Index beamId) = 0; + virtual const BeamSection& getBeamSection(const sofa::Index beamId) = 0; /// Returns the BeamSection data depending on the beam position at the given beam, similar to @getBeamSection - virtual void getInterpolationParameters(sofa::Index beamId, Real& _L, Real& _A, Real& _Iy, Real& _Iz, Real& _Asy, Real& _Asz, Real& J) = 0; + virtual void getInterpolationParameters(const sofa::Index beamId, Real& _L, Real& _A, Real& _Iy, Real& _Iz, Real& _Asy, Real& _Asz, Real& J) = 0; /// Returns the Young modulus, Poisson's ratio and massDensity coefficient of the section at the given curvilinear abscissa - virtual void getMechanicalParameters(sofa::Index beamId, Real& youngModulus, Real& cPoisson, Real& massDensity) = 0; + virtual void getMechanicalParameters(const sofa::Index beamId, Real& youngModulus, Real& cPoisson, Real& massDensity) = 0; - virtual void getBeamAtCurvAbs(const Real& x_input, unsigned int& edgeInList_output, Real& baryCoord_output, unsigned int start = 0); + virtual void getBeamAtCurvAbs(const Real x_input, sofa::Index& edgeInList_output, Real& baryCoord_output, unsigned int start = 0); int computeTransform(const EdgeID edgeInList, Transform& global_H_local0, Transform& global_H_local1, const VecCoord& x); int computeTransform(const EdgeID edgeInList, const PointID node0Idx, const PointID node1Idx, Transform& global_H_local0, Transform& global_H_local1, const VecCoord& x); - void getDOFtoLocalTransform(unsigned int edgeInList, Transform& DOF0_H_local0, Transform& DOF1_H_local1); - void getDOFtoLocalTransformInGlobalFrame(unsigned int edgeInList, Transform& DOF0Global_H_local0, Transform& DOF1Global_H_local1, const VecCoord& x); - void setTransformBetweenDofAndNode(int beam, const Transform& DOF_H_Node, unsigned int zeroORone); + void getDOFtoLocalTransform(const EdgeID edgeInList, Transform& DOF0_H_local0, Transform& DOF1_H_local1); + void getDOFtoLocalTransformInGlobalFrame(const EdgeID edgeInList, Transform& DOF0Global_H_local0, Transform& DOF1Global_H_local1, const VecCoord& x); + void setTransformBetweenDofAndNode(const sofa::Index beam, const Transform& DOF_H_Node, unsigned int zeroORone); - void getTangent(Vec3& t, const Real& baryCoord, - const Transform& global_H_local0, const Transform& global_H_local1, const Real& L); + void getTangent(Vec3& t, const Real baryCoord, + const Transform& global_H_local0, const Transform& global_H_local1, const Real L); - int getNodeIndices(unsigned int edgeInList, unsigned int& node0Idx, unsigned int& node1Idx); + int getNodeIndices(const EdgeID edgeInList, unsigned int& node0Idx, unsigned int& node1Idx); - void getSplinePoints(unsigned int edgeInList, const VecCoord& x, Vec3& P0, Vec3& P1, Vec3& P2, Vec3& P3); + void getSplinePoints(const EdgeID edgeInList, const VecCoord& x, Vec3& P0, Vec3& P1, Vec3& P2, Vec3& P3); unsigned int getStateSize() const; void computeActualLength(Real& length, const Vec3& P0, const Vec3& P1, const Vec3& P2, const Vec3& P3); - void computeStrechAndTwist(unsigned int edgeInList, const VecCoord& x, Vec3& ResultNodeO, Vec3& ResultNode1); + void computeStrechAndTwist(const EdgeID edgeInList, const VecCoord& x, Vec3& ResultNodeO, Vec3& ResultNode1); ///vId_Out provides the id of the multiVecId which stores the position of the Bezier Points void updateBezierPoints(const VecCoord& x, sofa::core::VecCoordId& vId_Out); - void updateBezierPoints(const VecCoord& x, unsigned int index, VectorVec3& v); + void updateBezierPoints(const VecCoord& x, sofa::Index index, VectorVec3& v); /// spline base interpolation of points and transformation - void interpolatePointUsingSpline(unsigned int edgeInList, const Real& baryCoord, const Vec3& localPos, const VecCoord& x, Vec3& posResult) { + void interpolatePointUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, Vec3& posResult) { interpolatePointUsingSpline(edgeInList, baryCoord, localPos, x, posResult, true, sofa::core::vec_id::read_access::position); } - void interpolatePointUsingSpline(unsigned int edgeInList, const Real& baryCoord, const Vec3& localPos, + void interpolatePointUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, Vec3& posResult, bool recompute, const sofa::core::ConstVecCoordId& vecXId); - void InterpolateTransformUsingSpline(unsigned int edgeInList, const Real& baryCoord, const Vec3& localPos, + void InterpolateTransformUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, Transform& global_H_localInterpol); - void InterpolateTransformUsingSpline(Transform& global_H_localResult, const Real& baryCoord, - const Transform& global_H_local0, const Transform& global_H_local1, const Real& L); + void InterpolateTransformUsingSpline(Transform& global_H_localResult, const Real baryCoord, + const Transform& global_H_local0, const Transform& global_H_local1, const Real L); - void InterpolateTransformAndVelUsingSpline(unsigned int edgeInList, const Real& baryCoord, const Vec3& localPos, + void InterpolateTransformAndVelUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, const VecDeriv& v, Transform& global_H_localInterpol, Deriv& v_interpol); /// compute the total bending Rotation Angle while going through the Spline (to estimate the curvature) - Real ComputeTotalBendingRotationAngle(const Real& dx_computation, const Transform& global_H_local0, - const Transform& global_H_local1, const Real& L, - const Real& baryCoordMin, const Real& baryCoordMax); + Real ComputeTotalBendingRotationAngle(const Real dx_computation, const Transform& global_H_local0, + const Transform& global_H_local1, const Real L, + const Real baryCoordMin, const Real baryCoordMax); /// 3DOF mapping - void MapForceOnNodeUsingSpline(unsigned int edgeInList, const Real& baryCoord, const Vec3& localPos, + void MapForceOnNodeUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, const Vec3& finput, SpatialVector& FNode0output, SpatialVector& FNode1output); /// 6DoF mapping - void MapForceOnNodeUsingSpline(unsigned int edgeInList, const Real& baryCoord, const Vec3& localPos, + void MapForceOnNodeUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, const SpatialVector& f6DofInput, SpatialVector& FNode0output, SpatialVector& FNode1output); @@ -209,10 +214,9 @@ class BaseBeamInterpolation : public virtual sofa::core::objectmodel::BaseObject typename MechanicalObject::SPtr m_StateNodes; Data< VecEdgeID > d_edgeList; - const VecEdges* m_topologyEdges{ nullptr }; ///2. Vector of length of each beam. Same size as @sa d_edgeList - Data< type::vector< double > > d_lengthList; + Data< type::vector< Real > > d_lengthList; ///3. (optional) apply a rigid Transform between the degree of Freedom and the first node of the beam. Indexation based on the num of Edge Data< type::vector< Transform > > d_DOF0TransformNode0; @@ -224,16 +228,12 @@ class BaseBeamInterpolation : public virtual sofa::core::objectmodel::BaseObject Data< type::vector< Vec2 > > d_curvAbsList; ///5. (optional) list of the beams in m_edgeList that need to be considered for collision - Data< sofa::type::vector > d_beamCollision; + Data< sofa::type::vector > d_beamCollision; Data d_dofsAndBeamsAligned; - - - /// pointer to the topology - BaseMeshTopology* m_topology{ nullptr }; - - /// pointer on mechanical state - MechanicalState* m_mstate{ nullptr }; + + /// link to the (edge) topology + SingleLink, BaseMeshTopology, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_topology; }; diff --git a/src/BeamAdapter/component/BaseBeamInterpolation.inl b/src/BeamAdapter/component/BaseBeamInterpolation.inl index 233bba526..d87ae05d8 100644 --- a/src/BeamAdapter/component/BaseBeamInterpolation.inl +++ b/src/BeamAdapter/component/BaseBeamInterpolation.inl @@ -36,7 +36,7 @@ using sofa::core::objectmodel::BaseContext; template void BaseBeamInterpolation::getControlPointsFromFrame(const Transform& global_H_local0, const Transform& global_H_local1, - const Real& L, + const Real L, Vec3& P0, Vec3& P1, Vec3& P2, Vec3& P3) { @@ -96,8 +96,10 @@ void BaseBeamInterpolation::RotateFrameForAlignNormalizedX(const Quat template -BaseBeamInterpolation::BaseBeamInterpolation(/*sofa::component::engine::WireRestShape *_restShape*/) - : d_edgeList(initData(&d_edgeList, "edgeList", "list of the edge in the topology that are concerned by the Interpolation")) +BaseBeamInterpolation::BaseBeamInterpolation() + : Inherit() + , m_StateNodes(sofa::core::objectmodel::New< sofa::component::statecontainer::MechanicalObject >()) + , d_edgeList(initData(&d_edgeList, "edgeList", "list of the edge in the topology that are concerned by the Interpolation")) , d_lengthList(initData(&d_lengthList, "lengthList", "list of the length of each beam")) , d_DOF0TransformNode0(initData(&d_DOF0TransformNode0, "DOF0TransformNode0", "Optional rigid transformation between the degree of Freedom and the first node of the beam")) , d_DOF1TransformNode1(initData(&d_DOF1TransformNode1, "DOF1TransformNode1", "Optional rigid transformation between the degree of Freedom and the second node of the beam")) @@ -105,49 +107,43 @@ BaseBeamInterpolation::BaseBeamInterpolation(/*sofa::component::engin , d_beamCollision(initData(&d_beamCollision, "beamCollision", "list of beam (in edgeList) that needs to be considered for collision")) , d_dofsAndBeamsAligned(initData(&d_dofsAndBeamsAligned, true, "dofsAndBeamsAligned", "if false, a transformation for each beam is computed between the DOF and the beam nodes")) - , m_topology(nullptr) - , m_mstate(nullptr) - , m_StateNodes(sofa::core::objectmodel::New< sofa::component::statecontainer::MechanicalObject >()) + , l_topology(initLink("topology", "link to the topology (must contain edges)")) { - m_StateNodes->setName("bezierNodes"); - addSlave(m_StateNodes); + this->addSlave(m_StateNodes); } template -void BaseBeamInterpolation::bwdInit() +void BaseBeamInterpolation::init() { - BaseContext* context = getContext(); - - m_mstate = dynamic_cast *> (context->getMechanicalState()); - if (m_mstate == nullptr) + Inherit::init(); + + if (!l_topology) { - msg_error() << "No MechanicalState found. Component is de-activated."; - d_componentState.setValue(ComponentState::Invalid); - return; + l_topology.set(this->getMState()->getContext()->getMeshTopologyLink()); } - /// Get the topology from context and check if it is valid. - m_topology = context->getMeshTopology(); - if (!m_topology) + if (l_topology) + { + msg_info() << "Found topology named "<< l_topology->getName() ; + } + else { - msg_error() << "No Topology found. Component is de-activated."; - d_componentState.setValue(ComponentState::Invalid); + msg_error() << "Cannot find a topology container. Please specify the link to the topology or insert one in the same node."; + this->d_componentState.setValue(ComponentState::Invalid); return; } - - /// Check the topology properties - if (m_topology->getNbEdges() == 0) + + if(l_topology->getNbEdges() == 0) { - msg_error() << "Found a topology but it is empty. Component is de-activated"; - d_componentState.setValue(ComponentState::Invalid); + msg_error() << "Found a topology but it is empty (no edges). Component is de-activated"; + this->d_componentState.setValue(ComponentState::Invalid); return; } - - m_topologyEdges = &m_topology->getEdges(); - Size nbEdges = m_topology->getNbEdges(); + + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); } @@ -169,7 +165,7 @@ void BaseBeamInterpolation::clear() template -void BaseBeamInterpolation::addBeam(const BaseMeshTopology::EdgeID& eID, const Real& length, const Real& x0, const Real& x1, const Real& angle) +void BaseBeamInterpolation::addBeam(const EdgeID eID, const Real length, const Real x0, const Real x1, const Real angle) { auto edgeList = sofa::helper::getWriteOnlyAccessor(d_edgeList); auto lengthList = sofa::helper::getWriteOnlyAccessor(d_lengthList); @@ -194,14 +190,14 @@ void BaseBeamInterpolation::addBeam(const BaseMeshTopology::EdgeID& e template -void BaseBeamInterpolation::getAbsCurvXFromBeam(int beam, Real& x_curv) +void BaseBeamInterpolation::getAbsCurvXFromBeam(const sofa::Index beam, Real& x_curv) { x_curv = d_curvAbsList.getValue()[beam].y(); } template -void BaseBeamInterpolation::getAbsCurvXFromBeam(int beam, Real& x_curv_start, Real& x_curv_end) +void BaseBeamInterpolation::getAbsCurvXFromBeam(const sofa::Index beam, Real& x_curv_start, Real& x_curv_end) { Vec2 ca = d_curvAbsList.getValue()[beam]; x_curv_start = ca.x(); @@ -209,14 +205,14 @@ void BaseBeamInterpolation::getAbsCurvXFromBeam(int beam, Real& x_cur } template -typename BaseBeamInterpolation::Real BaseBeamInterpolation::getLength(unsigned int edgeInList) +typename BaseBeamInterpolation::Real BaseBeamInterpolation::getLength(const EdgeID edgeInList) { Real _L = d_lengthList.getValue()[edgeInList]; return _L; } template -void BaseBeamInterpolation::setLength(unsigned int edgeInList, Real& length) +void BaseBeamInterpolation::setLength(const EdgeID edgeInList, Real& length) { auto lengthList = sofa::helper::getWriteOnlyAccessor(d_lengthList); lengthList[edgeInList] = length; @@ -258,14 +254,14 @@ int BaseBeamInterpolation::computeTransform(const EdgeID edgeInList, template -void BaseBeamInterpolation::getBeamAtCurvAbs(const Real& x_input, unsigned int& edgeInList_output, Real& baryCoord_output, unsigned int start) +void BaseBeamInterpolation::getBeamAtCurvAbs(const Real x_input, unsigned int& edgeInList_output, Real& baryCoord_output, unsigned int start) { /// lTotalRest = total length of the Real lTotalRest = getRestTotalLength(); /// LTotal = length sum of the beams that are "out" Real LTotal = 0.0; - const unsigned int edgeListSize = this->d_edgeList.getValue().size(); + const sofa::Size edgeListSize = static_cast(this->d_edgeList.getValue().size()); /// we find the length of the beam that is "out" for (unsigned int e = start; e < edgeListSize; e++) @@ -322,7 +318,7 @@ int BaseBeamInterpolation::computeTransform(const EdgeID edgeInList, template -void BaseBeamInterpolation::getDOFtoLocalTransform(unsigned int edgeInList, Transform& DOF0_H_local0, Transform& DOF1_H_local1) +void BaseBeamInterpolation::getDOFtoLocalTransform(const EdgeID edgeInList, Transform& DOF0_H_local0, Transform& DOF1_H_local1) { if (d_dofsAndBeamsAligned.getValue()) { @@ -337,7 +333,7 @@ void BaseBeamInterpolation::getDOFtoLocalTransform(unsigned int edgeI template -void BaseBeamInterpolation::getDOFtoLocalTransformInGlobalFrame(unsigned int edgeInList, Transform& DOF0Global_H_local0, Transform& DOF1Global_H_local1, const VecCoord& x) +void BaseBeamInterpolation::getDOFtoLocalTransformInGlobalFrame(const EdgeID edgeInList, Transform& DOF0Global_H_local0, Transform& DOF1Global_H_local1, const VecCoord& x) { Transform DOF0_H_local0, DOF1_H_local1; @@ -357,9 +353,9 @@ void BaseBeamInterpolation::getDOFtoLocalTransformInGlobalFrame(unsig template -void BaseBeamInterpolation::setTransformBetweenDofAndNode(int beam, const Transform& DOF_H_Node, unsigned int zeroORone) +void BaseBeamInterpolation::setTransformBetweenDofAndNode(const sofa::Index beam, const Transform& DOF_H_Node, unsigned int zeroORone) { - if (beam > int(d_DOF0TransformNode0.getValue().size() - 1) || beam > int(d_DOF1TransformNode1.getValue().size() - 1)) + if (beam > (d_DOF0TransformNode0.getValue().size() - 1) || beam > (d_DOF1TransformNode1.getValue().size() - 1)) { msg_error() << "WARNING setTransformBetweenDofAndNode on non existing beam"; return; @@ -380,9 +376,9 @@ void BaseBeamInterpolation::setTransformBetweenDofAndNode(int beam, c /// getTangent : Computation of a Tangent for the beam (it is given by the derivative of the spline formulation) template -void BaseBeamInterpolation::getTangent(Vec3& t, const Real& baryCoord, +void BaseBeamInterpolation::getTangent(Vec3& t, const Real baryCoord, const Transform& global_H_local0, - const Transform& global_H_local1, const Real& L) + const Transform& global_H_local1, const Real L) { Vec3 P0, P1, P2, P3; @@ -397,19 +393,19 @@ void BaseBeamInterpolation::getTangent(Vec3& t, const Real& baryCoord template -int BaseBeamInterpolation::getNodeIndices(unsigned int edgeInList, +int BaseBeamInterpolation::getNodeIndices(const EdgeID edgeInList, unsigned int& node0Idx, unsigned int& node1Idx) { - if (m_topologyEdges == nullptr) + if(!this->isComponentStateValid()) { - msg_error() << "This object does not have edge topology defined (computation halted). "; + msg_error() << "(getNodeIndices) This component is invalid, check the other error messages. "; return -1; } /// 1. Get the indices of element and nodes const EdgeID& e = d_edgeList.getValue()[edgeInList]; - const BaseMeshTopology::Edge& edge = (*m_topologyEdges)[e]; + const BaseMeshTopology::Edge& edge = l_topology->getEdge(e); node0Idx = edge[0]; node1Idx = edge[1]; @@ -418,7 +414,7 @@ int BaseBeamInterpolation::getNodeIndices(unsigned int edgeInList, template -void BaseBeamInterpolation::getSplinePoints(unsigned int edgeInList, const VecCoord& x, Vec3& P0, Vec3& P1, Vec3& P2, Vec3& P3) +void BaseBeamInterpolation::getSplinePoints(const EdgeID edgeInList, const VecCoord& x, Vec3& P0, Vec3& P1, Vec3& P2, Vec3& P3) { Transform global_H_local0, global_H_local1; if (computeTransform(edgeInList, global_H_local0, global_H_local1, x) == -1) @@ -427,7 +423,7 @@ void BaseBeamInterpolation::getSplinePoints(unsigned int edgeInList, return; } - const Real& _L = d_lengthList.getValue()[edgeInList]; + const Real _L = d_lengthList.getValue()[edgeInList]; this->getControlPointsFromFrame(global_H_local0, global_H_local1, _L, P0, P1, P2, P3); } @@ -436,14 +432,14 @@ void BaseBeamInterpolation::getSplinePoints(unsigned int edgeInList, template unsigned int BaseBeamInterpolation::getStateSize() const { - if (m_mstate == nullptr) + if(!this->isComponentStateValid()) { - msg_error() << "No _mstate found (Aborting)"; + msg_error() << "(getStateSize) This component is invalid, check the other error messages. "; return 0; } else { - return m_mstate->getSize(); + return this->getMState()->getSize(); } } @@ -492,7 +488,7 @@ void BaseBeamInterpolation::computeActualLength(Real& length, const V template -void BaseBeamInterpolation::computeStrechAndTwist(unsigned int edgeInList, const VecCoord& x, Vec3& ResultNodeO, Vec3& ResultNode1) +void BaseBeamInterpolation::computeStrechAndTwist(const EdgeID edgeInList, const VecCoord& x, Vec3& ResultNodeO, Vec3& ResultNode1) { /// ResultNode = [half length of the beam (m), geometrical Twist angle (rad), additional mechanical Twist angle (rad)] @@ -581,13 +577,15 @@ void BaseBeamInterpolation::computeStrechAndTwist(unsigned int edgeIn ///vId_Out provides the id of the multiVecId which stores the position of the Bezier Points template -void BaseBeamInterpolation::updateBezierPoints(const VecCoord& x, sofa::core::VecCoordId& vId_Out) { +void BaseBeamInterpolation::updateBezierPoints(const VecCoord& x, sofa::core::VecCoordId& vId_Out) +{ + const sofa::Size edgeListSize = static_cast(d_edgeList.getValue().size()); ///Mechanical Object to stock Bezier points. - m_StateNodes->resize(d_edgeList.getValue().size() * 4); + m_StateNodes->resize(edgeListSize * 4); auto bezierPosVec = sofa::helper::getWriteOnlyAccessor(*m_StateNodes->write(vId_Out)); - bezierPosVec.resize(d_edgeList.getValue().size() * 4); + bezierPosVec.resize(edgeListSize * 4); - for (unsigned int i = 0; i < d_edgeList.getValue().size(); i++) { + for (unsigned int i = 0; i < edgeListSize; i++) { updateBezierPoints(x, i, bezierPosVec.wref()); } @@ -595,9 +593,9 @@ void BaseBeamInterpolation::updateBezierPoints(const VecCoord& x, so template void BaseBeamInterpolation::updateBezierPoints(const VecCoord& x, unsigned int index, - VectorVec3& bezierPosVec) { - /// <<" interpolatePointUsingSpline : "<< edgeInList<<" xbary="< make something faster ! @@ -613,8 +611,8 @@ void BaseBeamInterpolation::updateBezierPoints(const VecCoord& x, uns template -void BaseBeamInterpolation::interpolatePointUsingSpline(unsigned int edgeInList, - const Real& baryCoord, +void BaseBeamInterpolation::interpolatePointUsingSpline(const EdgeID edgeInList, + const Real baryCoord, const Vec3& localPos, const VecCoord& x, Vec3& posResult, @@ -624,7 +622,7 @@ void BaseBeamInterpolation::interpolatePointUsingSpline(unsigned int if (recompute) { /// <<" interpolatePointUsingSpline : "<< edgeInList<<" xbary="< _L * 1e-10) { @@ -661,7 +659,7 @@ void BaseBeamInterpolation::interpolatePointUsingSpline(unsigned int else { /// no need to recompute the positions of the Spline => we will read their value in the vector which id is vecXId - const Real& _L = d_lengthList.getValue()[edgeInList]; + const Real _L = d_lengthList.getValue()[edgeInList]; if (localPos.norm() > _L * 1e-10) { @@ -699,8 +697,8 @@ void BaseBeamInterpolation::interpolatePointUsingSpline(unsigned int template -void BaseBeamInterpolation::InterpolateTransformUsingSpline(unsigned int edgeInList, - const Real& baryCoord, +void BaseBeamInterpolation::InterpolateTransformUsingSpline(const EdgeID edgeInList, + const Real baryCoord, const Vec3& localPos, const VecCoord& x, Transform& global_H_localInterpol) @@ -712,7 +710,7 @@ void BaseBeamInterpolation::InterpolateTransformUsingSpline(unsigned return; } - const Real& _L = d_lengthList.getValue()[edgeInList]; + const Real _L = d_lengthList.getValue()[edgeInList]; if (localPos.norm() > 1e-10 * _L) msg_warning() << "Interpolate frame only on the central curve of the beam. "; @@ -726,10 +724,10 @@ void BaseBeamInterpolation::InterpolateTransformUsingSpline(unsigned /// the location of the frame is given by baryCoord template void BaseBeamInterpolation::InterpolateTransformUsingSpline(Transform& global_H_localResult, - const Real& baryCoord, + const Real baryCoord, const Transform& global_H_local0, const Transform& global_H_local1, - const Real& L) + const Real L) { Vec3NoInit P0, P1, P2, P3; @@ -788,7 +786,7 @@ void BaseBeamInterpolation::InterpolateTransformUsingSpline(Transform template -void BaseBeamInterpolation::InterpolateTransformAndVelUsingSpline(unsigned int edgeInList, const Real& baryCoord, +void BaseBeamInterpolation::InterpolateTransformAndVelUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, const VecDeriv& v, Transform& global_H_localInterpol, Deriv& v_interpol) { @@ -814,7 +812,7 @@ void BaseBeamInterpolation::InterpolateTransformAndVelUsingSpline(uns /// 4. Computes the transformation - const Real& _L = d_lengthList.getValue()[edgeInList]; + const Real _L = d_lengthList.getValue()[edgeInList]; if (localPos.norm() > 1e-10 * _L) msg_warning() << "Interpolate frame only on the central curve of the beam"; @@ -854,9 +852,9 @@ void BaseBeamInterpolation::InterpolateTransformAndVelUsingSpline(uns /// Principle: computation of several tangent between node0 to the node1 (given by numComputationPoints) /// and computation of an angle (acos) between these successive tangents template -typename BaseBeamInterpolation::Real BaseBeamInterpolation::ComputeTotalBendingRotationAngle(const Real& dx_computation, - const Transform& global_H_local0, const Transform& global_H_local1, const Real& L, - const Real& baryCoordMin, const Real& baryCoordMax) +typename BaseBeamInterpolation::Real BaseBeamInterpolation::ComputeTotalBendingRotationAngle(const Real dx_computation, + const Transform& global_H_local0, const Transform& global_H_local1, const Real L, + const Real baryCoordMin, const Real baryCoordMax) { Vec3 P0, P1, P2, P3, t0, t1; @@ -898,8 +896,8 @@ typename BaseBeamInterpolation::Real BaseBeamInterpolation /// Result: Forces (and Torques) on the nodes of this beam //////////////////////////////////////////////////////////////////////////////////////////////////// template -void BaseBeamInterpolation::MapForceOnNodeUsingSpline(unsigned int edgeInList, - const Real& baryCoord, const Vec3& localPos, +void BaseBeamInterpolation::MapForceOnNodeUsingSpline(const EdgeID edgeInList, + const Real baryCoord, const Vec3& localPos, const VecCoord& x, const Vec3& finput, SpatialVector& FNode0output, SpatialVector& FNode1output) { @@ -962,7 +960,7 @@ void BaseBeamInterpolation::MapForceOnNodeUsingSpline(unsigned int ed /// Result: Forces (and Torques) on the nodes of this beam //////////////////////////////////////////////////////////////////////////////////////////////////// template -void BaseBeamInterpolation::MapForceOnNodeUsingSpline(unsigned int edgeInList, const Real& baryCoord, +void BaseBeamInterpolation::MapForceOnNodeUsingSpline(const EdgeID edgeInList, const Real baryCoord, const Vec3& localPos, const VecCoord& x, const SpatialVector& f6DofInput, SpatialVector& FNode0output, SpatialVector& FNode1output) diff --git a/src/BeamAdapter/component/BeamInterpolation.h b/src/BeamAdapter/component/BeamInterpolation.h index 9e367d5f8..cfe9dd77e 100644 --- a/src/BeamAdapter/component/BeamInterpolation.h +++ b/src/BeamAdapter/component/BeamInterpolation.h @@ -69,6 +69,8 @@ class BeamInterpolation : public BaseBeamInterpolation { public: SOFA_CLASS( SOFA_TEMPLATE(BeamInterpolation, DataTypes) , SOFA_TEMPLATE(BaseBeamInterpolation, DataTypes)); + + using Inherit = BaseBeamInterpolation; using Coord = typename DataTypes::Coord; using VecCoord = typename DataTypes::VecCoord; @@ -88,6 +90,7 @@ class BeamInterpolation : public BaseBeamInterpolation using PointID = BaseMeshTopology::PointID; using ElementID = BaseMeshTopology::EdgeID; + using EdgeID = BaseMeshTopology::EdgeID; using VecElementID = type::vector; using VecEdges = type::vector; @@ -116,7 +119,6 @@ class BeamInterpolation : public BaseBeamInterpolation //////////////////////////////////// Inherited from Base /////////////////////////////////////// void init() override ; - void bwdInit() override ; void reinit() override ; void reset() override ; @@ -145,12 +147,23 @@ class BeamInterpolation : public BaseBeamInterpolation Real &_Asy, Real &_Asz, Real &J) override; void getMechanicalParameters(sofa::Index beamId, Real& youngModulus, Real& cPoisson, Real& massDensity) override; - void getTangentUsingSplinePoints(unsigned int edgeInList, const Real& baryCoord, const sofa::core::ConstVecCoordId &vecXId, Vec3& t ); + void getTangentUsingSplinePoints(const EdgeID edgeInList, const Real baryCoord, const sofa::core::ConstVecCoordId &vecXId, Vec3& t ); /// computeActualLength => given the 4 control points of the spline, it provides an estimate of the length (using gauss points integration) - virtual void getCurvAbsAtBeam(const unsigned int& edgeInList_input, const Real& baryCoord_input, Real& x_output) {} - virtual void getBeamAtCurvAbs(const Real& x_input, unsigned int& edgeInList_output, Real& baryCoord_output, unsigned int start = 0) {} + virtual void getCurvAbsAtBeam(const EdgeID edgeInList_input, const Real baryCoord_input, Real& x_output) override + { + SOFA_UNUSED(edgeInList_input); + SOFA_UNUSED(baryCoord_input); + SOFA_UNUSED(x_output); + } + virtual void getBeamAtCurvAbs(const Real x_input, unsigned int& edgeInList_output, Real& baryCoord_output, unsigned int start = 0) override + { + SOFA_UNUSED(x_input); + SOFA_UNUSED(edgeInList_output); + SOFA_UNUSED(baryCoord_output); + SOFA_UNUSED(start); + } Data crossSectionShape; @@ -182,16 +195,17 @@ class BeamInterpolation : public BaseBeamInterpolation Data d_straight; - virtual void clear(); - virtual void addBeam(const BaseMeshTopology::EdgeID &eID , const Real &length, const Real &x0, const Real &x1, const Real &angle); + virtual void clear() override; + virtual void addBeam(const EdgeID eID , const Real length, const Real x0, const Real x1, const Real angle) override; virtual void getSamplingParameters(type::vector& xP_noticeable, - type::vector& nbP_density) ; + type::vector& nbP_density) override; Real getRestTotalLength() override; - void getCollisionSampling(Real &dx, const Real& x_localcurv_abs) override; + void getMechanicalSampling(Real& dx, const Real x_localcurv_abs) override; + void getCollisionSampling(Real &dx, const Real x_localcurv_abs) override; void getNumberOfCollisionSegment(Real &dx, unsigned int &numLines) override; - - void setTransformBetweenDofAndNode(int beam, const Transform &DOF_H_Node, unsigned int zeroORone ); - void getSplineRestTransform(unsigned int edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) override; + + void setTransformBetweenDofAndNode(const sofa::Index beam, const Transform &DOF_H_Node, unsigned int zeroORone ); + void getSplineRestTransform(const EdgeID edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) override; /////////////////////////// Deprecated Methods ////////////////////////////////////////// [[deprecated("Releasing catheter or brokenIn2 mode is not anymore supported. Feature has been removed after release v23.06")]] @@ -218,8 +232,8 @@ protected : void checkDataSize(Real& defaultValue, Data>& dataList, const size_t& nbEdges); - void computeRectangularCrossSectionInertiaMatrix(const Real &Ly, const Real &Lz, BeamSection §ion); - void computeCircularCrossSectionInertiaMatrix(const Real &r, const Real &rInner, BeamSection §ion); + void computeRectangularCrossSectionInertiaMatrix(const Real Ly, const Real Lz, BeamSection §ion); + void computeCircularCrossSectionInertiaMatrix(const Real r, const Real rInner, BeamSection §ion); }; #if !defined(SOFA_PLUGIN_BEAMADAPTER_BEAMINTERPOLATION_CPP) diff --git a/src/BeamAdapter/component/BeamInterpolation.inl b/src/BeamAdapter/component/BeamInterpolation.inl index ecf325c99..d17c1ce6d 100644 --- a/src/BeamAdapter/component/BeamInterpolation.inl +++ b/src/BeamAdapter/component/BeamInterpolation.inl @@ -50,8 +50,9 @@ using sofa::helper::ReadAccessor ; //////////////////////////////////// BREAMINTERPOLATION //////////////////////////////////////////// template -BeamInterpolation::BeamInterpolation() : - crossSectionShape(initData(&crossSectionShape, +BeamInterpolation::BeamInterpolation() + : Inherit() + , crossSectionShape(initData(&crossSectionShape, {"circular","elliptic (not available)","rectangular"}, "crossSectionShape", "shape of the cross-section. Can be: circular, elliptic, square, rectangular. Default is circular" )) @@ -90,7 +91,7 @@ BeamInterpolation::BeamInterpolation() : } template -void BeamInterpolation::computeRectangularCrossSectionInertiaMatrix(const Real& Ly, const Real& Lz, BeamSection& section) +void BeamInterpolation::computeRectangularCrossSectionInertiaMatrix(const Real Ly, const Real Lz, BeamSection& section) { section._Iy=Ly*Lz*Lz*Lz/12.0; section._Iz=Lz*Ly*Ly*Ly/12.0; @@ -102,7 +103,7 @@ void BeamInterpolation::computeRectangularCrossSectionInertiaMatrix(c } template -void BeamInterpolation::computeCircularCrossSectionInertiaMatrix(const Real &r, const Real &rInner, BeamSection §ion) +void BeamInterpolation::computeCircularCrossSectionInertiaMatrix(const Real r, const Real rInner, BeamSection §ion) { section._r = r; section._rInner = rInner; @@ -139,7 +140,7 @@ void BeamInterpolation::computeCrossSectionInertiaMatrix() } else { - Size nbEdges = this->m_topology->getNbEdges(); + Size nbEdges = this->l_topology->getNbEdges(); m_section.resize(nbEdges); if ( crossSectionShape.getValue().getSelectedItem() == "elliptic") { @@ -149,13 +150,13 @@ void BeamInterpolation::computeCrossSectionInertiaMatrix() { const auto& lengthY = helper::getReadAccessor(d_lengthY); const auto& lengthZ = helper::getReadAccessor(d_lengthZ); - for (int beamId=0; beamId::computeCrossSectionInertiaMatrix() ////////////////////////////////// ADAPTIVE INTERPOLATION ////////////////////////////////////////// -template -void BeamInterpolation::init() -{ -} - template void BeamInterpolation::checkDataSize(Real& defaultValue, Data>& dataList, const size_t& nbEdges) { @@ -194,15 +190,15 @@ void BeamInterpolation::checkDataSize(Real& defaultValue, Data -void BeamInterpolation::bwdInit() +void BeamInterpolation::init() { this->d_componentState.setValue(ComponentState::Loading); - BaseBeamInterpolation::bwdInit(); + Inherit::init(); if (this->d_componentState.getValue() == ComponentState::Invalid) return; - Size nbEdges = this->m_topology->getNbEdges(); + Size nbEdges = this->l_topology->getNbEdges(); checkDataSize(m_defaultRadius, d_radius, nbEdges); checkDataSize(m_defaultInnerRadius, d_innerRadius, nbEdges); checkDataSize(m_defaultLengthY, d_lengthY, nbEdges); @@ -216,7 +212,7 @@ void BeamInterpolation::bwdInit() auto edgeList = sofa::helper::getWriteOnlyAccessor(this->d_edgeList); edgeList.clear(); - for (unsigned int i=0; im_topology->getNbEdges(); i++) + for (unsigned int i=0; il_topology->getNbEdges(); i++) { edgeList.push_back(i); } @@ -230,12 +226,12 @@ void BeamInterpolation::bwdInit() DOF1TransformNode1.resize(edgeList.size()); } - ReadAccessor > statePos = this->m_mstate->read(sofa::core::vec_id::read_access::position) ; + ReadAccessor > statePos = this->getMState()->read(sofa::core::vec_id::read_access::position) ; auto lengthList = sofa::helper::getWriteOnlyAccessor(this->d_lengthList); lengthList.clear(); - const unsigned int edgeListSize = this->d_edgeList.getValue().size(); + const auto edgeListSize = this->d_edgeList.getValue().size(); unsigned int nd0Id=0, nd1Id=0; @@ -293,8 +289,7 @@ void BeamInterpolation::bwdInit() template void BeamInterpolation::reinit() { - init(); - bwdInit(); + init(); } template @@ -312,7 +307,7 @@ void BeamInterpolation::reset() if(d_componentState.getValue()==ComponentState::Invalid) return ; - bwdInit(); + init(); } template @@ -321,7 +316,7 @@ bool BeamInterpolation::interpolationIsAlreadyInitialized() if (this->d_edgeList.getValue().size() == 0) return false; - const unsigned int nbEdges = this->d_edgeList.getValue().size(); + const auto nbEdges = this->d_edgeList.getValue().size(); if (this->d_DOF0TransformNode0.getValue().size() != nbEdges) return false; @@ -341,17 +336,18 @@ bool BeamInterpolation::interpolationIsAlreadyInitialized() template bool BeamInterpolation::verifyTopology() { + const auto nbEdges = this->l_topology->getNbEdges(); + //TODO(dmarchal) This contains "code" specific slang that cannot be understood by user. - dmsg_info() << "The vector _topologyEdges is now set with " << this->m_topologyEdges->size() << " edges" ; - + dmsg_info() << "The vector _topologyEdges is now set with " << nbEdges << " edges" ; const VecElementID &edgeList = this->d_edgeList.getValue(); for (unsigned int j = 0; j < edgeList.size(); j++) { - if(edgeList[j] > this->m_topologyEdges->size()) + if(edgeList[j] > nbEdges) { msg_error() << "The provided edge index '" << edgeList[j] << "'is larger than '" - << this->m_topologyEdges->size() << "' the amount of edges in the topology. " ; + << nbEdges << "' the amount of edges in the topology. " ; return false; } } @@ -378,7 +374,7 @@ void BeamInterpolation::clear() template -void BeamInterpolation::addBeam(const BaseMeshTopology::EdgeID &eID , const Real &length, const Real &x0, const Real &x1, const Real &angle) +void BeamInterpolation::addBeam(const EdgeID eID, const Real length, const Real x0, const Real x1, const Real angle) { auto edgeList = sofa::helper::getWriteOnlyAccessor(this->d_edgeList); auto lengthList = sofa::helper::getWriteOnlyAccessor(this->d_lengthList); @@ -403,7 +399,7 @@ void BeamInterpolation::addBeam(const BaseMeshTopology::EdgeID &eID template -void BeamInterpolation::getSamplingParameters(type::vector& /*xP_noticeable*/, type::vector< int>& /*nbP_density*/) +void BeamInterpolation::getSamplingParameters(type::vector& /*xP_noticeable*/, type::vector& /*nbP_density*/) { msg_error()<<"getSamplingParameters is not implemented when _restShape== nullptr : TODO !! "; } @@ -420,21 +416,32 @@ typename BeamInterpolation::Real BeamInterpolation::getRes return le; } + +template +void BeamInterpolation::getMechanicalSampling(Real& dx, const Real x_localcurv_abs) +{ + SOFA_UNUSED(x_localcurv_abs); + + const auto numLines = this->l_topology->getNbEdges(); + dx = getRestTotalLength()/numLines; +} + template -void BeamInterpolation::getCollisionSampling(Real &dx, const Real& /*x_localcurv_abs*/) +void BeamInterpolation::getCollisionSampling(Real &dx, const Real x_localcurv_abs) { - unsigned int numLines = 30; + SOFA_UNUSED(x_localcurv_abs); + + const auto numLines = this->l_topology->getNbEdges(); dx = getRestTotalLength()/numLines; } template void BeamInterpolation::getNumberOfCollisionSegment(Real &dx, unsigned int &numLines) { - numLines = 30; + numLines = static_cast(this->l_topology->getNbEdges()); dx = getRestTotalLength()/numLines; } - template void BeamInterpolation::getInterpolationParameters(sofa::Index beamId, Real& _L, Real& _A, Real& _Iy, Real& _Iz, Real& _Asy, Real& _Asz, Real& _J) @@ -455,11 +462,13 @@ void BeamInterpolation::getInterpolationParameters(sofa::Index beamId template void BeamInterpolation::getMechanicalParameters(sofa::Index beamId, Real& youngModulus, Real& cPoisson, Real& massDensity) { + SOFA_UNUSED(massDensity); + const auto& defaultYoungModuli = d_defaultYoungModulus.getValue(); - youngModulus = (beamId < int(defaultYoungModuli.size()))? defaultYoungModuli[beamId]: m_defaultYoungModulus; + youngModulus = (beamId < static_cast(defaultYoungModuli.size()))? defaultYoungModuli[beamId]: m_defaultYoungModulus; const auto& poissonRatios = d_poissonRatio.getValue(); - cPoisson = (beamId < int(defaultYoungModuli.size()))? poissonRatios[beamId]: m_defaultPoissonRatio; + cPoisson = (beamId < static_cast(defaultYoungModuli.size()))? poissonRatios[beamId]: m_defaultPoissonRatio; //TODO: massDensity?? } @@ -467,9 +476,9 @@ void BeamInterpolation::getMechanicalParameters(sofa::Index beamId, R template -void BeamInterpolation::setTransformBetweenDofAndNode(int beam, const Transform &DOF_H_Node, unsigned int zeroORone ) +void BeamInterpolation::setTransformBetweenDofAndNode(const sofa::Index beam, const Transform &DOF_H_Node, unsigned int zeroORone) { - if (beam > int(this->d_DOF0TransformNode0.getValue().size()-1) || beam > int(this->d_DOF1TransformNode1.getValue().size()-1)) + if (beam > this->d_DOF0TransformNode0.getValue().size()-1 || beam > this->d_DOF1TransformNode1.getValue().size()-1) { msg_error()<<"WARNING setTransformBetweenDofAndNode on non existing beam"; return; @@ -489,7 +498,7 @@ void BeamInterpolation::setTransformBetweenDofAndNode(int beam, const template -void BeamInterpolation::getSplineRestTransform(unsigned int edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) +void BeamInterpolation::getSplineRestTransform(const EdgeID edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) { if(d_straight.getValue()) { @@ -536,7 +545,7 @@ void BeamInterpolation::getSplineRestTransform(unsigned int edgeInLis template -void BeamInterpolation::getTangentUsingSplinePoints(unsigned int edgeInList, const Real& baryCoord, +void BeamInterpolation::getTangentUsingSplinePoints(const EdgeID edgeInList, const Real baryCoord, const sofa::core::ConstVecCoordId &vecXId, Vec3& t ) { @@ -573,7 +582,7 @@ void BeamInterpolation::updateInterpolation(){ dmsg_info() <<"entering updateInterpolation" ; const type::vector< Vec2 > &interpolationInputs = d_InterpolationInputs.getValue(); - unsigned int numInterpolatedPositions = interpolationInputs.size(); + const auto numInterpolatedPositions = interpolationInputs.size(); auto interpolatedPos = sofa::helper::getWriteOnlyAccessor(d_InterpolatedPos); auto interpolatedVel = sofa::helper::getWriteOnlyAccessor(d_InterpolatedVel); @@ -592,16 +601,16 @@ void BeamInterpolation::updateInterpolation(){ if(d_vecID.getValue().getSelectedItem() == "current") { dmsg_info() <<" position " << msgendl - << " ="<< this->m_mstate->read( sofa::core::vec_id::read_access::position )->getValue( ) ; - x=this->m_mstate->read( sofa::core::vec_id::read_access::position ); + << " ="<< this->getMState()->read( sofa::core::vec_id::read_access::position )->getValue( ) ; + x=this->getMState()->read( sofa::core::vec_id::read_access::position ); } else if(d_vecID.getValue().getSelectedItem() == "free") { - x=this->m_mstate->read( sofa::core::vec_id::read_access::freePosition ) ; + x=this->getMState()->read( sofa::core::vec_id::read_access::freePosition ) ; } else /// rest position { - x=this->m_mstate->read( sofa::core::vec_id::read_access::restPosition ) ; + x=this->getMState()->read( sofa::core::vec_id::read_access::restPosition ) ; computeVel = false; } diff --git a/src/BeamAdapter/component/WireBeamInterpolation.h b/src/BeamAdapter/component/WireBeamInterpolation.h index cfdcd3854..32f69ee1b 100644 --- a/src/BeamAdapter/component/WireBeamInterpolation.h +++ b/src/BeamAdapter/component/WireBeamInterpolation.h @@ -75,22 +75,24 @@ class WireBeamInterpolation : public BaseBeamInterpolation public: SOFA_CLASS(SOFA_TEMPLATE(WireBeamInterpolation, DataTypes) , SOFA_TEMPLATE(BaseBeamInterpolation, DataTypes) ); + + using Inherit = BaseBeamInterpolation; - typedef BaseBeamInterpolation Inherited; - - typedef typename Inherited::VecCoord VecCoord; - typedef typename Inherited::VecDeriv VecDeriv; - typedef typename Inherited::Coord Coord; - typedef typename Inherited::Deriv Deriv; + typedef typename Inherit::VecCoord VecCoord; + typedef typename Inherit::VecDeriv VecDeriv; + typedef typename Inherit::Coord Coord; + typedef typename Inherit::Deriv Deriv; - typedef typename Inherited::Real Real; + typedef typename Inherit::Real Real; - typedef typename Inherited::Transform Transform; - typedef typename Inherited::SpatialVector SpatialVector; + typedef typename Inherit::Transform Transform; + typedef typename Inherit::SpatialVector SpatialVector; - typedef typename Inherited::Vec2 Vec2; - typedef typename Inherited::Vec3 Vec3; - typedef typename Inherited::Quat Quat; + typedef typename Inherit::Vec2 Vec2; + typedef typename Inherit::Vec3 Vec3; + typedef typename Inherit::Quat Quat; + + using EdgeID = BaseMeshTopology::EdgeID; WireBeamInterpolation(WireRestShape *_restShape = nullptr); @@ -102,10 +104,10 @@ class WireBeamInterpolation : public BaseBeamInterpolation using BaseBeamInterpolation::addBeam; - void addBeam(const BaseMeshTopology::EdgeID &eID , const Real &length, const Real &x0, const Real &x1, + void addBeam(const EdgeID eID, const Real length, const Real x0, const Real x1, const Transform &DOF0_H_Node0, const Transform &DOF1_H_Node1); - void getSamplingParameters(type::vector& xP_noticeable, type::vector< int>& nbP_density) override + void getSamplingParameters(type::vector& xP_noticeable, type::vector& nbP_density) override { this->m_restShape->getSamplingParameters(xP_noticeable, nbP_density); } @@ -115,7 +117,12 @@ class WireBeamInterpolation : public BaseBeamInterpolation return this->m_restShape->getLength(); } - void getCollisionSampling(Real &dx, const Real& x_localcurv_abs) override + void getMechanicalSampling(Real &dx, const Real x_localcurv_abs) override + { + this->m_restShape->getMechanicalSampling(dx,x_localcurv_abs); + } + + void getCollisionSampling(Real &dx, const Real x_localcurv_abs) override { this->m_restShape->getCollisionSampling(dx,x_localcurv_abs); } @@ -124,12 +131,18 @@ class WireBeamInterpolation : public BaseBeamInterpolation { this->m_restShape->getNumberOfCollisionSegment(dx,numLines); } + + // this is the number of beams which can be simulated according to the rest shape (and its sections) + sofa::Size getTotalNumberOfPossibleBeams() const + { + return this->m_restShape->getTotalNumberOfBeams(); + } - virtual void getRestTransform(unsigned int edgeInList, Transform &local0_H_local1_rest); + virtual void getRestTransform(const EdgeID edgeInList, Transform &local0_H_local1_rest); - void getCurvAbsAtBeam(const unsigned int& edgeInList_input, const Real& baryCoord_input, Real& x_output) override; - void getSplineRestTransform(unsigned int edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) override; + void getCurvAbsAtBeam(const EdgeID edgeInList_input, const Real baryCoord_input, Real& x_output) override; + void getSplineRestTransform(const EdgeID edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) override; const BeamSection& getBeamSection(sofa::Index beamId) override; void getInterpolationParameters(sofa::Index beamId, Real& _L, Real& _A, Real& _Iy, Real& _Iz, Real& _Asy, Real& _Asz, Real& _J) override; @@ -141,7 +154,7 @@ class WireBeamInterpolation : public BaseBeamInterpolation void setPathToRestShape(const std::string &o){m_restShape.setPath(o);} - void getRestTransformOnX(Transform &global_H_local, const Real &x) + void getRestTransformOnX(Transform &global_H_local, const Real x) { if(this->m_restShape) { @@ -163,9 +176,9 @@ class WireBeamInterpolation : public BaseBeamInterpolation BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> m_restShape; /*! link on an external rest-shape*/ - ////////////////////////// Inherited attributes //////////////////////////// + ////////////////////////// Inherit attributes //////////////////////////// /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html - /// Bring inherited attributes and function in the current lookup context. + /// Bring Inherit attributes and function in the current lookup context. /// otherwise any access to the base::attribute would require /// the "this->" approach. using BaseBeamInterpolation::d_componentState ; @@ -176,7 +189,7 @@ class WireBeamInterpolation : public BaseBeamInterpolation template static bool canCreate(T* obj, sofa::core::objectmodel::BaseContext* context, sofa::core::objectmodel::BaseObjectDescription* arg) { - return Inherited::canCreate(obj,context,arg); + return Inherit::canCreate(obj,context,arg); } template @@ -185,7 +198,7 @@ class WireBeamInterpolation : public BaseBeamInterpolation /////////////////////////// Deprecated Methods ////////////////////////////////////////// /// For coils: a part of the coil instrument can be brokenIn2 (by default the point of release is the end of the straight length) [[deprecated("Releasing catheter or brokenIn2 mode is not anymore supported. Feature has been removed after release v23.06")]] - bool breaksInTwo(const Real& x_min_out, Real& x_break, int& numBeamsNotUnderControlled) { + bool breaksInTwo(const Real x_min_out, Real& x_break, int& numBeamsNotUnderControlled) { SOFA_UNUSED(x_min_out); SOFA_UNUSED(x_break); SOFA_UNUSED(numBeamsNotUnderControlled); diff --git a/src/BeamAdapter/component/WireBeamInterpolation.inl b/src/BeamAdapter/component/WireBeamInterpolation.inl index c57b7ee3f..74d471931 100644 --- a/src/BeamAdapter/component/WireBeamInterpolation.inl +++ b/src/BeamAdapter/component/WireBeamInterpolation.inl @@ -41,7 +41,8 @@ namespace beamadapter template WireBeamInterpolation::WireBeamInterpolation(WireRestShape *_restShape) - : m_restShape(initLink("WireRestShape", "link to the component on the scene"), _restShape) + : Inherit() + , m_restShape(initLink("WireRestShape", "link to the component on the scene"), _restShape) { @@ -51,15 +52,17 @@ WireBeamInterpolation::WireBeamInterpolation(WireRestShape template void WireBeamInterpolation::init() { + Inherit::init(); + if( m_restShape.get() == nullptr ) { msg_error() << "Missing WireRestShape. The component is thus de-activated" ; this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); return; } - + type::vector xP_noticeable; - type::vector< int> nbP_density; + type::vector nbP_density; m_restShape.get()->getSamplingParameters(xP_noticeable, nbP_density); @@ -70,7 +73,7 @@ void WireBeamInterpolation::init() template void WireBeamInterpolation::bwdInit() { - Inherited::bwdInit(); + Inherit::bwdInit(); if (this->isControlled()){ msg_info() << "external controller for this ForceField is detected" ; @@ -81,7 +84,7 @@ template template -void WireBeamInterpolation::addBeam(const BaseMeshTopology::EdgeID &eID , const Real &length, const Real &x0, const Real &x1, +void WireBeamInterpolation::addBeam(const EdgeID eID, const Real length, const Real x0, const Real x1, const Transform &DOF0_H_Node0, const Transform &DOF1_H_Node1) { auto edgeList = sofa::helper::getWriteOnlyAccessor(this->d_edgeList); @@ -104,7 +107,7 @@ void WireBeamInterpolation::addBeam(const BaseMeshTopology::EdgeID &e template -void WireBeamInterpolation::getRestTransform(unsigned int edgeInList, Transform &local0_H_local1_rest) +void WireBeamInterpolation::getRestTransform(const EdgeID edgeInList, Transform &local0_H_local1_rest) { msg_warning() << "GetRestTransform not implemented for not straightRestShape" ; @@ -114,7 +117,7 @@ void WireBeamInterpolation::getRestTransform(unsigned int edgeInList, template -void WireBeamInterpolation::getSplineRestTransform(unsigned int edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) +void WireBeamInterpolation::getSplineRestTransform(const EdgeID edgeInList, Transform &local_H_local0_rest, Transform &local_H_local1_rest) { if (this->isControlled() && this->m_restShape!=nullptr) { @@ -147,7 +150,7 @@ void WireBeamInterpolation::getSplineRestTransform(unsigned int edgeI template -void WireBeamInterpolation::getCurvAbsAtBeam(const unsigned int &edgeInList_input, const Real& baryCoord_input, Real& x_output) +void WireBeamInterpolation::getCurvAbsAtBeam(const EdgeID edgeInList_input, const Real baryCoord_input, Real& x_output) { ///TODO(dmarchal 2017-05-17): Please tell who and when it will be done. // TODO : version plus complete prenant en compte les coupures et autres particularites de ce modele ? @@ -175,7 +178,6 @@ void WireBeamInterpolation::getInterpolationParameters(sofa::Index be Real& _Asy, Real& _Asz, Real& _J) { _L = this->d_lengthList.getValue()[beamId]; - Real _rho; Real x_curv = 0; this->getAbsCurvXFromBeam(beamId, x_curv); @@ -208,11 +210,11 @@ bool WireBeamInterpolation::getApproximateCurvAbs(const Vec3& x_input Real closestDist = (x_input-globalHlocal0.getOrigin()).norm2(); Real beamBary = 0.0; bool projected = false; - unsigned int beamIndex = 0; + sofa::Index beamIndex = 0; // Just look for the closest point on the curve // Returns false if this point is not a projection on the curve - unsigned int nb = this->getNumBeams(); + const auto nb = this->getNumBeams(); for(unsigned int i=0; icomputeTransform(i, globalHlocal0, globalHlocal1, x); @@ -264,6 +266,8 @@ template template typename T::SPtr WireBeamInterpolation::create(T* tObj, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) { + SOFA_UNUSED(tObj); + WireRestShape* _restShape = nullptr; std::string _restShapePath; diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.h b/src/BeamAdapter/component/controller/InterventionalRadiologyController.h index 359f5b5f0..5ea336dfb 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.h +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.h @@ -86,7 +86,6 @@ class InterventionalRadiologyController : public sofa::component::controller::Me ////////////////////// Inherited from BaseObject /////////////////////////////////////////////// virtual void init() override ; - virtual void bwdInit() override ; //////////////////////////////////////////////////////////////////////////////////////////////// @@ -180,13 +179,16 @@ class InterventionalRadiologyController : public sofa::component::controller::Me SingleLink< InterventionalRadiologyController, FixedProjectiveConstraint, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_fixedConstraint; + SingleLink< + InterventionalRadiologyController, sofa::core::topology::BaseMeshTopology, + BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_mechanicalTopology; + DeprecatedAndRemoved m_fixedConstraint; type::vector m_sensorMotionData; unsigned int m_currentSensorData; type::vector m_nodeCurvAbs; type::vector< type::vector > m_idInstrumentCurvAbsTable; - unsigned int m_numControlledNodes; // Excluding the nodes that are "dropped" }; #if !defined(SOFA_PLUGIN_BEAMADAPTER_INTERVENTIONALRADIOCONTROLLER_CPP) diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index 7dce303b0..1165384a8 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -43,6 +43,7 @@ #include +#include namespace beamadapter @@ -70,6 +71,7 @@ InterventionalRadiologyController::InterventionalRadiologyController( , d_motionFilename(initData(&d_motionFilename, "motionFilename", "text file that includes tracked motion from optical sensor")) , d_indexFirstNode(initData(&d_indexFirstNode, (unsigned int) 0, "indexFirstNode", "first node (should be fixed with restshape)")) , l_fixedConstraint(initLink("fixedConstraint", "Path to the FixedProjectiveConstraint")) +, l_mechanicalTopology(initLink("topology", "Path to the mechanical topology")) { m_sensored =false; } @@ -80,6 +82,23 @@ void InterventionalRadiologyController::init() { BaseContext* context = getContext(); this->mState = nullptr; + + if(!l_mechanicalTopology) + { + msg_info() << "topology (path to the mechanical topology) has not been set, searching for one in the context."; + l_mechanicalTopology.set(this->getContext()->getMeshTopologyLink()); + } + + if (l_mechanicalTopology) + { + msg_info() << "Found topology named "<< l_mechanicalTopology->getName() ; + } + else + { + msg_error() << "Cannot find topology container. Please specify the link to the topology or insert one in the same node."; + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } //get the pointers of the WireBeamInterpolations const type::vector& instrumentPathList = d_instrumentsPath.getValue(); @@ -100,7 +119,8 @@ void InterventionalRadiologyController::init() } } - if (m_instrumentsList.empty()) { + if (m_instrumentsList.empty()) + { msg_error() << "No instrument found (no WireBeamInterpolation)! the component can not work and will be set to Invalid."; sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); return; @@ -109,6 +129,7 @@ void InterventionalRadiologyController::init() { msg_info() << m_instrumentsList.size() << " instrument(s) found (WireBeamInterpolation)"; } + const auto numberOfInstruments = m_instrumentsList.size(); m_activatedPointsBuf.clear(); @@ -124,15 +145,44 @@ void InterventionalRadiologyController::init() loadMotionData(d_motionFilename.getValue()); } - auto x_instr_tip = sofa::helper::getWriteOnlyAccessor(d_xTip); - x_instr_tip.resize(m_instrumentsList.size()); - - auto angle_Instrument = sofa::helper::getWriteOnlyAccessor(d_rotationInstrument); - angle_Instrument.resize(m_instrumentsList.size()); - - for(unsigned int i=0; isetControlled(true); + auto checkData = [&](auto& data) + { + if(data.isSet()) + { + const auto& values = data.getValue(); + const auto valueSize = values.size(); + + if(valueSize != numberOfInstruments) + { + msg_warning() << "Discrepancy for " << data.getName() << " value: it manages " << valueSize << " tools, but there are " << numberOfInstruments << " defined in instrumentPathList."; + if(valueSize > numberOfInstruments) + { + msg_warning() << "The superfluous values will be ignored."; + } + else + { + msg_warning() << "The missing values will be set as zero."; + } + } + } + }; + + { + checkData(d_xTip); + auto xTip = sofa::helper::getWriteOnlyAccessor(d_xTip); + xTip.resize(numberOfInstruments); + } + { + checkData(d_rotationInstrument); + auto angle_Instrument = sofa::helper::getWriteOnlyAccessor(d_rotationInstrument); + angle_Instrument.resize(numberOfInstruments); + } + for(auto* instrument : m_instrumentsList) + { + instrument->setControlled(true); + } + if (!l_fixedConstraint) { typename FixedProjectiveConstraint::SPtr fixedConstraint{}; @@ -157,15 +207,86 @@ void InterventionalRadiologyController::init() m_nodeCurvAbs.clear(); m_idInstrumentCurvAbsTable.clear(); + + // initiliaze current curvAbs + // if there are to be deployed at start (xtip set) then it should reflect that. + // it should always start with zero (origin) m_nodeCurvAbs.push_back(0.0); - type::vector listInit; + const auto& xTips = d_xTip.getValue(); + + //for(const auto xTip : xTips | std::views::reverse) + for(std::size_t i = xTips.size() ; i > 0 ; i--) + { + const auto xTip = xTips[i-1]; + if(xTip > 0.0) + { + m_nodeCurvAbs.push_back(xTip); + } + } + + // initialize curvAbsNode <-> instrument ID table + // i,e identify for each "simulated" node the corresponding tool(s) + // if xtip not initialized then only the 0 (origin) node is considered, and has all the tools + // and as before, if they are deployed at start (xtip set), then we need to take it into account + for(sofa::Index i = 0 ; i < m_nodeCurvAbs.size(); i++) + { + type::vector listTool; + for(sofa::Index id = 0 ; id < numberOfInstruments ; id++) + { + const auto xTip = xTips[id]; + + if(xTip >= m_nodeCurvAbs[i]) + { + listTool.push_back(id); + } + } + + m_idInstrumentCurvAbsTable.push_back(listTool); + } - for(unsigned int i=0; i listInit; + for(unsigned int i=0; imState) + { + msg_error() << "No MechanicalState found. The component can not work and will be set to Invalid."; + sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; + } + + // check if the provided mechanical state and topology can manage our tools + sofa::Size requiredNumberOfEdges = 0; + for (const auto* instrument : m_instrumentsList) + { + requiredNumberOfEdges += instrument->getTotalNumberOfPossibleBeams(); + } + + if(requiredNumberOfEdges + 1 > this->mState->getSize()) + { + msg_warning() << "The associated Mechanical Object does not contain enough nodes (" << this->mState->getSize() + << ") whereas the provided tool(s) need(s) at least " << requiredNumberOfEdges + 1 << " nodes."; + } + if(requiredNumberOfEdges > this->l_mechanicalTopology->getNbEdges()) + { + msg_warning() << "The associated Topology does not contain enough edges (" << this->l_mechanicalTopology->getNbEdges() + << ") whereas the provided tool(s) need(s) at least " << requiredNumberOfEdges << " edges."; + } + + WriteAccessor > x = *this->mState->write(sofa::core::vec_id::write_access::position); + for(unsigned int i=0; i::loadMotionData(std::string fi file.close(); } - -template -void InterventionalRadiologyController::bwdInit() -{ - // assign the starting pos to each point of the Mechanical State - Coord stPos =d_startingPos.getValue(); - stPos.getOrientation().normalize(); - d_startingPos.setValue(stPos); - - if (!this->mState) { - msg_error() << "No MechanicalState found. The component can not work and will be set to Invalid."; - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - - WriteAccessor > x = *this->mState->write(sofa::core::vec_id::write_access::position); - for(unsigned int i=0; i xP_noticeable_I; - type::vector< int > density_I; - m_instrumentsList[i]->getSamplingParameters(xP_noticeable_I, density_I); - - for (auto nb : density_I) - nbrBeam += nb; - } - - if (nbrBeam > m_numControlledNodes) - { - msg_warning() << "Parameter missmatch: According to the list of controlled instrument controlled. The number of potential beams: " - << nbrBeam << " exceed the number of degree of freedom in the MechanicalObject: " << m_numControlledNodes << ". This could lead to unespected behavior."; - } - - applyInterventionalRadiologyController(); - - sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); -} - - /*! * \todo fix the mouse event with better controls */ @@ -467,7 +545,7 @@ void InterventionalRadiologyController::computeInstrumentsCurvAbs(typ for (sofa::Size i=0; i xP_noticeable_I; - type::vector< int > density_I; + type::vector density_I; m_instrumentsList[i]->getSamplingParameters(xP_noticeable_I, density_I); // sampling of the different section of this instrument // check each interval of noticeable point to see if they go out (>0) and use corresponding density to sample the interval. @@ -574,7 +652,7 @@ void InterventionalRadiologyController::interventionalRadiologyCollis for (int i = static_cast(xPointList.size()) - 1; i>=0; i--) { //1. we determin if the poin ument - int instrumentId = idInstrumentList[i]; + const int instrumentId = idInstrumentList[i]; // x_max for the instrument that is controlled (not dropped part) Real xMaxControlled = m_instrumentsList[instrumentId]->getRestTotalLength(); @@ -592,9 +670,9 @@ void InterventionalRadiologyController::interventionalRadiologyCollis xPointList[i] = xAbsCurv - xBegin; // provides the "local" curv absc of the point (on the instrument reference) idInstrumentList[i] = firstInstruOnx; - // 3. we look for the collision sampling of the current instrument in order to "place" the following point + // 3. we look for the mechanical sampling of the current instrument in order to "place" the following point Real xIncr; - m_instrumentsList[firstInstruOnx]->getCollisionSampling(xIncr, xPointList[i]); + m_instrumentsList[firstInstruOnx]->getMechanicalSampling(xIncr, xPointList[i]); xAbsCurv -= xIncr; // the following point could not have x_abs_curv<0; @@ -633,7 +711,7 @@ void InterventionalRadiologyController::interventionalRadiologyCollis for (unsigned int i=0; igetRestTotalLength(); @@ -716,7 +794,7 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC // Create vectors with the CurvAbs of the noticiable points and the id of the corresponding instrument type::vector newCurvAbs; - type::vector> idInstrumentTable; + type::vector> idInstrumentTable; // i.e for each node -> [instID0, instID1...] // ## STEP 1: Find the total length of the COMBINED INSTRUMENTS and the one for which xtip > 0 (so the one which are simulated) helper::AdvancedTimer::stepBegin("step1"); @@ -774,23 +852,25 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC // => Get write access to current nodes/dofs Data* datax = this->getMechanicalState()->write(sofa::core::vec_id::write_access::position); auto x = sofa::helper::getWriteOnlyAccessor(*datax); - VecCoord xbuf = x.ref(); + const VecCoord xbuf = x.ref(); // make a copy of the positions, as it will changed meanwhile + + const sofa::Size numberOfNodes = x.size(); + sofa::Size numberOfSimulatedNodes = newCurvAbs.size(); // number of simulated nodes - sofa::Size nbrCurvAbs = newCurvAbs.size(); // number of simulated nodes - if (nbrCurvAbs > x.size()) + if (numberOfSimulatedNodes > numberOfNodes) { - msg_warning() << "Parameters missmatch. There are more curv abscisses '" << nbrCurvAbs << "' than the number of dof: " << x.size(); - nbrCurvAbs = x.size(); + msg_warning() << "Parameters missmatch. There are more curv abscisses '" << numberOfSimulatedNodes << "' than the number of dof: " << numberOfNodes; + numberOfSimulatedNodes = numberOfNodes; } - const sofa::Size prev_nbrCurvAbs = m_nodeCurvAbs.size(); // previous number of simulated nodes; + const sofa::Size prev_numberOfSimulatedNodes = m_nodeCurvAbs.size(); // previous number of simulated nodes; - const sofa::Size nbrUnactiveNode = (m_numControlledNodes > nbrCurvAbs) ? m_numControlledNodes - nbrCurvAbs : 0; // m_numControlledNodes == nbr Dof | nbr of CurvAbs > 0 - const sofa::Size prev_nbrUnactiveNode = (m_numControlledNodes > prev_nbrCurvAbs) ? m_numControlledNodes - prev_nbrCurvAbs : 0; + const sofa::Size numberOfUnactiveNodes = numberOfNodes - numberOfSimulatedNodes; // m_numControlledNodes == nbr Dof | nbr of CurvAbs > 0 + const sofa::Size prev_numberOfUnactiveNodes = numberOfNodes - prev_numberOfSimulatedNodes; - for (sofa::Index xId = 0; xId < nbrCurvAbs; xId++) + for (sofa::Index xId = 0; xId < numberOfSimulatedNodes; xId++) { - const sofa::Index globalNodeId = nbrUnactiveNode + xId; // position of the curvAbs in the dof buffer filled by the end + const sofa::Index globalNodeId = numberOfUnactiveNodes + xId; // position of the curvAbs in the dof buffer filled by the end const Real xCurvAbs = modifiedCurvAbs[xId]; if ((xCurvAbs - std::numeric_limits::epsilon()) > m_nodeCurvAbs.back() + threshold) @@ -811,7 +891,7 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC break; } - sofa::Index prev_globalNodeId = prev_nbrUnactiveNode + prev_xId; + sofa::Index prev_globalNodeId = prev_numberOfUnactiveNodes + prev_xId; const Real prev_xCurvAbs = m_nodeCurvAbs[prev_xId]; if (fabs(prev_xCurvAbs - xCurvAbs) < threshold) @@ -856,36 +936,37 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC // ## STEP 4: Assign the beams helper::AdvancedTimer::stepBegin("step4"); - sofa::Size nbrBeam = newCurvAbs.size() - 1; // number of simulated beams - const sofa::Size numEdges = m_numControlledNodes - 1; + sofa::Size numberOfBeams = newCurvAbs.size() - 1; // number of simulated beams + const sofa::Size numberOfEdges = x.size() - 1; - if (numEdges < nbrBeam) // verify that there is a sufficient number of Edge in the topology : TODO if not, modify topo ! + if (numberOfEdges < numberOfBeams) // verify that there is a sufficient number of Edge in the topology : TODO if not, modify topo ! { - msg_error() << "Not enough edges in the topology. Only: " << numEdges << " while nbrBeam = " << nbrBeam << ". Will simulate only " << numEdges << " beams."; - nbrBeam = numEdges; + msg_error() << "Not enough edges in the topology. Only: " << numberOfEdges << " while nbrBeam = " << numberOfBeams << ". Will simulate only " << numberOfEdges << " beams."; + + numberOfBeams = numberOfEdges; } const type::vector& rotInstruments = d_rotationInstrument.getValue(); - for (unsigned int b=0; b< nbrBeam; b++) + for (unsigned int b=0; b< numberOfBeams; b++) { - const Real& x0 = newCurvAbs[b]; - const Real& x1 = newCurvAbs[b+1]; + const Real x0 = newCurvAbs[b]; + const Real x1 = newCurvAbs[b+1]; for (unsigned int i=0; i(xmin- threshold) && x0<(xmax+ threshold) && x1>(xmin- threshold) && x1<(xmax+ threshold)) { - BaseMeshTopology::EdgeID eID = (BaseMeshTopology::EdgeID)(numEdges - nbrBeam + b); + const auto eID = static_cast(numberOfEdges - numberOfBeams + b); - Real length = x1 - x0; - Real x0_local = x0-xmin; - Real x1_local = x1-xmin; + const Real length = x1 - x0; + const Real x0_local = x0-xmin; + const Real x1_local = x1-xmin; - Real theta = rotInstruments[i]; + const Real theta = rotInstruments[i]; m_instrumentsList[i]->addBeam(eID, length, x0_local, x1_local,theta ); } @@ -896,7 +977,7 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC // ## STEP 5: Fix the not simulated nodes helper::AdvancedTimer::stepBegin("step5"); - unsigned int firstSimulatedNode = m_numControlledNodes - nbrBeam; + unsigned int firstSimulatedNode = numberOfNodes - numberOfBeams; // => 1. Fix the nodes (beginning of the instruments) that are not "out" fixFirstNodesWithUntil(firstSimulatedNode); @@ -1017,7 +1098,7 @@ void InterventionalRadiologyController::fillInstrumentCurvAbsTable(co xBegin -= threshold; xEnd += threshold; - // check curvAbs sorted value, if value is inside [xBegin, xBegin] of the tool add it to instrumentList. + // check curvAbs sorted value, if value is inside [xBegin, xEnd] of the tool add it to instrumentList. for (unsigned int i = 0; i < curvAbs.size(); i++) { if (curvAbs[i] < xBegin) // still not inside range @@ -1071,7 +1152,7 @@ const type::vector< type::vector >& InterventionalRadiologyController int InterventionalRadiologyController::getTotalNbEdges() const { - return getContext()->getMeshTopology()->getNbEdges(); + return l_mechanicalTopology->getNbEdges(); } diff --git a/src/BeamAdapter/component/controller/SutureController.inl b/src/BeamAdapter/component/controller/SutureController.inl index 85bbedfb9..cb0aad4de 100644 --- a/src/BeamAdapter/component/controller/SutureController.inl +++ b/src/BeamAdapter/component/controller/SutureController.inl @@ -121,7 +121,7 @@ void SutureController::initWireModel() } type::vector< Real > xP_noticeable; - type::vector< int > nbP_density; + type::vector nbP_density; l_adaptiveInterpolation->getSamplingParameters(xP_noticeable, nbP_density); // computation of the number of node on the structure: @@ -913,7 +913,7 @@ template void SutureController::computeSampling(type::vector &newCurvAbs, VecCoord &x) { type::vector xP_noticeable; - type::vector nbP_density; + type::vector nbP_density; l_adaptiveInterpolation->getSamplingParameters(xP_noticeable, nbP_density); diff --git a/src/BeamAdapter/component/engine/WireRestShape.h b/src/BeamAdapter/component/engine/WireRestShape.h index 8f56a7267..f866a226e 100644 --- a/src/BeamAdapter/component/engine/WireRestShape.h +++ b/src/BeamAdapter/component/engine/WireRestShape.h @@ -83,23 +83,23 @@ class WireRestShape : public core::objectmodel::BaseObject /////////////////////////// Methods of WireRestShape ////////////////////////////////////////// /// This function is called by the force field to evaluate the rest position of each beam - void getRestTransformOnX(Transform &global_H_local, const Real &x); + void getRestTransformOnX(Transform &global_H_local, const Real x); /// Returns the BeamSection @sa m_beamSection corresponding to the given curvilinear abscissa, will call @sa BaseRodSectionMaterial::getBeamSection - [[nodiscard]] const BeamSection& getBeamSectionAtX(const Real& x_curv) const; + [[nodiscard]] const BeamSection& getBeamSectionAtX(const Real x_curv) const; /// Returns the BeamSection data depending on the beam position at the given curvilinear abscissa, will call @sa BaseRodSectionMaterial::getInterpolationParameters - void getInterpolationParametersAtX(const Real& x_curv, Real &_A, Real &_Iy , Real &_Iz, Real &_Asy, Real &_Asz, Real &_J) const; + void getInterpolationParametersAtX(const Real x_curv, Real &_A, Real &_Iy , Real &_Iz, Real &_Asy, Real &_Asz, Real &_J) const; /// Returns the Young modulus, Poisson's ratio and massDensity coefficient of the section at the given curvilinear abscissa, will call @sa BaseRodSectionMaterial::getMechanicalParameters - void getMechanicalParametersAtX(const Real& x_curv, Real& youngModulus, Real& cPoisson, Real& massDensity) const; + void getMechanicalParametersAtX(const Real x_curv, Real& youngModulus, Real& cPoisson, Real& massDensity) const; /** * This function provides a type::vector with the curviliar abscissa of the noticeable point(s) * and the minimum density (number of points) between them. (Nb. nbP_density.size() == xP_noticeable.size() - 1) */ - void getSamplingParameters(type::vector& xP_noticeable, type::vector& nbP_density) const ; + void getSamplingParameters(type::vector& xP_noticeable, type::vector& nbP_density) const ; /// Functions enabling to load and use a geometry given from OBJ external file @@ -107,9 +107,11 @@ class WireRestShape : public core::objectmodel::BaseObject Real getLength() ; - void getCollisionSampling(Real &dx, const Real &x_curv); - void getNumberOfCollisionSegment(Real &dx, unsigned int &numLines) ; - + + void getMechanicalSampling(Real& dx, const Real x_localcurv_abs); + void getCollisionSampling(Real &dx, const Real x_curv); + void getNumberOfCollisionSegment(Real &dx, sofa::Size& numLines); + sofa::Size getTotalNumberOfBeams() const; /////////////////////////// Deprecated Methods ////////////////////////////////////////// @@ -134,7 +136,7 @@ class WireRestShape : public core::objectmodel::BaseObject public: - Data > d_density; + Data > d_density; Data > d_keyPoints; /// Vector or links to the Wire section material. The order of the linked material will define the WireShape structure. @@ -142,9 +144,7 @@ class WireRestShape : public core::objectmodel::BaseObject private: /// Link to be set to the topology container in the component graph. - SingleLink, TopologyContainer, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; - /// Pointer to the topology container, should be set using @sa l_topology, otherwise will search for one in current Node. - TopologyContainer* _topology{ nullptr }; + SingleLink, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; }; diff --git a/src/BeamAdapter/component/engine/WireRestShape.inl b/src/BeamAdapter/component/engine/WireRestShape.inl index ddf411c4a..dcd08e25d 100644 --- a/src/BeamAdapter/component/engine/WireRestShape.inl +++ b/src/BeamAdapter/component/engine/WireRestShape.inl @@ -58,7 +58,8 @@ WireRestShape::WireRestShape() , l_sectionMaterials(initLink("wireMaterials", "link to Wire Section Materials (to be ordered according to the instrument, from handle to tip)")) , l_topology(initLink("topology", "link to the topology container")) { - + d_density.setReadOnly(true); // density is supposed to be filled using the section materials + d_keyPoints.setReadOnly(true); // key points are supposed to be filled using the section materials } @@ -70,16 +71,15 @@ void WireRestShape::init() ////////////////////////////////////////////// ////////// get and fill local topology /////// ////////////////////////////////////////////// - - // Get pointer to given topology using the link. If not found will search in current context. - _topology = l_topology.get(); - if (!_topology) - this->getContext()->get(_topology); + if (!l_topology) + { + l_topology.set(this->getContext()->getMeshTopologyLink()); + } - if(_topology != nullptr) + if (l_topology) { - msg_info() << "found topology named "<< _topology->getName() ; + msg_info() << "Found topology named "<< l_topology->getName() ; } else { @@ -94,6 +94,16 @@ void WireRestShape::init() this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); return; } + + // Workaround around the fact that d_density and d_keyPoints are supposed to be output only + if(d_density.isSet()) + { + msg_warning() << "The density field will be ignored (output only Data)."; + } + if(d_keyPoints.isSet()) + { + msg_warning() << "The keyPoints field will be ignored (output only Data)."; + } //////////////////////////////////////////////////////// @@ -124,7 +134,7 @@ void WireRestShape::initLengths() { auto rodSection = l_sectionMaterials.get(i); keyPointList[i+1] = keyPointList[i] + rodSection->getLength(); - densityList[i] = rodSection->getNbCollisionEdges(); + densityList[i] = rodSection->getNbBeams(); } } @@ -133,8 +143,8 @@ template bool WireRestShape::initTopology() { /// fill topology : - _topology->clear(); - _topology->cleanup(); + l_topology->clear(); + l_topology->cleanup(); const type::vector& keyPts = d_keyPoints.getValue(); if (l_sectionMaterials.size() != keyPts.size() - 1) @@ -155,12 +165,12 @@ bool WireRestShape::initTopology() // add points from the material for (int i = startPtId; i < nbrVisuEdges + 1; i++) { - _topology->addPoint(prev_length + i * dx, 0, 0); + l_topology->addPoint(prev_length + i * dx, 0, 0); } // add segments from the material for (int i = prev_edges; i < prev_edges + nbrVisuEdges; i++) { - _topology->addEdge(i, i + 1); + l_topology->addEdge(i, i + 1); } prev_length = length; @@ -174,7 +184,7 @@ bool WireRestShape::initTopology() template void WireRestShape::getSamplingParameters(type::vector& xP_noticeable, - type::vector& nbP_density) const + type::vector& nbP_density) const { xP_noticeable = d_keyPoints.getValue(); nbP_density = d_density.getValue(); @@ -182,9 +192,50 @@ void WireRestShape::getSamplingParameters(type::vector& xP_noti template -void WireRestShape::getCollisionSampling(Real &dx, const Real &x_curv) +void WireRestShape::getMechanicalSampling(Real &dx, const Real x_curv) { - unsigned int numLines; + unsigned int numLines = 0; + Real x_used = x_curv - EPSILON; + + const Real totalLength = this->getLength(); + x_used = std::clamp(x_used, static_cast(0.0), totalLength); + + const type::vector& keyPts = d_keyPoints.getValue(); + + // verify that size of number of materials == size of keyPoints-1 + if (l_sectionMaterials.size() != keyPts.size() - 1) + { + msg_error() << "Problem size of number of materials: " << l_sectionMaterials.size() + << " != size of keyPoints-1 " << keyPts.size()-1 + << ". Returning default values."; + numLines = 20; + dx = totalLength / numLines; + return; + } + + // Check in which section x_used belongs to and get access to this section material + for (sofa::Size i = 1; i< keyPts.size(); ++i) + { + if (x_used <= keyPts[i]) + { + numLines = l_sectionMaterials.get(i-1)->getNbBeams(); + + Real length = fabs(keyPts[i] - keyPts[i-1]); + dx = length / numLines; + return; + } + } + + // If x_used is out of bounds. Warn user and returns default value. + numLines = 20; + dx = totalLength / numLines; + msg_error() << " problem in getMechanicalSampling : x_curv " << x_used << " is not between keyPoints" << d_keyPoints.getValue(); +} + +template +void WireRestShape::getCollisionSampling(Real &dx, const Real x_curv) +{ + unsigned int numLines = 0; Real x_used = x_curv - EPSILON; const Real totalLength = this->getLength(); @@ -227,9 +278,31 @@ void WireRestShape::getCollisionSampling(Real &dx, const Real &x_curv msg_error() << " problem in getCollisionSampling : x_curv " << x_used << " is not between keyPoints" << d_keyPoints.getValue(); } +template +void WireRestShape::getNumberOfCollisionSegment(Real &dx, sofa::Size& numLines) +{ + numLines = 0; + for (sofa::Size i = 0; i < l_sectionMaterials.size(); ++i) + { + numLines += l_sectionMaterials.get(i)->getNbCollisionEdges(); + } + dx = getLength() / numLines; +} + +template +sofa::Size WireRestShape::getTotalNumberOfBeams() const +{ + sofa::Size numBeams = 0; + for (sofa::Size i = 0; i < l_sectionMaterials.size(); ++i) + { + numBeams += l_sectionMaterials.get(i)->getNbBeams(); + } + + return numBeams; +} template -void WireRestShape::getRestTransformOnX(Transform &global_H_local, const Real &x) +void WireRestShape::getRestTransformOnX(Transform &global_H_local, const Real x) { Real x_used = x - EPSILON; @@ -255,7 +328,7 @@ void WireRestShape::getRestTransformOnX(Transform &global_H_local, co template -const BeamSection& WireRestShape::getBeamSectionAtX(const Real& x_curv) const +const BeamSection& WireRestShape::getBeamSectionAtX(const Real x_curv) const { const Real x_used = x_curv - Real(EPSILON); const type::vector& keyPts = d_keyPoints.getValue(); @@ -276,7 +349,7 @@ const BeamSection& WireRestShape::getBeamSectionAtX(const Real& x_cur template -void WireRestShape::getInterpolationParametersAtX(const Real& x_curv, Real &_A, Real &_Iy , Real &_Iz, Real &_Asy, Real &_Asz, Real &_J) const +void WireRestShape::getInterpolationParametersAtX(const Real x_curv, Real &_A, Real &_Iy , Real &_Iz, Real &_Asy, Real &_Asz, Real &_J) const { const Real x_used = x_curv - Real(EPSILON); const type::vector& keyPts = d_keyPoints.getValue(); @@ -295,13 +368,13 @@ void WireRestShape::getInterpolationParametersAtX(const Real& x_curv, template -void WireRestShape::getMechanicalParametersAtX(const Real& x_curv, Real& youngModulus, Real& cPoisson, Real& massDensity) const +void WireRestShape::getMechanicalParametersAtX(const Real x_curv, Real& youngModulus, Real& cPoisson, Real& massDensity) const { const Real x_used = x_curv - Real(EPSILON); const type::vector& keyPts = d_keyPoints.getValue(); // Check in which section x_used belongs to and get access to this section material - for (auto i = 1; i < keyPts.size(); ++i) + for (std::size_t i = 1; i < keyPts.size(); ++i) { if (x_used <= keyPts[i]) { @@ -319,19 +392,6 @@ typename WireRestShape::Real WireRestShape::getLength() return d_keyPoints.getValue().back(); } - -template -void WireRestShape::getNumberOfCollisionSegment(Real &dx, unsigned int &numLines) -{ - numLines = 0; - for (sofa::Size i = 0; i < l_sectionMaterials.size(); ++i) - { - numLines += l_sectionMaterials.get(i)->getNbCollisionEdges(); - } - dx = getLength() / numLines; -} - - template void WireRestShape::computeOrientation(const Vec3& AB, const Quat& Q, Quat &result) { diff --git a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h index a46193c4e..2bea4791e 100644 --- a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h +++ b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.h @@ -32,10 +32,12 @@ // #pragma once -//////////////////////// Inclusion of headers...from wider to narrower/closer ////////////////////// +#include + +#include +#include #include #include -#include #include #include @@ -43,10 +45,6 @@ namespace beamadapter { -using sofa::core::behavior::MultiMatrixAccessor; -using sofa::core::visual::VisualParams; -using sofa::core::MechanicalParams; - /*! * \class AdaptiveBeamForceFieldAndMass * @brief AdaptiveBeamForceFieldAndMass Class @@ -77,8 +75,8 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass using Vec6NoInit = sofa::type::VecNoInit<6, Real>; using Matrix6x6 = sofa::type::Mat<6, 6, Real>; using Matrix6x6NoInit = sofa::type::MatNoInit<6, 6, Real>; - using Transform = typename sofa::defaulttype::SolidTypes::Transform; - using SpatialVector = typename sofa::defaulttype::SolidTypes::SpatialVector; + using Transform = sofa::type::Transform; + using SpatialVector = sofa::type::SpatialVector; using BInterpolation = BaseBeamInterpolation; using core::behavior::Mass::mstate; @@ -89,8 +87,8 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass * \class BeamLocalMatrices * @brief BeamLocalMatrices Class */ - class BeamLocalMatrices{ - + class BeamLocalMatrices + { public: BeamLocalMatrices() = default; @@ -114,29 +112,29 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass ///////////////////////////////////// void init() override ; void reinit() override ; - void draw(const VisualParams* vparams) override ; + void draw(const sofa::core::visual::VisualParams* vparams) override ; ///////////////////////////////////// /// Mass Interface ///////////////////////////////////// - void addMDx(const MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor) override; - void addMToMatrix(const MechanicalParams *mparams, const MultiMatrixAccessor* matrix) override; - void addMBKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix) override; + void addMDx(const sofa::core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecDeriv& dx, SReal factor) override; + void addMToMatrix(const sofa::core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addMBKToMatrix(const sofa::core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; void buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) override; void buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) override; void buildDampingMatrix(core::behavior::DampingMatrix* matrices) override; //TODO(dmarchal 2017-05-17) So what do we do ? For who is this message intended for ? How can we make this code "more" manageable. - void accFromF(const MechanicalParams* mparams, DataVecDeriv& , const DataVecDeriv& ) override + void accFromF(const sofa::core::MechanicalParams* mparams, DataVecDeriv& , const DataVecDeriv& ) override { SOFA_UNUSED(mparams); msg_error()<<"accFromF can not be implemented easily: It necessitates a solver because M^-1 is not available"; } //TODO(dmarchal 2017-05-17) So what do we do ? For who is this message intended for ? How can we make this code "more" manageable. - SReal getKineticEnergy(const MechanicalParams* mparams, const DataVecDeriv& ) const override ///< vMv/2 using dof->getV() + SReal getKineticEnergy(const sofa::core::MechanicalParams* mparams, const DataVecDeriv& ) const override ///< vMv/2 using dof->getV() { SOFA_UNUSED(mparams); msg_error() << "getKineticEnergy not yet implemented"; @@ -144,7 +142,7 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass } //TODO(dmarchal 2017-05-17) So what do we do ? For who is this message intended for ? How can we make this code "more" manageable. - void addGravityToV(const MechanicalParams* mparams, DataVecDeriv& ) override + void addGravityToV(const sofa::core::MechanicalParams* mparams, DataVecDeriv& ) override { SOFA_UNUSED(mparams); msg_error() << "addGravityToV not implemented yet"; @@ -155,12 +153,12 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass ///////////////////////////////////// /// ForceField Interface ///////////////////////////////////// - void addForce(const MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; + void addForce(const sofa::core::MechanicalParams* mparams, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v) override; - void addDForce(const MechanicalParams* mparams, DataVecDeriv& datadF , const DataVecDeriv& datadX ) override; + void addDForce(const sofa::core::MechanicalParams* mparams, DataVecDeriv& datadF , const DataVecDeriv& datadX ) override; //TODO(dmarchal 2017-05-17) So what do we do ? For who is this message intended for ? How can we make this code "more" manageable. - SReal getPotentialEnergy(const MechanicalParams* mparams, const DataVecCoord& )const override + SReal getPotentialEnergy(const sofa::core::MechanicalParams* mparams, const DataVecCoord& )const override { SOFA_UNUSED(mparams); msg_error()<<"getPotentialEnergy not yet implemented"; @@ -168,11 +166,11 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass } using sofa::core::behavior::ForceField::addKToMatrix; - void addKToMatrix(const MechanicalParams* mparams, - const MultiMatrixAccessor* matrix) override; + void addKToMatrix(const sofa::core::MechanicalParams* mparams, + const sofa::core::behavior::MultiMatrixAccessor* matrix) override; - void computeStiffness(int beam, BeamLocalMatrices& beamLocalMatrices); - void computeMass(int beam, BeamLocalMatrices& beamMatrices); + void computeStiffness(const sofa::Index beamID, BeamLocalMatrices& beamLocalMatrices); + void computeMass(const sofa::Index beamID, BeamLocalMatrices& beamMatrices); Data d_computeMass; ///< if false, only compute the stiff elastic model Real m_defaultMassDensity; @@ -183,16 +181,16 @@ class AdaptiveBeamForceFieldAndMass : public core::behavior::Mass protected : SingleLink, BInterpolation, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_interpolation; - void applyMassLarge( VecDeriv& df, int bIndex, Index nd0Id, Index nd1Id, SReal factor); - void applyStiffnessLarge( VecDeriv& df, const VecDeriv& dx, int beam, Index nd0Id, Index nd1Id, SReal factor ); + void applyMassLarge( VecDeriv& df, const sofa::Index beamID, const sofa::Index nd0Id, const sofa::Index nd1Id, const SReal factor); + void applyStiffnessLarge( VecDeriv& df, const VecDeriv& dx, const sofa::Index beamID, const sofa::Index nd0Id, const sofa::Index nd1Id, const SReal factor ); void computeGravityVector(); private: type::vector m_localBeamMatrices; Vec6 m_gravity; - void drawElement(const VisualParams* vparams, int beam, - Transform &global_H0_local, Transform &global_H1_local) ; + void drawElement(const sofa::core::visual::VisualParams* vparams, const sofa::Index beamID, + const Transform &global_H0_local, const Transform &global_H1_local) ; }; /// Instantiate the templates so that they are not instiated in each translation unit (see ) diff --git a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl index c2286d8da..509600e71 100644 --- a/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl +++ b/src/BeamAdapter/component/forcefield/AdaptiveBeamForceFieldAndMass.inl @@ -60,7 +60,7 @@ AdaptiveBeamForceFieldAndMass::AdaptiveBeamForceFieldAndMass() , m_defaultMassDensity(Real(1.)) , d_massDensity(initData(&d_massDensity,type::vector(1, m_defaultMassDensity),"massDensity", "Density of the mass" )) , d_useShearStressComputation(initData(&d_useShearStressComputation, true, "shearStressComputation","if false, suppress the shear stress in the computation")) - , d_reinforceLength(initData(&d_reinforceLength, false, "reinforceLength", "if true, a separate computation for the error in elongation is peformed")) + , d_reinforceLength(initData(&d_reinforceLength, false, "reinforceLength", "if true, a separate computation for the error in elongation is performed")) , l_interpolation(initLink("interpolation","Path to the Interpolation component on scene")) { @@ -74,16 +74,16 @@ void AdaptiveBeamForceFieldAndMass::init() if(!l_interpolation) l_interpolation.set(dynamic_cast(this->getContext())->get(BaseContext::Local)); - if (!l_interpolation) { + if (!l_interpolation) + { msg_error() << "No Beam Interpolation found, the component can not work."; this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); } else { - BaseContext* context = this->getContext(); - core::topology::BaseMeshTopology* topology = context->getMeshTopology(); + core::topology::BaseMeshTopology* topology = l_interpolation->l_topology.get(); auto massDensity = sofa::helper::getWriteOnlyAccessor(d_massDensity); - if (topology) + if(topology) { const auto& nbEdges = topology->getNbEdges(); if (massDensity.size() != nbEdges) @@ -123,17 +123,19 @@ void AdaptiveBeamForceFieldAndMass::computeGravityVector() template -void AdaptiveBeamForceFieldAndMass::computeStiffness(int beamId, BeamLocalMatrices& beamLocalMatrices) +void AdaptiveBeamForceFieldAndMass::computeStiffness(const sofa::Index beamId, BeamLocalMatrices& beamLocalMatrices) { + SOFA_UNUSED(beamId); + /// material parameters - Real _G = beamLocalMatrices._youngM / (2.0 * (1.0 + beamLocalMatrices._cPoisson)); + const Real _G = beamLocalMatrices._youngM / (2.0 * (1.0 + beamLocalMatrices._cPoisson)); - Real phiy, phiz; - Real L2 = (Real) (beamLocalMatrices._L * beamLocalMatrices._L); - Real L3 = (Real) (L2 * beamLocalMatrices._L); - Real EIy = (Real)(beamLocalMatrices._youngM * beamLocalMatrices._Iy); - Real EIz = (Real)(beamLocalMatrices._youngM * beamLocalMatrices._Iz); + const Real L2 = (Real) (beamLocalMatrices._L * beamLocalMatrices._L); + const Real L3 = (Real) (L2 * beamLocalMatrices._L); + const Real EIy = (Real)(beamLocalMatrices._youngM * beamLocalMatrices._Iy); + const Real EIz = (Real)(beamLocalMatrices._youngM * beamLocalMatrices._Iz); + Real phiy{0.0}, phiz{0.0}; /// Find shear-deformation parameters if (beamLocalMatrices._Asy == 0) phiy = 0.0; @@ -156,8 +158,8 @@ void AdaptiveBeamForceFieldAndMass::computeStiffness(int beamId, Beam beamLocalMatrices.m_K00[5][5] = beamLocalMatrices.m_K11[5][5] = (beamLocalMatrices._L == 0.0)? 0.0 : (Real)((4.0+phiy)*EIz/(beamLocalMatrices._L*(1.0+phiy))); /// diagonal blocks - beamLocalMatrices.m_K00[4][2] =(L2 == 0.0)? 0.0 : (Real)(-6.0*EIy/(L2*(1.0+phiz))); - beamLocalMatrices.m_K00[5][1] =(L2 == 0.0)? 0.0 : (Real)( 6.0*EIz/(L2*(1.0+phiy))); + beamLocalMatrices.m_K00[4][2] = (L2 == 0.0)? 0.0 : (Real)(-6.0*EIy/(L2*(1.0+phiz))); + beamLocalMatrices.m_K00[5][1] = (L2 == 0.0)? 0.0 : (Real)( 6.0*EIz/(L2*(1.0+phiy))); beamLocalMatrices.m_K11[5][1] = -beamLocalMatrices.m_K00[5][1]; beamLocalMatrices.m_K11[4][2] = -beamLocalMatrices.m_K00[4][2]; @@ -168,10 +170,10 @@ void AdaptiveBeamForceFieldAndMass::computeStiffness(int beamId, Beam beamLocalMatrices.m_K10[2][2] = -beamLocalMatrices.m_K00[2][2]; beamLocalMatrices.m_K10[2][4] = -beamLocalMatrices.m_K00[4][2]; beamLocalMatrices.m_K10[3][3] = -beamLocalMatrices.m_K00[3][3]; - beamLocalMatrices.m_K10[4][2] = beamLocalMatrices.m_K00[4][2]; - beamLocalMatrices.m_K10[4][4] =(beamLocalMatrices._L == 0.0)? 0.0 : (Real)((2.0-phiz)*EIy/(beamLocalMatrices._L*(1.0+phiz))); - beamLocalMatrices.m_K10[5][1] = beamLocalMatrices.m_K00[5][1]; - beamLocalMatrices.m_K10[5][5] =(beamLocalMatrices._L == 0.0)? 0.0 : (Real)((2.0-phiy)*EIz/(beamLocalMatrices._L*(1.0+phiy))); + beamLocalMatrices.m_K10[4][2] = beamLocalMatrices.m_K00[4][2]; + beamLocalMatrices.m_K10[4][4] = (beamLocalMatrices._L == 0.0)? 0.0 : (Real)((2.0-phiz)*EIy/(beamLocalMatrices._L*(1.0+phiz))); + beamLocalMatrices.m_K10[5][1] = beamLocalMatrices.m_K00[5][1]; + beamLocalMatrices.m_K10[5][5] = (beamLocalMatrices._L == 0.0)? 0.0 : (Real)((2.0-phiy)*EIz/(beamLocalMatrices._L*(1.0+phiy))); /// Make a symetric matrix with diagonal blocks for (int i=0; i<=5; i++) @@ -189,15 +191,15 @@ void AdaptiveBeamForceFieldAndMass::computeStiffness(int beamId, Beam template -void AdaptiveBeamForceFieldAndMass::computeMass(int beamId, BeamLocalMatrices& beamLocalMatrix) +void AdaptiveBeamForceFieldAndMass::computeMass(const sofa::Index beamID, BeamLocalMatrices& beamLocalMatrix) { - SOFA_UNUSED(beamId); - Real L2 = (Real) (beamLocalMatrix._L * beamLocalMatrix._L); + SOFA_UNUSED(beamID); + const Real L2 = beamLocalMatrix._L * beamLocalMatrix._L; beamLocalMatrix.m_M00.clear(); beamLocalMatrix.m_M01.clear(); beamLocalMatrix.m_M10.clear(); beamLocalMatrix.m_M11.clear(); - Real AL = beamLocalMatrix._A * beamLocalMatrix._L; - Real Iz_A = (beamLocalMatrix._A == 0.0) ? 0.0 : beamLocalMatrix._Iz / beamLocalMatrix._A; - Real Iy_A = (beamLocalMatrix._A == 0.0) ? 0.0 : beamLocalMatrix._Iy / beamLocalMatrix._A; + const Real AL = beamLocalMatrix._A * beamLocalMatrix._L; + const Real Iz_A = (beamLocalMatrix._A == 0.0) ? 0.0 : beamLocalMatrix._Iz / beamLocalMatrix._A; + const Real Iy_A = (beamLocalMatrix._A == 0.0) ? 0.0 : beamLocalMatrix._Iy / beamLocalMatrix._A; /// diagonal values beamLocalMatrix.m_M00[0][0] = beamLocalMatrix.m_M11[0][0] = (Real)(1.0 / 3.0); @@ -232,9 +234,9 @@ void AdaptiveBeamForceFieldAndMass::computeMass(int beamId, BeamLocal beamLocalMatrix.m_M10 *= beamLocalMatrix._rho * AL; /// Make a symetric matrix with diagonal blocks - for (int i=0; i<=5; i++) + for (sofa::Index i=0; i<=5; i++) { - for (int j=i+1; j<6; j++) + for (sofa::Index j=i+1; j<6; j++) { beamLocalMatrix.m_M00[i][j] = beamLocalMatrix.m_M00[j][i]; beamLocalMatrix.m_M11[i][j] = beamLocalMatrix.m_M11[j][i]; @@ -248,14 +250,14 @@ void AdaptiveBeamForceFieldAndMass::computeMass(int beamId, BeamLocal template void AdaptiveBeamForceFieldAndMass::applyStiffnessLarge( VecDeriv& df, const VecDeriv& dx, - int bIndex, Index nd0Id, Index nd1Id, - SReal factor ) + const sofa::Index beamID, const sofa::Index nd0Id, const sofa::Index nd1Id, + const SReal factor ) { - if(nd0Id==nd1Id) /// Return in case of rigidification + if(nd0Id == nd1Id) /// Return in case of rigidification return; Vec6NoInit U0, U1, u0, u1, f0, f1, F0, F1; - BeamLocalMatrices &beamLocalMatrix = m_localBeamMatrices[bIndex]; + const BeamLocalMatrices &beamLocalMatrix = m_localBeamMatrices[beamID]; for (unsigned int i=0; i<6; i++) { @@ -285,21 +287,21 @@ void AdaptiveBeamForceFieldAndMass::applyStiffnessLarge( VecDeriv& df template -void AdaptiveBeamForceFieldAndMass::applyMassLarge( VecDeriv& df, int bIndex, Index nd0Id, Index nd1Id, SReal factor) +void AdaptiveBeamForceFieldAndMass::applyMassLarge(VecDeriv& df, const sofa::Index beamID, const sofa::Index nd0Id, const sofa::Index nd1Id, const SReal factor) { - const BeamLocalMatrices &beamLocalMatrix = m_localBeamMatrices[bIndex]; + const BeamLocalMatrices &beamLocalMatrix = m_localBeamMatrices[beamID]; /// displacement in local frame (only gravity as external force) - Vec6 a0 = beamLocalMatrix.m_A0Ref * m_gravity; - Vec6 a1 = beamLocalMatrix.m_A1Ref * m_gravity; + const Vec6 a0 = beamLocalMatrix.m_A0Ref * m_gravity; + const Vec6 a1 = beamLocalMatrix.m_A1Ref * m_gravity; /// internal force in local frame - Vec6 f0 = beamLocalMatrix.m_M00*a0 + beamLocalMatrix.m_M01*a1; - Vec6 f1 = beamLocalMatrix.m_M10*a0 + beamLocalMatrix.m_M11*a1; + const Vec6 f0 = beamLocalMatrix.m_M00*a0 + beamLocalMatrix.m_M01*a1; + const Vec6 f1 = beamLocalMatrix.m_M10*a0 + beamLocalMatrix.m_M11*a1; /// force in global frame - Vec6 F0 = beamLocalMatrix.m_A0Ref.multTranspose(f0); - Vec6 F1 = beamLocalMatrix.m_A1Ref.multTranspose(f1); + const Vec6 F0 = beamLocalMatrix.m_A0Ref.multTranspose(f0); + const Vec6 F1 = beamLocalMatrix.m_A1Ref.multTranspose(f1); /// put the result in df for (unsigned int i=0; i<6; i++) @@ -316,21 +318,21 @@ void AdaptiveBeamForceFieldAndMass::applyMassLarge( VecDeriv& df, int ///////////////////////////////////// template -void AdaptiveBeamForceFieldAndMass::addMDx(const MechanicalParams* mparams , DataVecDeriv& dataf, const DataVecDeriv& datadx, SReal factor) +void AdaptiveBeamForceFieldAndMass::addMDx(const sofa::core::MechanicalParams* mparams , DataVecDeriv& dataf, const DataVecDeriv& datadx, const SReal factor) { SOFA_UNUSED(mparams); SOFA_UNUSED(datadx); auto f = sofa::helper::getWriteOnlyAccessor(dataf); - auto size = l_interpolation->getStateSize(); + const auto size = l_interpolation->getStateSize(); if (f.size() != size) f.resize(size); - unsigned int numBeams = l_interpolation->getNumBeams(); - for (unsigned int b=0; bgetNumBeams(); + for (sofa::Index b=0; bgetNodeIndices( b, node0Idx, node1Idx ); applyMassLarge( f.wref(), b, node0Idx, node1Idx, factor); @@ -339,35 +341,35 @@ void AdaptiveBeamForceFieldAndMass::addMDx(const MechanicalParams* mp template -void AdaptiveBeamForceFieldAndMass::addMToMatrix(const MechanicalParams *mparams, - const MultiMatrixAccessor* matrix) +void AdaptiveBeamForceFieldAndMass::addMToMatrix(const sofa::core::MechanicalParams *mparams, + const sofa::core::behavior::MultiMatrixAccessor* matrix) { - MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(mstate); - Real mFact = (Real)mparams->mFactor(); + sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(mstate); + const Real mFact = (Real)mparams->mFactor(); - unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); - for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); /// matrices in global frame - Matrix6x6 M00 = bLM.m_A0Ref.multTranspose((bLM.m_M00 * bLM.m_A0Ref)); - Matrix6x6 M01 = bLM.m_A0Ref.multTranspose((bLM.m_M01 * bLM.m_A1Ref)); - Matrix6x6 M10 = bLM.m_A1Ref.multTranspose((bLM.m_M10 * bLM.m_A0Ref)); - Matrix6x6 M11 = bLM.m_A1Ref.multTranspose((bLM.m_M11 * bLM.m_A1Ref)); + const Matrix6x6 M00 = bLM.m_A0Ref.multTranspose((bLM.m_M00 * bLM.m_A0Ref)); + const Matrix6x6 M01 = bLM.m_A0Ref.multTranspose((bLM.m_M01 * bLM.m_A1Ref)); + const Matrix6x6 M10 = bLM.m_A1Ref.multTranspose((bLM.m_M10 * bLM.m_A0Ref)); + const Matrix6x6 M11 = bLM.m_A1Ref.multTranspose((bLM.m_M11 * bLM.m_A1Ref)); - int index0[6], index1[6]; - for (int i=0;i<6;i++) + sofa::Index index0[6], index1[6]; + for (sofa::Index i=0;i<6;i++) index0[i] = r.offset+node0Idx*6+i; - for (int i=0;i<6;i++) + for (sofa::Index i=0;i<6;i++) index1[i] = r.offset+node1Idx*6+i; - for (int i=0;i<6;i++) + for (sofa::Index i=0;i<6;i++) { - for (int j=0;j<6;j++) + for (sofa::Index j=0;j<6;j++) { r.matrix->add(index0[i], index0[j], M00(i,j)*mFact); r.matrix->add(index0[i], index1[j], M01(i,j)*mFact); @@ -382,13 +384,12 @@ void AdaptiveBeamForceFieldAndMass::addMToMatrix(const MechanicalPara template void AdaptiveBeamForceFieldAndMass::buildMassMatrix(sofa::core::behavior::MassMatrixAccumulator* matrices) { - const unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); - - for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); /// matrices in global frame @@ -407,39 +408,39 @@ void AdaptiveBeamForceFieldAndMass::buildMassMatrix(sofa::core::behav template -void AdaptiveBeamForceFieldAndMass::addMBKToMatrix(const MechanicalParams* mparams, - const MultiMatrixAccessor* matrix) +void AdaptiveBeamForceFieldAndMass::addMBKToMatrix(const sofa::core::MechanicalParams* mparams, + const sofa::core::behavior::MultiMatrixAccessor* matrix) { - MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(mstate); - Real kFact = (Real)mparams->kFactor(); - Real mFact = (Real)mparams->mFactor(); + sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(mstate); + const Real kFact = (Real)mparams->kFactor(); + const Real mFact = (Real)mparams->mFactor(); Real totalMass = 0; - unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); - for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); - int index0[6], index1[6]; - for (int i=0;i<6;i++) + sofa::Index index0[6], index1[6]; + for (sofa::Index i=0;i<6;i++) index0[i] = r.offset+node0Idx*6+i; - for (int i=0;i<6;i++) + for (sofa::Index i=0;i<6;i++) index1[i] = r.offset+node1Idx*6+i; - if(node0Idx!=node1Idx) // no rigidification + if(node0Idx != node1Idx) // no rigidification { // matrices in global frame - Matrix6x6 K00 = bLM.m_A0Ref.multTranspose((bLM.m_K00 * bLM.m_A0Ref)); - Matrix6x6 K01 = bLM.m_A0Ref.multTranspose((bLM.m_K01 * bLM.m_A1Ref)); - Matrix6x6 K10 = bLM.m_A1Ref.multTranspose((bLM.m_K10 * bLM.m_A0Ref)); - Matrix6x6 K11 = bLM.m_A1Ref.multTranspose((bLM.m_K11 * bLM.m_A1Ref)); + const Matrix6x6 K00 = bLM.m_A0Ref.multTranspose((bLM.m_K00 * bLM.m_A0Ref)); + const Matrix6x6 K01 = bLM.m_A0Ref.multTranspose((bLM.m_K01 * bLM.m_A1Ref)); + const Matrix6x6 K10 = bLM.m_A1Ref.multTranspose((bLM.m_K10 * bLM.m_A0Ref)); + const Matrix6x6 K11 = bLM.m_A1Ref.multTranspose((bLM.m_K11 * bLM.m_A1Ref)); - for (int i=0;i<6;i++) + for (sofa::Index i=0;i<6;i++) { - for (int j=0;j<6;j++) + for (sofa::Index j=0;j<6;j++) { r.matrix->add(index0[i], index0[j], - K00(i,j)*kFact); r.matrix->add(index0[i], index1[j], - K01(i,j)*kFact); @@ -450,14 +451,14 @@ void AdaptiveBeamForceFieldAndMass::addMBKToMatrix(const MechanicalPa } // matrices in global frame - Matrix6x6 M00 = bLM.m_A0Ref.multTranspose((bLM.m_M00 * bLM.m_A0Ref)); - Matrix6x6 M01 = bLM.m_A0Ref.multTranspose((bLM.m_M01 * bLM.m_A1Ref)); - Matrix6x6 M10 = bLM.m_A1Ref.multTranspose((bLM.m_M10 * bLM.m_A0Ref)); - Matrix6x6 M11 = bLM.m_A1Ref.multTranspose((bLM.m_M11 * bLM.m_A1Ref)); + const Matrix6x6 M00 = bLM.m_A0Ref.multTranspose((bLM.m_M00 * bLM.m_A0Ref)); + const Matrix6x6 M01 = bLM.m_A0Ref.multTranspose((bLM.m_M01 * bLM.m_A1Ref)); + const Matrix6x6 M10 = bLM.m_A1Ref.multTranspose((bLM.m_M10 * bLM.m_A0Ref)); + const Matrix6x6 M11 = bLM.m_A1Ref.multTranspose((bLM.m_M11 * bLM.m_A1Ref)); - for (int i=0;i<6;i++) + for (sofa::Index i=0;i<6;i++) { - for (int j=0;j<6;j++) + for (sofa::Index j=0;j<6;j++) { totalMass += M00(i,j)*mFact + M11(i,j)*mFact; r.matrix->add(index0[i], index0[j], M00(i,j)*mFact); @@ -481,7 +482,7 @@ void AdaptiveBeamForceFieldAndMass::buildDampingMatrix(core::behavior ///////////////////////////////////// template -void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* mparams , +void AdaptiveBeamForceFieldAndMass::addForce (const sofa::core::MechanicalParams* mparams , DataVecDeriv& dataf, const DataVecCoord& datax, const DataVecDeriv& v) @@ -494,7 +495,7 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* f.resize(x.size()); // current content of the vector will remain the same (http://www.cplusplus.com/reference/vector/vector/resize/) - unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); m_localBeamMatrices.resize(numBeams); if(d_computeMass.getValue()) @@ -505,8 +506,8 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* ///* Calculer les rotation et les transformations ///* Calculer la matrice "locale" ///* Calculer la force exercée par chaque beam - ///* Calculer la force exercée par la gravitée - for (unsigned int beamId=0; beamId ::addForce (const MechanicalParams* /// 2. Computes the frame of the beam based on the spline interpolation: Transform global_H_local; - Real baryX = 0.5; - Real L = l_interpolation->getLength(beamId); + constexpr Real baryX = 0.5; + const Real L = l_interpolation->getLength(beamId); l_interpolation->InterpolateTransformUsingSpline(global_H_local, baryX, global_H_local0, global_H_local1, L); @@ -565,7 +566,7 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* const auto& massDensity = helper::getReadAccessor(d_massDensity); // for BeamInterpolation which is not overidding the _rho - if (beamId < int(massDensity.size())) + if (beamId < static_cast(massDensity.size())) { beamMatrices._rho = massDensity[beamId]; } @@ -582,7 +583,7 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* } /// IF RIGIDIFICATION: no stiffness forces: - if(node0Idx==node1Idx) + if(node0Idx == node1Idx) continue; /// compute the local stiffness matrices @@ -595,8 +596,8 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* l_interpolation->getSplineRestTransform(beamId, local_H_local0_rest, local_H_local1_rest); ///2. computes the local displacement of 0 and 1 in frame local: - SpatialVector u0 = local_H_local0.CreateSpatialVector() - local_H_local0_rest.CreateSpatialVector(); - SpatialVector u1 = local_H_local1.CreateSpatialVector() - local_H_local1_rest.CreateSpatialVector(); + const SpatialVector u0 = local_H_local0.CreateSpatialVector() - local_H_local0_rest.CreateSpatialVector(); + const SpatialVector u1 = local_H_local1.CreateSpatialVector() - local_H_local1_rest.CreateSpatialVector(); /// 3. put the result in a Vec6 Vec6NoInit U0local, U1local; @@ -613,7 +614,7 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* { Vec3 P0,P1,P2,P3; Real length; - Real rest_length = l_interpolation->getLength(beamId); + const Real rest_length = l_interpolation->getLength(beamId); l_interpolation->getSplinePoints(beamId, x, P0, P1, P2, P3); l_interpolation->computeActualLength(length, P0, P1, P2, P3); @@ -628,8 +629,8 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* Vec3 ResultNode0, ResultNode1; l_interpolation->computeStrechAndTwist(beamId, x, ResultNode0, ResultNode1); - Real ux0 =-ResultNode0[0] + l_interpolation->getLength(beamId)/2; - Real ux1 = ResultNode1[0] - l_interpolation->getLength(beamId)/2; + const Real ux0 =-ResultNode0[0] + l_interpolation->getLength(beamId)/2; + const Real ux1 = ResultNode1[0] - l_interpolation->getLength(beamId)/2; U0local[0] = ux0; U1local[0] = ux1; @@ -641,12 +642,12 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* } /// compute the force in the local frame: - Vec6 f0 = beamMatrices.m_K00 * U0local + beamMatrices.m_K01 * U1local; - Vec6 f1 = beamMatrices.m_K10 * U0local + beamMatrices.m_K11 * U1local; + const Vec6 f0 = beamMatrices.m_K00 * U0local + beamMatrices.m_K01 * U1local; + const Vec6 f1 = beamMatrices.m_K10 * U0local + beamMatrices.m_K11 * U1local; /// compute the force in the global frame - Vec6 F0_ref = beamMatrices.m_A0Ref.multTranspose(f0); - Vec6 F1_ref = beamMatrices.m_A1Ref.multTranspose(f1); + const Vec6 F0_ref = beamMatrices.m_A0Ref.multTranspose(f0); + const Vec6 F1_ref = beamMatrices.m_A1Ref.multTranspose(f1); /// Add this force to vector f for (unsigned int i=0; i<6; i++) @@ -666,7 +667,7 @@ void AdaptiveBeamForceFieldAndMass::addForce (const MechanicalParams* template -void AdaptiveBeamForceFieldAndMass::addDForce(const MechanicalParams* mparams, +void AdaptiveBeamForceFieldAndMass::addDForce(const sofa::core::MechanicalParams* mparams, DataVecDeriv& datadF, const DataVecDeriv& datadX ) { auto df = sofa::helper::getWriteOnlyAccessor(datadF); @@ -675,11 +676,11 @@ void AdaptiveBeamForceFieldAndMass::addDForce(const MechanicalParams* df.resize(dx.size()); // current content of the vector will remain the same (http://www.cplusplus.com/reference/vector/vector/resize/) - unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); - for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); applyStiffnessLarge( df.wref(), dx, b, node0Idx, node1Idx, kFactor); @@ -689,38 +690,38 @@ void AdaptiveBeamForceFieldAndMass::addDForce(const MechanicalParams* template -void AdaptiveBeamForceFieldAndMass::addKToMatrix(const MechanicalParams* mparams, - const MultiMatrixAccessor* matrix) +void AdaptiveBeamForceFieldAndMass::addKToMatrix(const sofa::core::MechanicalParams* mparams, + const sofa::core::behavior::MultiMatrixAccessor* matrix) { - MultiMatrixAccessor::MatrixRef matrixRef = matrix->getMatrix(mstate); - Real k = (Real)mparams->kFactor(); + sofa::core::behavior::MultiMatrixAccessor::MatrixRef matrixRef = matrix->getMatrix(mstate); + const Real k = (Real)mparams->kFactor(); - unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); - for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); - if(node0Idx==node1Idx) + if(node0Idx == node1Idx) continue; // matrices in global frame - Matrix6x6 K00 = beamLocalMatrix.m_A0Ref.multTranspose((beamLocalMatrix.m_K00 * beamLocalMatrix.m_A0Ref)); - Matrix6x6 K01 = beamLocalMatrix.m_A0Ref.multTranspose((beamLocalMatrix.m_K01 * beamLocalMatrix.m_A1Ref)); - Matrix6x6 K10 = beamLocalMatrix.m_A1Ref.multTranspose((beamLocalMatrix.m_K10 * beamLocalMatrix.m_A0Ref)); - Matrix6x6 K11 = beamLocalMatrix.m_A1Ref.multTranspose((beamLocalMatrix.m_K11 * beamLocalMatrix.m_A1Ref)); + const Matrix6x6 K00 = beamLocalMatrix.m_A0Ref.multTranspose((beamLocalMatrix.m_K00 * beamLocalMatrix.m_A0Ref)); + const Matrix6x6 K01 = beamLocalMatrix.m_A0Ref.multTranspose((beamLocalMatrix.m_K01 * beamLocalMatrix.m_A1Ref)); + const Matrix6x6 K10 = beamLocalMatrix.m_A1Ref.multTranspose((beamLocalMatrix.m_K10 * beamLocalMatrix.m_A0Ref)); + const Matrix6x6 K11 = beamLocalMatrix.m_A1Ref.multTranspose((beamLocalMatrix.m_K11 * beamLocalMatrix.m_A1Ref)); - int index0[6], index1[6]; - for (int i=0;i<6;i++) + sofa::Index index0[6], index1[6]; + for (sofa::Index i=0;i<6;i++) index0[i] = matrixRef.offset+node0Idx*6+i; - for (int i=0;i<6;i++) + for (sofa::Index i=0;i<6;i++) index1[i] = matrixRef.offset+node1Idx*6+i; - for (int i=0;i<6;i++) + for (sofa::Index i=0;i<6;i++) { - for (int j=0;j<6;j++) + for (sofa::Index j=0;j<6;j++) { matrixRef.matrix->add(index0[i], index0[j], - K00(i,j)*k); matrixRef.matrix->add(index0[i], index1[j], - K01(i,j)*k); @@ -734,27 +735,26 @@ void AdaptiveBeamForceFieldAndMass::addKToMatrix(const MechanicalPara template void AdaptiveBeamForceFieldAndMass::buildStiffnessMatrix(core::behavior::StiffnessMatrix* matrix) { - const unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); auto dfdx = matrix->getForceDerivativeIn(this->mstate) .withRespectToPositionsIn(this->mstate); - for (unsigned int b=0; bgetNodeIndices( b, node0Idx, node1Idx ); - if(node0Idx==node1Idx) + if(node0Idx == node1Idx) continue; // matrices in global frame - Matrix6x6 K00 = beamLocalMatrix.m_A0Ref.multTranspose(beamLocalMatrix.m_K00 * beamLocalMatrix.m_A0Ref); - Matrix6x6 K01 = beamLocalMatrix.m_A0Ref.multTranspose(beamLocalMatrix.m_K01 * beamLocalMatrix.m_A1Ref); - Matrix6x6 K10 = beamLocalMatrix.m_A1Ref.multTranspose(beamLocalMatrix.m_K10 * beamLocalMatrix.m_A0Ref); - Matrix6x6 K11 = beamLocalMatrix.m_A1Ref.multTranspose(beamLocalMatrix.m_K11 * beamLocalMatrix.m_A1Ref); - + const Matrix6x6 K00 = beamLocalMatrix.m_A0Ref.multTranspose(beamLocalMatrix.m_K00 * beamLocalMatrix.m_A0Ref); + const Matrix6x6 K01 = beamLocalMatrix.m_A0Ref.multTranspose(beamLocalMatrix.m_K01 * beamLocalMatrix.m_A1Ref); + const Matrix6x6 K10 = beamLocalMatrix.m_A1Ref.multTranspose(beamLocalMatrix.m_K10 * beamLocalMatrix.m_A0Ref); + const Matrix6x6 K11 = beamLocalMatrix.m_A1Ref.multTranspose(beamLocalMatrix.m_K11 * beamLocalMatrix.m_A1Ref); dfdx(node0Idx*6, node0Idx*6) += - K00; dfdx(node0Idx*6, node1Idx*6) += - K01; @@ -769,7 +769,7 @@ void AdaptiveBeamForceFieldAndMass::buildStiffnessMatrix(core::behavi ///////////////////////////////////// template -void AdaptiveBeamForceFieldAndMass::draw(const VisualParams *vparams) +void AdaptiveBeamForceFieldAndMass::draw(const sofa::core::visual::VisualParams *vparams) { if (!vparams->displayFlags().getShowForceFields() && !vparams->displayFlags().getShowBehaviorModels()) return; if (!mstate) return; @@ -778,9 +778,10 @@ void AdaptiveBeamForceFieldAndMass::draw(const VisualParams *vparams) ReadAccessor > x = mstate->read(sofa::core::vec_id::read_access::position) ; - unsigned int numBeams = l_interpolation->getNumBeams(); + const auto numBeams = l_interpolation->getNumBeams(); + constexpr Vec3 localPos(0.0,0.0,0.0); - for (unsigned int b=0; b::draw(const VisualParams *vparams) l_interpolation->getNodeIndices(b, node0Idx, node1Idx); l_interpolation->computeTransform(b, node0Idx, node1Idx, globalH0Local, globalH1Local, x.ref()); - if (vparams->displayFlags().getShowBehaviorModels() && node0Idx!=node1Idx) + if (vparams->displayFlags().getShowBehaviorModels() && node0Idx != node1Idx) drawElement(vparams, b, globalH0Local, globalH1Local); if(vparams->displayFlags().getShowForceFields()) { - // / test /// + constexpr double nbDiscretization = 50.0; + constexpr double step = 1.0/nbDiscretization; + type::vector points; Vec3 pos = globalH0Local.getOrigin(); - for (double i=0.0; i<1.00001; i+=0.02) + + for (double i=0.0; i<1.00001; i += step) { points.push_back(pos); - Vec3 localPos(0.0,0.0,0.0); this->l_interpolation->interpolatePointUsingSpline(b, i, localPos, x.ref(), pos); points.push_back(pos); } - if(node0Idx==node1Idx) /// rigidification case + if(node0Idx == node1Idx) /// rigidification case { - vparams->drawTool()->drawLines(points,2, sofa::type::RGBAColor(0,0,1,1)); + vparams->drawTool()->drawLines(points,2, sofa::type::RGBAColor::blue()); continue; } else - vparams->drawTool()->drawLines(points,2, sofa::type::RGBAColor(1,0,0,1)); - - double length = (double) l_interpolation->getLength(b); + { + vparams->drawTool()->drawLines(points,2, sofa::type::RGBAColor::red()); + } + const float length = static_cast(l_interpolation->getLength(b)); - Vec3 localPos(0.0,0.0,0.0); - Real baryX = 0.5; + constexpr Real baryX = 0.5; Transform globalHLocalInterpol; l_interpolation->InterpolateTransformUsingSpline(b, baryX, localPos, x.ref(), globalHLocalInterpol); Quat q = globalHLocalInterpol.getOrientation(); q.normalize(); - Vec3 P1, x,y,z; - P1 = globalHLocalInterpol.getOrigin(); - x= q.rotate(Vec3(length/6.0,0,0)); - y= q.rotate(Vec3(0,length/8.0,0)); - z= q.rotate(Vec3(0,0,length/8.0)); - float radius_arrow = (float)length/60.0f; + const Vec3 P1 = globalHLocalInterpol.getOrigin(); + const Vec3 x = q.rotate(Vec3(length/6.0,0,0)); + const Vec3 y = q.rotate(Vec3(0,length/8.0,0)); + const Vec3 z = q.rotate(Vec3(0,0,length/8.0)); + const float radius_arrow = static_cast(length/60.0f); - vparams->drawTool()->drawArrow(P1,P1 + x, radius_arrow, sofa::type::RGBAColor(1,0,0,1)); - vparams->drawTool()->drawArrow(P1,P1 + y, radius_arrow, sofa::type::RGBAColor(1,0,0,1)); - vparams->drawTool()->drawArrow(P1,P1 + z, radius_arrow, sofa::type::RGBAColor(1,0,0,1)); + vparams->drawTool()->drawArrow(P1,P1 + x, radius_arrow, sofa::type::RGBAColor::red()); + vparams->drawTool()->drawArrow(P1,P1 + y, radius_arrow, sofa::type::RGBAColor::red()); + vparams->drawTool()->drawArrow(P1,P1 + z, radius_arrow, sofa::type::RGBAColor::red()); } } @@ -841,13 +843,13 @@ void AdaptiveBeamForceFieldAndMass::draw(const VisualParams *vparams) template -void AdaptiveBeamForceFieldAndMass::drawElement(const VisualParams *vparams, int beam, - Transform &global_H0_local, Transform &global_H1_local) +void AdaptiveBeamForceFieldAndMass::drawElement(const sofa::core::visual::VisualParams *vparams, const sofa::Index beamID, + const Transform &global_H0_local, const Transform &global_H1_local) { - double length = (double) l_interpolation->getLength(beam); + const float length = static_cast(l_interpolation->getLength(beamID)); /// ARROWS - Vec3 sizeArrows (length/4., length/8., length/8.); + const type::Vec3f sizeArrows (length/4.0f, length/8.0f, length/8.0f); vparams->drawTool()->drawFrame(global_H0_local.getOrigin(), global_H0_local.getOrientation(), sizeArrows); vparams->drawTool()->drawFrame(global_H1_local.getOrigin(), global_H1_local.getOrientation(), sizeArrows); diff --git a/src/BeamAdapter/component/forcefield/AdaptiveInflatableBeamForceField.inl b/src/BeamAdapter/component/forcefield/AdaptiveInflatableBeamForceField.inl index 85da90d6b..9566c6019 100644 --- a/src/BeamAdapter/component/forcefield/AdaptiveInflatableBeamForceField.inl +++ b/src/BeamAdapter/component/forcefield/AdaptiveInflatableBeamForceField.inl @@ -122,7 +122,6 @@ void AdaptiveInflatableBeamForceField::computeGravityVector() template void AdaptiveInflatableBeamForceField::computeStiffness(int beam, BeamLocalMatrices& beamLocalMatrices) { - Real x_curv = 0.0 ; Real _rho = 0.0 ; Real _nu = 0.0 ; Real _E = 0.0 ; diff --git a/src/BeamAdapter/component/mapping/AdaptiveBeamMapping.inl b/src/BeamAdapter/component/mapping/AdaptiveBeamMapping.inl index 7c1b7bc51..67416ee7d 100644 --- a/src/BeamAdapter/component/mapping/AdaptiveBeamMapping.inl +++ b/src/BeamAdapter/component/mapping/AdaptiveBeamMapping.inl @@ -273,9 +273,9 @@ void AdaptiveBeamMapping< TIn, TOut>::apply(const MechanicalParams* mparams, Dat // HACK for init: In case the number of output points is bigger to the number of distribution, set all points to 0 - for (int i = m_pointBeamDistribution.size(); i < d_points.getValue().size(); i++) + for (std::size_t i = m_pointBeamDistribution.size(); i < d_points.getValue().size(); i++) { - out[i] = Vec<3, InReal>(0.0, 0.0, 0.0); + out[i].clear(); } diff --git a/src/BeamAdapter/component/mapping/MultiAdaptiveBeamMapping.inl b/src/BeamAdapter/component/mapping/MultiAdaptiveBeamMapping.inl index 3417ecaa6..2d8c12a28 100644 --- a/src/BeamAdapter/component/mapping/MultiAdaptiveBeamMapping.inl +++ b/src/BeamAdapter/component/mapping/MultiAdaptiveBeamMapping.inl @@ -213,7 +213,7 @@ void MultiAdaptiveBeamMapping< TIn, TOut>::assignSubMappingFromControllerInfo() } else { - msg_error() << "Trying to remove baseEdge which is alreay empty. This case is not supposed to happened."; + msg_error() << "Trying to remove baseEdge which is already empty. This case is not supposed to happened."; } if (edgeToRemove.size()>0) @@ -308,7 +308,7 @@ void MultiAdaptiveBeamMapping< TIn, TOut>::init() unsigned int numSeg, numLinesInstrument; numSeg=0; InReal DX=0; - // we chose the collision parameters of the most discrestized instrument + // we chose the collision parameters of the most discretized instrument for (unsigned int i=0; i::addBaryPoint(const int& edgeId,const V int nbUnControlledEdges = totalNbEdges - nbControlledEdge; assert(nbUnControlledEdges>=0); - if (edgeId < (totalNbEdges-nbControlledEdge) ) + if (edgeId < nbUnControlledEdges ) { //if the edge in question is not under control, dont need to compute for collision } diff --git a/src/BeamAdapter/component/model/BaseRodSectionMaterial.h b/src/BeamAdapter/component/model/BaseRodSectionMaterial.h index 6f03bd7fc..03a450ad6 100644 --- a/src/BeamAdapter/component/model/BaseRodSectionMaterial.h +++ b/src/BeamAdapter/component/model/BaseRodSectionMaterial.h @@ -43,7 +43,7 @@ using sofa::core::loader::MeshLoader; * Method @sa initSection and @sa getRestTransformOnX should be overriden to provide the correct creation and interpolation. * * The rod section is described by: - * - Topology parameters: vertices and edges @sa d_nbEdgesVisu and @sa d_nbEdgesCollis + * - Topology parameters: vertices and edges @sa d_nbEdgesVisu, @sa d_nbEdgesCollis and @sa d_nbBeams * - Geometry parameters: radius @sa d_radius, @sa d_innerRadius and length @sa d_length * - Mechanical parameters: @sa d_poissonRatio and @sa d_youngModulus */ @@ -72,13 +72,16 @@ class BaseRodSectionMaterial : public core::objectmodel::BaseObject /////////////////////////// Geometry and physics Getter ////////////////////////////////////////// /// Returns the number of visual edges of this section. To be set or computed by child. - [[nodiscard]] int getNbVisualEdges() const { return d_nbEdgesVisu.getValue(); } + [[nodiscard]] auto getNbVisualEdges() const { return d_nbEdgesVisu.getValue(); } /// Returns the number of collision edges of this section. To be set or computed by child. - [[nodiscard]] int getNbCollisionEdges() const { return d_nbEdgesCollis.getValue(); } + [[nodiscard]] auto getNbCollisionEdges() const { return d_nbEdgesCollis.getValue(); } + + /// Returns the number of collision edges of this section. To be set or computed by child. + [[nodiscard]] auto getNbBeams() const { return d_nbBeams.getValue(); } /// Returns the total length of this section. To be set or computed by child. - [[nodiscard]] Real getLength() const { return d_length.getValue(); } + [[nodiscard]] auto getLength() const { return d_length.getValue(); } /// Returns the BeamSection @sa m_beamSection corresponding to this section @@ -91,7 +94,7 @@ class BaseRodSectionMaterial : public core::objectmodel::BaseObject void getMechanicalParameters(Real& youngModulus, Real& cPoisson, Real& massDensity) const; /// This function is called to get the rest position of the beam depending on the current curved abscisse given in parameter - virtual void getRestTransformOnX(Transform& global_H_local, const Real& x_used, const Real& x_start) + virtual void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) { SOFA_UNUSED(global_H_local); SOFA_UNUSED(x_used); @@ -111,6 +114,7 @@ class BaseRodSectionMaterial : public core::objectmodel::BaseObject Data d_innerRadius; ///< Data defining the geometry internal radius of this section is hollow Data d_length; ///< Data defining the geometry length of this section + Data d_nbBeams; ///< Data defining the number of (mechanical) beams composing this section Data d_nbEdgesVisu; ///< Data defining the number of visual edges composing this section Data d_nbEdgesCollis; ///< Data defining the number of collision edges composing this section diff --git a/src/BeamAdapter/component/model/BaseRodSectionMaterial.inl b/src/BeamAdapter/component/model/BaseRodSectionMaterial.inl index 957363dcc..b9adaac51 100644 --- a/src/BeamAdapter/component/model/BaseRodSectionMaterial.inl +++ b/src/BeamAdapter/component/model/BaseRodSectionMaterial.inl @@ -34,8 +34,9 @@ BaseRodSectionMaterial::BaseRodSectionMaterial() , d_radius(initData(&d_radius, (Real)1.0, "radius", "Full radius of this section")) , d_innerRadius(initData(&d_innerRadius, (Real)0.0, "innerRadius", "Inner radius of this section if hollow")) , d_length(initData(&d_length, (Real)1.0, "length", "Total length of this section")) - , d_nbEdgesVisu(initData(&d_nbEdgesVisu, (Size)10, "nbEdgesVisu", "number of Edges for the visual model")) - , d_nbEdgesCollis(initData(&d_nbEdgesCollis, (Size)20, "nbEdgesCollis", "number of Edges for the collision model")) + , d_nbBeams(initData(&d_nbBeams, (Size)5, "nbBeams", "Number of Beams for the mechanical model")) + , d_nbEdgesVisu(initData(&d_nbEdgesVisu, (Size)10, "nbEdgesVisu", "Number of Edges for the visual model")) + , d_nbEdgesCollis(initData(&d_nbEdgesCollis, (Size)20, "nbEdgesCollis", "Number of Edges for the collision model")) { } @@ -46,6 +47,12 @@ void BaseRodSectionMaterial::init() { this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Loading); + if(!d_nbBeams.isSet()) + { + msg_deprecated() << "nbBeams is now required but it was not set. Its value will be copied from nbEdgesCollis as a temporary compatibility solution."; + d_nbBeams.setValue(d_nbEdgesCollis.getValue()); + } + // Prepare beam sections double r = this->d_radius.getValue(); double rInner = this->d_innerRadius.getValue(); diff --git a/src/BeamAdapter/component/model/RodMeshSection.h b/src/BeamAdapter/component/model/RodMeshSection.h index 476cfcf73..da8183b9f 100644 --- a/src/BeamAdapter/component/model/RodMeshSection.h +++ b/src/BeamAdapter/component/model/RodMeshSection.h @@ -54,7 +54,7 @@ class RodMeshSection : public BaseRodSectionMaterial RodMeshSection(); /// Override method to get the rest position of the beam. In this implementation, it will interpolate along the loaded mesh geometry - void getRestTransformOnX(Transform& global_H_local, const Real& x_used, const Real& x_start) override; + void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) override; protected: /// Internal method to init the section. Called by @sa BaseRodSectionMaterial::init() method diff --git a/src/BeamAdapter/component/model/RodMeshSection.inl b/src/BeamAdapter/component/model/RodMeshSection.inl index e7dd498b3..81b57bd0d 100644 --- a/src/BeamAdapter/component/model/RodMeshSection.inl +++ b/src/BeamAdapter/component/model/RodMeshSection.inl @@ -62,7 +62,7 @@ bool RodMeshSection::initSection() template -void RodMeshSection::getRestTransformOnX(Transform& global_H_local, const Real& x_used, const Real& x_start) +void RodMeshSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) { Real abs_curr = x_used - x_start; abs_curr = abs_curr /(this->d_length.getValue()) * m_absOfGeometry; diff --git a/src/BeamAdapter/component/model/RodSpireSection.h b/src/BeamAdapter/component/model/RodSpireSection.h index b333c0a41..83a9c631e 100644 --- a/src/BeamAdapter/component/model/RodSpireSection.h +++ b/src/BeamAdapter/component/model/RodSpireSection.h @@ -51,7 +51,7 @@ class RodSpireSection : public BaseRodSectionMaterial RodSpireSection(); /// Override method to get the rest position of the beam. In this implementation, it will compute the current position given the spire parameters - void getRestTransformOnX(Transform& global_H_local, const Real& x_used, const Real& x_start) override; + void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) override; protected: /// Internal method to init the section. Called by @sa BaseRodSectionMaterial::init() method diff --git a/src/BeamAdapter/component/model/RodSpireSection.inl b/src/BeamAdapter/component/model/RodSpireSection.inl index 93ca2a933..8bc409a67 100644 --- a/src/BeamAdapter/component/model/RodSpireSection.inl +++ b/src/BeamAdapter/component/model/RodSpireSection.inl @@ -48,25 +48,12 @@ bool RodSpireSection::initSection() return false; } - if (int nbrEdgesVisu = this->d_nbEdgesVisu.getValue() <= 0) - { - msg_warning() << "Number of visual edges has been set to an invalid value: " << nbrEdgesVisu << ". Value should be a positive integer. Setting to default value: 10"; - this->d_nbEdgesVisu.setValue(10); - } - - - if (int nbEdgesCollis = this->d_nbEdgesCollis.getValue() <= 0) - { - msg_warning() << "Number of collision edges has been set to an invalid value: " << nbEdgesCollis << ". Value should be a positive integer. Setting to default value: 20"; - this->d_nbEdgesCollis.setValue(10); - } - return true; } template -void RodSpireSection::getRestTransformOnX(Transform& global_H_local, const Real& x_used, const Real& x_start) +void RodSpireSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) { Real projetedLength = d_spireDiameter.getValue() * M_PI; Real lengthSpire = sqrt(d_spireHeight.getValue() * d_spireHeight.getValue() + projetedLength * projetedLength); diff --git a/src/BeamAdapter/component/model/RodStraightSection.h b/src/BeamAdapter/component/model/RodStraightSection.h index 7da823642..e5fe157bb 100644 --- a/src/BeamAdapter/component/model/RodStraightSection.h +++ b/src/BeamAdapter/component/model/RodStraightSection.h @@ -48,7 +48,7 @@ class RodStraightSection : public BaseRodSectionMaterial RodStraightSection(); /// Override method to get the rest position of the beam. In this implementation, it will basically returns Vec3(x_start + x_used, 0 0) - void getRestTransformOnX(Transform& global_H_local, const Real& x_used, const Real& x_start) override; + void getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) override; protected: /// Internal method to init the section. Called by @sa BaseRodSectionMaterial::init() method diff --git a/src/BeamAdapter/component/model/RodStraightSection.inl b/src/BeamAdapter/component/model/RodStraightSection.inl index 67b96249a..7d939fd00 100644 --- a/src/BeamAdapter/component/model/RodStraightSection.inl +++ b/src/BeamAdapter/component/model/RodStraightSection.inl @@ -46,25 +46,12 @@ bool RodStraightSection::initSection() return false; } - if (int nbrEdgesVisu = this->d_nbEdgesVisu.getValue() <= 0) - { - msg_warning() << "Number of visual edges has been set to an invalid value: " << nbrEdgesVisu << ". Value should be a positive integer. Setting to default value: 10"; - this->d_nbEdgesVisu.setValue(10); - } - - - if (int nbEdgesCollis = this->d_nbEdgesCollis.getValue() <= 0) - { - msg_warning() << "Number of collision edges has been set to an invalid value: " << nbEdgesCollis << ". Value should be a positive integer. Setting to default value: 20"; - this->d_nbEdgesCollis.setValue(10); - } - return true; } template -void RodStraightSection::getRestTransformOnX(Transform& global_H_local, const Real& x_used, const Real& x_start) +void RodStraightSection::getRestTransformOnX(Transform& global_H_local, const Real x_used, const Real x_start) { global_H_local.set(type::Vec3(x_start + x_used, 0.0, 0.0), Quat()); } diff --git a/src/BeamAdapter/utils/BeamActions.h b/src/BeamAdapter/utils/BeamActions.h index 465979fe2..531a1cc0c 100644 --- a/src/BeamAdapter/utils/BeamActions.h +++ b/src/BeamAdapter/utils/BeamActions.h @@ -21,6 +21,9 @@ ******************************************************************************/ #pragma once +#include +#include + namespace beamadapter { @@ -55,7 +58,7 @@ namespace beamadapter }; /// static method to convert an action as string into enum class using @sa beamActionNames - static BeamAdapterAction convertBeamAdapterAction(const std::string& sAction) + inline BeamAdapterAction convertBeamAdapterAction(const std::string& sAction) { auto bAction = beamActionNames.find(sAction);