Simplifies GetSoonestContact & exports it for easier testability

Library/include/playrho/StepStats.hpp

@@ -84,7 +84,6 @@ struct ToiStepStats {
     counter_type proxiesMoved = 0; ///< Proxies moved count.
     counter_type sumPosIters = 0; ///< Sum position iterations count.
     counter_type sumVelIters = 0; ///< Sum velocity iterations count.
-    counter_type maxSimulContacts = 0; ///< Max contacts occurring simultaneously.
 
     /// @brief Distance iteration type.
     using dist_iter_type = std::remove_const_t<decltype(DefaultMaxDistanceIters)>;

Library/include/playrho/d2/AabbTreeWorld.hpp

@@ -108,7 +108,7 @@ using ManifoldContactListener = std::function<void(ContactID, const Manifold&)>;
 
 /// @brief Impulses contact listener.
 using ImpulsesContactListener =
-    std::function<void(ContactID, const ContactImpulsesList&, unsigned)>;
+std::function<void(ContactID, const ContactImpulsesList&, unsigned)>;
 
 /// @name AabbTreeWorld Listener Non-Member Functions
 /// @{
@@ -643,7 +643,7 @@ class AabbTreeWorld {
     /// @return Island solver results.
     ///
     IslandStats SolveRegIslandViaGS(const StepConf& conf, const Island& island);
-    
+
     /// @brief Adds to the island based off of a given "seed" body.
     /// @post Contacts are listed in the island in the order that bodies provide those contacts.
     /// @post Joints are listed the island in the order that bodies provide those joints.
@@ -678,17 +678,14 @@ class AabbTreeWorld {
     ToiStepStats SolveToi(const StepConf& conf);
 
     /// @brief Solves collisions for the given time of impact.
-    ///
     /// @param contactID Identifier of contact to solve for.
     /// @param conf Time step configuration to solve for.
-    ///
     /// @pre The identified contact has a valid TOI, is enabled, is active, and is impenetrable.
     /// @pre The identified contact is **not** a sensor.
     /// @pre There is no contact having a lower TOI in this time step that has
     ///   not already been solved for.
     /// @pre There is not a lower TOI in the time step for which collisions have
     ///   not already been processed.
-    ///
     IslandStats SolveToi(ContactID contactID, const StepConf& conf);
 
     /// @brief Solves the time of impact for bodies 0 and 1 of the given island.
@@ -810,7 +807,7 @@ class AabbTreeWorld {
     /// contact listener as its argument.
     /// Essentially this really just purges contacts that are no longer relevant.
     DestroyContactsStats DestroyContacts(KeyedContactIDs& contacts);
-    
+
     /// @brief Update contacts.
     UpdateContactsStats UpdateContacts(const StepConf& conf);
 
@@ -912,7 +909,7 @@ class AabbTreeWorld {
     BodyIDs m_bodies; ///< Body collection.
 
     JointIDs m_joints; ///< Joint collection.
-    
+
     /// @brief Container of contacts.
     /// @note In the <em>add pair</em> stress-test, 401 bodies can have some 31000 contacts
     ///   during a given time step.
@@ -928,7 +925,7 @@ class AabbTreeWorld {
     Listeners m_listeners;
 
     FlagsType m_flags = e_stepComplete; ///< Flags.
-    
+
     /// Inverse delta-t from previous step.
     /// @details Used to compute time step ratio to support a variable time step.
     /// @see Step.
@@ -1057,6 +1054,13 @@ inline void SetPostSolveContactListener(AabbTreeWorld& world, ImpulsesContactLis
     world.m_listeners.postSolveContact = std::move(listener);
 }
 
+/// @brief Gets the identifier of the contact with the lowest time of impact.
+/// @details This finds the contact with the lowest (soonest) time of impact that's under one
+///   and returns its identifier.
+/// @return Identifier of contact with the least time of impact under 1, or invalid contact ID.
+ContactID GetSoonestContact(const Span<const KeyedContactID>& ids,
+                            const Span<const Contact>& contacts) noexcept;
+
 } // namespace d2
 } // namespace playrho
 

Library/source/playrho/d2/AabbTreeWorld.cpp

@@ -662,49 +662,6 @@ RemoveUnspeedablesFromIslanded(const Span<const BodyID>& bodies,
     return numRemoved;
 }
 
-/// @brief Contact TOI data.
-struct ContactToiData
-{
-    ContactID contact = InvalidContactID; ///< Contact for which the time of impact is relevant.
-    ZeroToUnderOneFF<Real> toi; ///< Time of impact (TOI) as a fractional value 0 to under 1.
-    ContactCounter simultaneous = 0; ///< Count of simultaneous contacts at this TOI.
-};
-
-/// @brief Gets the soonest contact.
-/// @details This finds the contact with the lowest (soonest) time of impact.
-/// @return Contact with the least time of impact and its time of impact, or null contact.
-///  A non-null contact will be enabled, not have sensors, be active, and impenetrable.
-ContactToiData GetSoonestContact(const KeyedContactIDs& contacts,
-                                 const ObjectPool<Contact>& buffer) noexcept
-{
-    auto minToi = ZeroToUnderOneFF<Real>{nextafter(Real{1}, Real{0})};
-    auto found = InvalidContactID;
-    auto count = ContactCounter{0};
-    for (const auto& contact: contacts)
-    {
-        const auto contactID = std::get<ContactID>(contact);
-        const auto& c = buffer[to_underlying(contactID)];
-        if (const auto toi = c.GetToi())
-        {
-            if (minToi > *toi)
-            {
-                minToi = ZeroToUnderOneFF<Real>(*toi);
-                found = contactID;
-                count = 1;
-            }
-            else if (minToi == *toi)
-            {
-                // Have multiple contacts at the current minimum time of impact.
-                if (found == InvalidContactID) {
-                    found = contactID;
-                }
-                ++count;
-            }
-        }
-    }
-    return ContactToiData{found, minToi, count};
-}
-
 auto FindContacts(pmr::memory_resource& resource,
                   const DynamicTree& tree,
                   ProxyIDs&& proxies)
@@ -1663,18 +1620,16 @@ ToiStepStats AabbTreeWorld::SolveToi(const StepConf& conf)
         stats.maxToiIters = std::max(stats.maxToiIters, updateData.maxToiIters);
 
         const auto next = GetSoonestContact(m_contacts, m_contactBuffer);
-        if (next.contact == InvalidContactID) {
+        if (next == InvalidContactID) {
             // No more TOI events to handle within the current time step. Done!
             m_flags |= e_stepComplete;
             break;
         }
 
-        stats.maxSimulContacts = std::max(stats.maxSimulContacts,
-                                          static_cast<decltype(stats.maxSimulContacts)>(next.simultaneous));
-        stats.contactsFound += next.simultaneous;
+        ++stats.contactsFound;
         auto islandsFound = 0u;
-        if (!m_islanded.contacts[to_underlying(next.contact)]) {
-            const auto solverResults = SolveToi(next.contact, conf);
+        if (!m_islanded.contacts[to_underlying(next)]) {
+            const auto solverResults = SolveToi(next, conf);
             stats.minSeparation = std::min(stats.minSeparation, solverResults.minSeparation);
             stats.maxIncImpulse = std::max(stats.maxIncImpulse, solverResults.maxIncImpulse);
             stats.islandsSolved += solverResults.solved;
@@ -1723,7 +1678,6 @@ IslandStats AabbTreeWorld::SolveToi(ContactID contactID, const StepConf& conf)
     // Note:
     //   This function is what used to be b2World::SolveToi(const b2TimeStep& step).
     //   It also differs internally from Erin's implementation.
-    //
     //   Here's some specific behavioral differences:
     //   1. Bodies don't get their under-active times reset (like they do in Erin's code).
 
@@ -1734,7 +1688,7 @@ IslandStats AabbTreeWorld::SolveToi(ContactID contactID, const StepConf& conf)
 
     /*
      * Confirm that contact is as it's supposed to be according to contract of the
-     * GetSoonestContacts function from which this contact should have been obtained.
+     * GetSoonestContact function from which this contact should have been obtained.
      */
     assert(IsEnabled(contact));
     assert(!IsSensor(contact));
@@ -2782,5 +2736,26 @@ const Manifold& GetManifold(const AabbTreeWorld& world, ContactID id)
     return world.m_manifoldBuffer.at(to_underlying(id));
 }
 
+ContactID GetSoonestContact(const Span<const KeyedContactID>& ids,
+                            const Span<const Contact>& contacts) noexcept
+{
+    auto found = InvalidContactID;
+    auto minToi = UnitIntervalFF<Real>{Real(1)};
+    for (const auto& id: ids)
+    {
+        const auto contactID = std::get<ContactID>(id);
+        assert(to_underlying(contactID) < contacts.size());
+        const auto& c = contacts[to_underlying(contactID)];
+        if (const auto toi = c.GetToi())
+        {
+            if (minToi > *toi) {
+                minToi = *toi;
+                found = contactID;
+            }
+        }
+    }
+    return found;
+}
+
 } // namespace d2
 } // namespace playrho

Testbed/Framework/Test.cpp

@@ -630,7 +630,6 @@ void ShowStats(const StepConf& stepConf, UiState& ui, const World& world, const
         stream << ", max-dist-iter=" << unsigned{stats.m_maxDistIters} << "/" << unsigned{stepConf.maxDistanceIters};
         stream << ", max-toi-iter=" << unsigned{stats.m_maxToiIters} << "/" << unsigned{stepConf.maxToiIters};
         stream << ", max-root-iter=" << unsigned{stats.m_maxRootIters} << "/" << unsigned{stepConf.maxToiRootIters};
-        stream << ", max-simul-cts=" << stats.m_maxSimulContacts;
         stream << ".";
         ImGui::TextUnformatted(stream.str());
         ImGui::NextColumn();
@@ -1198,8 +1197,6 @@ void Test::Step(const Settings& settings, Drawer& drawer, UiState& ui)
     m_stats.m_sumToiContactsSkippedTouching += stepStats.toi.contactsSkippedTouching;
     m_stats.m_sumContactsAtMaxSubSteps += stepStats.toi.contactsAtMaxSubSteps;
 
-    m_stats.m_maxSimulContacts = std::max(m_stats.m_maxSimulContacts,
-                                          stepStats.toi.maxSimulContacts);
     m_stats.m_maxDistIters = std::max(m_stats.m_maxDistIters, stepStats.toi.maxDistIters);
     m_stats.m_maxRootIters = std::max(m_stats.m_maxRootIters, stepStats.toi.maxRootIters);
     m_stats.m_maxToiIters = std::max(m_stats.m_maxToiIters, stepStats.toi.maxToiIters);

Testbed/Framework/Test.hpp

@@ -166,8 +166,6 @@ struct Stats
     Length m_maxRegSep = -std::numeric_limits<Length>::infinity();
     Length m_minToiSep = 0;
 
-    std::uint32_t m_maxSimulContacts = 0;
-
     using dist_iter_type = std::remove_const_t<decltype(DefaultMaxDistanceIters)>;
     using toi_iter_type = std::remove_const_t<decltype(DefaultMaxToiIters)>;
     using root_iter_type = std::remove_const_t<decltype(DefaultMaxToiRootIters)>;

UnitTests/AabbTreeWorld.cpp

@@ -388,7 +388,6 @@ TEST(AabbTreeWorld, CreateDestroyContactingBodies)
     EXPECT_EQ(stats0.toi.proxiesMoved, static_cast<decltype(stats0.toi.proxiesMoved)>(0));
     EXPECT_EQ(stats0.toi.sumPosIters, static_cast<decltype(stats0.toi.sumPosIters)>(0));
     EXPECT_EQ(stats0.toi.sumVelIters, static_cast<decltype(stats0.toi.sumVelIters)>(0));
-    EXPECT_EQ(stats0.toi.maxSimulContacts, static_cast<decltype(stats0.toi.maxSimulContacts)>(0));
     EXPECT_EQ(stats0.toi.maxDistIters, static_cast<decltype(stats0.toi.maxDistIters)>(0));
     EXPECT_EQ(stats0.toi.maxToiIters, static_cast<decltype(stats0.toi.maxToiIters)>(0));
     EXPECT_EQ(stats0.toi.maxRootIters, static_cast<decltype(stats0.toi.maxRootIters)>(0));
@@ -1033,3 +1032,27 @@ TEST(Templates, EraseFirst)
     EXPECT_EQ(size(container), 2u);
     EXPECT_EQ(container, (std::vector<int>{0, 2}));
 }
+
+TEST(AabbTreeWorld, GetSoonestContact)
+{
+    auto ids = std::vector<KeyedContactID>{};
+    auto contacts = std::vector<Contact>{};
+    EXPECT_EQ(GetSoonestContact(ids, contacts), InvalidContactID);
+    auto c = Contact{};
+    contacts.push_back(c);
+    EXPECT_EQ(GetSoonestContact(ids, contacts), InvalidContactID);
+    ids.emplace_back(ContactKey(), ContactID(0));
+    EXPECT_EQ(GetSoonestContact(ids, contacts), InvalidContactID);
+    c.SetToi(Real(0.5));
+    contacts.push_back(c);
+    ids.emplace_back(ContactKey(), ContactID(1));
+    EXPECT_EQ(GetSoonestContact(ids, contacts), ContactID(1));
+    c.SetToi(Real(0.2));
+    contacts.push_back(c);
+    ids.emplace_back(ContactKey(), ContactID(2));
+    EXPECT_EQ(GetSoonestContact(ids, contacts), ContactID(2));
+    c.SetToi(Real(0.6));
+    contacts.push_back(c);
+    ids.emplace_back(ContactKey(), ContactID(3));
+    EXPECT_EQ(GetSoonestContact(ids, contacts), ContactID(2));
+}

UnitTests/StepStats.cpp

@@ -24,52 +24,6 @@
 
 using namespace playrho;
 
-TEST(StepStats, PreStatsByteSize)
-{
-    switch (sizeof(Real))
-    {
-        case  4: EXPECT_EQ(sizeof(PreStepStats), std::size_t(24)); break;
-        case  8: EXPECT_EQ(sizeof(PreStepStats), std::size_t(24)); break;
-        case 16: EXPECT_EQ(sizeof(PreStepStats), std::size_t(24)); break;
-        default: FAIL(); break;
-    }
-}
-
-TEST(StepStats, RegStatsByteSize)
-{
-    switch (sizeof(Real))
-    {
-        case  4: EXPECT_EQ(sizeof(RegStepStats), std::size_t(32)); break;
-        case  8: EXPECT_EQ(sizeof(RegStepStats), std::size_t(40)); break;
-        case 16: EXPECT_EQ(sizeof(RegStepStats), std::size_t(64)); break;
-        default: FAIL(); break;
-    }
-}
-
-TEST(StepStats, ToiStatsByteSize)
-{
-    switch (sizeof(Real))
-    {
-        case  4: EXPECT_EQ(sizeof(ToiStepStats), std::size_t(60)); break;
-        case  8: EXPECT_EQ(sizeof(ToiStepStats), std::size_t(72)); break;
-        case 16: EXPECT_EQ(sizeof(ToiStepStats), std::size_t(96)); break;
-        default: FAIL(); break;
-    }
-}
-
-TEST(StepStats, ByteSize)
-{
-    // Check size at test runtime instead of compile-time via static_assert to avoid stopping
-    // builds and to report actual size rather than just reporting that expected size is wrong.
-    switch (sizeof(Real))
-    {
-        case  4: EXPECT_EQ(sizeof(StepStats), std::size_t(116)); break;
-        case  8: EXPECT_EQ(sizeof(StepStats), std::size_t(136)); break;
-        case 16: EXPECT_EQ(sizeof(StepStats), std::size_t(192)); break;
-        default: FAIL(); break;
-    }
-}
-
 TEST(StepStats, Traits)
 {
     EXPECT_TRUE(std::is_default_constructible_v<StepStats>);

UnitTests/World.cpp

@@ -654,7 +654,6 @@ TEST(World, CreateDestroyContactingBodies)
     EXPECT_EQ(stats0.toi.proxiesMoved, static_cast<decltype(stats0.toi.proxiesMoved)>(0));
     EXPECT_EQ(stats0.toi.sumPosIters, static_cast<decltype(stats0.toi.sumPosIters)>(0));
     EXPECT_EQ(stats0.toi.sumVelIters, static_cast<decltype(stats0.toi.sumVelIters)>(0));
-    EXPECT_EQ(stats0.toi.maxSimulContacts, static_cast<decltype(stats0.toi.maxSimulContacts)>(0));
     EXPECT_EQ(stats0.toi.maxDistIters, static_cast<decltype(stats0.toi.maxDistIters)>(0));
     EXPECT_EQ(stats0.toi.maxToiIters, static_cast<decltype(stats0.toi.maxToiIters)>(0));
     EXPECT_EQ(stats0.toi.maxRootIters, static_cast<decltype(stats0.toi.maxRootIters)>(0));