[StateContainer] Allow coord difference in vOp for rigids

sofa-framework · Jan 31, 2025 · 313cec2 · 313cec2
1 parent 5448a14
commit 313cec2
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 0 deletions.
diff --git a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl
@@ -2096,6 +2096,19 @@ void MechanicalObject<DataTypes>::vOp(const core::ExecParams* params, core::VecI
                     APPLY_PREDICATE_THREEPARAMS(vOp_vab, v, a, b)
                     msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v = a+b (" << v << ',' << a << ',' << b << ',' << f << ")";
                 }
+                else if (f == -1._sreal &&
+                    (v.type == core::V_DERIV && a.type == core::V_COORD && b.type == core::V_COORD))
+                {
+                    // v = a-b
+                    auto vv = this->getWriteOnlyAccessor<core::V_DERIV>(v);
+                    auto va = this->getReadAccessor<core::V_COORD>(a);
+                    auto vb = this->getReadAccessor<core::V_COORD>(b);
+                    vv.resize(vb.size());
+                    for (unsigned int i = 0; i < vv.size(); ++i)
+                    {
+                        vv[i] = DataTypes::coordDifference(va[i], vb[i]);
+                    }
+                }
                 else
                 {
                     // v = a+b*f

diff --git a/Sofa/Component/StateContainer/tests/MechanicalObjectVOp_test.cpp b/Sofa/Component/StateContainer/tests/MechanicalObjectVOp_test.cpp
@@ -731,6 +731,31 @@ struct MechanicalObjectVOpTest : public testing::BaseTest
         checkVecValues<sofa::core::vec_id::read_access::velocity>(isRigid ? velocityCoefficient : forceCoefficient + positionCoefficient * 12);
     }
 
+    void equalCoordDifference() const
+    {
+        // v = a-b
+        m_mechanicalObject->vOp(nullptr, core::vec_id::write_access::velocity, core::vec_id::read_access::restPosition, core::vec_id::read_access::position, -1_sreal);
+
+        unsigned int index {};
+        auto vv = sofa::helper::getReadAccessor(*m_mechanicalObject->read(core::vec_id::read_access::velocity));
+        auto va = sofa::helper::getReadAccessor(*m_mechanicalObject->read(core::vec_id::read_access::restPosition));
+        auto vb = sofa::helper::getReadAccessor(*m_mechanicalObject->read(core::vec_id::read_access::position));
+
+        ASSERT_EQ(vv.size(), 10);
+        for (std::size_t i = 0; i < vv.size(); ++i)
+        {
+            const auto& v = vv[i];
+            const auto diff = DataTypes::coordDifference(va[i], vb[i]);
+            for (std::size_t j = 0; j < v.size(); ++j)
+            {
+                EXPECT_FLOATINGPOINT_EQ(v[j], diff[j])
+            }
+            ++index;
+        }
+
+        checkVecValues<core::vec_id::read_access::position>(positionCoefficient);
+    }
+
     typename MO::SPtr m_mechanicalObject;
 };
 
@@ -949,4 +974,9 @@ TYPED_TEST(MechanicalObjectVOpTest, equalSumWithScalarVelocityMix2)
     this->equalSumWithScalarVelocityMix2();
 }
 
+TYPED_TEST(MechanicalObjectVOpTest, equalCoordDifference)
+{
+    this->equalCoordDifference();
+}
+
 }