space-wizards/RobustToolbox
1
0
Fork 0
mirror of https://github.com/space-wizards/RobustToolbox.git synced 2026-02-15 03:30:53 +01:00
Code Issues Packages Projects Releases Wiki Activity

Make phys contacts per-world rather than per-map (#3619)

Browse Source
This commit is contained in:
metalgearsloth
2022-12-28 13:54:36 +11:00
committed by GitHub
parent 360db24f0a
commit 6c4b71e06b
10 changed files with 615 additions and 703 deletions
Download Patch File Download Diff File Expand all files Collapse all files

616
Robust.Shared/Physics/Dynamics/ContactManager.cs
View File

@@ -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
/// <summary>
/// Ordering is under <see cref="ShapeType"/>
/// uses enum to work out which collision evaluation to use.
/// </summary>
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<Contact> _contactPool;
internal readonly LinkedList<Contact> _activeContacts = new();
// Didn't use the eventbus because muh allocs on something being run for every collision every frame.
/// <summary>
/// Invoked whenever a KinematicController body collides. The first body is always guaranteed to be a KinematicController
/// </summary>
internal event Action<Fixture, Fixture, float, Vector2>? 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<Contact>
{
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<Contact>(
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<EntityLookupSystem>();
_physics = _entityManager.EntitySysManager.GetEntitySystem<SharedPhysicsSystem>();
_transform = _entityManager.EntitySysManager.GetEntitySystem<SharedTransformSystem>();
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;
}
/// <summary>
/// Try to create a contact between these 2 fixtures.
/// </summary>
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);
}
/// <summary>
/// Go through the cached broadphase movement and update contacts.
/// </summary>
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<Contact>.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<TransformComponent>();
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<ContactStatus>.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<Contact>.Shared.Return(contacts);
ArrayPool<ContactStatus>.Shared.Return(status);
}
private void BuildManifolds(Contact[] contacts, int count, ContactStatus[] status)
{
var wake = ArrayPool<bool>.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<bool>.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<Vector2> 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,
}
}

2
Robust.Shared/Physics/Dynamics/PhysicsMapComponent.cs
View File

@@ -41,8 +41,6 @@ namespace Robust.Shared.Physics.Dynamics
internal SharedPhysicsSystem Physics = default!;
internal ContactManager ContactManager = default!;
public bool AutoClearForces;
/// <summary>

8
Robust.Shared/Physics/Systems/FixtureSystem.cs
View File

@@ -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);

10
Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs
View File

@@ -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<MapGridComponent> movedGrids,
EntityQuery<PhysicsComponent> bodyQuery,
EntityQuery<TransformComponent> 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;

20
Robust.Shared/Physics/Systems/SharedPhysicsSystem.Components.cs
View File

@@ -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<PhysicsMapComponent>(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);

547
Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs
View File

@@ -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
/// <summary>
/// Ordering is under <see cref="ShapeType"/>
/// uses enum to work out which collision evaluation to use.
/// </summary>
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<Contact> _contactPool = default!;
private readonly LinkedList<Contact> _activeContacts = new();
private sealed class ContactPoolPolicy : IPooledObjectPolicy<Contact>
{
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<Contact>(
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;
}
/// <summary>
/// Try to create a contact between these 2 fixtures.
/// </summary>
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);
}
/// <summary>
/// Go through the cached broadphase movement and update contacts.
/// </summary>
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<Contact>.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<TransformComponent>();
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<ContactStatus>.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<Contact>.Shared.Return(contacts);
ArrayPool<ContactStatus>.Shared.Return(status);
}
private void BuildManifolds(Contact[] contacts, int count, ContactStatus[] status)
{
var wake = ArrayPool<bool>.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<bool>.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]);
}
}
/// <summary>
/// Used to prevent bodies from colliding; may lie depending on joints.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
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,
}

22
Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs
View File

@@ -277,22 +277,9 @@ public abstract partial class SharedPhysicsSystem
/// </summary>
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);

40
Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs
View File

@@ -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<Fixture, Fixture, float, Vector2>? KinematicControllerCollision;
private int _substeps;
public bool MetricsEnabled { get; protected set; }
@@ -81,12 +77,12 @@ namespace Robust.Shared.Physics.Systems
SubscribeLocalEvent<PhysicsComponent, EntGotRemovedFromContainerMessage>(HandleContainerRemoved);
SubscribeLocalEvent<EntParentChangedMessage>(OnParentChange);
SubscribeLocalEvent<PhysicsMapComponent, ComponentInit>(HandlePhysicsMapInit);
SubscribeLocalEvent<PhysicsMapComponent, ComponentRemove>(HandlePhysicsMapRemove);
SubscribeLocalEvent<PhysicsComponent, ComponentInit>(OnPhysicsInit);
SubscribeLocalEvent<PhysicsComponent, ComponentRemove>(OnPhysicsRemove);
SubscribeLocalEvent<PhysicsComponent, ComponentGetState>(OnPhysicsGetState);
SubscribeLocalEvent<PhysicsComponent, ComponentHandleState>(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<PhysicsMapComponent, TransformComponent>();
// 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<PhysicsMapComponent>();
while (enumerator.MoveNext(out var comp))

47
Robust.UnitTesting/Shared/Physics/Collision_Test.cs
View File

@@ -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)));
}
/// <summary>
/// Asserts that cross-map contacts correctly destroy
/// </summary>
[Test]
public void CrossMapContacts()
{
var sim = RobustServerSimulation.NewSimulation().InitializeInstance();
var entManager = sim.Resolve<IEntityManager>();
var mapManager = sim.Resolve<IMapManager>();
var fixtures = entManager.System<FixtureSystem>();
var physics = entManager.System<SharedPhysicsSystem>();
var xformSystem = entManager.System<SharedTransformSystem>();
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<PhysicsComponent>(ent1);
physics.SetBodyType(body1, BodyType.Dynamic);
var body2 = entManager.AddComponent<PhysicsComponent>(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);
}
}

6
Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs
View File

@@ -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<PhysicsMapComponent>(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<TransformComponent>(grid.Owner).LocalPosition = new Vector2(10f, 10f);
physSystem.Step(physicsMap, 0.001f, false);
physSystem.Update(0.001f);
Assert.That(onGridBody.ContactCount, Is.EqualTo(1));
});