Implement ContactPhaseList::getPresentPhase() method

CHANGELOG.md

@@ -4,6 +4,7 @@ All notable changes to this project are documented in this file.
 ## [Unreleased]
 ### Added
 - Implement `AdvanceableRunner::isRunning()` method (https://github.com/dic-iit/bipedal-locomotion-framework/pull/395)
+- Implement `ContactPhaseList::getPresentPhase()` method (https://github.com/dic-iit/bipedal-locomotion-framework/pull/396)
 
 ### Changed
 

src/Contacts/include/BipedalLocomotion/Contacts/ContactPhase.h

@@ -21,6 +21,12 @@ namespace Contacts
 /**
  * @brief Struct defining a contact phase.
  * Each phase is characterized by a set of contacts which remain active for the entirety of the phase.
+ * @note Mathematically speaking the interval of the phase is defined as following
+ * \f[
+ * I = [t_b \; t_e)
+ * \f]
+ * where \f$t_b\f$ is the ContactPhase::beginTime and \f$t_e\f$ is the ContactPhase::endTime.
+ * The end time is not included in the phase.
  */
 struct ContactPhase
 {

src/Contacts/include/BipedalLocomotion/Contacts/ContactPhaseList.h

@@ -105,6 +105,17 @@ class ContactPhaseList
      */
     bool setLists(const std::initializer_list<ContactList>& contactLists);
 
+    /**
+     * @brief Get the phase given the time.
+     *
+     * It returns the contact phase with the highest begin time lower than time.
+     * If no contacts phase has a begin time lower than time, it returns an iterator to the end.
+     * @param time The present time.
+     * @return an iterator to the last phase having an activation time lower than time.
+     * If no phase satisfies this condition, it returns a pointer to the end.
+     */
+    const_iterator getPresentPhase(double time) const;
+
     /**
      * @brief A reference to the lists stored in this class.
      * @warning All the iterators stored inside the contact phases refer to the lists viewable via this method.

src/Contacts/src/ContactPhaseList.cpp

@@ -140,6 +140,25 @@ bool ContactPhaseList::setLists(const std::initializer_list<ContactList>& contac
     return true;
 }
 
+ContactPhaseList::const_iterator ContactPhaseList::getPresentPhase(double time) const
+{
+    // With the reverse iterator we find the last phase such that the begin time is smaller that
+    // time
+    auto presentReverse = std::find_if(rbegin(), rend(), [time](const ContactPhase& a) -> bool {
+        return a.beginTime <= time;
+    });
+
+    if (presentReverse == rend())
+    {
+        // No phase has begin time lower than the specified time.
+        return end();
+    }
+
+    // This is to convert a reverse iterator to a forward iterator. The -- is because base() returns
+    // a forward iterator to the next element.
+    return --(presentReverse.base());
+}
+
 const BipedalLocomotion::Contacts::ContactListMap&
 BipedalLocomotion::Contacts::ContactPhaseList::lists() const
 {

src/Contacts/tests/Contacts/ContactPhaseListTest.cpp

@@ -100,6 +100,25 @@ TEST_CASE("ContactPhaseList")
 
     REQUIRE(phaseList.setLists({contactListAdditional, contactListLeft, contactListRight}));
 
+    SECTION("Present phase")
+    {
+        auto it = phaseList.begin();
+        std::advance(it, 1);
+        bool same = phaseList.getPresentPhase(it->beginTime) == it;
+        REQUIRE(same);
+
+        std::advance(it, 1);
+        same = phaseList.getPresentPhase((it->beginTime + it->endTime) / 2.0) == it;
+        REQUIRE(same);
+
+        std::advance(it, 1);
+
+        // the interval of a phase is defined as t = [t_begin, t_end) (i.e. t_end is not included)
+        same = phaseList.getPresentPhase(it->endTime) == std::next(it, 1);
+        REQUIRE(same);
+    }
+
+
     SECTION("Check phases")
     {
         REQUIRE(phaseList.size() == 8);