From 0bc3c517070ba81ae88e4249d520f4d850c0ebc1 Mon Sep 17 00:00:00 2001
From: metalgearsloth <31366439+metalgearsloth@users.noreply.github.com>
Date: Sun, 25 Aug 2024 11:54:05 +1000
Subject: [PATCH] Contact QOL stuff (#5385)

* Contact QOL stuff

Only just made the enumerator version need to test with AI branch had an IEnumerable before.

* Fix
---
 .../Physics/Dynamics/Contacts/Contact.cs      | 23 +++++
 .../Physics/Systems/FixtureSystem.cs          |  1 +
 .../Physics/Systems/SharedBroadphaseSystem.cs |  2 +-
 .../Systems/SharedPhysicsSystem.Contacts.cs   | 93 +++++++++++++++++++
 4 files changed, 118 insertions(+), 1 deletion(-)

diff --git a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs
index 6bb2f8e8779..e588fc93952 100644
--- a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs
+++ b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs
@@ -349,6 +349,29 @@ public override int GetHashCode()
             // TODO: Need to suss this out
             return HashCode.Combine(EntityA, EntityB);
         }
+
+        /// <summary>
+        /// Gets the other ent for this contact.
+        /// </summary>
+        public EntityUid OtherEnt(EntityUid uid)
+        {
+            if (uid == EntityA)
+                return EntityB;
+            else if (uid == EntityB)
+                return EntityA;
+
+            throw new InvalidOperationException();
+        }
+
+        public (string Id, Fixture) OtherFixture(EntityUid uid)
+        {
+            if (uid == EntityA)
+                return (FixtureBId, FixtureB!);
+            else if (uid == EntityB)
+                return (FixtureAId, FixtureA!);
+
+            throw new InvalidOperationException();
+        }
     }
 
     [Flags]
diff --git a/Robust.Shared/Physics/Systems/FixtureSystem.cs b/Robust.Shared/Physics/Systems/FixtureSystem.cs
index c07c6831f08..ac22c96cc6a 100644
--- a/Robust.Shared/Physics/Systems/FixtureSystem.cs
+++ b/Robust.Shared/Physics/Systems/FixtureSystem.cs
@@ -116,6 +116,7 @@ internal void CreateFixture(
                 // Don't need to ResetMassData as FixtureUpdate already does it.
                 Dirty(uid, manager);
             }
+
             // TODO: Set newcontacts to true.
         }
 
diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs
index ba1a80e029c..c57b0fe1a30 100644
--- a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs
+++ b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs
@@ -416,7 +416,7 @@ public void RegenerateContacts(EntityUid uid, PhysicsComponent body, FixturesCom
             }
         }
 
-        private void TouchProxies(EntityUid mapId, Matrix3x2 broadphaseMatrix, Fixture fixture)
+        internal void TouchProxies(EntityUid mapId, Matrix3x2 broadphaseMatrix, Fixture fixture)
         {
             foreach (var proxy in fixture.Proxies)
             {
diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs
index e98839a50c6..f7cee57fa47 100644
--- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs
+++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs
@@ -30,7 +30,10 @@
 using System;
 using System.Buffers;
 using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Diagnostics.CodeAnalysis;
 using System.Numerics;
+using JetBrains.Annotations;
 using Microsoft.Extensions.ObjectPool;
 using Robust.Shared.GameObjects;
 using Robust.Shared.Maths;
@@ -741,6 +744,96 @@ protected bool ShouldCollide(
 
         return true;
     }
+
+    /// <summary>
+    /// Will destroy all contacts and queue for rebuild.
+    /// Useful if you have one that may no longer be relevant and don't want to destroy it directly.
+    /// </summary>
+    public void RegenerateContacts(Entity<PhysicsComponent?> entity)
+    {
+        if (!PhysicsQuery.Resolve(entity.Owner, ref entity.Comp))
+            return;
+
+        _broadphase.RegenerateContacts(entity.Owner, entity.Comp);
+    }
+
+    /// <summary>
+    /// Returns the number of touching contacts this entity has.
+    /// </summary>
+    /// <param name="ignoredFixtureId">Fixture we should ignore if applicable</param>
+    [Pure]
+    public int GetTouchingContacts(Entity<FixturesComponent?> entity, string? ignoredFixtureId = null)
+    {
+        if (!_fixturesQuery.Resolve(entity.Owner, ref entity.Comp))
+            return 0;
+
+        var count = 0;
+
+        foreach (var (id, fixture) in entity.Comp.Fixtures)
+        {
+            if (ignoredFixtureId == id)
+                continue;
+
+            foreach (var contact in fixture.Contacts.Values)
+            {
+                if (!contact.IsTouching)
+                    continue;
+
+                count++;
+            }
+        }
+
+        return count;
+    }
+
+    /// <summary>
+    /// Returns all of this entity's contacts.
+    /// </summary>
+    [Pure]
+    public ContactEnumerator GetContacts(Entity<FixturesComponent?> entity)
+    {
+        _fixturesQuery.Resolve(entity.Owner, ref entity.Comp);
+        return new ContactEnumerator(entity.Comp);
+    }
+}
+
+public record struct ContactEnumerator
+{
+    public static readonly ContactEnumerator Empty = new(null);
+
+    private Dictionary<string, Fixture>.ValueCollection.Enumerator _fixtureEnumerator;
+    private Dictionary<Fixture, Contact>.ValueCollection.Enumerator _contactEnumerator;
+
+    public ContactEnumerator(FixturesComponent? fixtures)
+    {
+        if (fixtures == null || fixtures.Fixtures.Count == 0)
+        {
+            this = Empty;
+            return;
+        }
+
+        _fixtureEnumerator = fixtures.Fixtures.Values.GetEnumerator();
+        _fixtureEnumerator.MoveNext();
+        _contactEnumerator = _fixtureEnumerator.Current.Contacts.Values.GetEnumerator();
+    }
+
+    public bool MoveNext([NotNullWhen(true)] out Contact? contact)
+    {
+        if (!_contactEnumerator.MoveNext())
+        {
+            if (!_fixtureEnumerator.MoveNext())
+            {
+                contact = null;
+                return false;
+            }
+
+            _contactEnumerator = _fixtureEnumerator.Current.Contacts.Values.GetEnumerator();
+            return MoveNext(out contact);
+        }
+
+        contact = _contactEnumerator.Current;
+        return true;
+    }
 }
 
 internal enum ContactStatus : byte