Fix exception in DestroyContacts (#5619)

RELEASE-NOTES.md

@@ -43,7 +43,7 @@ END TEMPLATE-->
 
 ### Bugfixes
 
-*None yet*
+* Fixed an exception in `PhysicsSystem.DestroyContacts()` that could result in entities getting stuck with broken physics.
 
 ### Other
 

Robust.Shared/Physics/Dynamics/Contacts/Contact.cs

@@ -406,5 +406,10 @@ internal enum ContactFlags : byte
         /// Set right before the contact is deleted
         /// </summary>
         Deleting = 1 << 4,
+
+        /// <summary>
+        /// Set after a contact has been deleted and returned to the contact pool.
+        /// </summary>
+        Deleted = 1 << 5,
     }
 }

Robust.Shared/Physics/Systems/SharedPhysicsSystem.Components.cs

@@ -24,6 +24,7 @@
 
 using System;
 using System.Collections.Generic;
+using System.Linq;
 using System.Numerics;
 using Robust.Shared.Collections;
 using Robust.Shared.GameObjects;
@@ -32,6 +33,7 @@
 using Robust.Shared.Maths;
 using Robust.Shared.Physics.Components;
 using Robust.Shared.Physics.Dynamics;
+using Robust.Shared.Physics.Dynamics.Contacts;
 using Robust.Shared.Physics.Events;
 using Robust.Shared.Utility;
 
@@ -242,27 +244,31 @@ public void ApplyLinearImpulse(EntityUid uid, Vector2 impulse, Vector2 point, Fi
 
     public void DestroyContacts(PhysicsComponent body)
     {
-        if (body.Contacts.Count == 0) return;
+        if (body.Contacts.Count == 0)
+            return;
 
-        // This variable is only used in edge-case scenario when contact flagged Deleting raises
-        // EndCollideEvent which will QueueDelete contact's entity
-        ushort contactsFlaggedDeleting = 0;
         var node = body.Contacts.First;
 
         while (node != null)
         {
             var contact = node.Value;
-            node = node.Next;
 
-            // Destroy last so the linked-list doesn't get touched.
-            if (!DestroyContact(contact))
-            {
-                contactsFlaggedDeleting++;
-            }
+            // The Start/End collide events can result in other contacts in this list being destroyed, and maybe being
+            // created elsewhere. We want to ensure that the "next" node from a previous iteration wasn't somehow
+            // destroyed, returned to the pool, and then re-assigned to a new body.
+            // AFAIK this shouldn't be possible anymore, now that the next node is returned by DestroyContacts() after
+            // all events were raised.
+            DebugTools.Assert(contact.BodyA == body || contact.BodyB == body || contact.Flags == ContactFlags.Deleted);
+            DebugTools.AssertNotEqual(node, node.Next);
+
+            DestroyContact(contact, node, out var next);
+            DebugTools.AssertNotEqual(node, next);
+            node = next;
         }
 
-        // This contact will be deleted before SimulateWorld runs since it is already set to be Deleted
-        DebugTools.Assert(body.Contacts.Count == contactsFlaggedDeleting);
+        // It is possible that this DestroyContacts was called while another DestroyContacts was still being processed.
+        // The only remaining contacts should be those that are still getting deleted.
+        DebugTools.Assert(body.Contacts.All(x => (x.Flags & ContactFlags.Deleting) != 0));
     }
 
     /// <summary>

Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs

@@ -123,6 +123,7 @@ public Contact Create()
 
         public bool Return(Contact obj)
         {
+            DebugTools.Assert(obj.Flags is ContactFlags.None or ContactFlags.Deleted);
             SetContact(obj,
                 false,
                 EntityUid.Invalid, EntityUid.Invalid,
@@ -145,7 +146,7 @@ private static void SetContact(Contact contact,
     {
         contact.Enabled = enabled;
         contact.IsTouching = false;
-        contact.Flags = ContactFlags.None | ContactFlags.PreInit;
+        DebugTools.Assert(contact.Flags is ContactFlags.None or ContactFlags.PreInit or ContactFlags.Deleted);
         // TOIFlag = false;
 
         contact.EntityA = uidA;
@@ -229,6 +230,8 @@ private Contact CreateContact(
 
         // Pull out a spare contact object
         var contact = _contactPool.Get();
+        DebugTools.Assert(contact.Flags is ContactFlags.None or ContactFlags.Deleted);
+        contact.Flags = ContactFlags.PreInit;
 
         // Edge+Polygon is non-symmetrical due to the way Erin handles collision type registration.
         if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
@@ -314,13 +317,26 @@ internal static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
                  (fixtureB.CollisionMask & fixtureA.CollisionLayer) == 0x0);
     }
 
-    public bool DestroyContact(Contact contact)
+    public void DestroyContact(Contact contact)
     {
-        if ((contact.Flags & ContactFlags.Deleting) != 0x0)
-            return false;
+        DestroyContact(contact, null, out _);
+    }
 
-        Fixture fixtureA = contact.FixtureA!;
-        Fixture fixtureB = contact.FixtureB!;
+    internal void DestroyContact(Contact contact, LinkedListNode<Contact>? node, out LinkedListNode<Contact>? next)
+    {
+        // EndCollideEvent may cause knock on effects that cause contacts to be destroyed.
+        // This check prevents us from trying to destroy a contact that is already being, or already has been, destroyed.
+        if ((contact.Flags & (ContactFlags.Deleting | ContactFlags.Deleted)) != 0x0)
+        {
+            next = node?.Next;
+            return;
+        }
+
+        DebugTools.Assert((contact.Flags & ContactFlags.PreInit) == 0);
+        // Contact flag might be None here as CollideContacts() might destroy the contact after having removed the PreInit flag
+
+        var fixtureA = contact.FixtureA!;
+        var fixtureB = contact.FixtureB!;
         var bodyA = contact.BodyA!;
         var bodyB = contact.BodyB!;
         var aUid = contact.EntityA;
@@ -344,6 +360,11 @@ public bool DestroyContact(Contact contact)
                 SetAwake((bUid, bodyB), true);
         }
 
+        // Fetch next node AFTER all event raising has finished.
+        // This ensures that we actually get the next node in case the linked list was modified by the events that were
+        // raised
+        next = node?.Next;
+
         // Remove from the world
         _activeContacts.Remove(contact.MapNode);
 
@@ -359,10 +380,8 @@ public bool DestroyContact(Contact contact)
         DebugTools.Assert(bodyB.Contacts.Contains(contact.BodyBNode.Value));
         bodyB.Contacts.Remove(contact.BodyBNode);
 
-        // Insert into the pool.
+        contact.Flags = ContactFlags.Deleted;
         _contactPool.Return(contact);
-
-        return true;
     }
 
     internal void CollideContacts()