From 5c1d753a8df01eedeade5c7e3aaee49df8a037bd Mon Sep 17 00:00:00 2001 From: metalgearsloth <31366439+metalgearsloth@users.noreply.github.com> Date: Mon, 26 Dec 2022 10:51:00 +1100 Subject: [PATCH] Physics solver refactor (#3479) --- .../Physics/DebugPhysicsIslandSystem.cs | 3 +- Robust.Client/Physics/PhysicsMapComponent.cs | 59 - Robust.Client/Physics/PhysicsSystem.cs | 73 ++ Robust.Shared/CVars.cs | 18 - .../BroadPhase/DynamicTreeBroadPhase.cs | 303 +++-- .../Physics/Dynamics/ContactManager.cs | 36 +- .../Physics/Dynamics/Contacts/Contact.cs | 3 +- .../Dynamics/Contacts/ContactSolver.cs | 935 --------------- .../Contacts/ContactVelocityConstraint.cs | 8 +- .../Physics/Dynamics/Joints/DistanceJoint.cs | 88 +- .../Physics/Dynamics/Joints/FrictionJoint.cs | 64 +- .../Physics/Dynamics/Joints/Joint.cs | 36 +- .../Physics/Dynamics/Joints/MouseJoint.cs | 46 +- .../Physics/Dynamics/Joints/PrismaticJoint.cs | 88 +- .../Physics/Dynamics/Joints/RevoluteJoint.cs | 88 +- .../Physics/Dynamics/Joints/WeldJoint.cs | 82 +- .../Physics/Dynamics/PhysicsIsland.cs | 550 +-------- .../Dynamics/SharedPhysicsMapComponent.cs | 292 +---- Robust.Shared/Physics/IBroadPhase.cs | 193 ++-- Robust.Shared/Physics/IslandManager.cs | 271 ----- Robust.Shared/Physics/IslandSolveMessage.cs | 5 +- .../Physics/Systems/SharedBroadphaseSystem.cs | 4 +- .../Systems/SharedPhysicsSystem.Island.cs | 1020 +++++++++++++++++ .../Systems/SharedPhysicsSystem.Solver.cs | 912 +++++++++++++++ .../Physics/Systems/SharedPhysicsSystem.cs | 93 +- Robust.Shared/SharedIoC.cs | 1 - .../Server/RobustServerSimulation.cs | 1 - .../Shared/Physics/GridMovement_Test.cs | 4 +- 28 files changed, 2634 insertions(+), 2642 deletions(-) delete mode 100644 Robust.Shared/Physics/Dynamics/Contacts/ContactSolver.cs delete mode 100644 Robust.Shared/Physics/IslandManager.cs create mode 100644 Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs create mode 100644 Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs diff --git a/Robust.Client/Physics/DebugPhysicsIslandSystem.cs b/Robust.Client/Physics/DebugPhysicsIslandSystem.cs index 1db0aa690..47cbd5f9a 100644 --- a/Robust.Client/Physics/DebugPhysicsIslandSystem.cs +++ b/Robust.Client/Physics/DebugPhysicsIslandSystem.cs @@ -7,6 +7,7 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Maths; using Robust.Shared.Physics; +using Robust.Shared.Physics.Components; using Robust.Shared.Timing; namespace Robust.Client.Physics @@ -43,7 +44,7 @@ namespace Robust.Client.Physics * This will draw above every body involved in a particular island solve. */ - public readonly Queue<(TimeSpan Time, List Bodies)> IslandSolve = new(); + public readonly Queue<(TimeSpan Time, List Bodies)> IslandSolve = new(); public const float SolveDuration = 0.1f; public override void Initialize() diff --git a/Robust.Client/Physics/PhysicsMapComponent.cs b/Robust.Client/Physics/PhysicsMapComponent.cs index 57b05a796..024c4f4a0 100644 --- a/Robust.Client/Physics/PhysicsMapComponent.cs +++ b/Robust.Client/Physics/PhysicsMapComponent.cs @@ -1,10 +1,4 @@ -using System; -using System.Collections.Generic; -using Robust.Shared; -using Robust.Shared.Configuration; using Robust.Shared.GameObjects; -using Robust.Shared.IoC; -using Robust.Shared.Physics.Components; using Robust.Shared.Physics.Dynamics; namespace Robust.Client.Physics @@ -13,58 +7,5 @@ namespace Robust.Client.Physics [ComponentReference(typeof(SharedPhysicsMapComponent))] public sealed class PhysicsMapComponent : SharedPhysicsMapComponent { - private float _timeToSleep; - private float _linSleepTolerance; - private float _angSleepTolerance; - - protected override void Initialize() - { - base.Initialize(); - var configManager = IoCManager.Resolve(); - configManager.OnValueChanged(CVars.TimeToSleep, SetTimeToSleep, true); - configManager.OnValueChanged(CVars.LinearSleepTolerance, SetLinearSleepTolerance, true); - configManager.OnValueChanged(CVars.AngularSleepTolerance, SetAngularSleepTolerance, true); - } - - protected override void OnRemove() - { - base.OnRemove(); - var configManager = IoCManager.Resolve(); - configManager.UnsubValueChanged(CVars.TimeToSleep, SetTimeToSleep); - configManager.UnsubValueChanged(CVars.LinearSleepTolerance, SetLinearSleepTolerance); - configManager.UnsubValueChanged(CVars.AngularSleepTolerance, SetAngularSleepTolerance); - } - - private void SetTimeToSleep(float value) => _timeToSleep = value; - - private void SetLinearSleepTolerance(float value) => _linSleepTolerance = value; - - private void SetAngularSleepTolerance(float value) => _angSleepTolerance = value; - - protected override void Cleanup(float frameTime) - { - var toRemove = new List(); - - // Because we're not predicting 99% of bodies its sleep timer never gets incremented so we'll just do it ourselves. - // (and serializing it over the network isn't necessary?) - // This is a client-only problem. - // Also need to suss out having the client build the island anyway and just... not solving it? - foreach (var body in AwakeBodies) - { - if (body.LinearVelocity.Length > _linSleepTolerance / 2f || body.AngularVelocity * body.AngularVelocity > _angSleepTolerance / 2f) continue; - body.SleepTime += frameTime; - if (body.SleepTime > _timeToSleep) - { - toRemove.Add(body); - } - } - - foreach (var body in toRemove) - { - body.Awake = false; - } - - base.Cleanup(frameTime); - } } } diff --git a/Robust.Client/Physics/PhysicsSystem.cs b/Robust.Client/Physics/PhysicsSystem.cs index 7f36897e2..6d61a4156 100644 --- a/Robust.Client/Physics/PhysicsSystem.cs +++ b/Robust.Client/Physics/PhysicsSystem.cs @@ -1,5 +1,10 @@ +using System.Collections.Generic; using JetBrains.Annotations; +using Robust.Shared.GameObjects; using Robust.Shared.IoC; +using Robust.Shared.Physics; +using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Dynamics; using Robust.Shared.Physics.Systems; using Robust.Shared.Timing; @@ -14,5 +19,73 @@ namespace Robust.Client.Physics { SimulateWorld(frameTime, _gameTiming.InPrediction); } + + protected override void Cleanup(SharedPhysicsMapComponent component, float frameTime) + { + var toRemove = new List(); + + // Because we're not predicting 99% of bodies its sleep timer never gets incremented so we'll just do it ourselves. + // (and serializing it over the network isn't necessary?) + // This is a client-only problem. + // Also need to suss out having the client build the island anyway and just... not solving it? + foreach (var body in component.AwakeBodies) + { + if (body.LinearVelocity.Length > LinearToleranceSqr / 2f || body.AngularVelocity * body.AngularVelocity > AngularToleranceSqr / 2f) continue; + body.SleepTime += frameTime; + if (body.SleepTime > TimeToSleep) + { + toRemove.Add(body); + } + } + + foreach (var body in toRemove) + { + body.Awake = false; + } + + base.Cleanup(component, frameTime); + } + + protected override void UpdateLerpData(SharedPhysicsMapComponent component, List bodies, EntityQuery xformQuery) + { + foreach (var body in bodies) + { + if (body.BodyType == BodyType.Static || + component.LerpData.TryGetValue(body.Owner, out var lerpData) || + !xformQuery.TryGetComponent(body.Owner, out var xform) || + lerpData.ParentUid == xform.ParentUid) + { + continue; + } + + component.LerpData[xform.Owner] = (xform.ParentUid, xform.LocalPosition, xform.LocalRotation); + } + } + + /// + /// Flush all of our lerping data. + /// + protected override void FinalStep(SharedPhysicsMapComponent component) + { + base.FinalStep(component); + var xformQuery = GetEntityQuery(); + + foreach (var (uid, (parentUid, position, rotation)) in component.LerpData) + { + if (!xformQuery.TryGetComponent(uid, out var xform) || + !parentUid.IsValid()) + { + continue; + } + + xform.PrevPosition = position; + xform.PrevRotation = rotation; + xform.LerpParent = parentUid; + xform.NextPosition = xform.LocalPosition; + xform.NextRotation = xform.LocalRotation; + } + + component.LerpData.Clear(); + } } } diff --git a/Robust.Shared/CVars.cs b/Robust.Shared/CVars.cs index f02bce20a..4c0545550 100644 --- a/Robust.Shared/CVars.cs +++ b/Robust.Shared/CVars.cs @@ -1019,13 +1019,6 @@ namespace Robust.Shared public static readonly CVarDef GridFixtureEnlargement = CVarDef.Create("physics.grid_fixture_enlargement", -PhysicsConstants.PolygonRadius, CVar.ARCHIVE | CVar.REPLICATED); - // - Contacts - public static readonly CVarDef ContactMultithreadThreshold = - CVarDef.Create("physics.contact_multithread_threshold", 32); - - public static readonly CVarDef ContactMinimumThreads = - CVarDef.Create("physics.contact_minimum_threads", 2); - // - Sleep public static readonly CVarDef AngularSleepTolerance = CVarDef.Create("physics.angsleeptol", 0.3f / 180.0f * MathF.PI); @@ -1041,17 +1034,6 @@ namespace Robust.Shared CVarDef.Create("physics.timetosleep", 0.2f); // - Solver - public static readonly CVarDef PositionConstraintsPerThread = - CVarDef.Create("physics.position_constraints_per_thread", 32); - - public static readonly CVarDef PositionConstraintsMinimumThread = - CVarDef.Create("physics.position_constraints_minimum_threads", 2); - - public static readonly CVarDef VelocityConstraintsPerThread = - CVarDef.Create("physics.velocity_constraints_per_thread", 32); - - public static readonly CVarDef VelocityConstraintMinimumThreads = - CVarDef.Create("physics.velocity_constraints_minimum_threads", 2); // These are the minimum recommended by Box2D with the standard being 8 velocity 3 position iterations. // Trade-off is obviously performance vs how long it takes to stabilise. diff --git a/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs b/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs index 795e78a35..f747454fe 100644 --- a/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs +++ b/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs @@ -2,188 +2,187 @@ using System.Collections.Generic; using Robust.Shared.Maths; using Robust.Shared.Physics.Dynamics; -namespace Robust.Shared.Physics.BroadPhase +namespace Robust.Shared.Physics.BroadPhase; + +public sealed class DynamicTreeBroadPhase : IBroadPhase { - public sealed class DynamicTreeBroadPhase : IBroadPhase + private readonly B2DynamicTree _tree; + + private readonly DynamicTree.ExtractAabbDelegate _extractAabb = ExtractAabbFunc; + + public DynamicTreeBroadPhase(int capacity) { - private readonly B2DynamicTree _tree; + _tree = new B2DynamicTree(capacity: capacity); + } - private readonly DynamicTree.ExtractAabbDelegate _extractAabb = ExtractAabbFunc; + public DynamicTreeBroadPhase() : this(256) {} - public DynamicTreeBroadPhase(int capacity) + private static Box2 ExtractAabbFunc(in FixtureProxy proxy) + { + return proxy.AABB; + } + + public int Count => _tree.NodeCount; + + public Box2 GetFatAabb(DynamicTree.Proxy proxy) + { + return _tree.GetFatAabb(proxy); + } + + public DynamicTree.Proxy AddProxy(ref FixtureProxy proxy) + { + var proxyId = _tree.CreateProxy(proxy.AABB, proxy); + return proxyId; + } + + public bool MoveProxy(DynamicTree.Proxy proxy, in Box2 aabb, Vector2 displacement) + { + return _tree.MoveProxy(proxy, in aabb, displacement); + } + + public void RemoveProxy(DynamicTree.Proxy proxy) + { + _tree.DestroyProxy(proxy); + } + + public FixtureProxy? GetProxy(DynamicTree.Proxy proxy) + { + return _tree.GetUserData(proxy); + } + + public void QueryAabb(DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx = false) + { + QueryAabb(ref callback, EasyQueryCallback, aabb, approx); + } + + public void QueryAabb(ref TState state, DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx = false) + { + var tuple = (state, _tree, callback, aabb, approx, _extractAabb); + _tree.Query(ref tuple, DelegateCache.AabbQueryState, aabb); + state = tuple.state; + } + + public IEnumerable QueryAabb(Box2 aabb, bool approx = false) + { + var list = new List(); + return QueryAabb(list, aabb, approx); + } + + public IEnumerable QueryAabb(List proxies, Box2 aabb, bool approx = false) + { + QueryAabb(ref proxies, (ref List lst, in FixtureProxy i) => { - _tree = new B2DynamicTree(capacity: capacity); - } + lst.Add(i); + return true; + }, aabb, approx); - public DynamicTreeBroadPhase() : this(256) {} + return proxies; + } - private static Box2 ExtractAabbFunc(in FixtureProxy proxy) - { - return proxy.AABB; - } + public void QueryPoint(DynamicTree.QueryCallbackDelegate callback, Vector2 point, bool approx = false) + { + QueryPoint(ref callback, EasyQueryCallback, point, approx); + } - public int Count => _tree.NodeCount; - - public Box2 GetFatAabb(DynamicTree.Proxy proxy) - { - return _tree.GetFatAabb(proxy); - } - - public DynamicTree.Proxy AddProxy(ref FixtureProxy proxy) - { - var proxyId = _tree.CreateProxy(proxy.AABB, proxy); - return proxyId; - } - - public void MoveProxy(DynamicTree.Proxy proxy, in Box2 aabb, Vector2 displacement) - { - _tree.MoveProxy(proxy, in aabb, displacement); - } - - public void RemoveProxy(DynamicTree.Proxy proxy) - { - _tree.DestroyProxy(proxy); - } - - public FixtureProxy? GetProxy(DynamicTree.Proxy proxy) - { - return _tree.GetUserData(proxy); - } - - public void QueryAabb(DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx = false) - { - QueryAabb(ref callback, EasyQueryCallback, aabb, approx); - } - - public void QueryAabb(ref TState state, DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx = false) - { - var tuple = (state, _tree, callback, aabb, approx, _extractAabb); - _tree.Query(ref tuple, DelegateCache.AabbQueryState, aabb); - state = tuple.state; - } - - public IEnumerable QueryAabb(Box2 aabb, bool approx = false) - { - var list = new List(); - return QueryAabb(list, aabb, approx); - } - - public IEnumerable QueryAabb(List proxies, Box2 aabb, bool approx = false) - { - QueryAabb(ref proxies, (ref List lst, in FixtureProxy i) => + public void QueryPoint(ref TState state, DynamicTree.QueryCallbackDelegate callback, Vector2 point, bool approx = false) + { + var tuple = (state, _tree, callback, point, approx, _extractAabb); + _tree.Query(ref tuple, + (ref (TState state, B2DynamicTree tree, DynamicTree.QueryCallbackDelegate callback, Vector2 point, bool approx, DynamicTree.ExtractAabbDelegate extract) tuple, + DynamicTree.Proxy proxy) => { - lst.Add(i); - return true; - }, aabb, approx); + var item = tuple.tree.GetUserData(proxy)!; - return proxies; - } - - public void QueryPoint(DynamicTree.QueryCallbackDelegate callback, Vector2 point, bool approx = false) - { - QueryPoint(ref callback, EasyQueryCallback, point, approx); - } - - public void QueryPoint(ref TState state, DynamicTree.QueryCallbackDelegate callback, Vector2 point, bool approx = false) - { - var tuple = (state, _tree, callback, point, approx, _extractAabb); - _tree.Query(ref tuple, - (ref (TState state, B2DynamicTree tree, DynamicTree.QueryCallbackDelegate callback, Vector2 point, bool approx, DynamicTree.ExtractAabbDelegate extract) tuple, - DynamicTree.Proxy proxy) => + if (!tuple.approx) { - var item = tuple.tree.GetUserData(proxy)!; - - if (!tuple.approx) + var precise = tuple.extract(item); + if (!precise.Contains(tuple.point)) { - var precise = tuple.extract(item); - if (!precise.Contains(tuple.point)) - { - return true; - } + return true; } + } - return tuple.callback(ref tuple.state, item); - }, Box2.CenteredAround(point, new Vector2(0.1f, 0.1f))); - state = tuple.state; - } + return tuple.callback(ref tuple.state, item); + }, Box2.CenteredAround(point, new Vector2(0.1f, 0.1f))); + state = tuple.state; + } - public IEnumerable QueryPoint(Vector2 point, bool approx = false) + public IEnumerable QueryPoint(Vector2 point, bool approx = false) + { + var list = new List(); + + QueryPoint(ref list, (ref List list, in FixtureProxy i) => { - var list = new List(); + list.Add(i); + return true; + }, point, approx); - QueryPoint(ref list, (ref List list, in FixtureProxy i) => + return list; + } + + private static readonly DynamicTree.QueryCallbackDelegate.QueryCallbackDelegate> EasyQueryCallback = + (ref DynamicTree.QueryCallbackDelegate s, in FixtureProxy v) => s(v); + + public void QueryRay(ref TState state, DynamicTree.RayQueryCallbackDelegate callback, in Ray ray, bool approx = false) + { + var tuple = (state, callback, _tree, approx ? null : _extractAabb, ray); + _tree.RayCast(ref tuple, DelegateCache.RayQueryState, ray); + state = tuple.state; + } + + private static bool AabbQueryStateCallback(ref (TState state, B2DynamicTree tree, DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx, DynamicTree.ExtractAabbDelegate extract) tuple, DynamicTree.Proxy proxy) + { + var item = tuple.tree.GetUserData(proxy)!; + if (!tuple.approx) + { + var precise = tuple.extract(item); + if (!precise.Intersects(tuple.aabb)) { - list.Add(i); return true; - }, point, approx); - - return list; - } - - private static readonly DynamicTree.QueryCallbackDelegate.QueryCallbackDelegate> EasyQueryCallback = - (ref DynamicTree.QueryCallbackDelegate s, in FixtureProxy v) => s(v); - - public void QueryRay(ref TState state, DynamicTree.RayQueryCallbackDelegate callback, in Ray ray, bool approx = false) - { - var tuple = (state, callback, _tree, approx ? null : _extractAabb, ray); - _tree.RayCast(ref tuple, DelegateCache.RayQueryState, ray); - state = tuple.state; - } - - private static bool AabbQueryStateCallback(ref (TState state, B2DynamicTree tree, DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx, DynamicTree.ExtractAabbDelegate extract) tuple, DynamicTree.Proxy proxy) - { - var item = tuple.tree.GetUserData(proxy)!; - if (!tuple.approx) - { - var precise = tuple.extract(item); - if (!precise.Intersects(tuple.aabb)) - { - return true; - } } - - return tuple.callback(ref tuple.state, item); } - private static bool RayQueryStateCallback(ref (TState state, DynamicTree.RayQueryCallbackDelegate callback, B2DynamicTree tree, DynamicTree.ExtractAabbDelegate? extract, Ray srcRay) tuple, DynamicTree.Proxy proxy, in Vector2 hitPos, float distance) - { - var item = tuple.tree.GetUserData(proxy)!; - var hit = hitPos; + return tuple.callback(ref tuple.state, item); + } - if (tuple.extract != null) + private static bool RayQueryStateCallback(ref (TState state, DynamicTree.RayQueryCallbackDelegate callback, B2DynamicTree tree, DynamicTree.ExtractAabbDelegate? extract, Ray srcRay) tuple, DynamicTree.Proxy proxy, in Vector2 hitPos, float distance) + { + var item = tuple.tree.GetUserData(proxy)!; + var hit = hitPos; + + if (tuple.extract != null) + { + var precise = tuple.extract(item); + if (!tuple.srcRay.Intersects(precise, out distance, out hit)) { - var precise = tuple.extract(item); - if (!tuple.srcRay.Intersects(precise, out distance, out hit)) - { - return true; - } + return true; } - - return tuple.callback(ref tuple.state, item, hit, distance); } - public void QueryRay(DynamicTree.RayQueryCallbackDelegate callback, in Ray ray, bool approx = false) - { - QueryRay(ref callback, RayQueryDelegateCallbackInst, ray, approx); - } + return tuple.callback(ref tuple.state, item, hit, distance); + } - private static readonly DynamicTree.RayQueryCallbackDelegate.RayQueryCallbackDelegate> RayQueryDelegateCallbackInst = RayQueryDelegateCallback; + public void QueryRay(DynamicTree.RayQueryCallbackDelegate callback, in Ray ray, bool approx = false) + { + QueryRay(ref callback, RayQueryDelegateCallbackInst, ray, approx); + } - private static bool RayQueryDelegateCallback(ref DynamicTree.RayQueryCallbackDelegate state, in FixtureProxy value, in Vector2 point, float distFromOrigin) - { - return state(value, point, distFromOrigin); - } + private static readonly DynamicTree.RayQueryCallbackDelegate.RayQueryCallbackDelegate> RayQueryDelegateCallbackInst = RayQueryDelegateCallback; - private static class DelegateCache - { - public static readonly - B2DynamicTree.QueryCallback<(TState state, B2DynamicTree tree, DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx, DynamicTree.ExtractAabbDelegate extract)> AabbQueryState = - AabbQueryStateCallback; + private static bool RayQueryDelegateCallback(ref DynamicTree.RayQueryCallbackDelegate state, in FixtureProxy value, in Vector2 point, float distFromOrigin) + { + return state(value, point, distFromOrigin); + } - public static readonly - B2DynamicTree.RayQueryCallback<(TState state, DynamicTree.RayQueryCallbackDelegate callback, - B2DynamicTree tree, DynamicTree.ExtractAabbDelegate? extract, Ray srcRay)> RayQueryState = - RayQueryStateCallback; - } + private static class DelegateCache + { + public static readonly + B2DynamicTree.QueryCallback<(TState state, B2DynamicTree tree, DynamicTree.QueryCallbackDelegate callback, Box2 aabb, bool approx, DynamicTree.ExtractAabbDelegate extract)> AabbQueryState = + AabbQueryStateCallback; + + public static readonly + B2DynamicTree.RayQueryCallback<(TState state, DynamicTree.RayQueryCallbackDelegate callback, + B2DynamicTree tree, DynamicTree.ExtractAabbDelegate? extract, Ray srcRay)> RayQueryState = + RayQueryStateCallback; } } diff --git a/Robust.Shared/Physics/Dynamics/ContactManager.cs b/Robust.Shared/Physics/Dynamics/ContactManager.cs index da981d367..679652162 100644 --- a/Robust.Shared/Physics/Dynamics/ContactManager.cs +++ b/Robust.Shared/Physics/Dynamics/ContactManager.cs @@ -32,7 +32,6 @@ using System.Buffers; using System.Collections.Generic; using System.Threading.Tasks; using Microsoft.Extensions.ObjectPool; -using Robust.Shared.Configuration; using Robust.Shared.GameObjects; using Robust.Shared.Map; using Robust.Shared.Maths; @@ -50,7 +49,6 @@ namespace Robust.Shared.Physics.Dynamics { private readonly IEntityManager _entityManager; private readonly IPhysicsManager _physicsManager; - private readonly IConfigurationManager _cfg; private EntityLookupSystem _lookup = default!; private SharedPhysicsSystem _physics = default!; @@ -100,7 +98,7 @@ namespace Robust.Shared.Physics.Dynamics private readonly ObjectPool _contactPool; - internal LinkedList _activeContacts = new(); + internal readonly LinkedList _activeContacts = new(); // Didn't use the eventbus because muh allocs on something being run for every collision every frame. /// @@ -108,8 +106,7 @@ namespace Robust.Shared.Physics.Dynamics /// internal event Action? KinematicControllerCollision; - private int _contactMultithreadThreshold; - private int _contactMinimumThreads; + private const int ContactsPerThread = 32; // TODO: Also need to clean the station up to not have 160 contacts on roundstart @@ -149,16 +146,14 @@ namespace Robust.Shared.Physics.Dynamics SharedDebugPhysicsSystem debugPhysicsSystem, IManifoldManager manifoldManager, IEntityManager entityManager, - IPhysicsManager physicsManager, - IConfigurationManager cfg) + IPhysicsManager physicsManager) { _entityManager = entityManager; _physicsManager = physicsManager; - _cfg = cfg; _contactPool = new DefaultObjectPool( new ContactPoolPolicy(debugPhysicsSystem, manifoldManager), - 1024); + 4096); } private static void SetContact(Contact contact, Fixture? fixtureA, int indexA, Fixture? fixtureB, int indexB) @@ -192,26 +187,11 @@ namespace Robust.Shared.Physics.Dynamics _physics = _entityManager.EntitySysManager.GetEntitySystem(); _transform = _entityManager.EntitySysManager.GetEntitySystem(); - _cfg.OnValueChanged(CVars.ContactMultithreadThreshold, OnContactMultithreadThreshold, true); - _cfg.OnValueChanged(CVars.ContactMinimumThreads, OnContactMinimumThreads, true); - InitializePool(); } public void Shutdown() { - _cfg.UnsubValueChanged(CVars.ContactMultithreadThreshold, OnContactMultithreadThreshold); - _cfg.UnsubValueChanged(CVars.ContactMinimumThreads, OnContactMinimumThreads); - } - - private void OnContactMultithreadThreshold(int value) - { - _contactMultithreadThreshold = value; - } - - private void OnContactMinimumThreads(int value) - { - _contactMinimumThreads = value; } private void InitializePool() @@ -552,14 +532,14 @@ namespace Robust.Shared.Physics.Dynamics { var wake = ArrayPool.Shared.Rent(count); - if (count > _contactMultithreadThreshold * _contactMinimumThreads) + if (count > ContactsPerThread * 2) { - var (batches, batchSize) = SharedPhysicsSystem.GetBatch(count, _contactMultithreadThreshold); + var batches = (int) Math.Ceiling((float) count / ContactsPerThread); Parallel.For(0, batches, i => { - var start = i * batchSize; - var end = Math.Min(start + batchSize, count); + var start = i * ContactsPerThread; + var end = Math.Min(start + ContactsPerThread, count); UpdateContacts(contacts, start, end, status, wake); }); diff --git a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs index 506ecf7ea..8b93cff79 100644 --- a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs +++ b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs @@ -35,6 +35,7 @@ using Robust.Shared.Maths; using Robust.Shared.Physics.Collision; using Robust.Shared.Physics.Collision.Shapes; using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Systems; namespace Robust.Shared.Physics.Dynamics.Contacts { @@ -139,7 +140,7 @@ namespace Robust.Shared.Physics.Dynamics.Contacts var bodyATransform = physicsManager.EnsureTransform(bodyA); var bodyBTransform = physicsManager.EnsureTransform(bodyB); - ContactSolver.InitializeManifold(ref Manifold, bodyATransform, bodyBTransform, shapeA.Radius, shapeB.Radius, out normal, points); + SharedPhysicsSystem.InitializeManifold(ref Manifold, bodyATransform, bodyBTransform, shapeA.Radius, shapeB.Radius, out normal, points); } /// diff --git a/Robust.Shared/Physics/Dynamics/Contacts/ContactSolver.cs b/Robust.Shared/Physics/Dynamics/Contacts/ContactSolver.cs deleted file mode 100644 index 53e0fb9a7..000000000 --- a/Robust.Shared/Physics/Dynamics/Contacts/ContactSolver.cs +++ /dev/null @@ -1,935 +0,0 @@ -/* -* 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.Numerics; -using System.Threading; -using System.Threading.Tasks; -using Robust.Shared.GameObjects; -using Robust.Shared.Physics.Collision; -using Robust.Shared.Physics.Systems; -using Robust.Shared.Utility; -using Vector2 = Robust.Shared.Maths.Vector2; - -namespace Robust.Shared.Physics.Dynamics.Contacts -{ - internal sealed class ContactSolver - { - private bool _warmStarting; - private float _velocityThreshold; - private float _baumgarte; - private float _maxLinearCorrection; - private float _maxAngularCorrection; - - private Vector2[] _linearVelocities = Array.Empty(); - private float[] _angularVelocities = Array.Empty(); - - private Vector2[] _positions = Array.Empty(); - private float[] _angles = Array.Empty(); - - private Contact[] _contacts = Array.Empty(); - private int _contactCount; - - private ContactVelocityConstraint[] _velocityConstraints = Array.Empty(); - private ContactPositionConstraint[] _positionConstraints = Array.Empty(); - - private int _velocityConstraintsPerThread; - private int _velocityConstraintsMinimumThreads; - private int _positionConstraintsPerThread; - private int _positionConstraintsMinimumThreads; - - public void LoadConfig(in IslandCfg cfg) - { - _warmStarting = cfg.WarmStarting; - _velocityThreshold = cfg.VelocityThreshold; - _baumgarte = cfg.Baumgarte; - _maxLinearCorrection = cfg.MaxLinearCorrection; - _maxAngularCorrection = cfg.MaxAngularCorrection; - _positionConstraintsPerThread = cfg.PositionConstraintsPerThread; - _positionConstraintsMinimumThreads = cfg.PositionConstraintsMinimumThreads; - _velocityConstraintsPerThread = cfg.VelocityConstraintsPerThread; - _velocityConstraintsMinimumThreads = cfg.VelocityConstraintsMinimumThreads; - } - - public void Reset(SolverData data, int contactCount, Contact[] contacts) - { - _linearVelocities = data.LinearVelocities; - _angularVelocities = data.AngularVelocities; - - _positions = data.Positions; - _positions = data.Positions; - _angles = data.Angles; - - _contactCount = contactCount; - _contacts = contacts; - - // If we need more constraints then grow the cached arrays - if (_velocityConstraints.Length < contactCount) - { - var oldLength = _velocityConstraints.Length; - - Array.Resize(ref _velocityConstraints, contactCount * 2); - Array.Resize(ref _positionConstraints, contactCount * 2); - - for (var i = oldLength; i < _velocityConstraints.Length; i++) - { - var velocity = new ContactVelocityConstraint - { - K = new Vector4(), - Points = new VelocityConstraintPoint[2], - NormalMass = new Vector4(), - }; - - for (var j = 0; j < 2; j++) - { - velocity.Points[j] = new VelocityConstraintPoint(); - } - - _velocityConstraints[i] = velocity; - - var position = new ContactPositionConstraint() - { - LocalPoints = new Vector2[2], - }; - - for (var j = 0; j < 2; j++) - { - position.LocalPoints[j] = Vector2.Zero; - } - - _positionConstraints[i] = position; - } - } - - // Build constraints - // For now these are going to be bare but will change - for (var i = 0; i < _contactCount; i++) - { - var contact = contacts[i]; - Fixture fixtureA = contact.FixtureA!; - Fixture fixtureB = contact.FixtureB!; - var shapeA = fixtureA.Shape; - var shapeB = fixtureB.Shape; - float radiusA = shapeA.Radius; - float radiusB = shapeB.Radius; - var bodyA = fixtureA.Body; - var bodyB = fixtureB.Body; - var manifold = contact.Manifold; - - int pointCount = manifold.PointCount; - DebugTools.Assert(pointCount > 0); - - ref var velocityConstraint = ref _velocityConstraints[i]; - velocityConstraint.Friction = contact.Friction; - velocityConstraint.Restitution = contact.Restitution; - velocityConstraint.TangentSpeed = contact.TangentSpeed; - velocityConstraint.IndexA = bodyA.IslandIndex[data.IslandIndex]; - velocityConstraint.IndexB = bodyB.IslandIndex[data.IslandIndex]; - - var (invMassA, invMassB) = GetInvMass(bodyA, bodyB); - - (velocityConstraint.InvMassA, velocityConstraint.InvMassB) = (invMassA, invMassB); - velocityConstraint.InvIA = bodyA.InvI; - velocityConstraint.InvIB = bodyB.InvI; - velocityConstraint.ContactIndex = i; - velocityConstraint.PointCount = pointCount; - - velocityConstraint.K = Vector4.Zero; - velocityConstraint.NormalMass = Vector4.Zero; - - ref var positionConstraint = ref _positionConstraints[i]; - positionConstraint.IndexA = bodyA.IslandIndex[data.IslandIndex]; - positionConstraint.IndexB = bodyB.IslandIndex[data.IslandIndex]; - (positionConstraint.InvMassA, positionConstraint.InvMassB) = (invMassA, invMassB); - positionConstraint.LocalCenterA = bodyA.LocalCenter; - positionConstraint.LocalCenterB = bodyB.LocalCenter; - - positionConstraint.InvIA = bodyA.InvI; - positionConstraint.InvIB = bodyB.InvI; - positionConstraint.LocalNormal = manifold.LocalNormal; - positionConstraint.LocalPoint = manifold.LocalPoint; - positionConstraint.PointCount = pointCount; - positionConstraint.RadiusA = radiusA; - positionConstraint.RadiusB = radiusB; - positionConstraint.Type = manifold.Type; - - for (var j = 0; j < pointCount; ++j) - { - var contactPoint = manifold.Points[j]; - ref var constraintPoint = ref velocityConstraint.Points[j]; - - if (_warmStarting) - { - constraintPoint.NormalImpulse = data.DtRatio * contactPoint.NormalImpulse; - constraintPoint.TangentImpulse = data.DtRatio * contactPoint.TangentImpulse; - } - else - { - constraintPoint.NormalImpulse = 0.0f; - constraintPoint.TangentImpulse = 0.0f; - } - - constraintPoint.RelativeVelocityA = Vector2.Zero; - constraintPoint.RelativeVelocityB = Vector2.Zero; - constraintPoint.NormalMass = 0.0f; - constraintPoint.TangentMass = 0.0f; - constraintPoint.VelocityBias = 0.0f; - - positionConstraint.LocalPoints[j] = contactPoint.LocalPoint; - } - } - } - - private (float, float) GetInvMass(IPhysBody bodyA, IPhysBody bodyB) - { - // God this is shitcodey but uhhhh we need to snowflake KinematicController for nice collisions. - // TODO: Might need more finagling with the kinematic bodytype - switch (bodyA.BodyType) - { - case BodyType.Kinematic: - case BodyType.Static: - return (bodyA.InvMass, bodyB.InvMass); - case BodyType.KinematicController: - switch (bodyB.BodyType) - { - case BodyType.Kinematic: - case BodyType.Static: - return (bodyA.InvMass, bodyB.InvMass); - case BodyType.Dynamic: - return (bodyA.InvMass, 0f); - case BodyType.KinematicController: - return (0f, 0f); - default: - throw new ArgumentOutOfRangeException(); - } - case BodyType.Dynamic: - switch (bodyB.BodyType) - { - case BodyType.Kinematic: - case BodyType.Static: - case BodyType.Dynamic: - return (bodyA.InvMass, bodyB.InvMass); - case BodyType.KinematicController: - return (0f, bodyB.InvMass); - default: - throw new ArgumentOutOfRangeException(); - } - default: - throw new ArgumentOutOfRangeException(); - } - } - - public void InitializeVelocityConstraints() - { - Span points = stackalloc Vector2[2]; - - for (var i = 0; i < _contactCount; ++i) - { - ref var velocityConstraint = ref _velocityConstraints[i]; - var positionConstraint = _positionConstraints[i]; - - var radiusA = positionConstraint.RadiusA; - var radiusB = positionConstraint.RadiusB; - var manifold = _contacts[velocityConstraint.ContactIndex].Manifold; - - var indexA = velocityConstraint.IndexA; - var indexB = velocityConstraint.IndexB; - - var invMassA = velocityConstraint.InvMassA; - var invMassB = velocityConstraint.InvMassB; - var invIA = velocityConstraint.InvIA; - var invIB = velocityConstraint.InvIB; - var localCenterA = positionConstraint.LocalCenterA; - var localCenterB = positionConstraint.LocalCenterB; - - var centerA = _positions[indexA]; - var angleA = _angles[indexA]; - var linVelocityA = _linearVelocities[indexA]; - var angVelocityA = _angularVelocities[indexA]; - - var centerB = _positions[indexB]; - var angleB = _angles[indexB]; - var linVelocityB = _linearVelocities[indexB]; - var angVelocityB = _angularVelocities[indexB]; - - DebugTools.Assert(manifold.PointCount > 0); - - var xfA = new Transform(angleA); - var xfB = new Transform(angleB); - xfA.Position = centerA - Transform.Mul(xfA.Quaternion2D, localCenterA); - xfB.Position = centerB - Transform.Mul(xfB.Quaternion2D, localCenterB); - - InitializeManifold(ref manifold, xfA, xfB, radiusA, radiusB, out var normal, points); - - velocityConstraint.Normal = normal; - - int pointCount = velocityConstraint.PointCount; - - for (int j = 0; j < pointCount; ++j) - { - ref var vcp = ref velocityConstraint.Points[j]; - - vcp.RelativeVelocityA = points[j] - centerA; - vcp.RelativeVelocityB = points[j] - centerB; - - float rnA = Vector2.Cross(vcp.RelativeVelocityA, velocityConstraint.Normal); - float rnB = Vector2.Cross(vcp.RelativeVelocityB, velocityConstraint.Normal); - - float kNormal = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; - - vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - - Vector2 tangent = Vector2.Cross(velocityConstraint.Normal, 1.0f); - - float rtA = Vector2.Cross(vcp.RelativeVelocityA, tangent); - float rtB = Vector2.Cross(vcp.RelativeVelocityB, tangent); - - float kTangent = invMassA + invMassB + invIA * rtA * rtA + invIB * rtB * rtB; - - vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; - - // Setup a velocity bias for restitution. - vcp.VelocityBias = 0.0f; - float vRel = Vector2.Dot(velocityConstraint.Normal, linVelocityB + Vector2.Cross(angVelocityB, vcp.RelativeVelocityB) - linVelocityA - Vector2.Cross(angVelocityA, vcp.RelativeVelocityA)); - if (vRel < -_velocityThreshold) - { - vcp.VelocityBias = -velocityConstraint.Restitution * vRel; - } - } - - // If we have two points, then prepare the block solver. - if (velocityConstraint.PointCount == 2) - { - var vcp1 = velocityConstraint.Points[0]; - var vcp2 = velocityConstraint.Points[1]; - - var rn1A = Vector2.Cross(vcp1.RelativeVelocityA, velocityConstraint.Normal); - var rn1B = Vector2.Cross(vcp1.RelativeVelocityB, velocityConstraint.Normal); - var rn2A = Vector2.Cross(vcp2.RelativeVelocityA, velocityConstraint.Normal); - var rn2B = Vector2.Cross(vcp2.RelativeVelocityB, velocityConstraint.Normal); - - var k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; - var k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; - var k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; - - // Ensure a reasonable condition number. - const float k_maxConditionNumber = 1000.0f; - if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) - { - // K is safe to invert. - velocityConstraint.K = new Vector4(k11, k12, k12, k22); - - velocityConstraint.NormalMass = Vector4Helpers.Inverse(velocityConstraint.K); - } - else - { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - velocityConstraint.PointCount = 1; - } - } - } - } - - public void WarmStart() - { - for (var i = 0; i < _contactCount; ++i) - { - var velocityConstraint = _velocityConstraints[i]; - - var indexA = velocityConstraint.IndexA; - var indexB = velocityConstraint.IndexB; - var invMassA = velocityConstraint.InvMassA; - var invIA = velocityConstraint.InvIA; - var invMassB = velocityConstraint.InvMassB; - var invIB = velocityConstraint.InvIB; - var pointCount = velocityConstraint.PointCount; - - ref var linVelocityA = ref _linearVelocities[indexA]; - ref var angVelocityA = ref _angularVelocities[indexA]; - ref var linVelocityB = ref _linearVelocities[indexB]; - ref var angVelocityB = ref _angularVelocities[indexB]; - - var normal = velocityConstraint.Normal; - var tangent = Vector2.Cross(normal, 1.0f); - - for (var j = 0; j < pointCount; ++j) - { - var constraintPoint = velocityConstraint.Points[j]; - var P = normal * constraintPoint.NormalImpulse + tangent * constraintPoint.TangentImpulse; - angVelocityA -= invIA * Vector2.Cross(constraintPoint.RelativeVelocityA, P); - linVelocityA -= P * invMassA; - angVelocityB += invIB * Vector2.Cross(constraintPoint.RelativeVelocityB, P); - linVelocityB += P * invMassB; - } - } - } - - public void SolveVelocityConstraints() - { - if (_contactCount > _velocityConstraintsPerThread * _velocityConstraintsMinimumThreads) - { - var (batches, batchSize) = SharedPhysicsSystem.GetBatch(_contactCount, _velocityConstraintsPerThread); - Parallel.For(0, batches, i => - { - var start = i * batchSize; - var end = Math.Min(start + batchSize, _contactCount); - SolveVelocityConstraints(start, end); - }); - } - else - { - SolveVelocityConstraints(0, _contactCount); - } - } - - public void SolveVelocityConstraints(int start, int end) - { - // Here be dragons - for (var i = start; i < end; ++i) - { - ref var velocityConstraint = ref _velocityConstraints[i]; - - var indexA = velocityConstraint.IndexA; - var indexB = velocityConstraint.IndexB; - var mA = velocityConstraint.InvMassA; - var iA = velocityConstraint.InvIA; - var mB = velocityConstraint.InvMassB; - var iB = velocityConstraint.InvIB; - var pointCount = velocityConstraint.PointCount; - - ref var vA = ref _linearVelocities[indexA]; - ref var wA = ref _angularVelocities[indexA]; - ref var vB = ref _linearVelocities[indexB]; - ref var wB = ref _angularVelocities[indexB]; - - var normal = velocityConstraint.Normal; - var tangent = Vector2.Cross(normal, 1.0f); - var friction = velocityConstraint.Friction; - - DebugTools.Assert(pointCount == 1 || pointCount == 2); - - // Solve tangent constraints first because non-penetration is more important - // than friction. - for (var j = 0; j < pointCount; ++j) - { - ref var velConstraintPoint = ref velocityConstraint.Points[j]; - - // Relative velocity at contact - var dv = vB + Vector2.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2.Cross(wA, velConstraintPoint.RelativeVelocityA); - - // Compute tangent force - float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed; - float lambda = velConstraintPoint.TangentMass * (-vt); - - // b2Clamp the accumulated force - var maxFriction = friction * velConstraintPoint.NormalImpulse; - var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - velConstraintPoint.TangentImpulse; - velConstraintPoint.TangentImpulse = newImpulse; - - // Apply contact impulse - Vector2 P = tangent * lambda; - - vA -= P * mA; - wA -= iA * Vector2.Cross(velConstraintPoint.RelativeVelocityA, P); - - vB += P * mB; - wB += iB * Vector2.Cross(velConstraintPoint.RelativeVelocityB, P); - } - - // Solve normal constraints - if (velocityConstraint.PointCount == 1) - { - ref var vcp = ref velocityConstraint.Points[0]; - - // Relative velocity at contact - Vector2 dv = vB + Vector2.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2.Cross(wA, vcp.RelativeVelocityA); - - // Compute normal impulse - float vn = Vector2.Dot(dv, normal); - float lambda = -vcp.NormalMass * (vn - vcp.VelocityBias); - - // b2Clamp the accumulated impulse - float newImpulse = Math.Max(vcp.NormalImpulse + lambda, 0.0f); - lambda = newImpulse - vcp.NormalImpulse; - vcp.NormalImpulse = newImpulse; - - // Apply contact impulse - Vector2 P = normal * lambda; - vA -= P * mA; - wA -= iA * Vector2.Cross(vcp.RelativeVelocityA, P); - - vB += P * mB; - wB += iB * Vector2.Cross(vcp.RelativeVelocityB, P); - } - else - { - // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). - // Build the mini LCP for this contact patch - // - // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 - // - // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) - // b = vn0 - velocityBias - // - // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i - // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases - // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid - // solution that satisfies the problem is chosen. - // - // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires - // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). - // - // Substitute: - // - // x = a + d - // - // a := old total impulse - // x := new total impulse - // d := incremental impulse - // - // For the current iteration we extend the formula for the incremental impulse - // to compute the new total impulse: - // - // vn = A * d + b - // = A * (x - a) + b - // = A * x + b - A * a - // = A * x + b' - // b' = b - A * a; - - ref var cp1 = ref velocityConstraint.Points[0]; - ref var cp2 = ref velocityConstraint.Points[1]; - - Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse); - DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f); - - // Relative velocity at contact - Vector2 dv1 = vB + Vector2.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2.Cross(wA, cp1.RelativeVelocityA); - Vector2 dv2 = vB + Vector2.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2.Cross(wA, cp2.RelativeVelocityA); - - // Compute normal velocity - float vn1 = Vector2.Dot(dv1, normal); - float vn2 = Vector2.Dot(dv2, normal); - - Vector2 b = new Vector2 - { - X = vn1 - cp1.VelocityBias, - Y = vn2 - cp2.VelocityBias - }; - - // Compute b' - b -= Transform.Mul(velocityConstraint.K, a); - - //const float k_errorTol = 1e-3f; - //B2_NOT_USED(k_errorTol); - - for (; ; ) - { - // - // Case 1: vn = 0 - // - // 0 = A * x + b' - // - // Solve for x: - // - // x = - inv(A) * b' - // - Vector2 x = -Transform.Mul(velocityConstraint.NormalMass, b); - - if (x.X >= 0.0f && x.Y >= 0.0f) - { - // Get the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = normal * d.X; - Vector2 P2 = normal * d.Y; - vA -= (P1 + P2) * mA; - wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); - - vB += (P1 + P2) * mB; - wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - - break; - } - - // - // Case 2: vn1 = 0 and x2 = 0 - // - // 0 = a11 * x1 + a12 * 0 + b1' - // vn2 = a21 * x1 + a22 * 0 + b2' - // - x.X = -cp1.NormalMass * b.X; - x.Y = 0.0f; - vn1 = 0.0f; - vn2 = velocityConstraint.K.Y * x.X + b.Y; - - if (x.X >= 0.0f && vn2 >= 0.0f) - { - // Get the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = normal * d.X; - Vector2 P2 = normal * d.Y; - vA -= (P1 + P2) * mA; - wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); - - vB += (P1 + P2) * mB; - wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - - break; - } - - - // - // Case 3: vn2 = 0 and x1 = 0 - // - // vn1 = a11 * 0 + a12 * x2 + b1' - // 0 = a21 * 0 + a22 * x2 + b2' - // - x.X = 0.0f; - x.Y = -cp2.NormalMass * b.Y; - vn1 = velocityConstraint.K.Z * x.Y + b.X; - vn2 = 0.0f; - - if (x.Y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = normal * d.X; - Vector2 P2 = normal * d.Y; - vA -= (P1 + P2) * mA; - wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); - - vB += (P1 + P2) * mB; - wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.X = 0.0f; - x.Y = 0.0f; - vn1 = b.X; - vn2 = b.Y; - - if (vn1 >= 0.0f && vn2 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = normal * d.X; - Vector2 P2 = normal * d.Y; - vA -= (P1 + P2) * mA; - wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); - - vB += (P1 + P2) * mB; - wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } - } - } - } - - public void StoreImpulses() - { - for (var i = 0; i < _contactCount; ++i) - { - ContactVelocityConstraint velocityConstraint = _velocityConstraints[i]; - ref var manifold = ref _contacts[velocityConstraint.ContactIndex].Manifold; - - for (var j = 0; j < velocityConstraint.PointCount; ++j) - { - ref var point = ref manifold.Points[j]; - point.NormalImpulse = velocityConstraint.Points[j].NormalImpulse; - point.TangentImpulse = velocityConstraint.Points[j].TangentImpulse; - } - } - } - - public bool SolvePositionConstraints() - { - if (_contactCount > _positionConstraintsPerThread * _positionConstraintsMinimumThreads) - { - var unsolved = 0; - - var (batches, batchSize) = SharedPhysicsSystem.GetBatch(_contactCount, _positionConstraintsPerThread); - Parallel.For(0, batches, i => - { - var start = i * batchSize; - var end = Math.Min(start + batchSize, _contactCount); - if (!SolvePositionConstraints(start, end)) - Interlocked.Increment(ref unsolved); - }); - - return unsolved == 0; - } - - return SolvePositionConstraints(0, _contactCount); - } - - /// - /// Tries to solve positions for all contacts specified. - /// - /// true if all positions solved - public bool SolvePositionConstraints(int start, int end) - { - float minSeparation = 0.0f; - - for (int i = start; i < end; ++i) - { - var pc = _positionConstraints[i]; - - int indexA = pc.IndexA; - int indexB = pc.IndexB; - Vector2 localCenterA = pc.LocalCenterA; - float mA = pc.InvMassA; - float iA = pc.InvIA; - Vector2 localCenterB = pc.LocalCenterB; - float mB = pc.InvMassB; - float iB = pc.InvIB; - int pointCount = pc.PointCount; - - ref var centerA = ref _positions[indexA]; - ref var angleA = ref _angles[indexA]; - ref var centerB = ref _positions[indexB]; - ref var angleB = ref _angles[indexB]; - - // Solve normal constraints - for (int j = 0; j < pointCount; ++j) - { - Transform xfA = new Transform(angleA); - Transform xfB = new Transform(angleB); - xfA.Position = centerA - Transform.Mul(xfA.Quaternion2D, localCenterA); - xfB.Position = centerB - Transform.Mul(xfB.Quaternion2D, localCenterB); - - Vector2 normal; - Vector2 point; - float separation; - - PositionSolverManifoldInitialize(pc, j, xfA, xfB, out normal, out point, out separation); - - Vector2 rA = point - centerA; - Vector2 rB = point - centerB; - - // Track max constraint error. - minSeparation = Math.Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float C = Math.Clamp(_baumgarte * (separation + PhysicsConstants.LinearSlop), -_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float rnA = Vector2.Cross(rA, normal); - float rnB = Vector2.Cross(rB, normal); - float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float impulse = K > 0.0f ? -C / K : 0.0f; - - Vector2 P = normal * impulse; - - centerA -= P * mA; - angleA -= iA * Vector2.Cross(rA, P); - - centerB += P * mB; - angleB += iB * Vector2.Cross(rB, P); - } - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -3.0f * PhysicsConstants.LinearSlop; - } - - /// - /// Evaluate the manifold with supplied transforms. This assumes - /// modest motion from the original state. This does not change the - /// point count, impulses, etc. The radii must come from the Shapes - /// that generated the manifold. - /// - internal static void InitializeManifold( - ref Manifold manifold, - in Transform xfA, - in Transform xfB, - float radiusA, - float radiusB, - out Vector2 normal, - Span points) - { - normal = Vector2.Zero; - - if (manifold.PointCount == 0) - { - return; - } - - switch (manifold.Type) - { - case ManifoldType.Circles: - { - normal = new Vector2(1.0f, 0.0f); - Vector2 pointA = Transform.Mul(xfA, manifold.LocalPoint); - Vector2 pointB = Transform.Mul(xfB, manifold.Points[0].LocalPoint); - - if ((pointA - pointB).LengthSquared > float.Epsilon * float.Epsilon) - { - normal = pointB - pointA; - normal = normal.Normalized; - } - - Vector2 cA = pointA + normal * radiusA; - Vector2 cB = pointB - normal * radiusB; - points[0] = (cA + cB) * 0.5f; - } - break; - - case ManifoldType.FaceA: - { - normal = Transform.Mul(xfA.Quaternion2D, manifold.LocalNormal); - Vector2 planePoint = Transform.Mul(xfA, manifold.LocalPoint); - - for (int i = 0; i < manifold.PointCount; ++i) - { - Vector2 clipPoint = Transform.Mul(xfB, manifold.Points[i].LocalPoint); - Vector2 cA = clipPoint + normal * (radiusA - Vector2.Dot(clipPoint - planePoint, normal)); - Vector2 cB = clipPoint - normal * radiusB; - points[i] = (cA + cB) * 0.5f; - } - } - break; - - case ManifoldType.FaceB: - { - normal = Transform.Mul(xfB.Quaternion2D, manifold.LocalNormal); - Vector2 planePoint = Transform.Mul(xfB, manifold.LocalPoint); - - for (int i = 0; i < manifold.PointCount; ++i) - { - Vector2 clipPoint = Transform.Mul(xfA, manifold.Points[i].LocalPoint); - Vector2 cB = clipPoint + normal * (radiusB - Vector2.Dot(clipPoint - planePoint, normal)); - Vector2 cA = clipPoint - normal * radiusA; - points[i] = (cA + cB) * 0.5f; - } - - // Ensure normal points from A to B. - normal = -normal; - } - break; - default: - // Shouldn't happentm - throw new InvalidOperationException(); - - } - } - - private static void PositionSolverManifoldInitialize( - in ContactPositionConstraint pc, - int index, - in Transform xfA, - in Transform xfB, - out Vector2 normal, - out Vector2 point, - out float separation) - { - DebugTools.Assert(pc.PointCount > 0); - - switch (pc.Type) - { - case ManifoldType.Circles: - { - Vector2 pointA = Transform.Mul(xfA, pc.LocalPoint); - Vector2 pointB = Transform.Mul(xfB, pc.LocalPoints[0]); - normal = pointB - pointA; - - //FPE: Fix to handle zero normalization - if (normal != Vector2.Zero) - normal = normal.Normalized; - - point = (pointA + pointB) * 0.5f; - separation = Vector2.Dot(pointB - pointA, normal) - pc.RadiusA - pc.RadiusB; - } - break; - - case ManifoldType.FaceA: - { - normal = Transform.Mul(xfA.Quaternion2D, pc.LocalNormal); - Vector2 planePoint = Transform.Mul(xfA, pc.LocalPoint); - - Vector2 clipPoint = Transform.Mul(xfB, pc.LocalPoints[index]); - separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB; - point = clipPoint; - } - break; - - case ManifoldType.FaceB: - { - normal = Transform.Mul(xfB.Quaternion2D, pc.LocalNormal); - Vector2 planePoint = Transform.Mul(xfB, pc.LocalPoint); - - Vector2 clipPoint = Transform.Mul(xfA, pc.LocalPoints[index]); - separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB; - point = clipPoint; - - // Ensure normal points from A to B - normal = -normal; - } - break; - default: - normal = Vector2.Zero; - point = Vector2.Zero; - separation = 0; - break; - - } - } - } -} diff --git a/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs b/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs index a5655183f..ce15c1d15 100644 --- a/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs +++ b/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs @@ -26,17 +26,17 @@ namespace Robust.Shared.Physics.Dynamics.Contacts { internal struct ContactVelocityConstraint { - public int ContactIndex { get; set; } + public int ContactIndex; /// /// Index of BodyA in the island. /// - public int IndexA { get; set; } + public int IndexA; /// /// Index of BodyB in the island. /// - public int IndexB { get; set; } + public int IndexB; // Use 2 as its the max number of manifold points. public VelocityConstraintPoint[] Points; @@ -48,11 +48,9 @@ namespace Robust.Shared.Physics.Dynamics.Contacts public System.Numerics.Vector4 K; public float InvMassA; - public float InvMassB; public float InvIA; - public float InvIB; public float Friction; diff --git a/Robust.Shared/Physics/Dynamics/Joints/DistanceJoint.cs b/Robust.Shared/Physics/Dynamics/Joints/DistanceJoint.cs index 4a5ce91e1..bbb6f4564 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/DistanceJoint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/DistanceJoint.cs @@ -30,6 +30,7 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Maths; using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; using Robust.Shared.ViewVariables; @@ -284,10 +285,19 @@ namespace Robust.Shared.Physics.Dynamics.Joints return 0.0f; } - internal override void InitVelocityConstraints(SolverData data, PhysicsComponent bodyA, PhysicsComponent bodyB) + internal override void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) { - _indexA = bodyA.IslandIndex[data.IslandIndex]; - _indexB = bodyB.IslandIndex[data.IslandIndex]; + var offset = island.Offset; + _indexA = bodyA.IslandIndex[island.Index]; + _indexB = bodyB.IslandIndex[island.Index]; _localCenterA = bodyA.LocalCenter; _localCenterB = bodyB.LocalCenter; _invMassA = bodyA.InvMass; @@ -295,15 +305,15 @@ namespace Robust.Shared.Physics.Dynamics.Joints _invIA = bodyA.InvI; _invIB = bodyB.InvI; - var cA = data.Positions[_indexA]; - float aA = data.Angles[_indexA]; - var vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; + var cA = positions[_indexA]; + float aA = angles[_indexA]; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; - var cB = data.Positions[_indexB]; - float aB = data.Angles[_indexB]; - var vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + var cB = positions[_indexB]; + float aB = angles[_indexB]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -313,7 +323,7 @@ namespace Robust.Shared.Physics.Dynamics.Joints // Handle singularity. _currentLength = _u.Length; - if (_currentLength > data.LinearSlop) + if (_currentLength > PhysicsConstants.LinearSlop) { _u *= 1.0f / _currentLength; } @@ -377,18 +387,23 @@ namespace Robust.Shared.Physics.Dynamics.Joints _impulse = 0.0f; } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override void SolveVelocityConstraints(SolverData data) + internal override void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities) { - var vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; - var vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + var offset = island.Offset; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; if (_minLength < _maxLength) { @@ -471,19 +486,22 @@ namespace Robust.Shared.Physics.Dynamics.Joints wB += _invIB * Vector2.Cross(_rB, P); } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override bool SolvePositionConstraints(SolverData data) + internal override bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles) { - var cA = data.Positions[_indexA]; - float aA = data.Angles[_indexA]; - var cB = data.Positions[_indexB]; - float aB = data.Angles[_indexB]; + var cA = positions[_indexA]; + float aA = angles[_indexA]; + var cB = positions[_indexB]; + float aB = angles[_indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -519,12 +537,12 @@ namespace Robust.Shared.Physics.Dynamics.Joints cB += P * _invMassB; aB += _invIB * Vector2.Cross(rB, P); - data.Positions[_indexA] = cA; - data.Angles[_indexA] = aA; - data.Positions[_indexB] = cB; - data.Angles[_indexB] = aB; + positions[_indexA] = cA; + angles[_indexA] = aA; + positions[_indexB] = cB; + angles[_indexB] = aB; - return MathF.Abs(C) < data.LinearSlop; + return MathF.Abs(C) < PhysicsConstants.LinearSlop; } public bool Equals(DistanceJoint? other) diff --git a/Robust.Shared/Physics/Dynamics/Joints/FrictionJoint.cs b/Robust.Shared/Physics/Dynamics/Joints/FrictionJoint.cs index 1f5050b52..82d105fe1 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/FrictionJoint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/FrictionJoint.cs @@ -26,6 +26,7 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Maths; using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; using Robust.Shared.ViewVariables; @@ -169,10 +170,19 @@ namespace Robust.Shared.Physics.Dynamics.Joints return invDt * _angularImpulse; } - internal override void InitVelocityConstraints(SolverData data, PhysicsComponent bodyA, PhysicsComponent bodyB) + internal override void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) { - _indexA = bodyA.IslandIndex[data.IslandIndex]; - _indexB = bodyB.IslandIndex[data.IslandIndex]; + var offset = island.Offset; + _indexA = bodyA.IslandIndex[island.Index]; + _indexB = bodyB.IslandIndex[island.Index]; _localCenterA = bodyA.LocalCenter; _localCenterB = bodyB.LocalCenter; _invMassA = bodyA.InvMass; @@ -180,13 +190,13 @@ namespace Robust.Shared.Physics.Dynamics.Joints _invIA = bodyA.InvI; _invIB = bodyB.InvI; - float aA = data.Angles[_indexA]; - Vector2 vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; + float aA = angles[_indexA]; + Vector2 vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; - float aB = data.Angles[_indexB]; - Vector2 vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + float aB = angles[_indexB]; + Vector2 vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -239,18 +249,23 @@ namespace Robust.Shared.Physics.Dynamics.Joints _angularImpulse = 0.0f; } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override void SolveVelocityConstraints(SolverData data) + internal override void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities) { - Vector2 vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; - Vector2 vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + var offset = island.Offset; + Vector2 vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; + Vector2 vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; @@ -296,13 +311,16 @@ namespace Robust.Shared.Physics.Dynamics.Joints wB += iB * Vector2.Cross(_rB, impulse); } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override bool SolvePositionConstraints(SolverData data) + internal override bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles) { return true; } diff --git a/Robust.Shared/Physics/Dynamics/Joints/Joint.cs b/Robust.Shared/Physics/Dynamics/Joints/Joint.cs index 88e559a1b..8e09394d1 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/Joint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/Joint.cs @@ -272,7 +272,15 @@ namespace Robust.Shared.Physics.Dynamics.Joints /// The inverse delta time. public abstract float GetReactionTorque(float invDt); - internal abstract void InitVelocityConstraints(SolverData data, PhysicsComponent bodyA, PhysicsComponent bodyB); + internal abstract void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities); internal float Validate(float invDt) { @@ -289,13 +297,20 @@ namespace Robust.Shared.Physics.Dynamics.Joints return jointErrorSquared; } - internal abstract void SolveVelocityConstraints(SolverData data); + internal abstract void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities); /// /// Solves the position constraints. /// /// returns true if the position errors are within tolerance. - internal abstract bool SolvePositionConstraints(SolverData data); + internal abstract bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles); public bool Equals(Joint? other) { @@ -319,17 +334,8 @@ namespace Robust.Shared.Physics.Dynamics.Joints hashcode = hashcode * 397 ^ JointType.GetHashCode(); return hashcode; } - - public sealed class JointBreakMessage : EntityEventArgs - { - public Joint Joint { get; } - public float JointError { get; } - - public JointBreakMessage(Joint joint, float jointError) - { - Joint = joint; - JointError = jointError; - } - } } + + [ByRefEvent] + public readonly record struct JointBreakEvent(Joint Joint, float JointError) {} } diff --git a/Robust.Shared/Physics/Dynamics/Joints/MouseJoint.cs b/Robust.Shared/Physics/Dynamics/Joints/MouseJoint.cs index 4ccaa76ff..05e810fdc 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/MouseJoint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/MouseJoint.cs @@ -25,6 +25,7 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Maths; using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; using Robust.Shared.ViewVariables; @@ -169,17 +170,26 @@ public sealed class MouseJoint : Joint, IEquatable private int _indexB; private Vector2 _localCenterB; - internal override void InitVelocityConstraints(SolverData data, PhysicsComponent bodyA, PhysicsComponent bodyB) + internal override void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) { - _indexB = bodyB.IslandIndex[data.IslandIndex]; + var offset = island.Offset; + _indexB = bodyB.IslandIndex[island.Index]; _localCenterB = bodyB.LocalCenter; _invMassB = bodyB.InvMass; _invIB = bodyB.InvI; - var cB = data.Positions[_indexB]; - var aB = data.Angles[_indexB]; - var vB = data.LinearVelocities[_indexB]; - var wB = data.AngularVelocities[_indexB]; + var cB = positions[_indexB]; + var aB = angles[_indexB]; + var vB = linearVelocities[offset + _indexB]; + var wB = angularVelocities[offset + _indexB]; Quaternion2D qB = new(aB); @@ -229,14 +239,19 @@ public sealed class MouseJoint : Joint, IEquatable _impulse = Vector2.Zero; } - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override void SolveVelocityConstraints(SolverData data) + internal override void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities) { - var vB = data.LinearVelocities[_indexB]; - var wB = data.AngularVelocities[_indexB]; + var offset = island.Offset; + var vB = linearVelocities[offset + _indexB]; + var wB = angularVelocities[offset + _indexB]; // Cdot = v + cross(w, r) var Cdot = vB + Vector2.Cross(wB, _rB); @@ -255,11 +270,14 @@ public sealed class MouseJoint : Joint, IEquatable vB += impulse * _invMassB; wB += _invIB * Vector2.Cross(_rB, impulse); - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override bool SolvePositionConstraints(SolverData data) + internal override bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles) { return true; } diff --git a/Robust.Shared/Physics/Dynamics/Joints/PrismaticJoint.cs b/Robust.Shared/Physics/Dynamics/Joints/PrismaticJoint.cs index 4ef064f9a..38909a404 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/PrismaticJoint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/PrismaticJoint.cs @@ -26,6 +26,7 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Maths; using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; @@ -256,10 +257,19 @@ namespace Robust.Shared.Physics.Dynamics.Joints return invDt * _impulse.Y; } - internal override void InitVelocityConstraints(SolverData data, PhysicsComponent bodyA, PhysicsComponent bodyB) + internal override void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) { - _indexA = bodyA.IslandIndex[data.IslandIndex]; - _indexB = bodyB.IslandIndex[data.IslandIndex]; + var offset = island.Offset; + _indexA = bodyA.IslandIndex[island.Index]; + _indexB = bodyB.IslandIndex[island.Index]; _localCenterA = bodyA.LocalCenter; _localCenterB = bodyB.LocalCenter; _invMassA = bodyA.InvMass; @@ -267,15 +277,15 @@ namespace Robust.Shared.Physics.Dynamics.Joints _invIA = bodyA.InvI; _invIB = bodyB.InvI; - var cA = data.Positions[_indexA]; - float aA = data.Angles[_indexA]; - var vA = data.Positions[_indexA]; - float wA = data.Angles[_indexA]; + var cA = positions[_indexA]; + float aA = angles[_indexA]; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; - var cB = data.Positions[_indexB]; - float aB = data.Angles[_indexB]; - var vB = data.Positions[_indexB]; - float wB = data.Angles[_indexB]; + var cB = positions[_indexB]; + float aB = angles[_indexB]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -361,18 +371,23 @@ namespace Robust.Shared.Physics.Dynamics.Joints _upperImpulse = 0.0f; } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override void SolveVelocityConstraints(SolverData data) + internal override void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities) { - Vector2 vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; - Vector2 vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + var offset = island.Offset; + Vector2 vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; + Vector2 vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; @@ -460,18 +475,21 @@ namespace Robust.Shared.Physics.Dynamics.Joints wB += iB * LB; } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override bool SolvePositionConstraints(SolverData data) + internal override bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles) { - Vector2 cA = data.Positions[_indexA]; - float aA = data.Angles[_indexA]; - Vector2 cB = data.Positions[_indexB]; - float aB = data.Angles[_indexB]; + Vector2 cA = positions[_indexA]; + float aA = angles[_indexA]; + Vector2 cB = positions[_indexB]; + float aB = angles[_indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -504,7 +522,7 @@ namespace Robust.Shared.Physics.Dynamics.Joints if (EnableLimit) { float translation = Vector2.Dot(axis, d); - if (MathF.Abs(UpperTranslation - LowerTranslation) < 2.0f * data.LinearSlop) + if (MathF.Abs(UpperTranslation - LowerTranslation) < 2.0f * PhysicsConstants.LinearSlop) { C2 = translation; linearError = MathF.Max(linearError, MathF.Abs(translation)); @@ -578,12 +596,12 @@ namespace Robust.Shared.Physics.Dynamics.Joints cB += P * mB; aB += iB * LB; - data.Positions[_indexA] = cA; - data.Angles[_indexA] = aA; - data.Positions[_indexB] = cB; - data.Angles[_indexB] = aB; + positions[_indexA] = cA; + angles[_indexA] = aA; + positions[_indexB] = cB; + angles[_indexB] = aB; - return linearError <= data.LinearSlop && angularError <= data.AngularSlop; + return linearError <= PhysicsConstants.LinearSlop && angularError <= PhysicsConstants.AngularSlop; } public bool Equals(PrismaticJoint? other) diff --git a/Robust.Shared/Physics/Dynamics/Joints/RevoluteJoint.cs b/Robust.Shared/Physics/Dynamics/Joints/RevoluteJoint.cs index 84689d0e2..9a82974ed 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/RevoluteJoint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/RevoluteJoint.cs @@ -25,6 +25,7 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Maths; using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; @@ -166,10 +167,19 @@ namespace Robust.Shared.Physics.Dynamics.Joints return invDt * (_motorImpulse + _lowerImpulse - _upperImpulse); } - internal override void InitVelocityConstraints(SolverData data, PhysicsComponent bodyA, PhysicsComponent bodyB) + internal override void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) { - _indexA = bodyA.IslandIndex[data.IslandIndex]; - _indexB = bodyB.IslandIndex[data.IslandIndex]; + var offset = island.Offset; + _indexA = bodyA.IslandIndex[island.Index]; + _indexB = bodyB.IslandIndex[island.Index]; _localCenterA = bodyA.LocalCenter; _localCenterB = bodyB.LocalCenter; _invMassA = bodyA.InvMass; @@ -177,13 +187,13 @@ namespace Robust.Shared.Physics.Dynamics.Joints _invIA = bodyA.InvI; _invIB = bodyB.InvI; - float aA = data.Angles[_indexA]; - var vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; + float aA = angles[_indexA]; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; - float aB = data.Angles[_indexB]; - var vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + float aB = angles[_indexB]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -254,18 +264,23 @@ namespace Robust.Shared.Physics.Dynamics.Joints _upperImpulse = 0.0f; } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override void SolveVelocityConstraints(SolverData data) + internal override void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities) { - var vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; - var vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + var offset = island.Offset; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; @@ -332,18 +347,21 @@ namespace Robust.Shared.Physics.Dynamics.Joints wB += iB * Vector2.Cross(_rB, impulse); } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override bool SolvePositionConstraints(SolverData data) + internal override bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles) { - var cA = data.Positions[_indexA]; - float aA = data.Angles[_indexA]; - var cB = data.Positions[_indexB]; - float aB = data.Angles[_indexB]; + var cA = positions[_indexA]; + float aA = angles[_indexA]; + var cB = positions[_indexB]; + float aB = angles[_indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -358,7 +376,7 @@ namespace Robust.Shared.Physics.Dynamics.Joints float angle = aB - aA - ReferenceAngle; float C = 0.0f; - if (Math.Abs(UpperAngle - LowerAngle) < 2.0f * data.AngularSlop) + if (Math.Abs(UpperAngle - LowerAngle) < 2.0f * PhysicsConstants.AngularSlop) { // Prevent large angular corrections C = Math.Clamp(angle - LowerAngle, -data.MaxAngularCorrection, data.MaxAngularCorrection); @@ -366,12 +384,12 @@ namespace Robust.Shared.Physics.Dynamics.Joints else if (angle <= LowerAngle) { // Prevent large angular corrections and allow some slop. - C = Math.Clamp(angle - LowerAngle + data.AngularSlop, -data.MaxAngularCorrection, 0.0f); + C = Math.Clamp(angle - LowerAngle + PhysicsConstants.AngularSlop, -data.MaxAngularCorrection, 0.0f); } else if (angle >= UpperAngle) { // Prevent large angular corrections and allow some slop. - C = Math.Clamp(angle - UpperAngle - data.AngularSlop, 0.0f, data.MaxAngularCorrection); + C = Math.Clamp(angle - UpperAngle - PhysicsConstants.AngularSlop, 0.0f, data.MaxAngularCorrection); } float limitImpulse = -_axialMass * C; @@ -410,12 +428,12 @@ namespace Robust.Shared.Physics.Dynamics.Joints aB += iB * Vector2.Cross(rB, impulse); } - data.Positions[_indexA] = cA; - data.Angles[_indexA] = aA; - data.Positions[_indexB] = cB; - data.Angles[_indexB] = aB; + positions[_indexA] = cA; + angles[_indexA] = aA; + positions[_indexB] = cB; + angles[_indexB] = aB; - return positionError <= data.LinearSlop && angularError <= data.AngularSlop; + return positionError <= PhysicsConstants.LinearSlop && angularError <= PhysicsConstants.AngularSlop; } public bool Equals(RevoluteJoint? other) diff --git a/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs b/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs index 657db9f7d..6e1d3dc84 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs @@ -3,6 +3,7 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Maths; using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; @@ -98,10 +99,19 @@ namespace Robust.Shared.Physics.Dynamics.Joints return invDt * _impulse.Z; } - internal override void InitVelocityConstraints(SolverData data, PhysicsComponent bodyA, PhysicsComponent bodyB) + internal override void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) { - _indexA = bodyA.IslandIndex[data.IslandIndex]; - _indexB = bodyB.IslandIndex[data.IslandIndex]; + var offset = island.Offset; + _indexA = bodyA.IslandIndex[island.Index]; + _indexB = bodyB.IslandIndex[island.Index]; _localCenterA = bodyA.LocalCenter; _localCenterB = bodyB.LocalCenter; _invMassA = bodyA.InvMass; @@ -109,13 +119,13 @@ namespace Robust.Shared.Physics.Dynamics.Joints _invIA = bodyA.InvI; _invIB = bodyB.InvI; - float aA = data.Angles[_indexA]; - var vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; + float aA = angles[_indexA]; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; - float aB = data.Angles[_indexB]; - var vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + float aB = angles[_indexB]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -199,18 +209,23 @@ namespace Robust.Shared.Physics.Dynamics.Joints _impulse = Vector3.Zero; } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override void SolveVelocityConstraints(SolverData data) + internal override void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities) { - var vA = data.LinearVelocities[_indexA]; - float wA = data.AngularVelocities[_indexA]; - var vB = data.LinearVelocities[_indexB]; - float wB = data.AngularVelocities[_indexB]; + var offset = island.Offset; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; @@ -257,18 +272,21 @@ namespace Robust.Shared.Physics.Dynamics.Joints wB += iB * (Vector2.Cross(_rB, P) + impulse.Z); } - data.LinearVelocities[_indexA] = vA; - data.AngularVelocities[_indexA] = wA; - data.LinearVelocities[_indexB] = vB; - data.AngularVelocities[_indexB] = wB; + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; } - internal override bool SolvePositionConstraints(SolverData data) + internal override bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles) { - var cA = data.Positions[_indexA]; - float aA = data.Angles[_indexA]; - var cB = data.Positions[_indexB]; - float aB = data.Angles[_indexB]; + var cA = positions[_indexA]; + float aA = angles[_indexA]; + var cB = positions[_indexB]; + float aB = angles[_indexB]; Quaternion2D qA = new(aA), qB = new(aB); @@ -336,12 +354,12 @@ namespace Robust.Shared.Physics.Dynamics.Joints aB += iB * (Vector2.Cross(rB, P) + impulse.Z); } - data.Positions[_indexA] = cA; - data.Angles[_indexA]= aA; - data.Positions[_indexB] = cB; - data.Angles[_indexB] = aB; + positions[_indexA] = cA; + angles[_indexA]= aA; + positions[_indexB] = cB; + angles[_indexB] = aB; - return positionError <= data.LinearSlop && angularError <= data.AngularSlop; + return positionError <= PhysicsConstants.LinearSlop && angularError <= PhysicsConstants.AngularSlop; } public bool Equals(WeldJoint? other) diff --git a/Robust.Shared/Physics/Dynamics/PhysicsIsland.cs b/Robust.Shared/Physics/Dynamics/PhysicsIsland.cs index 3418b0040..e28c8d4b2 100644 --- a/Robust.Shared/Physics/Dynamics/PhysicsIsland.cs +++ b/Robust.Shared/Physics/Dynamics/PhysicsIsland.cs @@ -139,542 +139,30 @@ constraint structures. The body velocities/positions are held in compact, tempor arrays to increase the number of cache hits. Linear and angular velocity are stored in a single array since multiple arrays lead to multiple misses. */ - public sealed class PhysicsIsland - { - [Dependency] private readonly IPhysicsManager _physicsManager = default!; - [Dependency] private readonly IEntityManager _entityManager = default!; - private SharedTransformSystem _transform = default!; - private SharedPhysicsSystem _physics = default!; -#if DEBUG - private List _debugBodies = new(8); -#endif - private readonly ContactSolver _contactSolver = new(); - - internal int ID { get; set; } = -1; - - internal bool LoneIsland { get; set; } - - private float _angTolSqr; - private float _linTolSqr; - private bool _warmStarting; - private int _velocityIterations; - private float _maxTranslation; - private float _maxRotation; - private float _maxLinearCorrection; - private float _maxAngularCorrection; - private int _positionIterations; - private bool _sleepAllowed; // BONAFIDE MONAFIED - private float _timeToSleep; - - public PhysicsComponent[] Bodies = Array.Empty(); - private Contact[] _contacts = Array.Empty(); - private (Joint Joint, PhysicsComponent BodyA, PhysicsComponent BodyB)[] _joints = Array.Empty<(Joint Joint, PhysicsComponent BodyA, PhysicsComponent BodyB)>(); - - private List<(Joint Joint, float ErrorSquared)> _brokenJoints = new(); - - // These are joint in box2d / derivatives - private Vector2[] _linearVelocities = Array.Empty(); - private float[] _angularVelocities = Array.Empty(); - - private Vector2[] _positions = Array.Empty(); - private float[] _angles = Array.Empty(); - - private bool _positionSolved = false; - - internal SolverData SolverData = new(); - - private const int BodyIncrease = 8; - private const int ContactIncrease = 4; - private const int JointIncrease = 4; - - /// - /// How many bodies we can fit in the island before needing to re-size. - /// - public int BodyCapacity { get; private set; } - - /// - /// How many bodies are in the island. - /// - public int BodyCount { get; private set; } - - /// - /// How many contacts we can fit in the island before needing to re-size. - /// - public int ContactCapacity { get; private set; } - - /// - /// How many contacts are in the island. - /// - public int ContactCount { get; private set; } - - /// - /// How many joints we can fit in the island before needing to re-size. - /// - public int JointCapacity { get; private set; } - - /// - /// How many joints are in the island. - /// - public int JointCount { get; private set; } - - internal void Initialize() - { - IoCManager.InjectDependencies(this); - _transform = _entityManager.EntitySysManager.GetEntitySystem(); - _physics = _entityManager.EntitySysManager.GetEntitySystem(); - } - - internal void LoadConfig(in IslandCfg cfg) - { - _angTolSqr = cfg.AngTolSqr; - _linTolSqr = cfg.LinTolSqr; - _warmStarting = cfg.WarmStarting; - _velocityIterations = cfg.VelocityIterations; - _maxTranslation = cfg.MaxTranslationPerTick; - _maxRotation = cfg.MaxRotationPerTick; - _maxLinearCorrection = cfg.MaxLinearCorrection; - _maxAngularCorrection = cfg.MaxAngularCorrection; - _positionIterations = cfg.PositionIterations; - _sleepAllowed = cfg.SleepAllowed; - _timeToSleep = cfg.TimeToSleep; - - _contactSolver.LoadConfig(cfg); - } - - public void Append(List bodies, List contacts, List joints) - { - Resize(BodyCount + bodies.Count, ContactCount + contacts.Count, JointCount + joints.Count); - foreach (var body in bodies) - { - Add(body); - } - - foreach (var contact in contacts) - { - Add(contact); - } - - var query = _entityManager.GetEntityQuery(); - foreach (var joint in joints) - { - var bodyA = query.GetComponent(joint.BodyAUid); - var bodyB = query.GetComponent(joint.BodyBUid); - Add((joint, bodyA, bodyB)); - } - } - - public void Add(PhysicsComponent body) - { - body.IslandIndex[ID] = BodyCount; - Bodies[BodyCount++] = body; - } - - public void Add(Contact contact) - { - _contacts[ContactCount++] = contact; - } - - public void Add((Joint Joint, PhysicsComponent BodyA, PhysicsComponent BodyB) joint) - { - _joints[JointCount++] = joint; - } - - public void Clear() - { - ID = -1; - BodyCount = 0; - ContactCount = 0; - JointCount = 0; - } - - /* - * Look there's a whole lot of stuff going on around here but all you need to know is it's trying to avoid - * allocations where possible so it does a whole lot of passing data around and using arrays. - */ - public void Resize(int bodyCount, int contactCount, int jointCount) - { - BodyCapacity = Math.Max(bodyCount, Bodies.Length); - ContactCapacity = Math.Max(contactCount, _contacts.Length); - JointCapacity = Math.Max(jointCount, _joints.Length); - - if (Bodies.Length < BodyCapacity) - { - BodyCapacity = BodyIncrease * (int) MathF.Ceiling(BodyCapacity / (float) BodyIncrease); - Array.Resize(ref Bodies, BodyCapacity); - Array.Resize(ref _linearVelocities, BodyCapacity); - Array.Resize(ref _angularVelocities, BodyCapacity); - Array.Resize(ref _positions, BodyCapacity); - Array.Resize(ref _angles, BodyCapacity); - } - - if (_contacts.Length < ContactCapacity) - { - ContactCapacity = ContactIncrease * (int) MathF.Ceiling(ContactCapacity / (float) ContactIncrease); - Array.Resize(ref _contacts, ContactCapacity * 2); - } - - if (_joints.Length < JointCapacity) - { - JointCapacity = JointIncrease * (int) MathF.Ceiling(JointCapacity / (float) JointIncrease); - Array.Resize(ref _joints, JointCapacity * 2); - } - } - - /// - /// Go through all the bodies in this island and solve. - /// - public void Solve(Vector2 gravity, float frameTime, float dtRatio, float invDt, bool prediction) - { -#if DEBUG - _debugBodies.Clear(); - for (var i = 0; i < BodyCount; i++) - { - _debugBodies.Add(Bodies[i]); - } - - _entityManager.EventBus.RaiseEvent(EventSource.Local, new IslandSolveMessage(_debugBodies)); -#endif - - for (var i = 0; i < BodyCount; i++) - { - var body = Bodies[i]; - - // Didn't use the old variable names because they're hard to read - var transform = _physicsManager.EnsureTransform(body); - var position = Transform.Mul(transform, body.LocalCenter); - // DebugTools.Assert(!float.IsNaN(position.X) && !float.IsNaN(position.Y)); - var angle = transform.Quaternion2D.Angle; - - // var bodyTransform = body.GetTransform(); - // DebugTools.Assert(bodyTransform.Position.EqualsApprox(position) && MathHelper.CloseTo(angle, bodyTransform.Quaternion2D.Angle)); - - var linearVelocity = body.LinearVelocity; - var angularVelocity = body.AngularVelocity; - - // if the body cannot move, nothing to do here - if (body.BodyType == BodyType.Dynamic) - { - if (body.IgnoreGravity) - linearVelocity += body.Force * frameTime * body.InvMass; - else - linearVelocity += (gravity + body.Force * body.InvMass) * frameTime; - - angularVelocity += frameTime * body.InvI * body.Torque; - - linearVelocity *= Math.Clamp(1.0f - frameTime * body.LinearDamping, 0.0f, 1.0f); - angularVelocity *= Math.Clamp(1.0f - frameTime * body.AngularDamping, 0.0f, 1.0f); - } - - _positions[i] = position; - _angles[i] = angle; - _linearVelocities[i] = linearVelocity; - _angularVelocities[i] = angularVelocity; - } - - // TODO: Do these up front of the world step. - SolverData.FrameTime = frameTime; - SolverData.DtRatio = dtRatio; - SolverData.InvDt = invDt; - SolverData.IslandIndex = ID; - SolverData.WarmStarting = _warmStarting; - SolverData.MaxLinearCorrection = _maxLinearCorrection; - SolverData.MaxAngularCorrection = _maxAngularCorrection; - - SolverData.LinearVelocities = _linearVelocities; - SolverData.AngularVelocities = _angularVelocities; - SolverData.Positions = _positions; - SolverData.Angles = _angles; - - // Pass the data into the solver - _contactSolver.Reset(SolverData, ContactCount, _contacts); - - _contactSolver.InitializeVelocityConstraints(); - - if (_warmStarting) - { - _contactSolver.WarmStart(); - } - - for (var i = 0; i < JointCount; i++) - { - var (joint, bodyA, bodyB) = _joints[i]; - if (!joint.Enabled) continue; - joint.InitVelocityConstraints(SolverData, bodyA, bodyB); - } - - // Velocity solver - for (var i = 0; i < _velocityIterations; i++) - { - for (var j = 0; j < JointCount; ++j) - { - Joint joint = _joints[j].Joint; - - if (!joint.Enabled) - continue; - - joint.SolveVelocityConstraints(SolverData); - - var error = joint.Validate(invDt); - - if (error > 0.0f) - _brokenJoints.Add((joint, error)); - } - - _contactSolver.SolveVelocityConstraints(); - } - - // Store for warm starting. - _contactSolver.StoreImpulses(); - - var maxVel = _maxTranslation / frameTime; - var maxVelSq = maxVel * maxVel; - var maxAngVel = _maxRotation / frameTime; - var maxAngVelSq = maxAngVel * maxAngVel; - - // Apply velocity limits and Integrate positions - for (var i = 0; i < BodyCount; i++) - { - var linearVelocity = _linearVelocities[i]; - var angularVelocity = _angularVelocities[i]; - - var velSqr = linearVelocity.LengthSquared; - if (velSqr > maxVelSq) - { - linearVelocity *= maxVel / MathF.Sqrt(velSqr); - _linearVelocities[i] = linearVelocity; - } - - if (angularVelocity * angularVelocity > maxAngVelSq) - { - angularVelocity *= maxAngVel / MathF.Abs(angularVelocity); - _angularVelocities[i] = angularVelocity; - } - - // Integrate - _positions[i] += linearVelocity * frameTime; - _angles[i] += angularVelocity * frameTime; - } - - _positionSolved = false; - - for (var i = 0; i < _positionIterations; i++) - { - var contactsOkay = _contactSolver.SolvePositionConstraints(); - var jointsOkay = true; - - for (int j = 0; j < JointCount; ++j) - { - var joint = _joints[j].Joint; - - if (!joint.Enabled) - continue; - - var jointOkay = joint.SolvePositionConstraints(SolverData); - - jointsOkay = jointsOkay && jointOkay; - } - - if (contactsOkay && jointsOkay) - { - _positionSolved = true; - break; - } - } - } - - internal void UpdateBodies(HashSet deferredUpdates, bool substepping) - { - foreach (var (joint, error) in _brokenJoints) - { - var msg = new Joint.JointBreakMessage(joint, MathF.Sqrt(error)); - var eventBus = _entityManager.EventBus; - eventBus.RaiseLocalEvent(joint.BodyAUid, msg, false); - eventBus.RaiseLocalEvent(joint.BodyBUid, msg, false); - eventBus.RaiseEvent(EventSource.Local, msg); - } - - _brokenJoints.Clear(); - - var xforms = _entityManager.GetEntityQuery(); - var meta = _entityManager.GetEntityQuery(); - - // Update data on bodies by copying the buffers back - for (var i = 0; i < BodyCount; i++) - { - var body = Bodies[i]; - - // So technically we don't /need/ to skip static bodies here but it saves us having to check for deferred updates so we'll do it anyway. - // Plus calcing worldpos can be costly so we skip that too which is nice. - if (body.BodyType == BodyType.Static) continue; - - /* - * Handle new position - */ - var bodyPos = _positions[i]; - var angle = _angles[i]; - - // Temporary NaN guards until PVS is fixed. - if (!float.IsNaN(bodyPos.X) && !float.IsNaN(bodyPos.Y)) - { - var q = new Quaternion2D(angle); - - bodyPos -= Transform.Mul(q, body.LocalCenter); - var transform = xforms.GetComponent(body.Owner); - - // Defer MoveEvent / RotateEvent until the end of the physics step so cache can be better. - // Apparantly lerping works with substepping here - transform.DeferUpdates = true; - _transform.SetWorldPositionRotation(transform, bodyPos, angle, xforms); - transform.DeferUpdates = false; - - // Unfortunately we can't cache the position and angle here because if our parent's position - // changes then this is immediately invalidated. - if (transform.UpdatesDeferred) - { - deferredUpdates.Add(transform); - } - } - - var linVelocity = _linearVelocities[i]; - var dirty = false; - - if (!float.IsNaN(linVelocity.X) && !float.IsNaN(linVelocity.Y)) - { - _physics.SetLinearVelocity(body, linVelocity, false); - dirty = true; - } - - var angVelocity = _angularVelocities[i]; - - if (!float.IsNaN(angVelocity)) - { - _physics.SetAngularVelocity(body, angVelocity, false); - dirty = true; - } - - if (dirty) - _entityManager.Dirty(body, meta.GetComponent(body.Owner)); - } - } - - internal void SleepBodies(bool prediction, float frameTime) - { - if (LoneIsland) - { - if (!prediction && _sleepAllowed) - { - for (var i = 0; i < BodyCount; i++) - { - var body = Bodies[i]; - - if (body.BodyType == BodyType.Static) continue; - - if (!body.SleepingAllowed || - body.AngularVelocity * body.AngularVelocity > _angTolSqr || - Vector2.Dot(body.LinearVelocity, body.LinearVelocity) > _linTolSqr) - { - _physics.SetSleepTime(body, 0f); - } - else - { - _physics.SetSleepTime(body, body.SleepTime + frameTime); - } - - if (body.SleepTime >= _timeToSleep && _positionSolved) - { - _physics.SetAwake(body, false); - } - } - } - } - else - { - // Sleep bodies if needed. Prediction won't accumulate sleep-time for bodies. - if (!prediction && _sleepAllowed) - { - var minSleepTime = float.MaxValue; - - for (var i = 0; i < BodyCount; i++) - { - var body = Bodies[i]; - - if (body.BodyType == BodyType.Static) continue; - - if (!body.SleepingAllowed || - body.AngularVelocity * body.AngularVelocity > _angTolSqr || - Vector2.Dot(body.LinearVelocity, body.LinearVelocity) > _linTolSqr) - { - _physics.SetSleepTime(body, 0f); - minSleepTime = 0.0f; - } - else - { - _physics.SetSleepTime(body, body.SleepTime + frameTime); - minSleepTime = MathF.Min(minSleepTime, body.SleepTime); - } - } - - if (minSleepTime >= _timeToSleep && _positionSolved) - { - for (var i = 0; i < BodyCount; i++) - { - var body = Bodies[i]; - _physics.SetAwake(body, false); - } - } - } - } - } - } /// /// Easy way of passing around the data required for the contact solver. /// - internal sealed class SolverData + internal readonly record struct SolverData( + float FrameTime, + float DtRatio, + float InvDt, + bool WarmStarting, + float MaxLinearCorrection, + float MaxAngularCorrection, + int VelocityIterations, + int PositionIterations, + float MaxLinearVelocity, + float MaxAngularVelocity, + float MaxTranslation, + float MaxRotation, + bool SleepAllowed, + float AngTolSqr, + float LinTolSqr, + float TimeToSleep, + float VelocityThreshold, + float Baumgarte) { - public int IslandIndex { get; set; } = -1; - - public float FrameTime { get; set; } - public float DtRatio { get; set; } - public float InvDt { get; set; } - - public bool WarmStarting { get; set; } - public float LinearSlop { get; set; } - public float AngularSlop { get; set; } - public float MaxLinearCorrection { get; set; } - public float MaxAngularCorrection { get; set; } - - public Vector2[] LinearVelocities { get; set; } = default!; - public float[] AngularVelocities { get; set; } = default!; - - public Vector2[] Positions { get; set; } = default!; - public float[] Angles { get; set; } = default!; - } - - /// - /// Contains all configuration parameters that need to be passed to physics islands. - /// - internal struct IslandCfg - { - public float AngTolSqr; - public float LinTolSqr; - public bool SleepAllowed; - public bool WarmStarting; - public int VelocityIterations; - public float MaxTranslationPerTick; - public float MaxRotationPerTick; - public int PositionIterations; - public float TimeToSleep; - public float VelocityThreshold; - public float Baumgarte; - public float LinearSlop; - public float AngularSlop; - public float MaxLinearCorrection; - public float MaxAngularCorrection; - public int VelocityConstraintsPerThread; - public int VelocityConstraintsMinimumThreads; - public int PositionConstraintsPerThread; - public int PositionConstraintsMinimumThreads; } } diff --git a/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs b/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs index 91810fa33..3b3ca58d0 100644 --- a/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs +++ b/Robust.Shared/Physics/Dynamics/SharedPhysicsMapComponent.cs @@ -27,10 +27,9 @@ using Robust.Shared.IoC; using Robust.Shared.Log; using Robust.Shared.Map; using Robust.Shared.Maths; -using Robust.Shared.Physics.Dynamics.Contacts; -using Robust.Shared.Physics.Dynamics.Joints; using Robust.Shared.Physics.Systems; using Robust.Shared.Utility; +using Robust.Shared.ViewVariables; using PhysicsComponent = Robust.Shared.Physics.Components.PhysicsComponent; namespace Robust.Shared.Physics.Dynamics @@ -38,10 +37,8 @@ namespace Robust.Shared.Physics.Dynamics public abstract class SharedPhysicsMapComponent : Component { [Dependency] private readonly IEntityManager _entityManager = default!; - [Dependency] private readonly IIslandManager _islandManager = default!; internal SharedPhysicsSystem Physics = default!; - internal SharedBroadphaseSystem BroadphaseSystem = default!; internal ContactManager ContactManager = default!; @@ -61,6 +58,7 @@ namespace Robust.Shared.Physics.Dynamics /// /// Change the global gravity vector. /// + [ViewVariables(VVAccess.ReadWrite)] public Vector2 Gravity { get => _gravity; @@ -106,30 +104,10 @@ namespace Robust.Shared.Physics.Dynamics /// public readonly HashSet AwakeBodies = new(); - /// - /// Temporary body storage during solving. - /// - private List _awakeBodyList = new(); - - /// - /// Temporary joint storage during solving - /// - private List _joints = new(); - - private Stack _bodyStack = new(64); - - /// - /// Temporarily store island-bodies for easier iteration. - /// - private HashSet _islandSet = new(); - private List _islandBodies = new(64); - private List _islandContacts = new(32); - private List _islandJoints = new(8); - /// /// Store last tick's invDT /// - private float _invDt0; + internal float _invDt0; public MapId MapId => _entityManager.GetComponent(Owner).MapID; @@ -162,42 +140,6 @@ namespace Robust.Shared.Physics.Dynamics #endregion - /// - /// Where the magic happens. - /// - public void Step(float frameTime, bool prediction, bool substepping = false) - { - // 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. - BroadphaseSystem.FindNewContacts(this, MapId); - - var invDt = frameTime > 0.0f ? 1.0f / frameTime : 0.0f; - var dtRatio = _invDt0 * frameTime; - - var updateBeforeSolve = new PhysicsUpdateBeforeMapSolveEvent(prediction, this, frameTime); - _entityManager.EventBus.RaiseEvent(EventSource.Local, ref updateBeforeSolve); - - ContactManager.Collide(); - // Don't run collision behaviors during FrameUpdate? - if (!prediction) - ContactManager.PreSolve(frameTime); - - // Integrate velocities, solve velocity constraints, and do integration. - Solve(frameTime, dtRatio, invDt, prediction, substepping); - - // TODO: SolveTOI - - var updateAfterSolve = new PhysicsUpdateAfterMapSolveEvent(prediction, this, frameTime); - _entityManager.EventBus.RaiseEvent(EventSource.Local, ref updateAfterSolve); - - // Box2d recommends clearing (if you are) during fixed updates rather than variable if you are using it - if (!prediction && AutoClearForces) - ClearForces(); - - _invDt0 = invDt; - } - /// /// Go through all of the deferred MoveEvents and then run them /// @@ -211,234 +153,6 @@ namespace Robust.Shared.Physics.Dynamics DeferredUpdates.Clear(); } - - private void Solve(float frameTime, float dtRatio, float invDt, bool prediction, bool substepping) - { - _islandManager.InitializePools(); - - DebugTools.Assert(_islandSet.Count == 0); - - var contactNode = 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(AwakeBodies.Count); - _islandSet.EnsureCapacity(AwakeBodies.Count); - _awakeBodyList.AddRange(AwakeBodies); - - var metaQuery = _entityManager.GetEntityQuery(); - var jointQuery = _entityManager.GetEntityQuery(); - - // Build the relevant islands / graphs for all bodies. - foreach (var seed in _awakeBodyList) - { - // I tried not running prediction for non-contacted entities but unfortunately it looked like shit - // when contact broke so if you want to try that then GOOD LUCK. - if (seed.Island) continue; - - if (!metaQuery.TryGetComponent(seed.Owner, out var metadata)) - { - Logger.ErrorS("physics", $"Found deleted entity {_entityManager.ToPrettyString(seed.Owner)} on map!"); - RemoveSleepBody(seed); - continue; - } - - if ((metadata.EntityPaused && !seed.IgnorePaused) || - (prediction && !seed.Predict) || - !seed.CanCollide || - seed.BodyType == BodyType.Static) - { - continue; - } - - // Start of a new island - _islandBodies.Clear(); - _islandContacts.Clear(); - _islandJoints.Clear(); - _bodyStack.Push(seed); - - // TODO: Probably don't need _islandSet anymore. - seed.Island = true; - - while (_bodyStack.TryPop(out var body)) - { - _islandBodies.Add(body); - _islandSet.Add(body); - - // Static bodies don't propagate islands - if (body.BodyType == BodyType.Static) continue; - - // As static bodies can never be awake (unlike Farseer) we'll set this after the check. - Physics.SetAwake(body, true, updateSleepTime: false); - - var node = body.Contacts.First; - - while (node != null) - { - var contact = node.Value; - node = node.Next; - - // Has this contact already been added to an island? - if ((contact.Flags & ContactFlags.Island) != 0x0) continue; - - // Is this contact solid and touching? - if (!contact.Enabled || !contact.IsTouching) continue; - - // Skip sensors. - if (contact.FixtureA?.Hard != true || contact.FixtureB?.Hard != true) continue; - - _islandContacts.Add(contact); - contact.Flags |= ContactFlags.Island; - var bodyA = contact.FixtureA!.Body; - var bodyB = contact.FixtureB!.Body; - - var other = bodyA == body ? bodyB : bodyA; - - // Was the other body already added to this island? - if (other.Island) continue; - - _bodyStack.Push(other); - other.Island = true; - } - - if (!jointQuery.TryGetComponent(body.Owner, out var jointComponent)) continue; - - foreach (var (_, joint) in jointComponent.Joints) - { - if (joint.IslandFlag) continue; - - var other = joint.BodyAUid == body.Owner - ? _entityManager.GetComponent(joint.BodyBUid) - : _entityManager.GetComponent(joint.BodyAUid); - - // Don't simulate joints connected to inactive bodies. - if (!other.CanCollide) continue; - - _islandJoints.Add(joint); - joint.IslandFlag = true; - - if (other.Island) continue; - - _bodyStack.Push(other); - other.Island = true; - } - } - - _islandManager - .AllocateIsland(_islandBodies.Count, _islandContacts.Count, _islandJoints.Count) - .Append(_islandBodies, _islandContacts, _islandJoints); - - _joints.AddRange(_islandJoints); - - // Allow static bodies to be re-used in other islands - for (var i = 0; i < _islandBodies.Count; i++) - { - var body = _islandBodies[i]; - - // Static bodies can participate in other islands - if (body.BodyType == BodyType.Static) - { - body.Island = false; - } - } - } - - SolveIslands(frameTime, dtRatio, invDt, prediction, substepping); - Cleanup(frameTime); - } - - protected virtual void Cleanup(float frameTime) - { - foreach (var body in _islandSet) - { - if (!body.Island || body.Deleted) - { - continue; - } - - body.IslandIndex.Clear(); - body.Island = false; - DebugTools.Assert(body.BodyType != BodyType.Static); - - // So Box2D would update broadphase here buutttt we'll just wait until MoveEvent queue is used. - } - - _islandSet.Clear(); - _awakeBodyList.Clear(); - - foreach (var joint in _joints) - { - joint.IslandFlag = false; - } - - _joints.Clear(); - } - - private void SolveIslands(float frameTime, float dtRatio, float invDt, bool prediction, bool substepping) - { - var islands = _islandManager.GetActive; - - // Update cached data for lerping if we're substepping before any writes happen - // TODO: Do it client only - var xformQuery = _entityManager.GetEntityQuery(); - - foreach (var island in islands) - { - for (var i = 0; i < island.BodyCount; i++) - { - var body = island.Bodies[i]; - - if (body.BodyType == BodyType.Static) - continue; - - if (LerpData.TryGetValue(body.Owner, out var data) || - !xformQuery.TryGetComponent(body.Owner, out var xform) || - data.ParentUid == xform.ParentUid) - { - continue; - } - - LerpData[xform.Owner] = (xform.ParentUid, xform.LocalPosition, xform.LocalRotation); - } - } - - // Islands are already pre-sorted - var iBegin = 0; - - while (iBegin < islands.Count) - { - var island = islands[iBegin]; - - island.Solve(Gravity, frameTime, dtRatio, invDt, prediction); - iBegin++; - // TODO: Submit rest in parallel if applicable - } - - // TODO: parallel dispatch here - - // Update bodies sequentially to avoid race conditions. May be able to do this parallel someday - // but easier to just do this for now. - foreach (var island in islands) - { - island.UpdateBodies(DeferredUpdates, substepping); - island.SleepBodies(prediction, frameTime); - } - } - - private void ClearForces() - { - foreach (var body in AwakeBodies) - { - body.Force = Vector2.Zero; - body.Torque = 0.0f; - } - } } [ByRefEvent] diff --git a/Robust.Shared/Physics/IBroadPhase.cs b/Robust.Shared/Physics/IBroadPhase.cs index 9cef43484..7783391b5 100644 --- a/Robust.Shared/Physics/IBroadPhase.cs +++ b/Robust.Shared/Physics/IBroadPhase.cs @@ -3,115 +3,114 @@ using System.Runtime.CompilerServices; using Robust.Shared.Maths; using Robust.Shared.Physics.Dynamics; -namespace Robust.Shared.Physics { +namespace Robust.Shared.Physics; - public interface IBroadPhase - { - int Count { get; } +public interface IBroadPhase +{ + int Count { get; } - Box2 GetFatAabb(DynamicTree.Proxy proxy); + Box2 GetFatAabb(DynamicTree.Proxy proxy); - DynamicTree.Proxy AddProxy(ref FixtureProxy proxy); + DynamicTree.Proxy AddProxy(ref FixtureProxy proxy); - void MoveProxy(DynamicTree.Proxy proxyId, in Box2 aabb, Vector2 displacement); + bool MoveProxy(DynamicTree.Proxy proxyId, in Box2 aabb, Vector2 displacement); - FixtureProxy? GetProxy(DynamicTree.Proxy proxy); + FixtureProxy? GetProxy(DynamicTree.Proxy proxy); - void RemoveProxy(DynamicTree.Proxy proxy); + void RemoveProxy(DynamicTree.Proxy proxy); - void QueryAabb( - ref TState state, - DynamicTree.QueryCallbackDelegate callback, - Box2 aabb, - bool approx = false); + void QueryAabb( + ref TState state, + DynamicTree.QueryCallbackDelegate callback, + Box2 aabb, + bool approx = false); - IEnumerable QueryAabb(Box2 aabb, bool approx = false); + IEnumerable QueryAabb(Box2 aabb, bool approx = false); - IEnumerable QueryAabb(List proxies, Box2 aabb, bool approx = false); + IEnumerable QueryAabb(List proxies, Box2 aabb, bool approx = false); - void QueryPoint(DynamicTree.QueryCallbackDelegate callback, - Vector2 point, - bool approx = false); + void QueryPoint(DynamicTree.QueryCallbackDelegate callback, + Vector2 point, + bool approx = false); - void QueryPoint( - ref TState state, - DynamicTree.QueryCallbackDelegate callback, - Vector2 point, - bool approx = false); + void QueryPoint( + ref TState state, + DynamicTree.QueryCallbackDelegate callback, + Vector2 point, + bool approx = false); - IEnumerable QueryPoint(Vector2 point, bool approx = false); + IEnumerable QueryPoint(Vector2 point, bool approx = false); - void QueryRay( - DynamicTree.RayQueryCallbackDelegate callback, - in Ray ray, - bool approx = false); + void QueryRay( + DynamicTree.RayQueryCallbackDelegate callback, + in Ray ray, + bool approx = false); - void QueryRay( - ref TState state, - DynamicTree.RayQueryCallbackDelegate callback, - in Ray ray, - bool approx = false); - } - - public interface IBroadPhase : ICollection where T : notnull { - - int Capacity { get; } - - int Height { - [MethodImpl(MethodImplOptions.AggressiveInlining)] - get; - } - - int MaxBalance { - [MethodImpl(MethodImplOptions.AggressiveOptimization | MethodImplOptions.NoInlining)] - get; - } - - float AreaRatio { - [MethodImpl(MethodImplOptions.AggressiveOptimization | MethodImplOptions.NoInlining)] - get; - } - - bool Add(in T item, Box2? newAABB = null); - - bool Remove(in T item); - - bool Update(in T item, Box2? newAABB = null); - - void QueryAabb( - DynamicTree.QueryCallbackDelegate callback, - Box2 aabb, - bool approx = false); - - void QueryAabb( - ref TState state, - DynamicTree.QueryCallbackDelegate callback, - Box2 aabb, - bool approx = false); - - IEnumerable QueryAabb(Box2 aabb, bool approx = false); - - void QueryPoint(DynamicTree.QueryCallbackDelegate callback, - Vector2 point, - bool approx = false); - - void QueryPoint( - ref TState state, - DynamicTree.QueryCallbackDelegate callback, - Vector2 point, - bool approx = false); - - IEnumerable QueryPoint(Vector2 point, bool approx = false); - - void QueryRay( - DynamicTree.RayQueryCallbackDelegate callback, - in Ray ray, - bool approx = false); - - void QueryRay( - ref TState state, - DynamicTree.RayQueryCallbackDelegate callback, - in Ray ray, - bool approx = false); - } + void QueryRay( + ref TState state, + DynamicTree.RayQueryCallbackDelegate callback, + in Ray ray, + bool approx = false); +} + +public interface IBroadPhase : ICollection where T : notnull { + + int Capacity { get; } + + int Height { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + get; + } + + int MaxBalance { + [MethodImpl(MethodImplOptions.AggressiveOptimization | MethodImplOptions.NoInlining)] + get; + } + + float AreaRatio { + [MethodImpl(MethodImplOptions.AggressiveOptimization | MethodImplOptions.NoInlining)] + get; + } + + bool Add(in T item, Box2? newAABB = null); + + bool Remove(in T item); + + bool Update(in T item, Box2? newAABB = null); + + void QueryAabb( + DynamicTree.QueryCallbackDelegate callback, + Box2 aabb, + bool approx = false); + + void QueryAabb( + ref TState state, + DynamicTree.QueryCallbackDelegate callback, + Box2 aabb, + bool approx = false); + + IEnumerable QueryAabb(Box2 aabb, bool approx = false); + + void QueryPoint(DynamicTree.QueryCallbackDelegate callback, + Vector2 point, + bool approx = false); + + void QueryPoint( + ref TState state, + DynamicTree.QueryCallbackDelegate callback, + Vector2 point, + bool approx = false); + + IEnumerable QueryPoint(Vector2 point, bool approx = false); + + void QueryRay( + DynamicTree.RayQueryCallbackDelegate callback, + in Ray ray, + bool approx = false); + + void QueryRay( + ref TState state, + DynamicTree.RayQueryCallbackDelegate callback, + in Ray ray, + bool approx = false); } diff --git a/Robust.Shared/Physics/IslandManager.cs b/Robust.Shared/Physics/IslandManager.cs deleted file mode 100644 index a9ea37de6..000000000 --- a/Robust.Shared/Physics/IslandManager.cs +++ /dev/null @@ -1,271 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ -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.Collections.Generic; -using Robust.Shared.Configuration; -using Robust.Shared.IoC; -using Robust.Shared.Physics.Dynamics; -using Robust.Shared.Utility; - -namespace Robust.Shared.Physics -{ - internal interface IIslandManager - { - void Initialize(); - void InitializePools(); - PhysicsIsland AllocateIsland(int numBodies, int numContacts, int numJoints); - - IReadOnlyList GetActive { get; } - } - - /// - /// Contains physics islands for use by PhysicsMaps. - /// Key difference from bullet3 is we won't merge small islands together due to the way box2d sleeping works. - /// - internal sealed class IslandManager : IIslandManager - { - [Dependency] private readonly IConfigurationManager _cfg = default!; - - private static readonly IslandBodyCapacitySort CapacitySort = new(); - private static readonly IslandBodyCountSort CountSort = new(); - - /// - /// The island all non-contact non-joint bodies are added to to batch together. This needs its own custom sleeping - /// given we cant wait for every body to be ready to sleep. - /// - private PhysicsIsland _loneIsland = new() {LoneIsland = true, ID = 0}; - - /// - /// Contains islands currently in use. - /// - private List _activeIslands = new(); - - private List _freeIslands = new(); - - /// - /// Contains every single PhysicsIsland. - /// - private List _allocatedIslands = new(); - - public IReadOnlyList GetActive - { - get - { - if (_loneIsland.BodyCount > 0) - { - _activeIslands.Add(_loneIsland); - } - - // Look this is kinda stinky but it's only called at the appropriate place for now - _activeIslands.Sort(CountSort); - return _activeIslands; - } - } - - private int _tickRate; - private float _maxLinearVelocityRaw; - private float _maxAngularVelocityRaw; - private IslandCfg _islandCfg; - - public void Initialize() - { - InitConfig(); - - _loneIsland.Initialize(); - // Set an initial size so we don't spam a bunch of array resizes at the start - _loneIsland.Resize(64, 32, 8); - } - - private void InitConfig() - { - // Config stuff. - CfgVar(CVars.AngularSleepTolerance, value => _islandCfg.AngTolSqr = value * value); - CfgVar(CVars.LinearSleepTolerance, value => _islandCfg.LinTolSqr = value * value); - CfgVar(CVars.WarmStarting, value => _islandCfg.WarmStarting = value); - CfgVar(CVars.VelocityIterations, value => _islandCfg.VelocityIterations = value); - CfgVar(CVars.PositionIterations, value => _islandCfg.PositionIterations = value); - CfgVar(CVars.SleepAllowed, value => _islandCfg.SleepAllowed = value); - CfgVar(CVars.TimeToSleep, value => _islandCfg.TimeToSleep = value); - CfgVar(CVars.VelocityThreshold, value => _islandCfg.VelocityThreshold = value); - CfgVar(CVars.Baumgarte, value => _islandCfg.Baumgarte = value); - CfgVar(CVars.MaxLinearCorrection, value => _islandCfg.MaxLinearCorrection = value); - CfgVar(CVars.MaxAngularCorrection, value => _islandCfg.MaxAngularCorrection = value); - CfgVar(CVars.VelocityConstraintsPerThread, value => _islandCfg.VelocityConstraintsPerThread = value); - CfgVar(CVars.VelocityConstraintMinimumThreads, value => _islandCfg.VelocityConstraintsMinimumThreads = value); - CfgVar(CVars.PositionConstraintsPerThread, value => _islandCfg.PositionConstraintsPerThread = value); - CfgVar(CVars.PositionConstraintsMinimumThread, value => _islandCfg.PositionConstraintsMinimumThreads = value); - CfgVar(CVars.MaxLinVelocity, value => - { - _maxLinearVelocityRaw = value; - UpdateMaxLinearVelocity(); - }); - CfgVar(CVars.MaxAngVelocity, value => - { - _maxAngularVelocityRaw = value; - UpdateMaxAngularVelocity(); - }); - CfgVar(CVars.NetTickrate, value => - { - _tickRate = value; - UpdateMaxLinearVelocity(); - UpdateMaxAngularVelocity(); - }); - - void UpdateMaxLinearVelocity() - { - _islandCfg.MaxTranslationPerTick = _maxLinearVelocityRaw / _tickRate; - } - - void UpdateMaxAngularVelocity() - { - _islandCfg.MaxRotationPerTick = (MathF.PI * 2 * _maxAngularVelocityRaw) / _tickRate; - } - - void CfgVar(CVarDef cVar, Action callback) where T : notnull - { - _cfg.OnValueChanged(cVar, value => - { - callback(value); - UpdateIslandCfg(); - }, true); - } - } - - private void UpdateIslandCfg() - { - // OOP bad - - _loneIsland.LoadConfig(_islandCfg); - - foreach (var island in _allocatedIslands) - { - island.LoadConfig(_islandCfg); - } - } - - public void InitializePools() - { - _loneIsland.Clear(); - _activeIslands.Clear(); - _freeIslands.Clear(); - - // Check whether allocated islands are sorted - var lastCapacity = 0; - var isSorted = true; - - foreach (var island in _allocatedIslands) - { - var capacity = island.BodyCapacity; - if (capacity > lastCapacity) - { - isSorted = false; - break; - } - - lastCapacity = capacity; - } - - if (!isSorted) - { - _allocatedIslands.Sort(CapacitySort); - } - - // TODO: Look at removing islands occasionally just to avoid memory bloat over time. - // e.g. every 30 seconds go through every island that hasn't been used in 30 seconds and remove it. - - // Free up islands - foreach (var island in _allocatedIslands) - { - island.Clear(); - _freeIslands.Add(island); - } - } - - public PhysicsIsland AllocateIsland(int bodyCount, int contactCount, int jointCount) - { - // If only 1 body then that means no contacts or joints. This also means that we can add it to the loneisland - if (bodyCount == 1) - { - // Island manages re-sizes internally so don't need to worry about this being hammered. - _loneIsland.Resize(_loneIsland.BodyCount + 1, 0, 0); - return _loneIsland; - } - - PhysicsIsland? island = null; - - // Because bullet3 combines islands it's more relevant for them to allocate by bodies but at least for now - // we don't combine. - if (_freeIslands.Count > 0) - { - // Try to use an existing island; with the smallest size that can hold us if possible. - var iFound = _freeIslands.Count; - - for (var i = _freeIslands.Count - 1; i >= 0; i--) - { - if (_freeIslands[i].BodyCapacity >= bodyCount) - { - iFound = i; - island = _freeIslands[i]; - DebugTools.Assert(island.BodyCount == 0 && island.ContactCount == 0 && island.JointCount == 0); - break; - } - } - - if (island != null) - { - var iDest = iFound; - var iSrc = iDest + 1; - while (iSrc < _freeIslands.Count) - { - _freeIslands[iDest++] = _freeIslands[iSrc++]; - } - - _freeIslands.RemoveAt(_freeIslands.Count - 1); - } - } - - if (island == null) - { - island = new PhysicsIsland(); - island.Initialize(); - island.LoadConfig(_islandCfg); - _allocatedIslands.Add(island); - } - - island.Resize(bodyCount, contactCount, jointCount); - // 0 ID taken up by LoneIsland - island.ID = _activeIslands.Count + 1; - _activeIslands.Add(island); - return island; - } - } - - internal sealed class IslandBodyCapacitySort : Comparer - { - public override int Compare(PhysicsIsland? x, PhysicsIsland? y) - { - if (x == null || y == null) return 0; - return x.BodyCapacity > y.BodyCapacity ? 1 : 0; - } - } - - internal sealed class IslandBodyCountSort : Comparer - { - public override int Compare(PhysicsIsland? x, PhysicsIsland? y) - { - if (x == null || y == null) return 0; - return x.BodyCount > y.BodyCount ? 1 : 0; - } - } -} diff --git a/Robust.Shared/Physics/IslandSolveMessage.cs b/Robust.Shared/Physics/IslandSolveMessage.cs index 79fc551f4..26f00bea0 100644 --- a/Robust.Shared/Physics/IslandSolveMessage.cs +++ b/Robust.Shared/Physics/IslandSolveMessage.cs @@ -1,13 +1,14 @@ using System.Collections.Generic; using Robust.Shared.GameObjects; +using Robust.Shared.Physics.Components; namespace Robust.Shared.Physics { internal sealed class IslandSolveMessage : EntityEventArgs { - public List Bodies { get; } + public List Bodies { get; } - public IslandSolveMessage(List bodies) + public IslandSolveMessage(List bodies) { Bodies = bodies; } diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs index 9df7a950d..b16e76fcd 100644 --- a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs @@ -186,7 +186,7 @@ namespace Robust.Shared.Physics.Systems pMoveBuffer[idx++] = (proxy, aabb); } - var options = new ParallelOptions() + var options = new ParallelOptions { MaxDegreeOfParallelism = _parallel.ParallelProcessCount, }; @@ -219,7 +219,7 @@ namespace Robust.Shared.Physics.Systems EntityQuery broadphaseQuery) tuple) => { ref var buffer = ref tuple.pairBuffer; - tuple.system.FindPairs(tuple.proxy, tuple.worldAABB, ((Component) grid).Owner, buffer, tuple.xformQuery, tuple.broadphaseQuery); + tuple.system.FindPairs(tuple.proxy, tuple.worldAABB, grid.Owner, buffer, tuple.xformQuery, tuple.broadphaseQuery); return true; }); diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs new file mode 100644 index 000000000..3a33e688e --- /dev/null +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs @@ -0,0 +1,1020 @@ +using System; +using System.Buffers; +using System.Collections.Generic; +using System.Threading.Tasks; +using Microsoft.Extensions.ObjectPool; +using Robust.Shared.GameObjects; +using Robust.Shared.Maths; +using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Dynamics.Contacts; +using Robust.Shared.Physics.Dynamics.Joints; +using Robust.Shared.Utility; +using YamlDotNet.Core.Tokens; + +namespace Robust.Shared.Physics.Systems; + +/* + * These comments scabbed directly from Box2D and the licence applies to them. + */ + +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +/* +Position Correction Notes +========================= +I tried the several algorithms for position correction of the 2D revolute joint. +I looked at these systems: +- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. +- suspension bridge with 30 1m long planks of length 1m. +- multi-link chain with 30 1m long links. +Here are the algorithms: +Baumgarte - A fraction of the position error is added to the velocity error. There is no +separate position solver. +Pseudo Velocities - After the velocity solver and position integration, +the position error, Jacobian, and effective mass are recomputed. Then +the velocity constraints are solved with pseudo velocities and a fraction +of the position error is added to the pseudo velocity error. The pseudo +velocities are initialized to zero and there is no warm-starting. After +the position solver, the pseudo velocities are added to the positions. +This is also called the First Order World method or the Position LCP method. +Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the +position error is re-computed for each constraint and the positions are updated +after the constraint is solved. The radius vectors (aka Jacobians) are +re-computed too (otherwise the algorithm has horrible instability). The pseudo +velocity states are not needed because they are effectively zero at the beginning +of each iteration. Since we have the current position error, we allow the +iterations to terminate early if the error becomes smaller than b2_linearSlop. +Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed +each time a constraint is solved. +Here are the results: +Baumgarte - this is the cheapest algorithm but it has some stability problems, +especially with the bridge. The chain links separate easily close to the root +and they jitter as they struggle to pull together. This is one of the most common +methods in the field. The big drawback is that the position correction artificially +affects the momentum, thus leading to instabilities and false bounce. I used a +bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller +factor makes joints and contacts more spongy. +Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is +stable. However, joints still separate with large angular velocities. Drag the +simple pendulum in a circle quickly and the joint will separate. The chain separates +easily and does not recover. I used a bias factor of 0.2. A larger value lead to +the bridge collapsing when a heavy cube drops on it. +Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo +Velocities, but in other ways it is worse. The bridge and chain are much more +stable, but the simple pendulum goes unstable at high angular velocities. +Full NGS - stable in all tests. The joints display good stiffness. The bridge +still sags, but this is better than infinite forces. +Recommendations +Pseudo Velocities are not really worthwhile because the bridge and chain cannot +recover from joint separation. In other cases the benefit over Baumgarte is small. +Modified NGS is not a robust method for the revolute joint due to the violent +instability seen in the simple pendulum. Perhaps it is viable with other constraint +types, especially scalar constraints where the effective mass is a scalar. +This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities +and is very fast. I don't think we can escape Baumgarte, especially in highly +demanding cases where high constraint fidelity is not needed. +Full NGS is robust and easy on the eyes. I recommend this as an option for +higher fidelity simulation and certainly for suspension bridges and long chains. +Full NGS might be a good choice for ragdolls, especially motorized ragdolls where +joint separation can be problematic. The number of NGS iterations can be reduced +for better performance without harming robustness much. +Each joint in a can be handled differently in the position solver. So I recommend +a system where the user can select the algorithm on a per joint basis. I would +probably default to the slower Full NGS and let the user select the faster +Baumgarte method in performance critical scenarios. +*/ + +/* +Cache Performance +The Box2D solvers are dominated by cache misses. Data structures are designed +to increase the number of cache hits. Much of misses are due to random access +to body data. The constraint structures are iterated over linearly, which leads +to few cache misses. +The bodies are not accessed during iteration. Instead read only data, such as +the mass values are stored with the constraints. The mutable data are the constraint +impulses and the bodies velocities/positions. The impulses are held inside the +constraint structures. The body velocities/positions are held in compact, temporary +arrays to increase the number of cache hits. Linear and angular velocity are +stored in a single array since multiple arrays lead to multiple misses. +*/ + +public abstract partial class SharedPhysicsSystem +{ + /* + * Handles island generation and constraints solver code. + */ + private const int MaxIslands = 256; + + private readonly ObjectPool> _islandBodyPool = + new DefaultObjectPool>(new ListPolicy(), MaxIslands); + + private readonly ObjectPool> _islandContactPool = + new DefaultObjectPool>(new ListPolicy(), MaxIslands); + + private readonly ObjectPool> _islandJointPool = new DefaultObjectPool>(new ListPolicy(), MaxIslands); + + internal record struct IslandData( + int Index, + bool LoneIsland, + List Bodies, + List Contacts, + List Joints, + List<(Joint Joint, float Error)> BrokenJoints) + { + /// + /// Island index to be used for bodies to identify which island they're in. + /// + public readonly int Index = Index; + + /// + /// Are we the special island that has all contact-less bodies in it? + /// This is treated separately for sleep purposes. + /// + public readonly bool LoneIsland = LoneIsland; + + /// + /// Offset in the data arrays + /// + public int Offset = 0; + + public readonly List Bodies = Bodies; + public readonly List Contacts = Contacts; + public readonly List Joints = Joints; + public bool PositionSolved = false; + public readonly List<(Joint Joint, float Error)> BrokenJoints = BrokenJoints; + } + + // Caching for island generation. + private readonly HashSet _islandSet = new(64); + private readonly Stack _bodyStack = new(64); + private readonly List _awakeBodyList = new(256); + + // Config + private bool _warmStarting; + private float _maxLinearCorrection; + private float _maxAngularCorrection; + private int _velocityIterations; + private int _positionIterations; + private float _maxLinearVelocity; + private float _maxAngularVelocity; + private float _maxTranslationPerTick; + private float _maxRotationPerTick; + private int _tickRate; + private bool _sleepAllowed; + protected float AngularToleranceSqr; + protected float LinearToleranceSqr; + protected float TimeToSleep; + private float _velocityThreshold; + private float _baumgarte; + + private const int VelocityConstraintsPerThread = 16; + private const int PositionConstraintsPerThread = 16; + + #region Setup + + private void InitializeIsland() + { + _configManager.OnValueChanged(CVars.NetTickrate, SetTickRate, true); + _configManager.OnValueChanged(CVars.WarmStarting, SetWarmStarting, true); + _configManager.OnValueChanged(CVars.MaxLinearCorrection, SetMaxLinearCorrection, true); + _configManager.OnValueChanged(CVars.MaxAngularCorrection, SetMaxAngularCorrection, true); + _configManager.OnValueChanged(CVars.VelocityIterations, SetVelocityIterations, true); + _configManager.OnValueChanged(CVars.PositionIterations, SetPositionIterations, true); + _configManager.OnValueChanged(CVars.MaxLinVelocity, SetMaxLinearVelocity, true); + _configManager.OnValueChanged(CVars.MaxAngVelocity, SetMaxAngularVelocity, true); + _configManager.OnValueChanged(CVars.SleepAllowed, SetSleepAllowed, true); + _configManager.OnValueChanged(CVars.AngularSleepTolerance, SetAngularToleranceSqr, true); + _configManager.OnValueChanged(CVars.LinearSleepTolerance, SetLinearToleranceSqr, true); + _configManager.OnValueChanged(CVars.TimeToSleep, SetTimeToSleep, true); + _configManager.OnValueChanged(CVars.VelocityThreshold, SetVelocityThreshold, true); + _configManager.OnValueChanged(CVars.Baumgarte, SetBaumgarte, true); + } + + private void ShutdownIsland() + { + _configManager.UnsubValueChanged(CVars.NetTickrate, SetTickRate); + _configManager.UnsubValueChanged(CVars.WarmStarting, SetWarmStarting); + _configManager.UnsubValueChanged(CVars.MaxLinearCorrection, SetMaxLinearCorrection); + _configManager.UnsubValueChanged(CVars.MaxAngularCorrection, SetMaxAngularCorrection); + _configManager.UnsubValueChanged(CVars.VelocityIterations, SetVelocityIterations); + _configManager.UnsubValueChanged(CVars.PositionIterations, SetPositionIterations); + _configManager.UnsubValueChanged(CVars.MaxLinVelocity, SetMaxLinearVelocity); + _configManager.UnsubValueChanged(CVars.MaxAngVelocity, SetMaxAngularVelocity); + _configManager.UnsubValueChanged(CVars.SleepAllowed, SetSleepAllowed); + _configManager.UnsubValueChanged(CVars.AngularSleepTolerance, SetAngularToleranceSqr); + _configManager.UnsubValueChanged(CVars.LinearSleepTolerance, SetLinearToleranceSqr); + _configManager.UnsubValueChanged(CVars.TimeToSleep, SetTimeToSleep); + _configManager.UnsubValueChanged(CVars.VelocityThreshold, SetVelocityThreshold); + _configManager.UnsubValueChanged(CVars.Baumgarte, SetBaumgarte); + } + + private void SetWarmStarting(bool value) => _warmStarting = value; + private void SetMaxLinearCorrection(float value) => _maxLinearCorrection = value; + private void SetMaxAngularCorrection(float value) => _maxAngularCorrection = value; + private void SetVelocityIterations(int value) => _velocityIterations = value; + private void SetPositionIterations(int value) => _positionIterations = value; + private void SetMaxLinearVelocity(float value) + { + _maxLinearVelocity = value; + UpdateMaxTranslation(); + } + + private void SetMaxAngularVelocity(float value) + { + _maxAngularVelocity = value; + UpdateMaxRotation(); + } + + private void SetTickRate(int value) + { + _tickRate = value; + UpdateMaxTranslation(); + UpdateMaxRotation(); + } + + private void SetSleepAllowed(bool value) => _sleepAllowed = value; + private void SetAngularToleranceSqr(float value) => AngularToleranceSqr = value; + private void SetLinearToleranceSqr(float value) => LinearToleranceSqr = value; + private void SetTimeToSleep(float value) => TimeToSleep = value; + private void SetVelocityThreshold(float value) => _velocityThreshold = value; + private void SetBaumgarte(float value) => _baumgarte = value; + + private void UpdateMaxTranslation() + { + _maxTranslationPerTick = _maxLinearVelocity / _tickRate; + } + + private void UpdateMaxRotation() + { + _maxRotationPerTick = (MathF.Tau * _maxAngularVelocity) / _tickRate; + } + + #endregion + + /// + /// Where the magic happens. + /// + public void Step(SharedPhysicsMapComponent 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); + + // TODO: SolveTOI + + var updateAfterSolve = new PhysicsUpdateAfterMapSolveEvent(prediction, component, frameTime); + RaiseLocalEvent(ref updateAfterSolve); + + // Box2d recommends clearing (if you are) during fixed updates rather than variable if you are using it + if (!prediction && component.AutoClearForces) + ClearForces(component); + + component._invDt0 = invDt; + } + + private void ClearForces(SharedPhysicsMapComponent component) + { + foreach (var body in component.AwakeBodies) + { + // TODO: Netsync + body.Force = Vector2.Zero; + body.Torque = 0.0f; + } + } + + private void Solve(SharedPhysicsMapComponent 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); + _awakeBodyList.AddRange(component.AwakeBodies); + + var bodyQuery = GetEntityQuery(); + var metaQuery = GetEntityQuery(); + var jointQuery = GetEntityQuery(); + var islandIndex = 0; + var loneIsland = new IslandData( + islandIndex++, + true, + _islandBodyPool.Get(), + _islandContactPool.Get(), + _islandJointPool.Get(), + new List<(Joint Joint, float Error)>()); + + var islands = new List(); + + // Build the relevant islands / graphs for all bodies. + foreach (var seed in _awakeBodyList) + { + // I tried not running prediction for non-contacted entities but unfortunately it looked like shit + // when contact broke so if you want to try that then GOOD LUCK. + if (seed.Island) continue; + + if (!metaQuery.TryGetComponent(seed.Owner, out var metadata)) + { + _sawmill.Error($"Found deleted entity {ToPrettyString(seed.Owner)} on map!"); + component.RemoveSleepBody(seed); + continue; + } + + if ((metadata.EntityPaused && !seed.IgnorePaused) || + (prediction && !seed.Predict) || + !seed.CanCollide || + seed.BodyType == BodyType.Static) + { + continue; + } + + // Start of a new island + var bodies = _islandBodyPool.Get(); + var contacts = _islandContactPool.Get(); + var joints = _islandJointPool.Get(); + _bodyStack.Push(seed); + + seed.Island = true; + + while (_bodyStack.TryPop(out var body)) + { + bodies.Add(body); + _islandSet.Add(body); + + // Static bodies don't propagate islands + if (body.BodyType == BodyType.Static) continue; + + // As static bodies can never be awake (unlike Farseer) we'll set this after the check. + SetAwake(body, true, updateSleepTime: false); + + var node = body.Contacts.First; + + while (node != null) + { + var contact = node.Value; + node = node.Next; + + // Has this contact already been added to an island? + if ((contact.Flags & ContactFlags.Island) != 0x0) continue; + + // Is this contact solid and touching? + if (!contact.Enabled || !contact.IsTouching) continue; + + // Skip sensors. + if (contact.FixtureA?.Hard != true || contact.FixtureB?.Hard != true) continue; + + contacts.Add(contact); + contact.Flags |= ContactFlags.Island; + var bodyA = contact.FixtureA!.Body; + var bodyB = contact.FixtureB!.Body; + + var other = bodyA == body ? bodyB : bodyA; + + // Was the other body already added to this island? + if (other.Island) continue; + + _bodyStack.Push(other); + other.Island = true; + } + + if (!jointQuery.TryGetComponent(body.Owner, out var jointComponent)) continue; + + foreach (var (_, joint) in jointComponent.Joints) + { + if (joint.IslandFlag) continue; + + var other = joint.BodyAUid == body.Owner + ? bodyQuery.GetComponent(joint.BodyBUid) + : bodyQuery.GetComponent(joint.BodyAUid); + + // Don't simulate joints connected to inactive bodies. + if (!other.CanCollide) continue; + + joints.Add(joint); + joint.IslandFlag = true; + + if (other.Island) continue; + + _bodyStack.Push(other); + other.Island = true; + } + } + + int idx; + + // Bodies not touching anything, hence we can just add it to the lone island. + if (contacts.Count == 0 && joints.Count == 0) + { + DebugTools.Assert(bodies.Count == 1 && bodies[0].BodyType != BodyType.Static); + loneIsland.Bodies.Add(bodies[0]); + idx = loneIsland.Index; + } + else + { + var data = new IslandData(islandIndex++, false, bodies, contacts, joints, new List<(Joint Joint, float Error)>()); + islands.Add(data); + idx = data.Index; + } + + // Allow static bodies to be re-used in other islands + for (var i = 0; i < bodies.Count; i++) + { + var body = bodies[i]; + + // Static bodies can participate in other islands + if (body.BodyType == BodyType.Static) + { + body.Island = false; + } + + body.IslandIndex[idx] = i; + } + } + + // If we didn't use lone island just return it. + if (loneIsland.Bodies.Count > 0) + { + islands.Add(loneIsland); + } + else + { + ReturnIsland(loneIsland); + } + + SolveIslands(component, islands, frameTime, dtRatio, invDt, prediction); + + foreach (var island in islands) + { + ReturnIsland(island); + } + + Cleanup(component, frameTime); + } + + private void ReturnIsland(IslandData island) + { + foreach (var body in island.Bodies) + { + body.IslandIndex.Remove(island.Index); + } + + _islandBodyPool.Return(island.Bodies); + _islandContactPool.Return(island.Contacts); + + foreach (var joint in island.Joints) + { + joint.IslandFlag = false; + } + + _islandJointPool.Return(island.Joints); + island.BrokenJoints.Clear(); + } + + protected virtual void Cleanup(SharedPhysicsMapComponent component, float frameTime) + { + foreach (var body in _islandSet) + { + if (!body.Island || body.Deleted) + { + continue; + } + + body.Island = false; + DebugTools.Assert(body.BodyType != BodyType.Static); + + // So Box2D would update broadphase here buutttt we'll just wait until MoveEvent queue is used. + } + + _islandSet.Clear(); + _awakeBodyList.Clear(); + } + + private void SolveIslands(SharedPhysicsMapComponent component, List islands, float frameTime, float dtRatio, float invDt, bool prediction) + { + var iBegin = 0; + var gravity = component.Gravity; + + var data = new SolverData( + frameTime, + dtRatio, + invDt, + _warmStarting, + _maxLinearCorrection, + _maxAngularCorrection, + _velocityIterations, + _positionIterations, + _maxLinearVelocity, + _maxAngularVelocity, + _maxTranslationPerTick, + _maxRotationPerTick, + _sleepAllowed, + AngularToleranceSqr, + LinearToleranceSqr, + TimeToSleep, + _velocityThreshold, + _baumgarte + ); + + // We'll sort islands from internally parallel (due to lots of contacts) to running all the islands in parallel + islands.Sort((x, y) => InternalParallel(y).CompareTo(InternalParallel(x))); + + var totalBodies = 0; + var actualIslands = islands.ToArray(); + var xformQuery = GetEntityQuery(); + + for (var i = 0; i < islands.Count; i++) + { + ref var island = ref actualIslands[i]; + island.Offset = totalBodies; + UpdateLerpData(component, island.Bodies, xformQuery); + +#if DEBUG + RaiseLocalEvent(new IslandSolveMessage(island.Bodies)); +#endif + + totalBodies += island.Bodies.Count; + } + + // Actual solver here; cache the data for later. + var solvedPositions = ArrayPool.Shared.Rent(totalBodies); + var solvedAngles = ArrayPool.Shared.Rent(totalBodies); + var linearVelocities = ArrayPool.Shared.Rent(totalBodies); + var angularVelocities = ArrayPool.Shared.Rent(totalBodies); + var sleepStatus = ArrayPool.Shared.Rent(totalBodies); + // Cleanup any potentially stale data first. + for (var i = 0; i < totalBodies; i++) + { + sleepStatus[i] = false; + } + + var options = new ParallelOptions() + { + MaxDegreeOfParallelism = _parallel.ParallelProcessCount, + }; + + while (iBegin < actualIslands.Length) + { + ref var island = ref actualIslands[iBegin]; + + if (!InternalParallel(island)) + break; + + SolveIsland(ref island, in data, options, gravity, prediction, solvedPositions, solvedAngles, linearVelocities, angularVelocities, sleepStatus); + iBegin++; + } + + Parallel.For(iBegin, actualIslands.Length, options, i => + { + ref var island = ref actualIslands[i]; + SolveIsland(ref island, in data, null, gravity, prediction, solvedPositions, solvedAngles, linearVelocities, angularVelocities, sleepStatus); + }); + + // Update data sequentially + var metaQuery = GetEntityQuery(); + + for (var i = 0; i < actualIslands.Length; i++) + { + var island = actualIslands[i]; + + UpdateBodies(in island, solvedPositions, solvedAngles, linearVelocities, angularVelocities, xformQuery, metaQuery); + SleepBodies(in island, sleepStatus); + } + + // Cleanup + ArrayPool.Shared.Return(solvedPositions); + ArrayPool.Shared.Return(solvedAngles); + ArrayPool.Shared.Return(linearVelocities); + ArrayPool.Shared.Return(angularVelocities); + ArrayPool.Shared.Return(sleepStatus); + } + + /// + /// If this is the first time a body has been updated this tick update its position for lerping. + /// Due to substepping we have to check it every time. + /// + protected virtual void UpdateLerpData(SharedPhysicsMapComponent component, List bodies, EntityQuery xformQuery) + { + + } + + /// + /// Can we run the island in parallel internally, otherwise solve it in parallel with the rest. + /// + /// + /// + private bool InternalParallel(IslandData island) + { + // Should lone island most times as well. + return island.Bodies.Count > 128 || island.Contacts.Count > 128 || island.Joints.Count > 128; + } + + /// + /// Go through all the bodies in this island and solve. + /// + private void SolveIsland( + ref IslandData island, + in SolverData data, + ParallelOptions? options, + Vector2 gravity, + bool prediction, + Vector2[] solvedPositions, + float[] solvedAngles, + Vector2[] linearVelocities, + float[] angularVelocities, + bool[] sleepStatus) + { + var bodyCount = island.Bodies.Count; + var positions = ArrayPool.Shared.Rent(bodyCount); + var angles = ArrayPool.Shared.Rent(bodyCount); + var offset = island.Offset; + var xformQuery = GetEntityQuery(); + + for (var i = 0; i < island.Bodies.Count; i++) + { + var body = island.Bodies[i]; + var (worldPos, worldRot) = + _transform.GetWorldPositionRotation(xformQuery.GetComponent(body.Owner), xformQuery); + + var transform = new Transform(worldPos, worldRot); + var position = Physics.Transform.Mul(transform, body.LocalCenter); + // DebugTools.Assert(!float.IsNaN(position.X) && !float.IsNaN(position.Y)); + var angle = transform.Quaternion2D.Angle; + + // var bodyTransform = body.GetTransform(); + // DebugTools.Assert(bodyTransform.Position.EqualsApprox(position) && MathHelper.CloseTo(angle, bodyTransform.Quaternion2D.Angle)); + + var linearVelocity = body.LinearVelocity; + var angularVelocity = body.AngularVelocity; + + // if the body cannot move, nothing to do here + if (body.BodyType == BodyType.Dynamic) + { + if (body.IgnoreGravity) + linearVelocity += body.Force * data.FrameTime * body.InvMass; + else + linearVelocity += (gravity + body.Force * body.InvMass) * data.FrameTime; + + angularVelocity += data.FrameTime * body.InvI * body.Torque; + + linearVelocity *= Math.Clamp(1.0f - data.FrameTime * body.LinearDamping, 0.0f, 1.0f); + angularVelocity *= Math.Clamp(1.0f - data.FrameTime * body.AngularDamping, 0.0f, 1.0f); + } + + positions[i] = position; + angles[i] = angle; + linearVelocities[i + offset] = linearVelocity; + angularVelocities[i + offset] = angularVelocity; + } + + var contactCount = island.Contacts.Count; + var velocityConstraints = ArrayPool.Shared.Rent(contactCount); + var positionConstraints = ArrayPool.Shared.Rent(contactCount); + + // Pass the data into the solver + ResetSolver(data, island, velocityConstraints, positionConstraints); + + InitializeVelocityConstraints(in data, in island, velocityConstraints, positionConstraints, positions, angles, linearVelocities, angularVelocities); + + if (data.WarmStarting) + { + WarmStart(data, island, velocityConstraints, linearVelocities, angularVelocities); + } + + var jointCount = island.Joints.Count; + var bodyQuery = GetEntityQuery(); + + if (jointCount > 0) + { + for (var i = 0; i < island.Joints.Count; i++) + { + var joint = island.Joints[i]; + if (!joint.Enabled) continue; + + var bodyA = bodyQuery.GetComponent(joint.BodyAUid); + var bodyB = bodyQuery.GetComponent(joint.BodyBUid); + joint.InitVelocityConstraints(in data, in island, bodyA, bodyB, positions, angles, linearVelocities, angularVelocities); + } + } + + // Velocity solver + for (var i = 0; i < data.VelocityIterations; i++) + { + for (var j = 0; j < jointCount; ++j) + { + var joint = island.Joints[j]; + + if (!joint.Enabled) + continue; + + joint.SolveVelocityConstraints(in data, in island, linearVelocities, angularVelocities); + + var error = joint.Validate(data.InvDt); + + if (error > 0.0f) + island.BrokenJoints.Add((joint, error)); + } + + SolveVelocityConstraints(island, options, velocityConstraints, linearVelocities, angularVelocities); + } + + // Store for warm starting. + StoreImpulses(in island, velocityConstraints); + + var maxVel = data.MaxTranslation / data.FrameTime; + var maxVelSq = maxVel * maxVel; + var maxAngVel = data.MaxRotation / data.FrameTime; + var maxAngVelSq = maxAngVel * maxAngVel; + + // Integrate positions + for (var i = 0; i < bodyCount; i++) + { + var linearVelocity = linearVelocities[offset + i]; + var angularVelocity = angularVelocities[offset + i]; + + var velSqr = linearVelocity.LengthSquared; + if (velSqr > maxVelSq) + { + linearVelocity *= maxVel / MathF.Sqrt(velSqr); + linearVelocities[offset + i] = linearVelocity; + } + + if (angularVelocity * angularVelocity > maxAngVelSq) + { + angularVelocity *= maxAngVel / MathF.Abs(angularVelocity); + angularVelocities[offset + i] = angularVelocity; + } + + // Integrate + positions[i] += linearVelocity * data.FrameTime; + angles[i] += angularVelocity * data.FrameTime; + } + + island.PositionSolved = false; + + for (var i = 0; i < data.PositionIterations; i++) + { + var contactsOkay = SolvePositionConstraints(data, in island, options, positionConstraints, positions, angles); + var jointsOkay = true; + + for (int j = 0; j < island.Joints.Count; ++j) + { + var joint = island.Joints[j]; + + if (!joint.Enabled) + continue; + + var jointOkay = joint.SolvePositionConstraints(in data, positions, angles); + + jointsOkay = jointsOkay && jointOkay; + } + + if (contactsOkay && jointsOkay) + { + island.PositionSolved = true; + break; + } + } + + // Transform the solved positions back into local terms + // This means we can run the entire solver in parallel and not have to worry about stale world positions later + // E.g. if a parent had its position updated then our worldposition is invalid + // We can safely do this in parallel. + + // Solve positions now and store for later; we can't write this safely in parallel. + var bodies = island.Bodies; + + if (options != null) + { + const int FinaliseBodies = 32; + var batches = (int)MathF.Ceiling((float) bodyCount / FinaliseBodies); + + Parallel.For(0, batches, options, i => + { + var start = i * FinaliseBodies; + var end = Math.Min(bodyCount, start + FinaliseBodies); + + FinalisePositions(start, end, offset, bodies, xformQuery, positions, angles, solvedPositions, solvedAngles); + }); + } + else + { + FinalisePositions(0, bodyCount, offset, bodies,xformQuery, positions, angles, solvedPositions, solvedAngles); + } + + // Check sleep status for all of the bodies + // Writing sleep timer is safe but updating awake or not is not safe. + + // We have a special island for no-contact no-joint bodies and just run this custom sleeping behaviour + // for it while still keeping the benefits of a big island. + if (island.LoneIsland) + { + if (!prediction && data.SleepAllowed) + { + for (var i = 0; i < bodyCount; i++) + { + var body = island.Bodies[i]; + + if (body.BodyType == BodyType.Static) continue; + + if (!body.SleepingAllowed || + body.AngularVelocity * body.AngularVelocity > data.AngTolSqr || + Vector2.Dot(body.LinearVelocity, body.LinearVelocity) > data.LinTolSqr) + { + SetSleepTime(body, 0f); + } + else + { + SetSleepTime(body, body.SleepTime + data.FrameTime); + } + + if (body.SleepTime >= data.TimeToSleep && island.PositionSolved) + { + sleepStatus[offset + i] = true; + } + } + } + } + else + { + // Sleep bodies if needed. Prediction won't accumulate sleep-time for bodies. + if (!prediction && data.SleepAllowed) + { + var minSleepTime = float.MaxValue; + + for (var i = 0; i < bodyCount; i++) + { + var body = island.Bodies[i]; + + if (body.BodyType == BodyType.Static) continue; + + if (!body.SleepingAllowed || + body.AngularVelocity * body.AngularVelocity > data.AngTolSqr || + Vector2.Dot(body.LinearVelocity, body.LinearVelocity) > data.LinTolSqr) + { + SetSleepTime(body, 0f); + minSleepTime = 0.0f; + } + else + { + SetSleepTime(body, body.SleepTime + data.FrameTime); + minSleepTime = MathF.Min(minSleepTime, body.SleepTime); + } + } + + if (minSleepTime >= data.TimeToSleep && island.PositionSolved) + { + for (var i = 0; i < island.Bodies.Count; i++) + { + sleepStatus[offset + i] = true; + } + } + } + } + + // Cleanup + ArrayPool.Shared.Return(velocityConstraints); + ArrayPool.Shared.Return(positionConstraints); + } + + private void FinalisePositions(int start, int end, int offset, List bodies, EntityQuery xformQuery, Vector2[] positions, float[] angles, Vector2[] solvedPositions, float[] solvedAngles) + { + for (var i = start; i < end; i++) + { + var body = bodies[i]; + + if (body.BodyType == BodyType.Static) + continue; + + var xform = xformQuery.GetComponent(body.Owner); + var parentXform = xformQuery.GetComponent(xform.ParentUid); + var (_, parentRot, parentInvMatrix) = parentXform.GetWorldPositionRotationInvMatrix(xformQuery); + var worldRot = (float) (parentRot + xform._localRotation); + + var angle = angles[i]; + + var q = new Quaternion2D(angle); + var adjustedPosition = positions[i] - Physics.Transform.Mul(q, body.LocalCenter); + + var solvedPosition = parentInvMatrix.Transform(adjustedPosition); + solvedPositions[offset + i] = solvedPosition - xform.LocalPosition; + solvedAngles[offset + i] = angles[i] - worldRot; + } + } + + /// + /// Updates the positions, rotations, and velocities of all of the solved bodies. + /// Run sequentially to avoid threading issues. + /// + private void UpdateBodies( + in IslandData island, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities, + EntityQuery xformQuery, + EntityQuery metaQuery) + { + foreach (var (joint, error) in island.BrokenJoints) + { + var ev = new JointBreakEvent(joint, MathF.Sqrt(error)); + RaiseLocalEvent(joint.BodyAUid, ref ev); + RaiseLocalEvent(joint.BodyBUid, ref ev); + RaiseLocalEvent(ref ev); + } + + var offset = island.Offset; + + for (var i = 0; i < island.Bodies.Count; i++) + { + var body = island.Bodies[i]; + + // So technically we don't /need/ to skip static bodies here but it saves us having to check for deferred updates so we'll do it anyway. + // Plus calcing worldpos can be costly so we skip that too which is nice. + if (body.BodyType == BodyType.Static) continue; + + var position = positions[offset + i]; + var angle = angles[offset + i]; + var xform = xformQuery.GetComponent(body.Owner); + + // Temporary NaN guards until PVS is fixed. + if (!float.IsNaN(position.X) && !float.IsNaN(position.Y)) + { + _transform.SetLocalPositionRotation(xform, xform.LocalPosition + position, xform.LocalRotation + angle); + } + + var linVelocity = linearVelocities[offset + i]; + + if (!float.IsNaN(linVelocity.X) && !float.IsNaN(linVelocity.Y)) + { + SetLinearVelocity(body, linVelocity, false); + } + + var angVelocity = angularVelocities[offset + i]; + + if (!float.IsNaN(angVelocity)) + { + SetAngularVelocity(body, angVelocity, false); + } + + // TODO: Should check if the values update. + Dirty(body, metaQuery.GetComponent(body.Owner)); + } + } + + private void SleepBodies(in IslandData island, bool[] sleepStatus) + { + var offset = island.Offset; + + for (var i = 0; i < island.Bodies.Count; i++) + { + var sleep = sleepStatus[offset + i]; + + if (!sleep) + continue; + + SetAwake(island.Bodies[i], false); + } + } +} diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs new file mode 100644 index 000000000..064218595 --- /dev/null +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs @@ -0,0 +1,912 @@ +/* +* 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.Threading; +using System.Threading.Tasks; +using Robust.Shared.Maths; +using Robust.Shared.Physics.Collision; +using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Dynamics.Contacts; +using Robust.Shared.Utility; + +namespace Robust.Shared.Physics.Systems; + +public abstract partial class SharedPhysicsSystem +{ + private void ResetSolver( + in SolverData data, + in IslandData island, + ContactVelocityConstraint[] velocityConstraints, + ContactPositionConstraint[] positionConstraints) + { + var contactCount = island.Contacts.Count; + + // Build constraints + // For now these are going to be bare but will change + for (var i = 0; i < contactCount; i++) + { + var contact = island.Contacts[i]; + Fixture fixtureA = contact.FixtureA!; + Fixture fixtureB = contact.FixtureB!; + var shapeA = fixtureA.Shape; + var shapeB = fixtureB.Shape; + float radiusA = shapeA.Radius; + float radiusB = shapeB.Radius; + var bodyA = fixtureA.Body; + var bodyB = fixtureB.Body; + var manifold = contact.Manifold; + + int pointCount = manifold.PointCount; + DebugTools.Assert(pointCount > 0); + + ref var velocityConstraint = ref velocityConstraints[i]; + velocityConstraint.Friction = contact.Friction; + velocityConstraint.Restitution = contact.Restitution; + velocityConstraint.TangentSpeed = contact.TangentSpeed; + velocityConstraint.IndexA = bodyA.IslandIndex[island.Index]; + velocityConstraint.IndexB = bodyB.IslandIndex[island.Index]; + velocityConstraint.Points = new VelocityConstraintPoint[2]; + + for (var j = 0; j < 2; j++) + { + velocityConstraint.Points[j] = new VelocityConstraintPoint(); + } + + var (invMassA, invMassB) = GetInvMass(bodyA, bodyB); + + (velocityConstraint.InvMassA, velocityConstraint.InvMassB) = (invMassA, invMassB); + velocityConstraint.InvIA = bodyA.InvI; + velocityConstraint.InvIB = bodyB.InvI; + velocityConstraint.ContactIndex = i; + velocityConstraint.PointCount = pointCount; + + velocityConstraint.K = System.Numerics.Vector4.Zero; + velocityConstraint.NormalMass = System.Numerics.Vector4.Zero; + + ref var positionConstraint = ref positionConstraints[i]; + positionConstraint.IndexA = bodyA.IslandIndex[island.Index]; + positionConstraint.IndexB = bodyB.IslandIndex[island.Index]; + (positionConstraint.InvMassA, positionConstraint.InvMassB) = (invMassA, invMassB); + positionConstraint.LocalCenterA = bodyA.LocalCenter; + positionConstraint.LocalCenterB = bodyB.LocalCenter; + positionConstraint.LocalPoints = new Vector2[2]; + + positionConstraint.InvIA = bodyA.InvI; + positionConstraint.InvIB = bodyB.InvI; + positionConstraint.LocalNormal = manifold.LocalNormal; + positionConstraint.LocalPoint = manifold.LocalPoint; + positionConstraint.PointCount = pointCount; + positionConstraint.RadiusA = radiusA; + positionConstraint.RadiusB = radiusB; + positionConstraint.Type = manifold.Type; + + for (var j = 0; j < pointCount; ++j) + { + var contactPoint = manifold.Points[j]; + ref var constraintPoint = ref velocityConstraint.Points[j]; + + if (_warmStarting) + { + constraintPoint.NormalImpulse = data.DtRatio * contactPoint.NormalImpulse; + constraintPoint.TangentImpulse = data.DtRatio * contactPoint.TangentImpulse; + } + else + { + constraintPoint.NormalImpulse = 0.0f; + constraintPoint.TangentImpulse = 0.0f; + } + + constraintPoint.RelativeVelocityA = Vector2.Zero; + constraintPoint.RelativeVelocityB = Vector2.Zero; + constraintPoint.NormalMass = 0.0f; + constraintPoint.TangentMass = 0.0f; + constraintPoint.VelocityBias = 0.0f; + + positionConstraint.LocalPoints[j] = contactPoint.LocalPoint; + } + } + } + + private (float, float) GetInvMass(IPhysBody bodyA, IPhysBody bodyB) + { + // God this is shitcodey but uhhhh we need to snowflake KinematicController for nice collisions. + // TODO: Might need more finagling with the kinematic bodytype + switch (bodyA.BodyType) + { + case BodyType.Kinematic: + case BodyType.Static: + return (bodyA.InvMass, bodyB.InvMass); + case BodyType.KinematicController: + switch (bodyB.BodyType) + { + case BodyType.Kinematic: + case BodyType.Static: + return (bodyA.InvMass, bodyB.InvMass); + case BodyType.Dynamic: + return (bodyA.InvMass, 0f); + case BodyType.KinematicController: + return (0f, 0f); + default: + throw new ArgumentOutOfRangeException(); + } + case BodyType.Dynamic: + switch (bodyB.BodyType) + { + case BodyType.Kinematic: + case BodyType.Static: + case BodyType.Dynamic: + return (bodyA.InvMass, bodyB.InvMass); + case BodyType.KinematicController: + return (0f, bodyB.InvMass); + default: + throw new ArgumentOutOfRangeException(); + } + default: + throw new ArgumentOutOfRangeException(); + } + } + + private void InitializeVelocityConstraints( + in SolverData data, + in IslandData island, + ContactVelocityConstraint[] velocityConstraints, + ContactPositionConstraint[] positionConstraints, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) + { + Span points = stackalloc Vector2[2]; + var contactCount = island.Contacts.Count; + var contacts = island.Contacts; + var offset = island.Offset; + + for (var i = 0; i < contactCount; ++i) + { + ref var velocityConstraint = ref velocityConstraints[i]; + var positionConstraint = positionConstraints[i]; + + var radiusA = positionConstraint.RadiusA; + var radiusB = positionConstraint.RadiusB; + var manifold = contacts[velocityConstraint.ContactIndex].Manifold; + + var indexA = velocityConstraint.IndexA; + var indexB = velocityConstraint.IndexB; + + var invMassA = velocityConstraint.InvMassA; + var invMassB = velocityConstraint.InvMassB; + var invIA = velocityConstraint.InvIA; + var invIB = velocityConstraint.InvIB; + var localCenterA = positionConstraint.LocalCenterA; + var localCenterB = positionConstraint.LocalCenterB; + + var centerA = positions[indexA]; + var angleA = angles[indexA]; + var linVelocityA = linearVelocities[offset + indexA]; + var angVelocityA = angularVelocities[offset + indexA]; + + var centerB = positions[indexB]; + var angleB = angles[indexB]; + var linVelocityB = linearVelocities[offset + indexB]; + var angVelocityB = angularVelocities[offset + indexB]; + + DebugTools.Assert(manifold.PointCount > 0); + + var xfA = new Transform(angleA); + var xfB = new Transform(angleB); + xfA.Position = centerA - Physics.Transform.Mul(xfA.Quaternion2D, localCenterA); + xfB.Position = centerB - Physics.Transform.Mul(xfB.Quaternion2D, localCenterB); + + InitializeManifold(ref manifold, xfA, xfB, radiusA, radiusB, out var normal, points); + + velocityConstraint.Normal = normal; + + int pointCount = velocityConstraint.PointCount; + + for (int j = 0; j < pointCount; ++j) + { + ref var vcp = ref velocityConstraint.Points[j]; + + vcp.RelativeVelocityA = points[j] - centerA; + vcp.RelativeVelocityB = points[j] - centerB; + + float rnA = Vector2.Cross(vcp.RelativeVelocityA, velocityConstraint.Normal); + float rnB = Vector2.Cross(vcp.RelativeVelocityB, velocityConstraint.Normal); + + float kNormal = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; + + vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + Vector2 tangent = Vector2.Cross(velocityConstraint.Normal, 1.0f); + + float rtA = Vector2.Cross(vcp.RelativeVelocityA, tangent); + float rtB = Vector2.Cross(vcp.RelativeVelocityB, tangent); + + float kTangent = invMassA + invMassB + invIA * rtA * rtA + invIB * rtB * rtB; + + vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // Setup a velocity bias for restitution. + vcp.VelocityBias = 0.0f; + float vRel = Vector2.Dot(velocityConstraint.Normal, linVelocityB + Vector2.Cross(angVelocityB, vcp.RelativeVelocityB) - linVelocityA - Vector2.Cross(angVelocityA, vcp.RelativeVelocityA)); + if (vRel < -data.VelocityThreshold) + { + vcp.VelocityBias = -velocityConstraint.Restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (velocityConstraint.PointCount == 2) + { + var vcp1 = velocityConstraint.Points[0]; + var vcp2 = velocityConstraint.Points[1]; + + var rn1A = Vector2.Cross(vcp1.RelativeVelocityA, velocityConstraint.Normal); + var rn1B = Vector2.Cross(vcp1.RelativeVelocityB, velocityConstraint.Normal); + var rn2A = Vector2.Cross(vcp2.RelativeVelocityA, velocityConstraint.Normal); + var rn2B = Vector2.Cross(vcp2.RelativeVelocityB, velocityConstraint.Normal); + + var k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; + var k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; + var k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float k_maxConditionNumber = 1000.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) + { + // K is safe to invert. + velocityConstraint.K = new System.Numerics.Vector4(k11, k12, k12, k22); + + velocityConstraint.NormalMass = Vector4Helpers.Inverse(velocityConstraint.K); + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + velocityConstraint.PointCount = 1; + } + } + } + } + + private void WarmStart( + in SolverData data, + in IslandData island, + ContactVelocityConstraint[] velocityConstraints, + Vector2[] linearVelocities, + float[] angularVelocities) + { + var offset = island.Offset; + + for (var i = 0; i < island.Contacts.Count; ++i) + { + var velocityConstraint = velocityConstraints[i]; + + var indexA = velocityConstraint.IndexA; + var indexB = velocityConstraint.IndexB; + var invMassA = velocityConstraint.InvMassA; + var invIA = velocityConstraint.InvIA; + var invMassB = velocityConstraint.InvMassB; + var invIB = velocityConstraint.InvIB; + var pointCount = velocityConstraint.PointCount; + + ref var linVelocityA = ref linearVelocities[offset + indexA]; + ref var angVelocityA = ref angularVelocities[offset + indexA]; + ref var linVelocityB = ref linearVelocities[offset + indexB]; + ref var angVelocityB = ref angularVelocities[offset + indexB]; + + var normal = velocityConstraint.Normal; + var tangent = Vector2.Cross(normal, 1.0f); + + for (var j = 0; j < pointCount; ++j) + { + var constraintPoint = velocityConstraint.Points[j]; + var P = normal * constraintPoint.NormalImpulse + tangent * constraintPoint.TangentImpulse; + angVelocityA -= invIA * Vector2.Cross(constraintPoint.RelativeVelocityA, P); + linVelocityA -= P * invMassA; + angVelocityB += invIB * Vector2.Cross(constraintPoint.RelativeVelocityB, P); + linVelocityB += P * invMassB; + } + } + } + + private void SolveVelocityConstraints(IslandData island, + ParallelOptions? options, + ContactVelocityConstraint[] velocityConstraints, + Vector2[] linearVelocities, + float[] angularVelocities) + { + var contactCount = island.Contacts.Count; + + if (options != null && contactCount > VelocityConstraintsPerThread * 2) + { + var batches = (int) Math.Ceiling((float) contactCount / VelocityConstraintsPerThread); + + Parallel.For(0, batches, i => + { + var start = i * VelocityConstraintsPerThread; + var end = Math.Min(start + VelocityConstraintsPerThread, contactCount); + SolveVelocityConstraints(island, start, end, velocityConstraints, linearVelocities, angularVelocities); + }); + } + else + { + SolveVelocityConstraints(island, 0, contactCount, velocityConstraints, linearVelocities, angularVelocities); + } + } + + private void SolveVelocityConstraints( + IslandData island, + int start, + int end, + ContactVelocityConstraint[] velocityConstraints, + Vector2[] linearVelocities, + float[] angularVelocities) + { + var offset = island.Offset; + + // Here be dragons + for (var i = start; i < end; ++i) + { + ref var velocityConstraint = ref velocityConstraints[i]; + + var indexA = velocityConstraint.IndexA; + var indexB = velocityConstraint.IndexB; + var mA = velocityConstraint.InvMassA; + var iA = velocityConstraint.InvIA; + var mB = velocityConstraint.InvMassB; + var iB = velocityConstraint.InvIB; + var pointCount = velocityConstraint.PointCount; + + ref var vA = ref linearVelocities[offset + indexA]; + ref var wA = ref angularVelocities[offset + indexA]; + ref var vB = ref linearVelocities[offset + indexB]; + ref var wB = ref angularVelocities[offset + indexB]; + + var normal = velocityConstraint.Normal; + var tangent = Vector2.Cross(normal, 1.0f); + var friction = velocityConstraint.Friction; + + DebugTools.Assert(pointCount is 1 or 2); + + // Solve tangent constraints first because non-penetration is more important + // than friction. + for (var j = 0; j < pointCount; ++j) + { + ref var velConstraintPoint = ref velocityConstraint.Points[j]; + + // Relative velocity at contact + var dv = vB + Vector2.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2.Cross(wA, velConstraintPoint.RelativeVelocityA); + + // Compute tangent force + float vt = Vector2.Dot(dv, tangent) - velocityConstraint.TangentSpeed; + float lambda = velConstraintPoint.TangentMass * (-vt); + + // b2Clamp the accumulated force + var maxFriction = friction * velConstraintPoint.NormalImpulse; + var newImpulse = Math.Clamp(velConstraintPoint.TangentImpulse + lambda, -maxFriction, maxFriction); + lambda = newImpulse - velConstraintPoint.TangentImpulse; + velConstraintPoint.TangentImpulse = newImpulse; + + // Apply contact impulse + Vector2 P = tangent * lambda; + + vA -= P * mA; + wA -= iA * Vector2.Cross(velConstraintPoint.RelativeVelocityA, P); + + vB += P * mB; + wB += iB * Vector2.Cross(velConstraintPoint.RelativeVelocityB, P); + } + + // Solve normal constraints + if (velocityConstraint.PointCount == 1) + { + ref var vcp = ref velocityConstraint.Points[0]; + + // Relative velocity at contact + Vector2 dv = vB + Vector2.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2.Cross(wA, vcp.RelativeVelocityA); + + // Compute normal impulse + float vn = Vector2.Dot(dv, normal); + float lambda = -vcp.NormalMass * (vn - vcp.VelocityBias); + + // b2Clamp the accumulated impulse + float newImpulse = Math.Max(vcp.NormalImpulse + lambda, 0.0f); + lambda = newImpulse - vcp.NormalImpulse; + vcp.NormalImpulse = newImpulse; + + // Apply contact impulse + Vector2 P = normal * lambda; + vA -= P * mA; + wA -= iA * Vector2.Cross(vcp.RelativeVelocityA, P); + + vB += P * mB; + wB += iB * Vector2.Cross(vcp.RelativeVelocityB, P); + } + else + { + // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). + // Build the mini LCP for this contact patch + // + // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 + // + // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) + // b = vn0 - velocityBias + // + // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i + // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases + // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid + // solution that satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires + // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). + // + // Substitute: + // + // x = a + d + // + // a := old total impulse + // x := new total impulse + // d := incremental impulse + // + // For the current iteration we extend the formula for the incremental impulse + // to compute the new total impulse: + // + // vn = A * d + b + // = A * (x - a) + b + // = A * x + b - A * a + // = A * x + b' + // b' = b - A * a; + + ref var cp1 = ref velocityConstraint.Points[0]; + ref var cp2 = ref velocityConstraint.Points[1]; + + Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse); + DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f); + + // Relative velocity at contact + Vector2 dv1 = vB + Vector2.Cross(wB, cp1.RelativeVelocityB) - vA - Vector2.Cross(wA, cp1.RelativeVelocityA); + Vector2 dv2 = vB + Vector2.Cross(wB, cp2.RelativeVelocityB) - vA - Vector2.Cross(wA, cp2.RelativeVelocityA); + + // Compute normal velocity + float vn1 = Vector2.Dot(dv1, normal); + float vn2 = Vector2.Dot(dv2, normal); + + Vector2 b = new Vector2 + { + X = vn1 - cp1.VelocityBias, + Y = vn2 - cp2.VelocityBias + }; + + // Compute b' + b -= Physics.Transform.Mul(velocityConstraint.K, a); + + //const float k_errorTol = 1e-3f; + //B2_NOT_USED(k_errorTol); + + for (; ; ) + { + // + // Case 1: vn = 0 + // + // 0 = A * x + b' + // + // Solve for x: + // + // x = - inv(A) * b' + // + Vector2 x = -Physics.Transform.Mul(velocityConstraint.NormalMass, b); + + if (x.X >= 0.0f && x.Y >= 0.0f) + { + // Get the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = normal * d.X; + Vector2 P2 = normal * d.Y; + vA -= (P1 + P2) * mA; + wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); + + vB += (P1 + P2) * mB; + wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); + + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; + + break; + } + + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 0 = a11 * x1 + a12 * 0 + b1' + // vn2 = a21 * x1 + a22 * 0 + b2' + // + x.X = -cp1.NormalMass * b.X; + x.Y = 0.0f; + vn1 = 0.0f; + vn2 = velocityConstraint.K.Y * x.X + b.Y; + + if (x.X >= 0.0f && vn2 >= 0.0f) + { + // Get the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = normal * d.X; + Vector2 P2 = normal * d.Y; + vA -= (P1 + P2) * mA; + wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); + + vB += (P1 + P2) * mB; + wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); + + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; + + break; + } + + + // + // Case 3: vn2 = 0 and x1 = 0 + // + // vn1 = a11 * 0 + a12 * x2 + b1' + // 0 = a21 * 0 + a22 * x2 + b2' + // + x.X = 0.0f; + x.Y = -cp2.NormalMass * b.Y; + vn1 = velocityConstraint.K.Z * x.Y + b.X; + vn2 = 0.0f; + + if (x.Y >= 0.0f && vn1 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = normal * d.X; + Vector2 P2 = normal * d.Y; + vA -= (P1 + P2) * mA; + wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); + + vB += (P1 + P2) * mB; + wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); + + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; + + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + x.X = 0.0f; + x.Y = 0.0f; + vn1 = b.X; + vn2 = b.Y; + + if (vn1 >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = normal * d.X; + Vector2 P2 = normal * d.Y; + vA -= (P1 + P2) * mA; + wA -= iA * (Vector2.Cross(cp1.RelativeVelocityA, P1) + Vector2.Cross(cp2.RelativeVelocityA, P2)); + + vB += (P1 + P2) * mB; + wB += iB * (Vector2.Cross(cp1.RelativeVelocityB, P1) + Vector2.Cross(cp2.RelativeVelocityB, P2)); + + // Accumulate + cp1.NormalImpulse = x.X; + cp2.NormalImpulse = x.Y; + + break; + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + break; + } + } + } + } + + private void StoreImpulses(in IslandData island, ContactVelocityConstraint[] velocityConstraints) + { + for (var i = 0; i < island.Contacts.Count; ++i) + { + ContactVelocityConstraint velocityConstraint = velocityConstraints[i]; + ref var manifold = ref island.Contacts[velocityConstraint.ContactIndex].Manifold; + + for (var j = 0; j < velocityConstraint.PointCount; ++j) + { + ref var point = ref manifold.Points[j]; + point.NormalImpulse = velocityConstraint.Points[j].NormalImpulse; + point.TangentImpulse = velocityConstraint.Points[j].TangentImpulse; + } + } + } + + private bool SolvePositionConstraints( + SolverData data, + in IslandData island, + ParallelOptions? options, + ContactPositionConstraint[] positionConstraints, + Vector2[] positions, + float[] angles) + { + var contactCount = island.Contacts.Count; + + // Parallel + if (options != null && contactCount > PositionConstraintsPerThread * 2) + { + var unsolved = 0; + var batches = (int) Math.Ceiling((float) contactCount / PositionConstraintsPerThread); + + Parallel.For(0, batches, i => + { + var start = i * PositionConstraintsPerThread; + var end = Math.Min(start + PositionConstraintsPerThread, contactCount); + + if (!SolvePositionConstraints(data, start, end, positionConstraints, positions, angles)) + Interlocked.Increment(ref unsolved); + }); + + return unsolved == 0; + } + + // No parallel + return SolvePositionConstraints(data, 0, contactCount, positionConstraints, positions, angles); + } + + /// + /// Tries to solve positions for all contacts specified. + /// + /// true if all positions solved + private bool SolvePositionConstraints( + SolverData data, + int start, + int end, + ContactPositionConstraint[] positionConstraints, + Vector2[] positions, + float[] angles) + { + float minSeparation = 0.0f; + + for (int i = start; i < end; ++i) + { + var pc = positionConstraints[i]; + + int indexA = pc.IndexA; + int indexB = pc.IndexB; + Vector2 localCenterA = pc.LocalCenterA; + float mA = pc.InvMassA; + float iA = pc.InvIA; + Vector2 localCenterB = pc.LocalCenterB; + float mB = pc.InvMassB; + float iB = pc.InvIB; + int pointCount = pc.PointCount; + + ref var centerA = ref positions[indexA]; + ref var angleA = ref angles[indexA]; + ref var centerB = ref positions[indexB]; + ref var angleB = ref angles[indexB]; + + // Solve normal constraints + for (int j = 0; j < pointCount; ++j) + { + Transform xfA = new Transform(angleA); + Transform xfB = new Transform(angleB); + xfA.Position = centerA - Physics.Transform.Mul(xfA.Quaternion2D, localCenterA); + xfB.Position = centerB - Physics.Transform.Mul(xfB.Quaternion2D, localCenterB); + + Vector2 normal; + Vector2 point; + float separation; + + PositionSolverManifoldInitialize(pc, j, xfA, xfB, out normal, out point, out separation); + + Vector2 rA = point - centerA; + Vector2 rB = point - centerB; + + // Track max constraint error. + minSeparation = Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = Math.Clamp(data.Baumgarte * (separation + PhysicsConstants.LinearSlop), -_maxLinearCorrection, 0.0f); + + // Compute the effective mass. + float rnA = Vector2.Cross(rA, normal); + float rnB = Vector2.Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float impulse = K > 0.0f ? -C / K : 0.0f; + + Vector2 P = normal * impulse; + + centerA -= P * mA; + angleA -= iA * Vector2.Cross(rA, P); + + centerB += P * mB; + angleB += iB * Vector2.Cross(rB, P); + } + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -3.0f * PhysicsConstants.LinearSlop; + } + + /// + /// Evaluate the manifold with supplied transforms. This assumes + /// modest motion from the original state. This does not change the + /// point count, impulses, etc. The radii must come from the Shapes + /// that generated the manifold. + /// + internal static void InitializeManifold( + ref Manifold manifold, + in Transform xfA, + in Transform xfB, + float radiusA, + float radiusB, + out Vector2 normal, + Span points) + { + normal = Vector2.Zero; + + if (manifold.PointCount == 0) + { + return; + } + + switch (manifold.Type) + { + case ManifoldType.Circles: + { + normal = new Vector2(1.0f, 0.0f); + Vector2 pointA = Physics.Transform.Mul(xfA, manifold.LocalPoint); + Vector2 pointB = Physics.Transform.Mul(xfB, manifold.Points[0].LocalPoint); + + if ((pointA - pointB).LengthSquared > float.Epsilon * float.Epsilon) + { + normal = pointB - pointA; + normal = normal.Normalized; + } + + Vector2 cA = pointA + normal * radiusA; + Vector2 cB = pointB - normal * radiusB; + points[0] = (cA + cB) * 0.5f; + } + break; + + case ManifoldType.FaceA: + { + normal = Physics.Transform.Mul(xfA.Quaternion2D, manifold.LocalNormal); + Vector2 planePoint = Physics.Transform.Mul(xfA, manifold.LocalPoint); + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 clipPoint = Physics.Transform.Mul(xfB, manifold.Points[i].LocalPoint); + Vector2 cA = clipPoint + normal * (radiusA - Vector2.Dot(clipPoint - planePoint, normal)); + Vector2 cB = clipPoint - normal * radiusB; + points[i] = (cA + cB) * 0.5f; + } + } + break; + + case ManifoldType.FaceB: + { + normal = Physics.Transform.Mul(xfB.Quaternion2D, manifold.LocalNormal); + Vector2 planePoint = Physics.Transform.Mul(xfB, manifold.LocalPoint); + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 clipPoint = Physics.Transform.Mul(xfA, manifold.Points[i].LocalPoint); + Vector2 cB = clipPoint + normal * (radiusB - Vector2.Dot(clipPoint - planePoint, normal)); + Vector2 cA = clipPoint - normal * radiusA; + points[i] = (cA + cB) * 0.5f; + } + + // Ensure normal points from A to B. + normal = -normal; + } + break; + default: + // Shouldn't happentm + throw new InvalidOperationException(); + + } + } + + private static void PositionSolverManifoldInitialize( + in ContactPositionConstraint pc, + int index, + in Transform xfA, + in Transform xfB, + out Vector2 normal, + out Vector2 point, + out float separation) + { + DebugTools.Assert(pc.PointCount > 0); + + switch (pc.Type) + { + case ManifoldType.Circles: + { + Vector2 pointA = Physics.Transform.Mul(xfA, pc.LocalPoint); + Vector2 pointB = Physics.Transform.Mul(xfB, pc.LocalPoints[0]); + normal = pointB - pointA; + + //FPE: Fix to handle zero normalization + if (normal != Vector2.Zero) + normal = normal.Normalized; + + point = (pointA + pointB) * 0.5f; + separation = Vector2.Dot(pointB - pointA, normal) - pc.RadiusA - pc.RadiusB; + } + break; + + case ManifoldType.FaceA: + { + normal = Physics.Transform.Mul(xfA.Quaternion2D, pc.LocalNormal); + Vector2 planePoint = Physics.Transform.Mul(xfA, pc.LocalPoint); + + Vector2 clipPoint = Physics.Transform.Mul(xfB, pc.LocalPoints[index]); + separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB; + point = clipPoint; + } + break; + + case ManifoldType.FaceB: + { + normal = Physics.Transform.Mul(xfB.Quaternion2D, pc.LocalNormal); + Vector2 planePoint = Physics.Transform.Mul(xfB, pc.LocalPoint); + + Vector2 clipPoint = Physics.Transform.Mul(xfA, pc.LocalPoints[index]); + separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB; + point = clipPoint; + + // Ensure normal points from A to B + normal = -normal; + } + break; + default: + normal = Vector2.Zero; + point = Vector2.Zero; + separation = 0; + break; + + } + } +} diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs index f52b1e4c9..e74d44d3d 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.cs @@ -1,5 +1,4 @@ using System; -using System.Collections.Generic; using Prometheus; using Robust.Shared.Configuration; using Robust.Shared.Containers; @@ -14,6 +13,7 @@ using Robust.Shared.Physics.Collision; using Robust.Shared.Physics.Components; using Robust.Shared.Physics.Dynamics; using Robust.Shared.Physics.Events; +using Robust.Shared.Threading; using Robust.Shared.Utility; using DependencyAttribute = Robust.Shared.IoC.DependencyAttribute; @@ -45,21 +45,24 @@ namespace Robust.Shared.Physics.Systems Buckets = Histogram.ExponentialBuckets(0.000_001, 1.5, 25) }); - [Dependency] private readonly SharedBroadphaseSystem _broadphase = default!; - [Dependency] private readonly EntityLookupSystem _lookup = default!; - [Dependency] private readonly SharedJointSystem _joints = default!; - [Dependency] private readonly SharedGridTraversalSystem _traversal = default!; - [Dependency] private readonly SharedDebugPhysicsSystem _debugPhysics = default!; - [Dependency] private readonly IManifoldManager _manifoldManager = default!; + [Dependency] private readonly IConfigurationManager _configManager = default!; + [Dependency] private readonly IManifoldManager _manifoldManager = default!; [Dependency] protected readonly IMapManager MapManager = default!; - [Dependency] private readonly IPhysicsManager _physicsManager = default!; - [Dependency] private readonly IIslandManager _islandManager = default!; - [Dependency] private readonly IConfigurationManager _cfg = default!; - [Dependency] private readonly IDependencyCollection _deps = default!; - [Dependency] private readonly IConfigurationManager _configurationManager = default!; + [Dependency] private readonly IParallelManager _parallel = default!; + [Dependency] private readonly IPhysicsManager _physicsManager = default!; + [Dependency] private readonly IConfigurationManager _cfg = default!; + [Dependency] private readonly IDependencyCollection _deps = default!; + [Dependency] private readonly SharedBroadphaseSystem _broadphase = default!; + [Dependency] private readonly EntityLookupSystem _lookup = default!; + [Dependency] private readonly SharedJointSystem _joints = default!; + [Dependency] private readonly SharedGridTraversalSystem _traversal = default!; + [Dependency] private readonly SharedTransformSystem _transform = default!; + [Dependency] private readonly SharedDebugPhysicsSystem _debugPhysics = default!; public Action? KinematicControllerCollision; + private int _substeps; + public bool MetricsEnabled { get; protected set; } private ISawmill _sawmill = default!; @@ -83,10 +86,11 @@ namespace Robust.Shared.Physics.Systems SubscribeLocalEvent(OnPhysicsRemove); SubscribeLocalEvent(OnPhysicsGetState); SubscribeLocalEvent(OnPhysicsHandleState); + InitializeIsland(); - _islandManager.Initialize(); - - _cfg.OnValueChanged(CVars.AutoClearForces, OnAutoClearChange); + _configManager.OnValueChanged(CVars.AutoClearForces, OnAutoClearChange); + _configManager.OnValueChanged(CVars.NetTickrate, UpdateSubsteps, true); + _configManager.OnValueChanged(CVars.TargetMinimumTickrate, UpdateSubsteps, true); } private void OnPhysicsRemove(EntityUid uid, PhysicsComponent component, ComponentRemove args) @@ -111,9 +115,8 @@ namespace Robust.Shared.Physics.Systems private void HandlePhysicsMapInit(EntityUid uid, SharedPhysicsMapComponent component, ComponentInit args) { _deps.InjectDependencies(component); - component.BroadphaseSystem = _broadphase; component.Physics = this; - component.ContactManager = new(_debugPhysics, _manifoldManager, EntityManager, _physicsManager, _cfg); + component.ContactManager = new(_debugPhysics, _manifoldManager, EntityManager, _physicsManager); component.ContactManager.Initialize(); component.ContactManager.MapId = component.MapId; component.AutoClearForces = _cfg.GetCVar(CVars.AutoClearForces); @@ -131,6 +134,13 @@ namespace Robust.Shared.Physics.Systems } } + private void UpdateSubsteps(int obj) + { + var targetMinTickrate = (float) _configManager.GetCVar(CVars.TargetMinimumTickrate); + var serverTickrate = (float) _configManager.GetCVar(CVars.NetTickrate); + _substeps = (int)Math.Ceiling(targetMinTickrate / serverTickrate); + } + private void HandlePhysicsMapRemove(EntityUid uid, SharedPhysicsMapComponent component, ComponentRemove args) { // THis entity might be getting deleted before ever having been initialized. @@ -254,7 +264,8 @@ namespace Robust.Shared.Physics.Systems { base.Shutdown(); - _cfg.UnsubValueChanged(CVars.AutoClearForces, OnAutoClearChange); + ShutdownIsland(); + _configManager.UnsubValueChanged(CVars.AutoClearForces, OnAutoClearChange); } private void OnWake(ref PhysicsWakeEvent @event) @@ -298,26 +309,18 @@ namespace Robust.Shared.Physics.Systems /// Should only predicted entities be considered in this simulation step? protected void SimulateWorld(float deltaTime, bool prediction) { - var targetMinTickrate = (float) _configurationManager.GetCVar(CVars.TargetMinimumTickrate); - var serverTickrate = (float) _configurationManager.GetCVar(CVars.NetTickrate); - var substeps = (int)Math.Ceiling(targetMinTickrate / serverTickrate); + var frameTime = deltaTime / _substeps; - var substepping = false; - var frameTime = deltaTime / substeps; - - for (int i = 0; i < substeps; i++) + for (int i = 0; i < _substeps; i++) { var updateBeforeSolve = new PhysicsUpdateBeforeSolveEvent(prediction, frameTime); RaiseLocalEvent(ref updateBeforeSolve); - if (substeps > 1) - substepping = true; - var enumerator = AllEntityQuery(); while (enumerator.MoveNext(out var comp)) { - comp.Step(frameTime, prediction, substepping); + Step(comp, frameTime, prediction); } var updateAfterSolve = new PhysicsUpdateAfterSolveEvent(prediction, frameTime); @@ -332,7 +335,8 @@ namespace Robust.Shared.Physics.Systems comp.ProcessQueue(); } - if (i == substeps - 1) + // On last substep (or main step where no substeps occured) we'll update all of the lerp data. + if (i == _substeps - 1) { enumerator = AllEntityQuery(); @@ -347,36 +351,9 @@ namespace Robust.Shared.Physics.Systems } } - private void FinalStep(SharedPhysicsMapComponent component) + protected virtual void FinalStep(SharedPhysicsMapComponent component) { - var xformQuery = GetEntityQuery(); - foreach (var (uid, (parentUid, position, rotation)) in component.LerpData) - { - if (!xformQuery.TryGetComponent(uid, out var xform) || - !parentUid.IsValid()) - { - continue; - } - - xform.PrevPosition = position; - xform.PrevRotation = rotation; - xform.LerpParent = parentUid; - xform.NextPosition = xform.LocalPosition; - xform.NextRotation = xform.LocalRotation; - } - - component.LerpData.Clear(); - } - - internal static (int Batches, int BatchSize) GetBatch(int count, int minimumBatchSize) - { - var batches = Math.Min( - (int) MathF.Ceiling((float) count / minimumBatchSize), - Math.Max(1, Environment.ProcessorCount)); - var batchSize = (int) MathF.Ceiling((float) count / batches); - - return (batches, batchSize); } } diff --git a/Robust.Shared/SharedIoC.cs b/Robust.Shared/SharedIoC.cs index 6ec235a69..391429926 100644 --- a/Robust.Shared/SharedIoC.cs +++ b/Robust.Shared/SharedIoC.cs @@ -48,7 +48,6 @@ namespace Robust.Shared deps.Register(); deps.Register(); deps.Register(); - deps.Register(); deps.Register(); deps.Register(); deps.Register(); diff --git a/Robust.UnitTesting/Server/RobustServerSimulation.cs b/Robust.UnitTesting/Server/RobustServerSimulation.cs index af443a0c8..c9ce0e124 100644 --- a/Robust.UnitTesting/Server/RobustServerSimulation.cs +++ b/Robust.UnitTesting/Server/RobustServerSimulation.cs @@ -218,7 +218,6 @@ namespace Robust.UnitTesting.Server container.Register(); container.Register(); container.Register(); - container.Register(); container.Register(); container.Register(); container.Register(); diff --git a/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs b/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs index 36f40fc4e..3bf79de95 100644 --- a/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs +++ b/Robust.UnitTesting/Shared/Physics/GridMovement_Test.cs @@ -63,13 +63,13 @@ public sealed class GridMovement_Test : RobustIntegrationTest // Alright just a quick validation then we start the actual damn test. var physicsMap = entManager.GetComponent(mapManager.GetMapEntityId(mapId)); - physicsMap.Step(0.001f, false); + physSystem.Step(physicsMap, 0.001f, false); 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); - physicsMap.Step(0.001f, false); + physSystem.Step(physicsMap, 0.001f, false); Assert.That(onGridBody.ContactCount, Is.EqualTo(1)); });