feat(constraint): Improve SurfacePressureConstraint logic and fix bug (#340)

* improve SurfacePressureConstraint logic and fix bug

This commit addresses a bug in the `SurfacePressureConstraintResolution` where the force was incorrectly calculated due to a flawed `if/if/else` control flow. The logic has been corrected to a proper `if/else if/else` structure.

Additionally, the clamping logic in `VolumeGrowthConstraintResolution` has been refactored to use `std::min` and `std::max`, improving code clarity and conciseness.

* refactoring of volume computation

* Apply suggestions from code review (renaming)

---------

Co-authored-by: EulalieCoevoet <eulalie.coevoet@gmail.com>

Author: adagolodjo

src/SoftRobots/component/constraint/SurfacePressureConstraint.cpp

@@ -29,6 +29,7 @@
 #include <SoftRobots/component/constraint/SurfacePressureConstraint.inl>
 
 #include <sofa/core/ObjectFactory.h>
+#include <algorithm>
 
 namespace softrobots::constraint
 {
@@ -56,26 +57,21 @@ void SurfacePressureConstraintResolution::init(int line, SReal**w, SReal*force)
 void SurfacePressureConstraintResolution::resolution(int line, SReal** w, SReal* d, SReal* force, SReal* dfree)
 {
     SOFA_UNUSED(w);
-    SOFA_UNUSED(d);
     SOFA_UNUSED(dfree);
 
 
-    double volumeGrowth = m_wActuatorActuator*m_imposedPressure + d[line];
+    const double volumeGrowth = m_wActuatorActuator*m_imposedPressure + d[line];
 
     if(volumeGrowth<m_minVolumeGrowth)
     {
-        volumeGrowth = m_minVolumeGrowth;
-        force[line] -= (d[line]-volumeGrowth) / m_wActuatorActuator ;
+        force[line] -= (d[line] - m_minVolumeGrowth) / m_wActuatorActuator ;
     }
-    if(volumeGrowth>m_maxVolumeGrowth)
+    else if(volumeGrowth>m_maxVolumeGrowth)
     {
-        volumeGrowth = m_maxVolumeGrowth;
-        force[line] -= (d[line]-volumeGrowth) / m_wActuatorActuator ;
+        force[line] -= (d[line] - m_maxVolumeGrowth) / m_wActuatorActuator ;
     }
     else
         force[line] = m_imposedPressure ;
-
-
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -106,10 +102,8 @@ void VolumeGrowthConstraintResolution::resolution(int line, SReal** w, SReal* d,
     // da=Waa*(lambda_a) + Sum Wai * lambda_i  = m_imposedVolumeGrowth
     lambda[line] -= (d[line]-m_imposedVolumeGrowth) / m_wActuatorActuator ;
 
-    if(lambda[line]<m_minPressure)
-        lambda[line] = m_minPressure;
-    if(lambda[line]>m_maxPressure)
-        lambda[line] = m_maxPressure;
+    lambda[line] = std::max(m_minPressure, lambda[line]);
+    lambda[line] = std::min(m_maxPressure, lambda[line]);
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////

src/SoftRobots/component/constraint/model/SurfacePressureModel.h

@@ -150,6 +150,7 @@ class SOFA_SOFTROBOTS_API SurfacePressureModel : virtual public SoftRobotsConstr
 
     void drawValue(const sofa::core::visual::VisualParams* vparams);
     void computeEdges();
+    Real getTriangleContributionToVolume(const Coord& p0, const Coord& p1, const Coord& p2);
 };
 
 extern template class SurfacePressureModel<sofa::defaulttype::Vec3Types>;

src/SoftRobots/component/constraint/model/SurfacePressureModel.inl

@@ -263,6 +263,12 @@ void SurfacePressureModel<DataTypes>::internalInit()
 }
 
 
+template<class DataTypes>
+typename SurfacePressureModel<DataTypes>::Real SurfacePressureModel<DataTypes>::getTriangleContributionToVolume(const Coord& p0, const Coord& p1, const Coord& p2)
+{
+    return ((p1[1]-p0[1])*(p2[2]-p0[2])-(p2[1]-p0[1])*(p1[2]-p0[2]))*(p0[0]+p1[0]+p2[0])/6;
+}
+
 template<class DataTypes>
 SReal SurfacePressureModel<DataTypes>::getCavityVolume(const VecCoord& positions)
 {
@@ -272,31 +278,17 @@ SReal SurfacePressureModel<DataTypes>::getCavityVolume(const VecCoord& positions
     ReadAccessor<Data<vector<Triangle>>>  triangles = d_triangles;
     ReadAccessor<Data<vector<Quad>>>      quads     = d_quads;
 
-    Coord p0, p1, p2;
     Real volume = 0;
 
     for (unsigned int t=0; t<triangles.size(); t++)
     {
-        p0 = positions[triangles[t][0]];
-        p1 = positions[triangles[t][1]];
-        p2 = positions[triangles[t][2]];
-
-        volume += ((p1[1]-p0[1])*(p2[2]-p0[2])-(p2[1]-p0[1])*(p1[2]-p0[2]))*(p0[0]+p1[0]+p2[0])/6;
+        volume += getTriangleContributionToVolume(positions[triangles[t][0]], positions[triangles[t][1]], positions[triangles[t][2]]);
     }
 
     for (unsigned int q=0; q<quads.size(); q++)
     {
-        p0 = positions[quads[q][0]];
-        p1 = positions[quads[q][1]];
-        p2 = positions[quads[q][2]];
-
-        volume += ((p1[1]-p0[1])*(p2[2]-p0[2])-(p2[1]-p0[1])*(p1[2]-p0[2]))*(p0[0]+p1[0]+p2[0])/6;
-
-        p0 = positions[quads[q][0]];
-        p1 = positions[quads[q][2]];
-        p2 = positions[quads[q][3]];
-
-        volume += ((p1[1]-p0[1])*(p2[2]-p0[2])-(p2[1]-p0[1])*(p1[2]-p0[2]))*(p0[0]+p1[0]+p2[0])/6;
+        volume += getTriangleContributionToVolume(positions[quads[q][0]], positions[quads[q][1]], positions[quads[q][2]]);
+        volume += getTriangleContributionToVolume(positions[quads[q][0]], positions[quads[q][2]], positions[quads[q][3]]);
     }
 
     if(volume<0)