[Bindings] Utility bindings for ContactListener and Simulation (#221)

* Added ContactListener bindings and utility bindings. Added step function to simualation to animate n steps in one call.

* Changed function name for coherency between the C++ function and the python binding.

Author: ScheiklP

bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ContactListener.cpp

@@ -39,15 +39,67 @@ using sofa::core::collision::ContactListener;
 namespace sofapython3
 {
 
+    pybind11::dict getContactData(const ContactListener& listener){
+        pybind11::dict contactData;
+        const sofa::Size numberOfContacts = listener.getNumberOfContacts();
+        contactData["numberOfContacts"] = numberOfContacts;
+
+        const std::vector<std::tuple<unsigned int, unsigned int, unsigned int, unsigned int>> contactElements = listener.getContactElements();
+        const std::vector<std::tuple<unsigned int, sofa::type::Vector3, unsigned int, sofa::type::Vector3>> contactPoints = listener.getContactPoints();
+
+        std::vector<unsigned int> collisionElementsModel1;
+        std::vector<unsigned int> collisionElementsModel2;
+        collisionElementsModel1.reserve(numberOfContacts);
+        collisionElementsModel2.reserve(numberOfContacts);
+
+        std::vector<sofa::type::Vector3> collisionPointsModel1;
+        std::vector<sofa::type::Vector3> collisionPointsModel2;
+        collisionPointsModel1.reserve(numberOfContacts);
+        collisionPointsModel2.reserve(numberOfContacts);
+
+        for (unsigned int i = 0; i < numberOfContacts; i++){
+
+            if(std::get<0>(contactElements[i]) == 0){
+                collisionElementsModel1.emplace_back(std::get<1>(contactElements[i]));
+                collisionElementsModel2.emplace_back(std::get<3>(contactElements[i]));
+            }
+            else{
+                collisionElementsModel1.emplace_back(std::get<3>(contactElements[i]));
+                collisionElementsModel2.emplace_back(std::get<1>(contactElements[i]));
+            }
+
+            if(std::get<0>(contactPoints[i]) == 0){
+                collisionPointsModel1.emplace_back(std::get<1>(contactPoints[i]));
+                collisionPointsModel2.emplace_back(std::get<3>(contactPoints[i]));
+            }
+            else{
+                collisionPointsModel1.emplace_back(std::get<3>(contactPoints[i]));
+                collisionPointsModel2.emplace_back(std::get<1>(contactPoints[i]));
+            }
+        }
+
+        contactData["collisionPointsModel1"] = collisionPointsModel1;
+        contactData["collisionPointsModel2"] = collisionPointsModel2;
+
+        contactData["collisionElementsModel1"] = collisionElementsModel1;
+        contactData["collisionElementsModel2"]= collisionElementsModel2;
+
+        return contactData;
+    }
+
+
 void moduleAddContactListener(pybind11::module &m)
 {
     /// register the ContactListener binding in the pybind11 typing sytem
     pybind11::class_<ContactListener,
                sofa::core::objectmodel::BaseObject,
                py_shared_ptr<ContactListener>> c(m, "ContactListener", sofapython3::doc::contactListener::contactListenerClass);
 
-    /// Commented out until feature is in SOFA master
-    /* c.def("getNumberOfContacts", &ContactListener::getNumberOfContacts); */
+    c.def("getNumberOfContacts", &ContactListener::getNumberOfContacts);
+    c.def("getContactData", &getContactData);
+    c.def("getDistances", &sofa::core::collision::ContactListener::getDistances);
+    c.def("getContactPoints", &sofa::core::collision::ContactListener::getContactPoints);
+    c.def("getContactElements", &sofa::core::collision::ContactListener::getContactElements);
 
     /// register the ContactListener binding in the downcasting subsystem
     PythonFactory::registerType<ContactListener>([](sofa::core::objectmodel::Base* object)

bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ContactListener_doc.h

@@ -43,6 +43,7 @@ static auto contactListenerClass =
                 collisionModel2=model2.getLinkPath(),
             )
             print(listener.getNumberOfContacts())
+            print(listener.getDistances())
         )";
 
 } // namespace sofapython3::doc::contactListener

bindings/Sofa/src/SofaPython3/Sofa/Simulation/Submodule_Simulation.cpp

@@ -69,6 +69,12 @@ PYBIND11_MODULE(Simulation, simulation)
         return node ? py::cast(node.get()) : py::none();
     }, sofapython3::doc::simulation::load);
     simulation.def("unload", [](Node* n){ sofa::simulation::getSimulation()->unload(n); }, sofapython3::doc::simulation::unload);
+    simulation.def("animateNSteps", [](Node *n, int n_steps, SReal dt=0.0){
+        for (int i = 0; i < n_steps; i++)
+        {
+            sofa::simulation::getSimulation()->animate(n, dt); //Execute one timestep. If dt is 0, the dt parameter in the graph will be used
+        }
+    }, sofapython3::doc::simulation::animateNSteps, py::arg("root_node"), py::arg("n_steps") = 1, py::arg("dt") = 0.0);
     simulation.def("updateVisual", [](Node* n){ sofa::simulation::getSimulation()->updateVisual(n); });
     simulation.def("initTextures", [](Node* n)
     {

bindings/Sofa/src/SofaPython3/Sofa/Simulation/Submodule_Simulation_doc.h

@@ -51,6 +51,7 @@ static auto Class =
             Sofa.Simulation.reset
             Sofa.Simulation.load
             Sofa.Simulation.unload
+            Sofa.Simulation.animateNSteps
         )";
 static auto print =
         R"(
@@ -85,4 +86,12 @@ static auto unload =
         R"(
         Unload a scene from a Node.
         )";
+
+static auto animateNSteps =
+        R"(
+        Advance the simulation by one time step (by default).
+
+        n_steps: specifies number of steps to advance the simulation by. The simulation visual is not updated between steps. Default is 1.
+        dt: specifies the time delta between two simulation steps. Default is 0 -> uses the dt from the simulation node.
+        )";
 }