WIP support for recreating worlds from parts

Library/include/playrho/d2/AabbTreeWorld.hpp

@@ -912,19 +912,16 @@ class AabbTreeWorld {
                                Real multiplier, Length extension);
 
     /// @brief Updates the touching related state and notifies listener (if one given).
-    ///
     /// @note Ideally this function is only called when a dependent change has occurred.
     /// @note Touching related state depends on the following data:
     ///   - The fixtures' sensor states.
     ///   - The fixtures bodies' transformations.
     ///   - The <code>maxCirclesRatio</code> per-step configuration state *OR* the
     ///     <code>maxDistanceIters</code> per-step configuration state.
-    ///
     /// @param id Identifies the contact to update.
     /// @param conf Per-step configuration information.
-    ///
+    /// @pre The identified contact needs updating.
     /// @see GetManifold, IsTouching
-    ///
     void Update(ContactID id, const ContactUpdateConf& conf);
 
     /******** Member variables. ********/

Library/source/playrho/d2/AabbTreeWorld.cpp

@@ -711,8 +711,9 @@ auto FindContacts(pmr::memory_resource& resource,
     // to eliminate any node pairs that have the same body here before the key pairs are
     // sorted.
     for_each(cbegin(proxies), cend(proxies), [&](DynamicTree::Size pid) {
-        const auto leaf0 = tree.GetLeafData(pid);
-        const auto aabb = tree.GetAABB(pid);
+        const auto &node = tree.GetNode(pid);
+        const auto aabb = node.GetAABB();
+        const auto leaf0 = node.AsLeaf();
         Query(tree, aabb, [pid,leaf0,&proxyKeys,&tree](DynamicTree::Size nodeId) {
             const auto leaf1 = tree.GetLeafData(nodeId);
             // A proxy cannot form a pair with itself.
@@ -1662,20 +1663,16 @@ AabbTreeWorld::UpdateContactTOIs(const StepConf& conf)
     auto results = UpdateContactsData{};
 
     const auto toiConf = GetToiConf(conf);
-    for (const auto& contact: m_contacts)
-    {
+    for (const auto& contact: m_contacts) {
         auto& c = m_contactBuffer[to_underlying(std::get<ContactID>(contact))];
-        if (HasValidToi(c))
-        {
+        if (HasValidToi(c)) {
             ++results.numValidTOI;
             continue;
         }
-        if (!IsEnabled(c) || IsSensor(c) || !IsImpenetrable(c))
-        {
+        if (!IsEnabled(c) || IsSensor(c) || !IsImpenetrable(c)) {
             continue;
         }
-        if (GetToiCount(c) >= conf.maxSubSteps)
-        {
+        if (GetToiCount(c) >= conf.maxSubSteps) {
             // What are the pros/cons of this?
             // Larger m_maxSubSteps slows down the simulation.
             // m_maxSubSteps of 44 and higher seems to decrease the occurrance of tunneling
@@ -1840,15 +1837,20 @@ IslandStats AabbTreeWorld::SolveToi(ContactID contactID, const StepConf& conf)
         // Advance the bodies to the TOI.
         assert((toi != Real(0)) || ((GetSweep(bA).alpha0 == Real(0)) && (GetSweep(bB).alpha0 == Real(0))));
         Advance(bA, toi);
+        if (GetPosition0(bA) != backupA.pos0 || GetPosition1(bA) != backupA.pos1) {
+            FlagForUpdating(m_contactBuffer, m_bodyContacts[to_underlying(bodyIdA)]);
+        }
         Advance(bB, toi);
-        FlagForUpdating(m_contactBuffer, m_bodyContacts[to_underlying(bodyIdA)]);
-        FlagForUpdating(m_contactBuffer, m_bodyContacts[to_underlying(bodyIdB)]);
+        if (GetPosition0(bB) != backupB.pos0 || GetPosition1(bB) != backupB.pos1) {
+            FlagForUpdating(m_contactBuffer, m_bodyContacts[to_underlying(bodyIdB)]);
+        }
 
         // The TOI contact likely has some new contact points.
         SetEnabled(contact);
-        assert(contact.NeedsUpdating());
-        Update(contactID, GetUpdateConf(conf));
-        ++numUpdated;
+        if (contact.NeedsUpdating()) {
+            Update(contactID, GetUpdateConf(conf));
+            ++numUpdated;
+        }
 
         SetToi(contact, {});
         contact.IncrementToiCount();
@@ -2055,7 +2057,9 @@ AabbTreeWorld::ProcessContactsForTOI( // NOLINT(readability-function-cognitive-c
                         const auto backup = GetSweep(other);
                         if (!otherIslanded /* && GetSweep(other).alpha0 != toi */) {
                             Advance(other, toi);
-                            FlagForUpdating(m_contactBuffer, m_bodyContacts[to_underlying(otherId)]);
+                            if (GetPosition0(other) != backup.pos0 || GetPosition1(other) != backup.pos1) {
+                                FlagForUpdating(m_contactBuffer, m_bodyContacts[to_underlying(otherId)]);
+                            }
                         }
 
                         // Update the contact points
@@ -2159,7 +2163,6 @@ StepStats Step(AabbTreeWorld& world, const StepConf& conf)
             stepStats.pre.contactsSkipped = updateStats.skipped;
         }
 
-
         // For any new fixtures added: need to find and create the new contacts.
         // Note: this may update bodies (in addition to the contacts container).
         stepStats.pre.contactsAdded = world.AddContacts(
@@ -2168,7 +2171,6 @@ StepStats Step(AabbTreeWorld& world, const StepConf& conf)
 
         if (conf.deltaTime != 0_s) {
             world.m_inv_dt0 = Real(1) / conf.deltaTime;
-
             // Integrate velocities, solve velocity constraints, and integrate positions.
             if (IsStepComplete(world)) {
                 stepStats.reg = world.SolveReg(conf);
@@ -2240,6 +2242,7 @@ void AabbTreeWorld::InternalDestroy(ContactID contactID, const Body* from)
         SetAwake(*bodyB);
     }
     m_contactBuffer.Free(to_underlying(contactID));
+    contact.SetDestroyed();
     m_manifoldBuffer.Free(to_underlying(contactID));
 }
 
@@ -2477,6 +2480,7 @@ AabbTreeWorld::AddContacts( // NOLINT(readability-function-cognitive-complexity)
         m_islanded.contacts.resize(size(m_contactBuffer));
         m_manifoldBuffer.Allocate();
         auto& contact = m_contactBuffer[to_underlying(contactID)];
+        contact.UnsetDestroyed();
         if (IsImpenetrable(bodyA) || IsImpenetrable(bodyB)) {
             SetImpenetrable(contact);
         }
@@ -2566,6 +2570,7 @@ void AabbTreeWorld::Update( // NOLINT(readability-function-cognitive-complexity)
     ContactID contactID, const ContactUpdateConf& conf)
 {
     auto& c = m_contactBuffer[to_underlying(contactID)];
+    assert(c.NeedsUpdating());
     auto& manifold = m_manifoldBuffer[to_underlying(contactID)];
     const auto oldManifold = manifold;
 

UnitTests/AabbTreeWorld.cpp

@@ -588,8 +588,9 @@ TEST(AabbTreeWorld, SetContact)
     step.deltaTime = {};
     Step(world, step);
     ASSERT_EQ(GetContactRange(world), 1u);
+    const auto originalContact = GetContact(world, ContactID(0));
     auto cB = Contactable{bodyId1, ShapeID(0), 0u};
-    auto contact0 = Contact{cA, cB};
+    auto contact0 = originalContact;
     contact0.UnsetImpenetrable();
     EXPECT_THROW(SetContact(world, ContactID(0), contact0), InvalidArgument);
     contact0.SetImpenetrable();