diff --git a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp index 326b44dc..0ddc88c6 100644 --- a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp +++ b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include @@ -136,7 +137,7 @@ namespace sofapython3 void ForceField_Trampoline::addKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* dfId) { PythonEnvironment::gil acquire; - + MultiMatrixAccessor::MatrixRef mref = dfId->getMatrix(this->mstate); sofa::linearalgebra::BaseMatrix* mat = mref.matrix; @@ -183,8 +184,74 @@ namespace sofapython3 } - template + py::object ForceField_Trampoline::_buildStiffnessMatrix(int nIndices, int nDofs) + { + PYBIND11_OVERRIDE_PURE(py::object, ForceField, buildStiffnessMatrix, nIndices, nDofs); + } + + template + void ForceField_Trampoline::buildStiffnessMatrix( + sofa::core::behavior::StiffnessMatrix *matrix) + { + PythonEnvironment::gil acquire; + + // nNodes is the number of nodes (positions) of the object whose K matrix we're computing + const int nNodes = static_cast(this->mstate->getSize()); + // nDofs is the number of degrees of freedom per-element of the object whose K matrix we're computing + static constexpr int nDofs = Coord::total_size; + + if (py::get_overload(this, "addKToMatrix") + && !py::get_overload(this, "buildStiffnessMatrix")) + { + msg_deprecated_once() << "The method `addKToMatrix` is deprecated. Please use `buildStiffnessMatrix` instead."; + + // Runs the compatibility layer with `addKToMatrix` + ForceField::buildStiffnessMatrix(matrix); + return; + } + + py::object ret = _buildStiffnessMatrix(nNodes, nDofs); + + if(!py::isinstance(ret)) + { + throw py::type_error("Can't read return value of `buildStiffnessMatrix`. A numpy array is expected"); + } + + auto dfdx = + matrix->getForceDerivativeIn(this->mstate) + .withRespectToPositionsIn(this->mstate); + + // if ret is numpy array + auto r = py::cast(ret); + if (r.ndim() == 3 && r.shape(2) == 1) + { + // read K as a plain 2D matrix + auto kMatrix = r.unchecked(); + for (size_t x = 0 ; x < size_t(kMatrix.shape(0)) ; ++x) + { + for (size_t y = 0 ; y < size_t(kMatrix.shape(1)) ; ++y) + { + dfdx(x, y) += kMatrix(x,y, 0); + } + } + } + else if (r.ndim() == 2 && r.shape(1) == 3) + { + // consider ret to be a list of tuples [(i,j,[val])] + auto kMatrix = r.unchecked(); + for (auto x = 0 ; x < kMatrix.shape(0) ; ++x) + { + dfdx(int(size_t(kMatrix(x,0))), int(size_t(kMatrix(x,1)))) += kMatrix(x,2); + } + } + else + { + throw py::type_error("Can't read return value of `buildStiffnessMatrix`. The method should return either a plain 2D matrix or a vector of tuples (i, j, val)"); + } + } + + template void declare_forcefield(py::module &m) { const std::string pyclass_name = std::string("ForceField") + TDOFType::Name(); py::class_, BaseComponent, ForceField_Trampoline, py_shared_ptr>> f(m, pyclass_name.c_str(), py::dynamic_attr(), py::multiple_inheritance(), sofapython3::doc::forceField::forceFieldClass); diff --git a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h index 3d34f3f9..d2f4f6d7 100644 --- a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h +++ b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h @@ -50,8 +50,13 @@ class ForceField_Trampoline : public sofa::core::behavior::ForceField pybind11::object _addKToMatrix(const sofa::core::MechanicalParams* mparams, int nNodes, int nDofs); void addKToMatrix(const sofa::core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* dfId) override; + void buildStiffnessMatrix(sofa::core::behavior::StiffnessMatrix* matrix) override; + SReal getPotentialEnergy(const sofa::core::MechanicalParams* /*mparams*/, const DataVecCoord& /*x*/) const override { return 0.0; } +private: + + pybind11::object _buildStiffnessMatrix(int nIndices, int nDofs); }; void moduleAddForceField(pybind11::module &m); diff --git a/examples/example-forcefield.py b/examples/example-forcefield.py index 5eea4d65..9f6f0800 100644 --- a/examples/example-forcefield.py +++ b/examples/example-forcefield.py @@ -17,7 +17,7 @@ def init(self): def addForce(self, m, out_force, pos, vel): with out_force.writeableArray() as wa: - wa[:] += ( (self.initpos-pos.value) * self.ks.value ) + wa[:] += ( (self.initpos-pos.value) * self.ks.value ) def addDForce(self, params, df, dx): kFactor = params['kFactor'] @@ -32,6 +32,12 @@ def addKToMatrix(self, params, number_of_nodes, number_of_dofs_per_node): K.append([number_of_dofs_per_node * n + d, number_of_dofs_per_node * n + d, -kFactor * self.ks.value]) return np.asarray(K) + def buildStiffnessMatrix(self, number_of_nodes, number_of_dofs_per_node): + K = [] + for n in range(0, number_of_nodes): + for d in range(0, number_of_dofs_per_node): + K.append([number_of_dofs_per_node * n + d, number_of_dofs_per_node * n + d, -self.ks.value]) + return np.asarray(K) def createScene(root):