diff --git a/Robust.Shared/Physics/Dynamics/ContactManager.cs b/Robust.Shared/Physics/Dynamics/ContactManager.cs deleted file mode 100644 index 679652162..000000000 --- a/Robust.Shared/Physics/Dynamics/ContactManager.cs +++ /dev/null @@ -1,616 +0,0 @@ -// Copyright (c) 2017 Kastellanos Nikolaos - -/* Original source Farseer Physics Engine: - * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com - * Microsoft Permissive License (Ms-PL) v1.1 - */ - -/* -* Farseer Physics Engine: -* Copyright (c) 2012 Ian Qvist -* -* Original source Box2D: -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Buffers; -using System.Collections.Generic; -using System.Threading.Tasks; -using Microsoft.Extensions.ObjectPool; -using Robust.Shared.GameObjects; -using Robust.Shared.Map; -using Robust.Shared.Maths; -using Robust.Shared.Physics.Collision; -using Robust.Shared.Physics.Collision.Shapes; -using Robust.Shared.Physics.Components; -using Robust.Shared.Physics.Dynamics.Contacts; -using Robust.Shared.Physics.Events; -using Robust.Shared.Physics.Systems; -using Robust.Shared.Utility; - -namespace Robust.Shared.Physics.Dynamics -{ - internal sealed class ContactManager - { - private readonly IEntityManager _entityManager; - private readonly IPhysicsManager _physicsManager; - - private EntityLookupSystem _lookup = default!; - private SharedPhysicsSystem _physics = default!; - private SharedTransformSystem _transform = default!; - - internal MapId MapId { get; set; } - - // TODO: Jesus we should really have a test for this - /// - /// Ordering is under - /// uses enum to work out which collision evaluation to use. - /// - private static Contact.ContactType[,] _registers = { - { - // Circle register - Contact.ContactType.Circle, - Contact.ContactType.EdgeAndCircle, - Contact.ContactType.PolygonAndCircle, - Contact.ContactType.ChainAndCircle, - }, - { - // Edge register - Contact.ContactType.EdgeAndCircle, - Contact.ContactType.NotSupported, // Edge - Contact.ContactType.EdgeAndPolygon, - Contact.ContactType.NotSupported, // Chain - }, - { - // Polygon register - Contact.ContactType.PolygonAndCircle, - Contact.ContactType.EdgeAndPolygon, - Contact.ContactType.Polygon, - Contact.ContactType.ChainAndPolygon, - }, - { - // Chain register - Contact.ContactType.ChainAndCircle, - Contact.ContactType.NotSupported, // Edge - Contact.ContactType.ChainAndPolygon, - Contact.ContactType.NotSupported, // Chain - } - }; - - public int ContactCount => _activeContacts.Count; - - private int ContactPoolInitialSize = 64; - - private readonly ObjectPool _contactPool; - - internal readonly LinkedList _activeContacts = new(); - - // Didn't use the eventbus because muh allocs on something being run for every collision every frame. - /// - /// Invoked whenever a KinematicController body collides. The first body is always guaranteed to be a KinematicController - /// - internal event Action? KinematicControllerCollision; - - private const int ContactsPerThread = 32; - - // TODO: Also need to clean the station up to not have 160 contacts on roundstart - - private sealed class ContactPoolPolicy : IPooledObjectPolicy - { - private readonly SharedDebugPhysicsSystem _debugPhysicsSystem; - private readonly IManifoldManager _manifoldManager; - - public ContactPoolPolicy(SharedDebugPhysicsSystem debugPhysicsSystem, IManifoldManager manifoldManager) - { - _debugPhysicsSystem = debugPhysicsSystem; - _manifoldManager = manifoldManager; - } - - public Contact Create() - { - var contact = new Contact(_manifoldManager); -#if DEBUG - contact._debugPhysics = _debugPhysicsSystem; -#endif - contact.Manifold = new Manifold - { - Points = new ManifoldPoint[2] - }; - - return contact; - } - - public bool Return(Contact obj) - { - SetContact(obj, null, 0, null, 0); - return true; - } - } - - public ContactManager( - SharedDebugPhysicsSystem debugPhysicsSystem, - IManifoldManager manifoldManager, - IEntityManager entityManager, - IPhysicsManager physicsManager) - { - _entityManager = entityManager; - _physicsManager = physicsManager; - - _contactPool = new DefaultObjectPool( - new ContactPoolPolicy(debugPhysicsSystem, manifoldManager), - 4096); - } - - private static void SetContact(Contact contact, Fixture? fixtureA, int indexA, Fixture? fixtureB, int indexB) - { - contact.Enabled = true; - contact.IsTouching = false; - contact.Flags = ContactFlags.None; - // TOIFlag = false; - - contact.FixtureA = fixtureA; - contact.FixtureB = fixtureB; - - contact.ChildIndexA = indexA; - contact.ChildIndexB = indexB; - - contact.Manifold.PointCount = 0; - - //FPE: We only set the friction and restitution if we are not destroying the contact - if (fixtureA != null && fixtureB != null) - { - contact.Friction = MathF.Sqrt(fixtureA.Friction * fixtureB.Friction); - contact.Restitution = MathF.Max(fixtureA.Restitution, fixtureB.Restitution); - } - - contact.TangentSpeed = 0; - } - - public void Initialize() - { - _lookup = _entityManager.EntitySysManager.GetEntitySystem(); - _physics = _entityManager.EntitySysManager.GetEntitySystem(); - _transform = _entityManager.EntitySysManager.GetEntitySystem(); - - InitializePool(); - } - - public void Shutdown() - { - } - - private void InitializePool() - { - var dummy = new Contact[ContactPoolInitialSize]; - - for (var i = 0; i < ContactPoolInitialSize; i++) - { - dummy[i] = _contactPool.Get(); - } - - for (var i = 0; i < ContactPoolInitialSize; i++) - { - _contactPool.Return(dummy[i]); - } - } - - private Contact CreateContact(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB) - { - var type1 = fixtureA.Shape.ShapeType; - var type2 = fixtureB.Shape.ShapeType; - - DebugTools.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount); - DebugTools.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount); - - // Pull out a spare contact object - var contact = _contactPool.Get(); - - // 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)) - { - SetContact(contact, fixtureA, indexA, fixtureB, indexB); - } - else - { - SetContact(contact, fixtureB, indexB, fixtureA, indexA); - } - - contact.Type = _registers[(int)type1, (int)type2]; - - return contact; - } - - /// - /// Try to create a contact between these 2 fixtures. - /// - internal void AddPair(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB, ContactFlags flags = ContactFlags.None) - { - PhysicsComponent bodyA = fixtureA.Body; - PhysicsComponent bodyB = fixtureB.Body; - - // Broadphase has already done the faster check for collision mask / layers - // so no point duplicating - - // Does a contact already exist? - if (fixtureA.Contacts.ContainsKey(fixtureB)) - return; - - DebugTools.Assert(!fixtureB.Contacts.ContainsKey(fixtureA)); - - // Does a joint override collision? Is at least one body dynamic? - if (!_physics.ShouldCollide(bodyB, bodyA)) - return; - - // Call the factory. - var contact = CreateContact(fixtureA, indexA, fixtureB, indexB); - contact.Flags = flags; - - // Contact creation may swap fixtures. - fixtureA = contact.FixtureA!; - fixtureB = contact.FixtureB!; - bodyA = fixtureA.Body; - bodyB = fixtureB.Body; - - // Insert into world - _activeContacts.AddLast(contact.MapNode); - - // Connect to body A - DebugTools.Assert(!fixtureA.Contacts.ContainsKey(fixtureB)); - fixtureA.Contacts.Add(fixtureB, contact); - bodyA.Contacts.AddLast(contact.BodyANode); - - // Connect to body B - DebugTools.Assert(!fixtureB.Contacts.ContainsKey(fixtureA)); - fixtureB.Contacts.Add(fixtureA, contact); - bodyB.Contacts.AddLast(contact.BodyBNode); - } - - /// - /// Go through the cached broadphase movement and update contacts. - /// - internal void AddPair(in FixtureProxy proxyA, in FixtureProxy proxyB) - { - AddPair(proxyA.Fixture, proxyA.ChildIndex, proxyB.Fixture, proxyB.ChildIndex); - } - - internal static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) - { - return !((fixtureA.CollisionMask & fixtureB.CollisionLayer) == 0x0 && - (fixtureB.CollisionMask & fixtureA.CollisionLayer) == 0x0); - } - - public void Destroy(Contact contact) - { - Fixture fixtureA = contact.FixtureA!; - Fixture fixtureB = contact.FixtureB!; - PhysicsComponent bodyA = fixtureA.Body; - PhysicsComponent bodyB = fixtureB.Body; - - if (contact.IsTouching) - { - var ev1 = new EndCollideEvent(fixtureA, fixtureB); - var ev2 = new EndCollideEvent(fixtureB, fixtureA); - _entityManager.EventBus.RaiseLocalEvent(bodyA.Owner, ref ev1); - _entityManager.EventBus.RaiseLocalEvent(bodyB.Owner, ref ev2); - } - - if (contact.Manifold.PointCount > 0 && contact.FixtureA?.Hard == true && contact.FixtureB?.Hard == true) - { - if (bodyA.CanCollide) - _physics.SetAwake(contact.FixtureA.Body, true); - - if (bodyB.CanCollide) - _physics.SetAwake(contact.FixtureB.Body, true); - } - - // Remove from the world - _activeContacts.Remove(contact.MapNode); - - // Remove from body 1 - DebugTools.Assert(fixtureA.Contacts.ContainsKey(fixtureB)); - fixtureA.Contacts.Remove(fixtureB); - DebugTools.Assert(bodyA.Contacts.Contains(contact.BodyANode!.Value)); - bodyA.Contacts.Remove(contact.BodyANode); - - // Remove from body 2 - DebugTools.Assert(fixtureB.Contacts.ContainsKey(fixtureA)); - fixtureB.Contacts.Remove(fixtureA); - bodyB.Contacts.Remove(contact.BodyBNode); - - // Insert into the pool. - _contactPool.Return(contact); - } - - internal void Collide() - { - // Due to the fact some contacts may be removed (and we need to update this array as we iterate). - // the length may not match the actual contact count, hence we track the index. - var contacts = ArrayPool.Shared.Rent(ContactCount); - var index = 0; - - // Can be changed while enumerating - // TODO: check for null instead? - // Work out which contacts are still valid before we decide to update manifolds. - var node = _activeContacts.First; - var xformQuery = _entityManager.GetEntityQuery(); - - while (node != null) - { - var contact = node.Value; - node = node.Next; - - Fixture fixtureA = contact.FixtureA!; - Fixture fixtureB = contact.FixtureB!; - int indexA = contact.ChildIndexA; - int indexB = contact.ChildIndexB; - - PhysicsComponent bodyA = fixtureA.Body; - PhysicsComponent bodyB = fixtureB.Body; - - // Do not try to collide disabled bodies - if (!bodyA.CanCollide || !bodyB.CanCollide) - { - Destroy(contact); - continue; - } - - // Is this contact flagged for filtering? - if ((contact.Flags & ContactFlags.Filter) != 0x0) - { - // Should these bodies collide? - if (_physics.ShouldCollide(bodyB, bodyA) == false) - { - Destroy(contact); - continue; - } - - // Check default filtering - if (ShouldCollide(fixtureA, fixtureB) == false) - { - Destroy(contact); - continue; - } - - // Clear the filtering flag. - contact.Flags &= ~ContactFlags.Filter; - } - - bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static; - bool activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static; - - // At least one body must be awake and it must be dynamic or kinematic. - if (activeA == false && activeB == false) - { - continue; - } - - // Special-case grid contacts. - if ((contact.Flags & ContactFlags.Grid) != 0x0) - { - var xformA = xformQuery.GetComponent(bodyA.Owner); - var xformB = xformQuery.GetComponent(bodyB.Owner); - - var gridABounds = fixtureA.Shape.ComputeAABB(_physics.GetPhysicsTransform(bodyA.Owner, xformA, xformQuery), 0); - var gridBBounds = fixtureB.Shape.ComputeAABB(_physics.GetPhysicsTransform(bodyB.Owner, xformB, xformQuery), 0); - - if (!gridABounds.Intersects(gridBBounds)) - { - Destroy(contact); - } - else - { - contacts[index++] = contact; - } - - continue; - } - - var proxyA = fixtureA.Proxies[indexA]; - var proxyB = fixtureB.Proxies[indexB]; - var broadphaseA = _lookup.GetCurrentBroadphase(xformQuery.GetComponent(bodyA.Owner)); - var broadphaseB = _lookup.GetCurrentBroadphase(xformQuery.GetComponent(bodyB.Owner)); - var overlap = false; - - // We can have cross-broadphase proxies hence need to change them to worldspace - if (broadphaseA != null && broadphaseB != null) - { - if (broadphaseA == broadphaseB) - { - overlap = proxyA.AABB.Intersects(proxyB.AABB); - } - else - { - // These should really be destroyed before map changes. - DebugTools.Assert(xformQuery.GetComponent(broadphaseA.Owner).MapID == xformQuery.GetComponent(broadphaseB.Owner).MapID); - - var proxyAWorldAABB = _transform.GetWorldMatrix(broadphaseA.Owner, xformQuery).TransformBox(proxyA.AABB); - var proxyBWorldAABB = _transform.GetWorldMatrix(broadphaseB.Owner, xformQuery).TransformBox(proxyB.AABB); - overlap = proxyAWorldAABB.Intersects(proxyBWorldAABB); - } - } - - // Here we destroy contacts that cease to overlap in the broad-phase. - if (!overlap) - { - Destroy(contact); - continue; - } - - contacts[index++] = contact; - } - - var status = ArrayPool.Shared.Rent(index); - - // To avoid race conditions with the dictionary we'll cache all of the transforms up front. - // Caching should provide better perf than multi-threading the GetTransform() as we can also re-use - // these in PhysicsIsland as well. - for (var i = 0; i < index; i++) - { - var contact = contacts[i]; - var bodyA = contact.FixtureA!.Body; - var bodyB = contact.FixtureB!.Body; - - _physicsManager.EnsureTransform(bodyA.Owner); - _physicsManager.EnsureTransform(bodyB.Owner); - } - - // Update contacts all at once. - BuildManifolds(contacts, index, status); - - // Single-threaded so content doesn't need to worry about race conditions. - for (var i = 0; i < index; i++) - { - var contact = contacts[i]; - - switch (status[i]) - { - case ContactStatus.StartTouching: - { - if (!contact.IsTouching) continue; - - var fixtureA = contact.FixtureA!; - var fixtureB = contact.FixtureB!; - var bodyA = fixtureA.Body; - var bodyB = fixtureB.Body; - var worldPoint = Transform.Mul(_physicsManager.EnsureTransform(bodyA), contact.Manifold.LocalPoint); - - var ev1 = new StartCollideEvent(fixtureA, fixtureB, worldPoint); - var ev2 = new StartCollideEvent(fixtureB, fixtureA, worldPoint); - - _entityManager.EventBus.RaiseLocalEvent(bodyA.Owner, ref ev1, true); - _entityManager.EventBus.RaiseLocalEvent(bodyB.Owner, ref ev2, true); - break; - } - case ContactStatus.Touching: - break; - case ContactStatus.EndTouching: - { - var fixtureA = contact.FixtureA; - var fixtureB = contact.FixtureB; - - // If something under StartCollideEvent potentially nukes other contacts (e.g. if the entity is deleted) - // then we'll just skip the EndCollide. - if (fixtureA == null || fixtureB == null) continue; - - var bodyA = fixtureA.Body; - var bodyB = fixtureB.Body; - - var ev1 = new EndCollideEvent(fixtureA, fixtureB); - var ev2 = new EndCollideEvent(fixtureB, fixtureA); - - _entityManager.EventBus.RaiseLocalEvent(bodyA.Owner, ref ev1); - _entityManager.EventBus.RaiseLocalEvent(bodyB.Owner, ref ev2); - break; - } - case ContactStatus.NoContact: - break; - default: - throw new ArgumentOutOfRangeException(); - } - } - - ArrayPool.Shared.Return(contacts); - ArrayPool.Shared.Return(status); - } - - private void BuildManifolds(Contact[] contacts, int count, ContactStatus[] status) - { - var wake = ArrayPool.Shared.Rent(count); - - if (count > ContactsPerThread * 2) - { - var batches = (int) Math.Ceiling((float) count / ContactsPerThread); - - Parallel.For(0, batches, i => - { - var start = i * ContactsPerThread; - var end = Math.Min(start + ContactsPerThread, count); - UpdateContacts(contacts, start, end, status, wake); - }); - - } - else - { - UpdateContacts(contacts, 0, count, status, wake); - } - - // Can't do this during UpdateContacts due to IoC threading issues. - for (var i = 0; i < count; i++) - { - var shouldWake = wake[i]; - if (!shouldWake) continue; - - var contact = contacts[i]; - var bodyA = contact.FixtureA!.Body; - var bodyB = contact.FixtureB!.Body; - - _physics.SetAwake(bodyA, true); - _physics.SetAwake(bodyB, true); - } - - ArrayPool.Shared.Return(wake); - } - - private void UpdateContacts(Contact[] contacts, int start, int end, ContactStatus[] status, bool[] wake) - { - for (var i = start; i < end; i++) - { - status[i] = contacts[i].Update(_physicsManager, out wake[i]); - } - } - - public void PreSolve(float frameTime) - { - Span points = stackalloc Vector2[2]; - - // We'll do pre and post-solve around all islands rather than each specific island as it seems cleaner with race conditions. - var node = _activeContacts.First; - - while (node != null) - { - var contact = node.Value; - node = node.Next; - - if (contact is not {IsTouching: true, Enabled: true}) continue; - - var bodyA = contact.FixtureA!.Body; - var bodyB = contact.FixtureB!.Body; - contact.GetWorldManifold(_physicsManager, out var worldNormal, points); - - // Didn't use an EntitySystemMessage as this is called FOR EVERY COLLISION AND IS REALLY EXPENSIVE - // so we just use the Action. Also we'll sort out BodyA / BodyB for anyone listening first. - if (bodyA.BodyType == BodyType.KinematicController) - { - KinematicControllerCollision?.Invoke(contact.FixtureA!, contact.FixtureB!, frameTime, -worldNormal); - } - else if (bodyB.BodyType == BodyType.KinematicController) - { - KinematicControllerCollision?.Invoke(contact.FixtureB!, contact.FixtureA!, frameTime, worldNormal); - } - } - } - } - - internal enum ContactStatus : byte - { - NoContact = 0, - StartTouching = 1, - Touching = 2, - EndTouching = 3, - } -} diff --git a/Robust.Shared/Physics/Dynamics/PhysicsMapComponent.cs b/Robust.Shared/Physics/Dynamics/PhysicsMapComponent.cs index 0c7df04fa..eafc73323 100644 --- a/Robust.Shared/Physics/Dynamics/PhysicsMapComponent.cs +++ b/Robust.Shared/Physics/Dynamics/PhysicsMapComponent.cs @@ -41,8 +41,6 @@ namespace Robust.Shared.Physics.Dynamics internal SharedPhysicsSystem Physics = default!; - internal ContactManager ContactManager = default!; - public bool AutoClearForces; /// diff --git a/Robust.Shared/Physics/Systems/FixtureSystem.cs b/Robust.Shared/Physics/Systems/FixtureSystem.cs index 582c1af07..0422157ab 100644 --- a/Robust.Shared/Physics/Systems/FixtureSystem.cs +++ b/Robust.Shared/Physics/Systems/FixtureSystem.cs @@ -36,10 +36,6 @@ namespace Robust.Shared.Physics.Systems private void OnShutdown(EntityUid uid, FixturesComponent component, ComponentShutdown args) { - var xform = Transform(uid); - if (xform.MapID == Map.MapId.Nullspace) - return; - // TODO: Need a better solution to this because the only reason I don't throw is that allcomponents test // Yes it is actively making the game buggier but I would essentially double the size of this PR trying to fix it // my best solution rn is move the broadphase property onto FixturesComponent and then refactor @@ -50,7 +46,7 @@ namespace Robust.Shared.Physics.Systems } // Can't just get physicscomp on shutdown as it may be touched completely independently. - _physics.DestroyContacts(body, xform.MapID, xform); + _physics.DestroyContacts(body); // TODO im 99% sure _broadphaseSystem.RemoveBody(body, component) gets triggered by this as well, so is this even needed? _physics.SetCanCollide(body, false); @@ -204,7 +200,7 @@ namespace Robust.Shared.Physics.Systems { foreach (var contact in fixture.Contacts.Values.ToArray()) { - physicsMap.ContactManager.Destroy(contact); + _physics.DestroyContact(contact); } } _lookup.DestroyProxies(fixture, xform, broadphase, physicsMap); diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs index 48bbdeec1..09c5bf35f 100644 --- a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs @@ -165,10 +165,9 @@ namespace Robust.Shared.Physics.Systems // FindNewContacts is inherently going to be a lot slower than Box2D's normal version so we need // to cache a bunch of stuff to make up for it. - var contactManager = component.ContactManager; // Handle grids first as they're not stored on map broadphase at all. - HandleGridCollisions(mapId, contactManager, movedGrids, physicsQuery, xformQuery); + HandleGridCollisions(mapId, movedGrids, physicsQuery, xformQuery); // EZ if (moveBuffer.Count == 0) @@ -249,7 +248,7 @@ namespace Robust.Shared.Physics.Systems _physicsSystem.WakeBody(otherBody, force: true); } - contactManager.AddPair(proxyA, other); + _physicsSystem.AddPair(proxyA, other); } _bufferPool.Return(contactBuffer[i]); @@ -264,7 +263,6 @@ namespace Robust.Shared.Physics.Systems private void HandleGridCollisions( MapId mapId, - ContactManager contactManager, HashSet movedGrids, EntityQuery bodyQuery, EntityQuery xformQuery) @@ -326,7 +324,7 @@ namespace Robust.Shared.Physics.Systems var otherAABB = otherFixture.Shape.ComputeAABB(otherTransform, j); if (!fixAABB.Intersects(otherAABB)) continue; - contactManager.AddPair(fixture, i, otherFixture, j, ContactFlags.Grid); + _physicsSystem.AddPair(fixture, i, otherFixture, j, ContactFlags.Grid); break; } } @@ -400,7 +398,7 @@ namespace Robust.Shared.Physics.Systems // Logger.DebugS("physics", $"Checking {proxy.Fixture.Body.Owner} against {other.Fixture.Body.Owner} at {aabb}"); if (tuple.proxy == other || - !ContactManager.ShouldCollide(tuple.proxy.Fixture, other.Fixture) || + !SharedPhysicsSystem.ShouldCollide(tuple.proxy.Fixture, other.Fixture) || tuple.proxy.Fixture.Body == other.Fixture.Body) { return true; diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Components.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Components.cs index 5fc947932..e71411914 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Components.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Components.cs @@ -164,23 +164,7 @@ public partial class SharedPhysicsSystem #region Setters - public void DestroyContacts(PhysicsComponent body, MapId? mapId = null, TransformComponent? xform = null) - { - if (body.Contacts.Count == 0) return; - - xform ??= Transform(body.Owner); - mapId ??= xform.MapID; - - if (!TryComp(MapManager.GetMapEntityId(mapId.Value), out var map)) - { - DebugTools.Assert("Attempted to destroy contacts, but entity has no physics map!"); - return; - } - - DestroyContacts(body, map); - } - - public void DestroyContacts(PhysicsComponent body, PhysicsMapComponent physMap) + public void DestroyContacts(PhysicsComponent body) { if (body.Contacts.Count == 0) return; @@ -191,7 +175,7 @@ public partial class SharedPhysicsSystem var contact = node.Value; node = node.Next; // Destroy last so the linked-list doesn't get touched. - physMap.ContactManager.Destroy(contact); + DestroyContact(contact); } DebugTools.Assert(body.Contacts.Count == 0); diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs index 49dc534c3..da242fbae 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs @@ -1,17 +1,548 @@ +// Copyright (c) 2017 Kastellanos Nikolaos + +/* Original source Farseer Physics Engine: + * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com + * Microsoft Permissive License (Ms-PL) v1.1 + */ + +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Buffers; +using System.Collections.Generic; +using System.Threading.Tasks; +using Microsoft.Extensions.ObjectPool; +using Robust.Shared.GameObjects; +using Robust.Shared.Physics.Collision; +using Robust.Shared.Physics.Collision.Shapes; 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; namespace Robust.Shared.Physics.Systems; public abstract partial class SharedPhysicsSystem { + // TODO: Jesus we should really have a test for this + /// + /// Ordering is under + /// uses enum to work out which collision evaluation to use. + /// + private static Contact.ContactType[,] _registers = + { + { + // Circle register + Contact.ContactType.Circle, + Contact.ContactType.EdgeAndCircle, + Contact.ContactType.PolygonAndCircle, + Contact.ContactType.ChainAndCircle, + }, + { + // Edge register + Contact.ContactType.EdgeAndCircle, + Contact.ContactType.NotSupported, // Edge + Contact.ContactType.EdgeAndPolygon, + Contact.ContactType.NotSupported, // Chain + }, + { + // Polygon register + Contact.ContactType.PolygonAndCircle, + Contact.ContactType.EdgeAndPolygon, + Contact.ContactType.Polygon, + Contact.ContactType.ChainAndPolygon, + }, + { + // Chain register + Contact.ContactType.ChainAndCircle, + Contact.ContactType.NotSupported, // Edge + Contact.ContactType.ChainAndPolygon, + Contact.ContactType.NotSupported, // Chain + } + }; + + private int ContactCount => _activeContacts.Count; + + private const int ContactPoolInitialSize = 128; + private const int ContactsPerThread = 32; + + private ObjectPool _contactPool = default!; + + private readonly LinkedList _activeContacts = new(); + + private sealed class ContactPoolPolicy : IPooledObjectPolicy + { + private readonly SharedDebugPhysicsSystem _debugPhysicsSystem; + private readonly IManifoldManager _manifoldManager; + + public ContactPoolPolicy(SharedDebugPhysicsSystem debugPhysicsSystem, IManifoldManager manifoldManager) + { + _debugPhysicsSystem = debugPhysicsSystem; + _manifoldManager = manifoldManager; + } + + public Contact Create() + { + var contact = new Contact(_manifoldManager); +#if DEBUG + contact._debugPhysics = _debugPhysicsSystem; +#endif + contact.Manifold = new Manifold + { + Points = new ManifoldPoint[2] + }; + + return contact; + } + + public bool Return(Contact obj) + { + SetContact(obj, null, 0, null, 0); + return true; + } + } + + private static void SetContact(Contact contact, Fixture? fixtureA, int indexA, Fixture? fixtureB, int indexB) + { + contact.Enabled = true; + contact.IsTouching = false; + contact.Flags = ContactFlags.None; + // TOIFlag = false; + + contact.FixtureA = fixtureA; + contact.FixtureB = fixtureB; + + contact.ChildIndexA = indexA; + contact.ChildIndexB = indexB; + + contact.Manifold.PointCount = 0; + + //FPE: We only set the friction and restitution if we are not destroying the contact + if (fixtureA != null && fixtureB != null) + { + contact.Friction = MathF.Sqrt(fixtureA.Friction * fixtureB.Friction); + contact.Restitution = MathF.Max(fixtureA.Restitution, fixtureB.Restitution); + } + + contact.TangentSpeed = 0; + } + + private void InitializeContacts() + { + _contactPool = new DefaultObjectPool( + new ContactPoolPolicy(_debugPhysics, _manifoldManager), + 4096); + + InitializePool(); + } + + private void InitializePool() + { + var dummy = new Contact[ContactPoolInitialSize]; + + for (var i = 0; i < ContactPoolInitialSize; i++) + { + dummy[i] = _contactPool.Get(); + } + + for (var i = 0; i < ContactPoolInitialSize; i++) + { + _contactPool.Return(dummy[i]); + } + } + + private Contact CreateContact(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB) + { + var type1 = fixtureA.Shape.ShapeType; + var type2 = fixtureB.Shape.ShapeType; + + DebugTools.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount); + DebugTools.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount); + + // Pull out a spare contact object + var contact = _contactPool.Get(); + + // 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)) + { + SetContact(contact, fixtureA, indexA, fixtureB, indexB); + } + else + { + SetContact(contact, fixtureB, indexB, fixtureA, indexA); + } + + contact.Type = _registers[(int)type1, (int)type2]; + + return contact; + } + + /// + /// Try to create a contact between these 2 fixtures. + /// + internal void AddPair(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB, ContactFlags flags = ContactFlags.None) + { + PhysicsComponent bodyA = fixtureA.Body; + PhysicsComponent bodyB = fixtureB.Body; + + // Broadphase has already done the faster check for collision mask / layers + // so no point duplicating + + // Does a contact already exist? + if (fixtureA.Contacts.ContainsKey(fixtureB)) + return; + + DebugTools.Assert(!fixtureB.Contacts.ContainsKey(fixtureA)); + + // Does a joint override collision? Is at least one body dynamic? + if (!ShouldCollide(bodyB, bodyA)) + return; + + // Call the factory. + var contact = CreateContact(fixtureA, indexA, fixtureB, indexB); + contact.Flags = flags; + + // Contact creation may swap fixtures. + fixtureA = contact.FixtureA!; + fixtureB = contact.FixtureB!; + bodyA = fixtureA.Body; + bodyB = fixtureB.Body; + + // Insert into world + _activeContacts.AddLast(contact.MapNode); + + // Connect to body A + DebugTools.Assert(!fixtureA.Contacts.ContainsKey(fixtureB)); + fixtureA.Contacts.Add(fixtureB, contact); + bodyA.Contacts.AddLast(contact.BodyANode); + + // Connect to body B + DebugTools.Assert(!fixtureB.Contacts.ContainsKey(fixtureA)); + fixtureB.Contacts.Add(fixtureA, contact); + bodyB.Contacts.AddLast(contact.BodyBNode); + } + + /// + /// Go through the cached broadphase movement and update contacts. + /// + internal void AddPair(in FixtureProxy proxyA, in FixtureProxy proxyB) + { + AddPair(proxyA.Fixture, proxyA.ChildIndex, proxyB.Fixture, proxyB.ChildIndex); + } + + internal static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) + { + return !((fixtureA.CollisionMask & fixtureB.CollisionLayer) == 0x0 && + (fixtureB.CollisionMask & fixtureA.CollisionLayer) == 0x0); + } + + public void DestroyContact(Contact contact) + { + Fixture fixtureA = contact.FixtureA!; + Fixture fixtureB = contact.FixtureB!; + PhysicsComponent bodyA = fixtureA.Body; + PhysicsComponent bodyB = fixtureB.Body; + + if (contact.IsTouching) + { + var ev1 = new EndCollideEvent(fixtureA, fixtureB); + var ev2 = new EndCollideEvent(fixtureB, fixtureA); + RaiseLocalEvent(bodyA.Owner, ref ev1); + RaiseLocalEvent(bodyB.Owner, ref ev2); + } + + if (contact.Manifold.PointCount > 0 && contact.FixtureA?.Hard == true && contact.FixtureB?.Hard == true) + { + if (bodyA.CanCollide) + SetAwake(contact.FixtureA.Body, true); + + if (bodyB.CanCollide) + SetAwake(contact.FixtureB.Body, true); + } + + // Remove from the world + _activeContacts.Remove(contact.MapNode); + + // Remove from body 1 + DebugTools.Assert(fixtureA.Contacts.ContainsKey(fixtureB)); + fixtureA.Contacts.Remove(fixtureB); + DebugTools.Assert(bodyA.Contacts.Contains(contact.BodyANode!.Value)); + bodyA.Contacts.Remove(contact.BodyANode); + + // Remove from body 2 + DebugTools.Assert(fixtureB.Contacts.ContainsKey(fixtureA)); + fixtureB.Contacts.Remove(fixtureA); + bodyB.Contacts.Remove(contact.BodyBNode); + + // Insert into the pool. + _contactPool.Return(contact); + } + + internal void CollideContacts() + { + // Due to the fact some contacts may be removed (and we need to update this array as we iterate). + // the length may not match the actual contact count, hence we track the index. + var contacts = ArrayPool.Shared.Rent(ContactCount); + var index = 0; + + // Can be changed while enumerating + // TODO: check for null instead? + // Work out which contacts are still valid before we decide to update manifolds. + var node = _activeContacts.First; + var xformQuery = GetEntityQuery(); + + while (node != null) + { + var contact = node.Value; + node = node.Next; + + Fixture fixtureA = contact.FixtureA!; + Fixture fixtureB = contact.FixtureB!; + int indexA = contact.ChildIndexA; + int indexB = contact.ChildIndexB; + + PhysicsComponent bodyA = fixtureA.Body; + PhysicsComponent bodyB = fixtureB.Body; + + // Do not try to collide disabled bodies + if (!bodyA.CanCollide || !bodyB.CanCollide) + { + DestroyContact(contact); + continue; + } + + // Is this contact flagged for filtering? + if ((contact.Flags & ContactFlags.Filter) != 0x0) + { + // Check default filtering + if (!ShouldCollide(fixtureA, fixtureB) || + !ShouldCollide(bodyB, bodyA)) + { + DestroyContact(contact); + continue; + } + + // Clear the filtering flag. + contact.Flags &= ~ContactFlags.Filter; + } + + bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static; + bool activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static; + + // At least one body must be awake and it must be dynamic or kinematic. + if (activeA == false && activeB == false) + { + continue; + } + + var xformA = xformQuery.GetComponent(bodyA.Owner); + var xformB = xformQuery.GetComponent(bodyB.Owner); + + if (xformA.MapUid == null || xformA.MapUid != xformB.MapUid) + { + DestroyContact(contact); + continue; + } + + // Special-case grid contacts. + if ((contact.Flags & ContactFlags.Grid) != 0x0) + { + var gridABounds = fixtureA.Shape.ComputeAABB(GetPhysicsTransform(bodyA.Owner, xformA, xformQuery), 0); + var gridBBounds = fixtureB.Shape.ComputeAABB(GetPhysicsTransform(bodyB.Owner, xformB, xformQuery), 0); + + if (!gridABounds.Intersects(gridBBounds)) + { + DestroyContact(contact); + } + else + { + // Grid contact is still alive. + contact.Flags &= ~ContactFlags.Island; + contacts[index++] = contact; + } + + continue; + } + + var proxyA = fixtureA.Proxies[indexA]; + var proxyB = fixtureB.Proxies[indexB]; + var broadphaseA = _lookup.GetCurrentBroadphase(xformA); + var broadphaseB = _lookup.GetCurrentBroadphase(xformB); + var overlap = false; + + // We can have cross-broadphase proxies hence need to change them to worldspace + if (broadphaseA != null && broadphaseB != null) + { + if (broadphaseA == broadphaseB) + { + overlap = proxyA.AABB.Intersects(proxyB.AABB); + } + else + { + var proxyAWorldAABB = _transform.GetWorldMatrix(broadphaseA.Owner, xformQuery).TransformBox(proxyA.AABB); + var proxyBWorldAABB = _transform.GetWorldMatrix(broadphaseB.Owner, xformQuery).TransformBox(proxyB.AABB); + overlap = proxyAWorldAABB.Intersects(proxyBWorldAABB); + } + } + + // Here we destroy contacts that cease to overlap in the broad-phase. + if (!overlap) + { + DestroyContact(contact); + continue; + } + + // Contact is actually going to live for manifold generation and solving. + // This can also short-circuit above for grid contacts. + contact.Flags &= ~ContactFlags.Island; + contacts[index++] = contact; + } + + var status = ArrayPool.Shared.Rent(index); + + // To avoid race conditions with the dictionary we'll cache all of the transforms up front. + // Caching should provide better perf than multi-threading the GetTransform() as we can also re-use + // these in PhysicsIsland as well. + for (var i = 0; i < index; i++) + { + var contact = contacts[i]; + var bodyA = contact.FixtureA!.Body; + var bodyB = contact.FixtureB!.Body; + + _physicsManager.EnsureTransform(bodyA.Owner); + _physicsManager.EnsureTransform(bodyB.Owner); + } + + // Update contacts all at once. + BuildManifolds(contacts, index, status); + + // Single-threaded so content doesn't need to worry about race conditions. + for (var i = 0; i < index; i++) + { + var contact = contacts[i]; + + switch (status[i]) + { + case ContactStatus.StartTouching: + { + if (!contact.IsTouching) continue; + + var fixtureA = contact.FixtureA!; + var fixtureB = contact.FixtureB!; + var bodyA = fixtureA.Body; + var bodyB = fixtureB.Body; + var worldPoint = Physics.Transform.Mul(_physicsManager.EnsureTransform(bodyA), contact.Manifold.LocalPoint); + + var ev1 = new StartCollideEvent(fixtureA, fixtureB, worldPoint); + var ev2 = new StartCollideEvent(fixtureB, fixtureA, worldPoint); + + RaiseLocalEvent(bodyA.Owner, ref ev1, true); + RaiseLocalEvent(bodyB.Owner, ref ev2, true); + break; + } + case ContactStatus.Touching: + break; + case ContactStatus.EndTouching: + { + var fixtureA = contact.FixtureA; + var fixtureB = contact.FixtureB; + + // If something under StartCollideEvent potentially nukes other contacts (e.g. if the entity is deleted) + // then we'll just skip the EndCollide. + if (fixtureA == null || fixtureB == null) continue; + + var bodyA = fixtureA.Body; + var bodyB = fixtureB.Body; + + var ev1 = new EndCollideEvent(fixtureA, fixtureB); + var ev2 = new EndCollideEvent(fixtureB, fixtureA); + + RaiseLocalEvent(bodyA.Owner, ref ev1); + RaiseLocalEvent(bodyB.Owner, ref ev2); + break; + } + case ContactStatus.NoContact: + break; + default: + throw new ArgumentOutOfRangeException(); + } + } + + ArrayPool.Shared.Return(contacts); + ArrayPool.Shared.Return(status); + } + + private void BuildManifolds(Contact[] contacts, int count, ContactStatus[] status) + { + var wake = ArrayPool.Shared.Rent(count); + + if (count > ContactsPerThread * 2) + { + var batches = (int) Math.Ceiling((float) count / ContactsPerThread); + + Parallel.For(0, batches, i => + { + var start = i * ContactsPerThread; + var end = Math.Min(start + ContactsPerThread, count); + UpdateContacts(contacts, start, end, status, wake); + }); + + } + else + { + UpdateContacts(contacts, 0, count, status, wake); + } + + // Can't do this during UpdateContacts due to IoC threading issues. + for (var i = 0; i < count; i++) + { + var shouldWake = wake[i]; + if (!shouldWake) continue; + + var contact = contacts[i]; + var bodyA = contact.FixtureA!.Body; + var bodyB = contact.FixtureB!.Body; + + SetAwake(bodyA, true); + SetAwake(bodyB, true); + } + + ArrayPool.Shared.Return(wake); + } + + private void UpdateContacts(Contact[] contacts, int start, int end, ContactStatus[] status, bool[] wake) + { + for (var i = start; i < end; i++) + { + status[i] = contacts[i].Update(_physicsManager, out wake[i]); + } + } + /// /// Used to prevent bodies from colliding; may lie depending on joints. /// - /// - /// - internal bool ShouldCollide(PhysicsComponent body, PhysicsComponent other) + private bool ShouldCollide(PhysicsComponent body, PhysicsComponent other) { if (((body.BodyType & (BodyType.Kinematic | BodyType.Static)) != 0 && (other.BodyType & (BodyType.Kinematic | BodyType.Static)) != 0) || @@ -31,7 +562,7 @@ public abstract partial class SharedPhysicsSystem var aUid = jointComponentA.Owner; var bUid = jointComponentB.Owner; - foreach (var (_, joint) in jointComponentA.Joints) + foreach (var joint in jointComponentA.Joints.Values) { // Check if either: the joint even allows collisions OR the other body on the joint is actually the other body we're checking. if (!joint.CollideConnected && @@ -55,3 +586,11 @@ public abstract partial class SharedPhysicsSystem return true; } } + +internal enum ContactStatus : byte +{ + NoContact = 0, + StartTouching = 1, + Touching = 2, + EndTouching = 3, +} diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs index 0f4e23265..545f22799 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs @@ -277,22 +277,9 @@ public abstract partial class SharedPhysicsSystem /// public void Step(PhysicsMapComponent component, float frameTime, bool prediction) { - // Box2D does this at the end of a step and also here when there's a fixture update. - // Given external stuff can move bodies we'll just do this here. - // Unfortunately this NEEDS to be predicted to make pushing remotely fucking good. - _broadphase.FindNewContacts(component, component.MapId); - var invDt = frameTime > 0.0f ? 1.0f / frameTime : 0.0f; var dtRatio = component._invDt0 * frameTime; - var updateBeforeSolve = new PhysicsUpdateBeforeMapSolveEvent(prediction, component, frameTime); - RaiseLocalEvent(ref updateBeforeSolve); - - component.ContactManager.Collide(); - // Don't run collision behaviors during FrameUpdate? - if (!prediction) - component.ContactManager.PreSolve(frameTime); - // Integrate velocities, solve velocity constraints, and do integration. Solve(component, frameTime, dtRatio, invDt, prediction); @@ -320,15 +307,6 @@ public abstract partial class SharedPhysicsSystem private void Solve(PhysicsMapComponent component, float frameTime, float dtRatio, float invDt, bool prediction) { - var contactNode = component.ContactManager._activeContacts.First; - - while (contactNode != null) - { - var contact = contactNode.Value; - contactNode = contactNode.Next; - contact.Flags &= ~ContactFlags.Island; - } - // Build and simulated islands from awake bodies. _bodyStack.EnsureCapacity(component.AwakeBodies.Count); _islandSet.EnsureCapacity(component.AwakeBodies.Count); diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs index 9f0bb8a26..4128ef089 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs @@ -8,7 +8,6 @@ using Robust.Shared.IoC; using Robust.Shared.Log; using Robust.Shared.Map; using Robust.Shared.Map.Components; -using Robust.Shared.Maths; using Robust.Shared.Physics.Collision; using Robust.Shared.Physics.Components; using Robust.Shared.Physics.Dynamics; @@ -25,7 +24,6 @@ namespace Robust.Shared.Physics.Systems * TODO: * Raycasts for non-box shapes. - * SetTransformIgnoreContacts for teleports (and anything else left on the physics body in Farseer) * TOI Solver (continuous collision detection) * Poly cutting * Chain shape @@ -59,8 +57,6 @@ namespace Robust.Shared.Physics.Systems [Dependency] private readonly SharedTransformSystem _transform = default!; [Dependency] private readonly SharedDebugPhysicsSystem _debugPhysics = default!; - public Action? KinematicControllerCollision; - private int _substeps; public bool MetricsEnabled { get; protected set; } @@ -81,12 +77,12 @@ namespace Robust.Shared.Physics.Systems SubscribeLocalEvent(HandleContainerRemoved); SubscribeLocalEvent(OnParentChange); SubscribeLocalEvent(HandlePhysicsMapInit); - SubscribeLocalEvent(HandlePhysicsMapRemove); SubscribeLocalEvent(OnPhysicsInit); SubscribeLocalEvent(OnPhysicsRemove); SubscribeLocalEvent(OnPhysicsGetState); SubscribeLocalEvent(OnPhysicsHandleState); InitializeIsland(); + InitializeContacts(); _configManager.OnValueChanged(CVars.AutoClearForces, OnAutoClearChange); _configManager.OnValueChanged(CVars.NetTickrate, UpdateSubsteps, true); @@ -116,12 +112,7 @@ namespace Robust.Shared.Physics.Systems { _deps.InjectDependencies(component); component.Physics = this; - component.ContactManager = new(_debugPhysics, _manifoldManager, EntityManager, _physicsManager); - component.ContactManager.Initialize(); - component.ContactManager.MapId = component.MapId; component.AutoClearForces = _cfg.GetCVar(CVars.AutoClearForces); - - component.ContactManager.KinematicControllerCollision += KinematicControllerCollision; } private void OnAutoClearChange(bool value) @@ -141,16 +132,6 @@ namespace Robust.Shared.Physics.Systems _substeps = (int)Math.Ceiling(targetMinTickrate / serverTickrate); } - private void HandlePhysicsMapRemove(EntityUid uid, PhysicsMapComponent component, ComponentRemove args) - { - // THis entity might be getting deleted before ever having been initialized. - if (component.ContactManager == null) - return; - - component.ContactManager.KinematicControllerCollision -= KinematicControllerCollision; - component.ContactManager.Shutdown(); - } - private void OnParentChange(ref EntParentChangedMessage args) { // We do not have a directed/body subscription, because the entity changing parents may not have a physics component, but one of its children might. @@ -226,11 +207,6 @@ namespace Robust.Shared.Physics.Systems } else DebugTools.Assert(oldMap?.AwakeBodies.Contains(body) != true); - - // TODO: Could potentially migrate these but would need more thinking - if (oldMap != null) - DestroyContacts(body, oldMap); // This can modify body.Awake - DebugTools.Assert(body.Contacts.Count == 0); } if (jointQuery.TryGetComponent(uid, out var joint)) @@ -316,6 +292,20 @@ namespace Robust.Shared.Physics.Systems var updateBeforeSolve = new PhysicsUpdateBeforeSolveEvent(prediction, frameTime); RaiseLocalEvent(ref updateBeforeSolve); + var contactEnumerator = AllEntityQuery(); + + // Find new contacts and (TODO: temporary) update any per-map virtual controllers + while (contactEnumerator.MoveNext(out var comp, out var xform)) + { + // Box2D does this at the end of a step and also here when there's a fixture update. + // Given external stuff can move bodies we'll just do this here. + _broadphase.FindNewContacts(comp, xform.MapID); + + var updateMapBeforeSolve = new PhysicsUpdateBeforeMapSolveEvent(prediction, comp, frameTime); + RaiseLocalEvent(ref updateMapBeforeSolve); + } + + CollideContacts(); var enumerator = AllEntityQuery(); while (enumerator.MoveNext(out var comp)) diff --git a/Robust.UnitTesting/Shared/Physics/Collision_Test.cs b/Robust.UnitTesting/Shared/Physics/Collision_Test.cs index 68035db49..04f563f30 100644 --- a/Robust.UnitTesting/Shared/Physics/Collision_Test.cs +++ b/Robust.UnitTesting/Shared/Physics/Collision_Test.cs @@ -22,11 +22,15 @@ using System; using NUnit.Framework; +using Robust.Shared.GameObjects; +using Robust.Shared.Map; using Robust.Shared.Maths; using Robust.Shared.Physics; using Robust.Shared.Physics.Collision.Shapes; +using Robust.Shared.Physics.Components; using Robust.Shared.Physics.Dynamics; using Robust.Shared.Physics.Systems; +using Robust.UnitTesting.Server; namespace Robust.UnitTesting.Shared.Physics; @@ -84,4 +88,47 @@ public sealed class Collision_Test Assert.That(MathF.Abs(massData2.Mass - mass), Is.LessThan(20.0f * (absTol + relTol * mass))); Assert.That(MathF.Abs(massData2.I - inertia), Is.LessThan(40.0f * (absTol + relTol * inertia))); } + + /// + /// Asserts that cross-map contacts correctly destroy + /// + [Test] + public void CrossMapContacts() + { + var sim = RobustServerSimulation.NewSimulation().InitializeInstance(); + var entManager = sim.Resolve(); + var mapManager = sim.Resolve(); + var fixtures = entManager.System(); + var physics = entManager.System(); + var xformSystem = entManager.System(); + var mapId = mapManager.CreateMap(); + var mapId2 = mapManager.CreateMap(); + + var ent1 = entManager.SpawnEntity(null, new MapCoordinates(Vector2.Zero, mapId)); + var ent2 = entManager.SpawnEntity(null, new MapCoordinates(Vector2.Zero, mapId)); + + var body1 = entManager.AddComponent(ent1); + physics.SetBodyType(body1, BodyType.Dynamic); + var body2 = entManager.AddComponent(ent2); + physics.SetBodyType(body2, BodyType.Dynamic); + + fixtures.CreateFixture(body1, new Fixture(new PhysShapeCircle() { Radius = 1f }, 1, 0, true)); + fixtures.CreateFixture(body2, new Fixture(new PhysShapeCircle() { Radius = 1f }, 0, 1, true)); + + physics.WakeBody(body1); + physics.WakeBody(body2); + + Assert.That(body1.Awake && body2.Awake); + Assert.That(body1.ContactCount == 0 && body2.ContactCount == 0); + + physics.Update(0.01f); + + Assert.That(body1.ContactCount == 1 && body2.ContactCount == 1); + + // Reparent body2 and assert the contact is destroyed + xformSystem.SetParent(ent2, mapManager.GetMapEntityId(mapId2)); + physics.Update(0.01f); + + Assert.That(body1.ContactCount == 0 && body2.ContactCount == 0); + } } diff --git a/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs b/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs index 1e471b0af..6b79642a2 100644 --- a/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs +++ b/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs @@ -61,15 +61,13 @@ public sealed class GridMovement_Test : RobustIntegrationTest physSystem.WakeBody(offGridBody); // Alright just a quick validation then we start the actual damn test. - - var physicsMap = entManager.GetComponent(mapManager.GetMapEntityId(mapId)); - physSystem.Step(physicsMap, 0.001f, false); + physSystem.Update(0.001f); Assert.That(onGridBody.ContactCount, Is.EqualTo(0)); // Alright now move the grid on top of the off grid body, run physics for a frame and see if they contact entManager.GetComponent(grid.Owner).LocalPosition = new Vector2(10f, 10f); - physSystem.Step(physicsMap, 0.001f, false); + physSystem.Update(0.001f); Assert.That(onGridBody.ContactCount, Is.EqualTo(1)); });