diff --git a/Robust.Shared/GameObjects/Systems/SharedMapSystem.Grid.cs b/Robust.Shared/GameObjects/Systems/SharedMapSystem.Grid.cs index 50fda58cd..317ad3cf5 100644 --- a/Robust.Shared/GameObjects/Systems/SharedMapSystem.Grid.cs +++ b/Robust.Shared/GameObjects/Systems/SharedMapSystem.Grid.cs @@ -7,6 +7,7 @@ using Robust.Shared.Physics.Components; using Robust.Shared.Physics.Systems; using Robust.Shared.Map.Components; using Robust.Shared.Maths; +using Robust.Shared.Physics; using Robust.Shared.Timing; using Robust.Shared.Utility; @@ -119,6 +120,86 @@ public abstract partial class SharedMapSystem SubscribeLocalEvent(OnGridInit); SubscribeLocalEvent(OnGridStartup); SubscribeLocalEvent(OnGridRemove); + SubscribeLocalEvent(OnGridMove); + } + + public void OnGridBoundsChange(EntityUid uid, MapGridComponent component) + { + // Just MapLoader things. + if (component.MapProxy == DynamicTree.Proxy.Free) return; + + var xform = EntityManager.GetComponent(uid); + var aabb = GetWorldAABB(uid, component, xform); + + if (TryComp(xform.MapUid, out var gridTree)) + { + gridTree.Tree.MoveProxy(component.MapProxy, in aabb, Vector2.Zero); + } + + if (TryComp(xform.MapUid, out var movedGrids)) + { + movedGrids.MovedGrids.Add(uid); + } + } + + private void OnGridMove(EntityUid uid, MapGridComponent component, ref MoveEvent args) + { + if (args.ParentChanged) + { + OnParentChange(uid, component, ref args); + return; + } + + // Just maploader / test things + if (component.MapProxy == DynamicTree.Proxy.Free) return; + + var xform = args.Component; + var aabb = GetWorldAABB(uid, component, xform); + + if (TryComp(xform.MapUid, out var gridTree)) + { + gridTree.Tree.MoveProxy(component.MapProxy, in aabb, Vector2.Zero); + } + + if (TryComp(xform.MapUid, out var movedGrids)) + { + movedGrids.MovedGrids.Add(uid); + } + } + + private void OnParentChange(EntityUid uid, MapGridComponent component, ref MoveEvent args) + { + if (EntityManager.HasComponent(uid)) + return; + + var lifestage = EntityManager.GetComponent(uid).EntityLifeStage; + + // oh boy + // Want gridinit to handle this hence specialcase those situations. + // oh boy oh boy, its even worse now. + // transform now raises parent change events on startup, because container code is a POS. + if (lifestage < EntityLifeStage.Initialized || args.Component.LifeStage == ComponentLifeStage.Starting) + return; + + // Make sure we cleanup old map for moved grid stuff. + var mapId = args.Component.MapID; + var oldMap = args.OldPosition.ToMap(EntityManager, _transform); + + // y'all need jesus + if (oldMap.MapId == mapId) return; + + if (component.MapProxy != DynamicTree.Proxy.Free && TryComp(MapManager.GetMapEntityId(oldMap.MapId), out var oldMovedGrids)) + { + oldMovedGrids.MovedGrids.Remove(uid); + RemoveGrid(uid, component, MapManager.GetMapEntityId(oldMap.MapId)); + } + + DebugTools.Assert(component.MapProxy == DynamicTree.Proxy.Free); + if (TryComp(MapManager.GetMapEntityId(mapId), out var newMovedGrids)) + { + newMovedGrids.MovedGrids.Add(uid); + AddGrid(uid, component, mapId); + } } private void OnGridHandleState(EntityUid uid, MapGridComponent component, ref ComponentHandleState args) @@ -312,6 +393,23 @@ public abstract partial class SharedMapSystem if (xform.MapUid != null && xform.MapUid != uid) _transform.SetParent(uid, xform, xform.MapUid.Value, xformQuery); + if (!HasComp(uid)) + { + var aabb = GetWorldAABB(uid, component); + + if (TryComp(xform.MapUid, out var gridTree)) + { + var proxy = gridTree.Tree.CreateProxy(in aabb, (uid, component)); + DebugTools.Assert(component.MapProxy == DynamicTree.Proxy.Free); + component.MapProxy = proxy; + } + + if (TryComp(xform.MapUid, out var movedGrids)) + { + movedGrids.MovedGrids.Add(uid); + } + } + var msg = new GridInitializeEvent(uid); RaiseLocalEvent(uid, msg, true); } @@ -324,6 +422,12 @@ public abstract partial class SharedMapSystem private void OnGridRemove(EntityUid uid, MapGridComponent component, ComponentShutdown args) { + if (TryComp(uid, out var xform) && xform.MapUid != null) + { + RemoveGrid(uid, component, xform.MapUid.Value); + } + + component.MapProxy = DynamicTree.Proxy.Free; RaiseLocalEvent(uid, new GridRemovalEvent(uid), true); if (uid == EntityUid.Invalid) @@ -334,4 +438,51 @@ public abstract partial class SharedMapSystem MapManager.DeleteGrid(uid); } + + private Box2 GetWorldAABB(EntityUid uid, MapGridComponent grid, TransformComponent? xform = null) + { + if (!Resolve(uid, ref xform)) + return new Box2(); + + var (worldPos, worldRot) = _transform.GetWorldPositionRotation(xform, GetEntityQuery()); + var aabb = grid.LocalAABB.Translated(worldPos); + + return new Box2Rotated(aabb, worldRot, worldPos).CalcBoundingBox(); + } + + private void AddGrid(EntityUid uid, MapGridComponent grid, MapId mapId) + { + DebugTools.Assert(!EntityManager.HasComponent(uid)); + var aabb = GetWorldAABB(uid, grid); + + if (!TryComp(uid, out var xform)) + return; + + if (TryComp(xform.MapUid, out var gridTree)) + { + var proxy = gridTree.Tree.CreateProxy(in aabb, (uid, grid)); + DebugTools.Assert(grid.MapProxy == DynamicTree.Proxy.Free); + grid.MapProxy = proxy; + } + + if (TryComp(xform.MapUid, out var movedGrids)) + { + movedGrids.MovedGrids.Add(uid); + } + } + + private void RemoveGrid(EntityUid uid, MapGridComponent grid, EntityUid mapUid) + { + if (grid.MapProxy != DynamicTree.Proxy.Free && TryComp(mapUid, out var gridTree)) + { + gridTree.Tree.DestroyProxy(grid.MapProxy); + } + + grid.MapProxy = DynamicTree.Proxy.Free; + + if (TryComp(mapUid, out var movedGrids)) + { + movedGrids.MovedGrids.Remove(uid); + } + } } diff --git a/Robust.Shared/GameObjects/Systems/SharedMapSystem.Map.cs b/Robust.Shared/GameObjects/Systems/SharedMapSystem.Map.cs index 931cf5aa0..6fd034bff 100644 --- a/Robust.Shared/GameObjects/Systems/SharedMapSystem.Map.cs +++ b/Robust.Shared/GameObjects/Systems/SharedMapSystem.Map.cs @@ -47,6 +47,9 @@ public abstract partial class SharedMapSystem private void OnMapInit(EntityUid uid, MapComponent component, ComponentInit args) { + EnsureComp(uid); + EnsureComp(uid); + var msg = new MapChangedEvent(uid, component.MapId, true); RaiseLocalEvent(uid, msg, true); } diff --git a/Robust.Shared/Map/Components/GridTreeComponent.cs b/Robust.Shared/Map/Components/GridTreeComponent.cs new file mode 100644 index 000000000..ebaef6f6c --- /dev/null +++ b/Robust.Shared/Map/Components/GridTreeComponent.cs @@ -0,0 +1,13 @@ +using Robust.Shared.GameObjects; +using Robust.Shared.GameStates; +using Robust.Shared.Physics; +using Robust.Shared.ViewVariables; + +namespace Robust.Shared.Map.Components; + +[RegisterComponent, NetworkedComponent] +public sealed class GridTreeComponent : Component +{ + [ViewVariables] + public readonly B2DynamicTree<(EntityUid Uid, MapGridComponent Grid)> Tree = new(); +} diff --git a/Robust.Shared/Map/Components/MapGridComponent.cs b/Robust.Shared/Map/Components/MapGridComponent.cs index ef810be34..fd46289ad 100644 --- a/Robust.Shared/Map/Components/MapGridComponent.cs +++ b/Robust.Shared/Map/Components/MapGridComponent.cs @@ -155,7 +155,7 @@ namespace Robust.Shared.Map.Components // TODO: Move this to the component when we combine. _entMan.EntitySysManager.GetEntitySystem().WakeBody(Owner); - _mapManager.OnGridBoundsChange(Owner, this); + _entMan.EntitySysManager.GetEntitySystem().OnGridBoundsChange(Owner, this); system?.RegenerateCollision(Owner, chunkRectangles, removedChunks); } diff --git a/Robust.Shared/Map/Components/MovedGridsComponent.cs b/Robust.Shared/Map/Components/MovedGridsComponent.cs new file mode 100644 index 000000000..f517527ed --- /dev/null +++ b/Robust.Shared/Map/Components/MovedGridsComponent.cs @@ -0,0 +1,16 @@ +using System.Collections.Generic; +using Robust.Shared.GameObjects; +using Robust.Shared.GameStates; +using Robust.Shared.ViewVariables; + +namespace Robust.Shared.Map.Components; + +/// +/// Stores what grids moved in a tick. +/// +[RegisterComponent, NetworkedComponent] +public sealed class MovedGridsComponent : Component +{ + [ViewVariables] + public HashSet MovedGrids = new(); +} diff --git a/Robust.Shared/Map/IMapManager.cs b/Robust.Shared/Map/IMapManager.cs index 1036190be..17a124be6 100644 --- a/Robust.Shared/Map/IMapManager.cs +++ b/Robust.Shared/Map/IMapManager.cs @@ -26,16 +26,6 @@ namespace Robust.Shared.Map /// bool SuppressOnTileChanged { get; set; } - /// - /// Get the set of grids that have moved on this map in this tick. - /// - HashSet GetMovedGrids(MapId mapId); - - /// - /// Clear the set of grids that have moved on this map in this tick. - /// - void ClearMovedGrids(MapId mapId); - /// /// Starts up the map system. /// diff --git a/Robust.Shared/Map/IMapManagerInternal.cs b/Robust.Shared/Map/IMapManagerInternal.cs index 728cc4fd2..86cb9242b 100644 --- a/Robust.Shared/Map/IMapManagerInternal.cs +++ b/Robust.Shared/Map/IMapManagerInternal.cs @@ -1,5 +1,4 @@ using Robust.Shared.GameObjects; -using Robust.Shared.Map.Components; using Robust.Shared.Timing; namespace Robust.Shared.Map @@ -19,6 +18,5 @@ namespace Robust.Shared.Map MapId CreateMap(MapId? mapId, EntityUid euid); void RemoveMapId(MapId mapId); - void OnGridBoundsChange(EntityUid uid, MapGridComponent grid); } } diff --git a/Robust.Shared/Map/MapManager.GridTrees.cs b/Robust.Shared/Map/MapManager.GridTrees.cs index 57f982c9e..249e46798 100644 --- a/Robust.Shared/Map/MapManager.GridTrees.cs +++ b/Robust.Shared/Map/MapManager.GridTrees.cs @@ -1,196 +1,12 @@ -using System.Collections.Generic; -using Robust.Shared.GameObjects; -using Robust.Shared.Log; -using Robust.Shared.Map.Components; -using Robust.Shared.Maths; -using Robust.Shared.Physics; -using Robust.Shared.Utility; - namespace Robust.Shared.Map; internal partial class MapManager { - // TODO: Move IMapManager stuff to the system - private Dictionary> _gridTrees = new(); - - private Dictionary> _movedGrids = new(); - - /// - /// Gets the grids that have moved this tick until broadphase has run. - /// - /// - /// - public HashSet GetMovedGrids(MapId mapId) - { - // Temporary until this is moved to SharedPhysicsMapComponent - if (!_movedGrids.TryGetValue(mapId, out var moved)) - { - Logger.ErrorS("map", $"Unable to get moved grids for {mapId}"); - moved = new HashSet(); - DebugTools.Assert(false); - } - - return moved; - } - - public void ClearMovedGrids(MapId mapId) - { - if (!_movedGrids.TryGetValue(mapId, out var moved)) - { - Logger.ErrorS("map", $"Unable to clear moved grids for {mapId}"); - return; - } - - moved.Clear(); - } - - private void StartupGridTrees() - { - // Needs to be done on mapmanager startup because the eventbus will clear on shutdown - // (and mapmanager initialize doesn't run upon connecting to a server every time). - EntityManager.EventBus.SubscribeEvent(EventSource.Local, this, OnGridInit); - EntityManager.EventBus.SubscribeEvent(EventSource.Local, this, OnGridRemove); - EntityManager.EventBus.SubscribeLocalEvent(OnGridMove); - EntityManager.EventBus.SubscribeLocalEvent(OnGridParentChange); - } - - private void ShutdownGridTrees() - { - EntityManager.EventBus.UnsubscribeEvent(EventSource.Local, this); - EntityManager.EventBus.UnsubscribeEvent(EventSource.Local, this); - EntityManager.EventBus.UnsubscribeLocalEvent(); - EntityManager.EventBus.UnsubscribeLocalEvent(); - - DebugTools.Assert(_gridTrees.Count == 0); - DebugTools.Assert(_movedGrids.Count == 0); - } - - private void OnMapCreatedGridTree(MapEventArgs e) - { - if (e.Map == MapId.Nullspace) return; - - _gridTrees.Add(e.Map, new B2DynamicTree<(EntityUid Uid, MapGridComponent Grid)>()); - _movedGrids.Add(e.Map, new HashSet()); - } - public void RemoveMapId(MapId mapId) { if (mapId == MapId.Nullspace) return; _mapEntities.Remove(mapId); - _gridTrees.Remove(mapId); - _movedGrids.Remove(mapId); - } - - private Box2 GetWorldAABB(EntityUid uid, MapGridComponent grid, TransformComponent? xform = null) - { - xform ??= EntityManager.GetComponent(uid); - - var (worldPos, worldRot) = xform.GetWorldPositionRotation(); - - return new Box2Rotated(grid.LocalAABB, worldRot).CalcBoundingBox().Translated(worldPos); - } - - private void OnGridInit(GridInitializeEvent args) - { - if (EntityManager.HasComponent(args.EntityUid)) - return; - - var xform = EntityManager.GetComponent(args.EntityUid); - var mapId = xform.MapID; - - if (mapId == MapId.Nullspace) return; - - var grid = EntityManager.GetComponent(args.EntityUid); - AddGrid(args.EntityUid, grid, mapId); - } - - private void AddGrid(EntityUid uid, MapGridComponent grid, MapId mapId) - { - DebugTools.Assert(!EntityManager.HasComponent(uid)); - var aabb = GetWorldAABB(uid, grid); - var proxy = _gridTrees[mapId].CreateProxy(in aabb, (uid, grid)); - DebugTools.Assert(grid.MapProxy == DynamicTree.Proxy.Free); - grid.MapProxy = proxy; - - _movedGrids[mapId].Add(uid); - } - - private void OnGridRemove(GridRemovalEvent args) - { - if (EntityManager.HasComponent(args.EntityUid)) - return; - - var xform = EntityManager.GetComponent(args.EntityUid); - - // Can't check for free proxy because DetachParentToNull gets called first woo! - if (xform.MapID == MapId.Nullspace) return; - - var grid = EntityManager.GetComponent(args.EntityUid); - RemoveGrid(args.EntityUid, grid, xform.MapID); - } - - private void RemoveGrid(EntityUid uid, MapGridComponent grid, MapId mapId) - { - _gridTrees[mapId].DestroyProxy(grid.MapProxy); - _movedGrids[mapId].Remove(uid); - grid.MapProxy = DynamicTree.Proxy.Free; - } - - private void OnGridMove(EntityUid uid, MapGridComponent component, ref MoveEvent args) - { - // Just maploader / test things - if (component.MapProxy == DynamicTree.Proxy.Free) return; - - var xform = args.Component; - var aabb = GetWorldAABB(uid, component, xform); - _gridTrees[xform.MapID].MoveProxy(component.MapProxy, in aabb, Vector2.Zero); - _movedGrids[xform.MapID].Add(uid); - } - - private void OnGridParentChange(EntityUid uid, MapGridComponent component, ref EntParentChangedMessage args) - { - if (EntityManager.HasComponent(uid)) - return; - - var lifestage = EntityManager.GetComponent(uid).EntityLifeStage; - - // oh boy - // Want gridinit to handle this hence specialcase those situations. - // oh boy oh boy, its even worse now. - // transform now raises parent change events on startup, because container code is a POS. - if (lifestage < EntityLifeStage.Initialized || args.Transform.LifeStage == ComponentLifeStage.Starting) - return; - - // Make sure we cleanup old map for moved grid stuff. - var mapId = args.Transform.MapID; - - // y'all need jesus - if (args.OldMapId == mapId) return; - - if (component.MapProxy != DynamicTree.Proxy.Free && _movedGrids.TryGetValue(args.OldMapId, out var oldMovedGrids)) - { - oldMovedGrids.Remove(uid); - RemoveGrid(uid, component, args.OldMapId); - } - - DebugTools.Assert(component.MapProxy == DynamicTree.Proxy.Free); - if (_movedGrids.TryGetValue(mapId, out var newMovedGrids)) - { - newMovedGrids.Add(uid); - AddGrid(uid, component, mapId); - } - } - - public void OnGridBoundsChange(EntityUid uid, MapGridComponent grid) - { - // Just MapLoader things. - if (grid.MapProxy == DynamicTree.Proxy.Free) return; - - var xform = EntityManager.GetComponent(uid); - var aabb = GetWorldAABB(uid, grid, xform); - _gridTrees[xform.MapID].MoveProxy(grid.MapProxy, in aabb, Vector2.Zero); - _movedGrids[xform.MapID].Add(uid); } } diff --git a/Robust.Shared/Map/MapManager.MapCollection.cs b/Robust.Shared/Map/MapManager.MapCollection.cs index e96ece81b..49754e359 100644 --- a/Robust.Shared/Map/MapManager.MapCollection.cs +++ b/Robust.Shared/Map/MapManager.MapCollection.cs @@ -144,8 +144,6 @@ internal partial class MapManager // Yeah this sucks but I just want to save maps for now, deal. if (raiseEvent) { - var args = new MapEventArgs(mapId); - OnMapCreatedGridTree(args); var ev = new MapChangedEvent(newMapEntity, mapId, true); EntityManager.EventBus.RaiseLocalEvent(newMapEntity, ev, true); } @@ -246,7 +244,6 @@ internal partial class MapManager } var args = new MapEventArgs(actualId); - OnMapCreatedGridTree(args); return actualId; } } diff --git a/Robust.Shared/Map/MapManager.Queries.cs b/Robust.Shared/Map/MapManager.Queries.cs index 51d0877cb..5ff4e9289 100644 --- a/Robust.Shared/Map/MapManager.Queries.cs +++ b/Robust.Shared/Map/MapManager.Queries.cs @@ -22,21 +22,23 @@ internal partial class MapManager public void FindGridsIntersecting(MapId mapId, Box2 worldAABB, GridCallback callback, bool approx = false) { - if (!_gridTrees.TryGetValue(mapId, out var gridTree)) + if (!_mapEntities.TryGetValue(mapId, out var mapEnt) || + !EntityManager.TryGetComponent(mapEnt, out var gridTree)) + { return; + } var physicsQuery = EntityManager.GetEntityQuery(); var xformQuery = EntityManager.GetEntityQuery(); var xformSystem = EntityManager.System(); - var state = (worldAABB, gridTree, callback, approx, physicsQuery, xformQuery, xformSystem); + var state = (worldAABB, gridTree.Tree, callback, approx, physicsQuery, xformQuery, xformSystem); - gridTree.Query(ref state, + gridTree.Tree.Query(ref state, static (ref (Box2 worldAABB, B2DynamicTree<(EntityUid Uid, MapGridComponent Grid)> gridTree, GridCallback callback, bool approx, - EntityQuery physicsQuery, - EntityQuery xformQuery, + EntityQuery physicsQuery, EntityQuery xformQuery, SharedTransformSystem xformSystem) tuple, DynamicTree.Proxy proxy) => { @@ -61,15 +63,17 @@ internal partial class MapManager public void FindGridsIntersecting(MapId mapId, Box2 worldAABB, ref TState state, GridCallback callback, bool approx = false) { - if (!_gridTrees.TryGetValue(mapId, out var gridTree)) + if (!_mapEntities.TryGetValue(mapId, out var mapEnt) || + !EntityManager.TryGetComponent(mapEnt, out var gridTree)) + { return; - + } var physicsQuery = EntityManager.GetEntityQuery(); var xformQuery = EntityManager.GetEntityQuery(); var xformSystem = EntityManager.System(); - var state2 = (state, worldAABB, gridTree, callback, approx, physicsQuery, xformQuery, xformSystem); + var state2 = (state, worldAABB, gridTree.Tree, callback, approx, physicsQuery, xformQuery, xformSystem); - gridTree.Query(ref state2, static (ref ( + gridTree.Tree.Query(ref state2, static (ref ( TState state, Box2 worldAABB, B2DynamicTree<(EntityUid Uid, MapGridComponent Grid)> gridTree, @@ -141,8 +145,6 @@ internal partial class MapManager [Obsolete("Use the FindGridsIntersecting callback")] public IEnumerable FindGridsIntersecting(MapId mapId, Box2 worldAabb, bool approx = false) { - if (!_gridTrees.ContainsKey(mapId)) return Enumerable.Empty(); - var grids = new List(); var state = grids; diff --git a/Robust.Shared/Map/MapManager.cs b/Robust.Shared/Map/MapManager.cs index 59764fd0f..c7f55fb02 100644 --- a/Robust.Shared/Map/MapManager.cs +++ b/Robust.Shared/Map/MapManager.cs @@ -42,8 +42,6 @@ internal partial class MapManager : IMapManagerInternal, IEntityEventSubscriber Logger.DebugS("map", "Starting..."); - StartupGridTrees(); - DebugTools.Assert(!GridExists(EntityUid.Invalid)); } @@ -59,7 +57,6 @@ internal partial class MapManager : IMapManagerInternal, IEntityEventSubscriber { EntityManager.DeleteEntity(mapComp.Owner); } - ShutdownGridTrees(); #if DEBUG DebugTools.Assert(!GridExists(EntityUid.Invalid)); diff --git a/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs b/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs index a4e8c0c6f..3ee300216 100644 --- a/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs +++ b/Robust.Shared/Physics/Dynamics/Joints/WeldJoint.cs @@ -6,397 +6,396 @@ using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; -namespace Robust.Shared.Physics.Dynamics.Joints +namespace Robust.Shared.Physics.Dynamics.Joints; + +[Serializable, NetSerializable] +internal sealed class WeldJointState : JointState { - [Serializable, NetSerializable] - internal sealed class WeldJointState : JointState + public float Stiffness { get; internal set; } + public float Damping { get; internal set; } + public float Bias { get; internal set; } + + public override Joint GetJoint() { - public float Stiffness { get; internal set; } - public float Damping { get; internal set; } - public float Bias { get; internal set; } - - public override Joint GetJoint() - { - return new WeldJoint(this); - } - } - - public sealed class WeldJoint : Joint, IEquatable - { - // Shared - private float _gamma; - private Vector3 _impulse; - - // Temporary - private int _indexA; - private int _indexB; - private Vector2 _rA; - private Vector2 _rB; - private Vector2 _localCenterA; - private Vector2 _localCenterB; - private float _invMassA; - private float _invMassB; - private float _invIA; - private float _invIB; - private Matrix33 _mass; - - // Settable - - [DataField("stiffness")] - public float Stiffness; - - [DataField("damping")] - public float Damping; - - [DataField("bias")] - public float Bias; - - /// - /// The bodyB angle minus bodyA angle in the reference state (radians). - /// - [DataField("referenceAngle")] - public float ReferenceAngle; - - /// - /// Used for Serv3 reasons - /// - public WeldJoint() {} - - internal WeldJoint(EntityUid bodyA, EntityUid bodyB, Vector2 anchorA, Vector2 anchorB, float referenceAngle) : base(bodyA, bodyB) - { - LocalAnchorA = anchorA; - LocalAnchorB = anchorB; - ReferenceAngle = referenceAngle; - } - - internal WeldJoint(EntityUid bodyAUid, EntityUid bodyBUid) : base(bodyAUid, bodyBUid) {} - - internal WeldJoint(WeldJointState state) : base(state) - { - Stiffness = state.Stiffness; - Damping = state.Damping; - Bias = state.Bias; - } - - public override JointType JointType => JointType.Weld; - public override JointState GetState() - { - var weldJointState = new WeldJointState(); - - base.GetState(weldJointState); - return weldJointState; - } - - public override Vector2 GetReactionForce(float invDt) - { - var P = new Vector2(_impulse.X, _impulse.Y); - return P * invDt; - } - - public override float GetReactionTorque(float invDt) - { - return invDt * _impulse.Z; - } - - internal override void InitVelocityConstraints( - in SolverData data, - in SharedPhysicsSystem.IslandData island, - PhysicsComponent bodyA, - PhysicsComponent bodyB, - Vector2[] positions, - float[] angles, - Vector2[] linearVelocities, - float[] angularVelocities) - { - var offset = island.Offset; - _indexA = bodyA.IslandIndex[island.Index]; - _indexB = bodyB.IslandIndex[island.Index]; - _localCenterA = bodyA.LocalCenter; - _localCenterB = bodyB.LocalCenter; - _invMassA = bodyA.InvMass; - _invMassB = bodyB.InvMass; - _invIA = bodyA.InvI; - _invIB = bodyB.InvI; - - float aA = angles[_indexA]; - var vA = linearVelocities[offset + _indexA]; - float wA = angularVelocities[offset + _indexA]; - - float aB = angles[_indexB]; - var vB = linearVelocities[offset + _indexB]; - float wB = angularVelocities[offset + _indexB]; - - Quaternion2D qA = new(aA), qB = new(aB); - - _rA = Transform.Mul(qA, LocalAnchorA - _localCenterA); - _rB = Transform.Mul(qB, LocalAnchorB - _localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float mA = _invMassA, mB = _invMassB; - float iA = _invIA, iB = _invIB; - - Matrix33 K; - K.EX.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB; - K.EY.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB; - K.EZ.X = -_rA.Y * iA - _rB.Y * iB; - K.EX.Y = K.EY.X; - K.EY.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB; - K.EZ.Y = _rA.X * iA + _rB.X * iB; - K.EX.Z = K.EZ.X; - K.EY.Z = K.EZ.Y; - K.EZ.Z = iA + iB; - - if (Stiffness > 0.0f) - { - K.GetInverse22(ref _mass); - - float invM = iA + iB; - - float C = aB - aA - ReferenceAngle; - - // Damping coefficient - float d = Damping; - - // Spring stiffness - float k = Stiffness; - - // magic formulas - float h = data.FrameTime; - _gamma = h * (d + h * k); - _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; - Bias = C * h * k * _gamma; - - invM += _gamma; - _mass.EZ.Z = invM != 0.0f ? 1.0f / invM : 0.0f; - } - else if (K.EZ.Z == 0.0f) - { - K.GetInverse22(ref _mass); - _gamma = 0.0f; - Bias = 0.0f; - } - else - { - K.GetSymInverse33(ref _mass); - _gamma = 0.0f; - Bias = 0.0f; - } - - if (data.WarmStarting) - { - // Scale impulses to support a variable time step. - _impulse *= data.DtRatio; - - var P = new Vector2(_impulse.X, _impulse.Y); - - vA -= P * mA; - wA -= iA * (Vector2.Cross(_rA, P) + _impulse.Z); - - vB += P * mB; - wB += iB * (Vector2.Cross(_rB, P) + _impulse.Z); - } - else - { - _impulse = Vector3.Zero; - } - - linearVelocities[offset + _indexA] = vA; - angularVelocities[offset + _indexA] = wA; - linearVelocities[offset + _indexB] = vB; - angularVelocities[offset + _indexB] = wB; - } - - internal override void SolveVelocityConstraints( - in SolverData data, - in SharedPhysicsSystem.IslandData island, - Vector2[] linearVelocities, - float[] angularVelocities) - { - 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; - - if (Stiffness > 0.0f) - { - float Cdot2 = wB - wA; - - float impulse2 = -_mass.EZ.Z * (Cdot2 + Bias + _gamma * _impulse.Z); - _impulse.Z += impulse2; - - wA -= iA * impulse2; - wB += iB * impulse2; - - var Cdot1 = vB + Vector2.Cross(wB, _rB) - vA - Vector2.Cross(wA, _rA); - - var impulse1 = -_mass.Mul22(Cdot1); - _impulse.X += impulse1.X; - _impulse.Y += impulse1.Y; - - var P = impulse1; - - vA -= P * mA; - wA -= iA * Vector2.Cross(_rA, P); - - vB += P * mB; - wB += iB * Vector2.Cross(_rB, P); - } - else - { - var Cdot1 = vB + Vector2.Cross(wB, _rB) - vA - Vector2.Cross(wA, _rA); - float Cdot2 = wB - wA; - var Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); - - var impulse = -_mass.Mul(Cdot); - _impulse += impulse; - - var P = new Vector2(impulse.X, impulse.Y); - - vA -= P * mA; - wA -= iA * (Vector2.Cross(_rA, P) + impulse.Z); - - vB += P * mB; - wB += iB * (Vector2.Cross(_rB, P) + impulse.Z); - } - - linearVelocities[offset + _indexA] = vA; - angularVelocities[offset + _indexA] = wA; - linearVelocities[offset + _indexB] = vB; - angularVelocities[offset + _indexB] = wB; - } - - internal override bool SolvePositionConstraints( - in SolverData data, - Vector2[] positions, - float[] angles) - { - var cA = positions[_indexA]; - float aA = angles[_indexA]; - var cB = positions[_indexB]; - float aB = angles[_indexB]; - - Quaternion2D qA = new(aA), qB = new(aB); - - float mA = _invMassA, mB = _invMassB; - float iA = _invIA, iB = _invIB; - - var rA = Transform.Mul(qA, LocalAnchorA - _localCenterA); - var rB = Transform.Mul(qB, LocalAnchorB - _localCenterB); - - float positionError, angularError; - - Matrix33 K; - K.EX.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; - K.EY.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; - K.EZ.X = -rA.Y * iA - rB.Y * iB; - K.EX.Y = K.EY.X; - K.EY.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; - K.EZ.Y = rA.X * iA + rB.X * iB; - K.EX.Z = K.EZ.X; - K.EY.Z = K.EZ.Y; - K.EZ.Z = iA + iB; - - if (Stiffness > 0.0f) - { - var C1 = cB + rB - cA - rA; - - positionError = C1.Length; - angularError = 0.0f; - - var P = -K.Solve22(C1); - - cA -= P * mA; - aA -= iA * Vector2.Cross(rA, P); - - cB += P * mB; - aB += iB * Vector2.Cross(rB, P); - } - else - { - var C1 = cB + rB - cA - rA; - float C2 = aB - aA - ReferenceAngle; - - positionError = C1.Length; - angularError = Math.Abs(C2); - - Vector3 C = new(C1.X, C1.Y, C2); - - Vector3 impulse; - if (K.EZ.Z > 0.0f) - { - impulse = -K.Solve33(C); - } - else - { - var impulse2 = -K.Solve22(C1); - impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); - } - - var P = new Vector2(impulse.X, impulse.Y); - - cA -= P * mA; - aA -= iA * (Vector2.Cross(rA, P) + impulse.Z); - - cB += P * mB; - aB += iB * (Vector2.Cross(rB, P) + impulse.Z); - } - - positions[_indexA] = cA; - angles[_indexA]= aA; - positions[_indexB] = cB; - angles[_indexB] = aB; - - return positionError <= PhysicsConstants.LinearSlop && angularError <= PhysicsConstants.AngularSlop; - } - - public override Joint Clone(EntityUid uidA, EntityUid uidB) - { - var weld = new WeldJoint(uidA, uidB, LocalAnchorA, LocalAnchorB, ReferenceAngle) - { - Enabled = Enabled, - Bias = Bias, - Damping = Damping, - Stiffness = Stiffness, - _impulse = _impulse, - Breakpoint = Breakpoint - }; - return weld; - } - - public override void CopyTo(Joint original) - { - if (original is not WeldJoint weld) - return; - - weld.Enabled = Enabled; - weld.Bias = Bias; - weld.Damping = Damping; - weld.Stiffness = Stiffness; - weld._impulse = _impulse; - weld.Breakpoint = Breakpoint; - } - - public bool Equals(WeldJoint? other) - { - if (ReferenceEquals(null, other)) return false; - if (ReferenceEquals(this, other)) return true; - if (!base.Equals(other)) return false; - - return Stiffness.Equals(other.Stiffness) && - Damping.Equals(other.Damping) && - Bias.Equals(other.Bias); - } + return new WeldJoint(this); + } +} + +public sealed class WeldJoint : Joint, IEquatable +{ + // Shared + private float _gamma; + private Vector3 _impulse; + + // Temporary + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Matrix33 _mass; + + // Settable + + [DataField("stiffness")] + public float Stiffness; + + [DataField("damping")] + public float Damping; + + [DataField("bias")] + public float Bias; + + /// + /// The bodyB angle minus bodyA angle in the reference state (radians). + /// + [DataField("referenceAngle")] + public float ReferenceAngle; + + /// + /// Used for Serv3 reasons + /// + public WeldJoint() {} + + internal WeldJoint(EntityUid bodyA, EntityUid bodyB, Vector2 anchorA, Vector2 anchorB, float referenceAngle) : base(bodyA, bodyB) + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + ReferenceAngle = referenceAngle; + } + + internal WeldJoint(EntityUid bodyAUid, EntityUid bodyBUid) : base(bodyAUid, bodyBUid) {} + + internal WeldJoint(WeldJointState state) : base(state) + { + Stiffness = state.Stiffness; + Damping = state.Damping; + Bias = state.Bias; + } + + public override JointType JointType => JointType.Weld; + public override JointState GetState() + { + var weldJointState = new WeldJointState(); + + base.GetState(weldJointState); + return weldJointState; + } + + public override Vector2 GetReactionForce(float invDt) + { + var P = new Vector2(_impulse.X, _impulse.Y); + return P * invDt; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _impulse.Z; + } + + internal override void InitVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + PhysicsComponent bodyA, + PhysicsComponent bodyB, + Vector2[] positions, + float[] angles, + Vector2[] linearVelocities, + float[] angularVelocities) + { + var offset = island.Offset; + _indexA = bodyA.IslandIndex[island.Index]; + _indexB = bodyB.IslandIndex[island.Index]; + _localCenterA = bodyA.LocalCenter; + _localCenterB = bodyB.LocalCenter; + _invMassA = bodyA.InvMass; + _invMassB = bodyB.InvMass; + _invIA = bodyA.InvI; + _invIB = bodyB.InvI; + + float aA = angles[_indexA]; + var vA = linearVelocities[offset + _indexA]; + float wA = angularVelocities[offset + _indexA]; + + float aB = angles[_indexB]; + var vB = linearVelocities[offset + _indexB]; + float wB = angularVelocities[offset + _indexB]; + + Quaternion2D qA = new(aA), qB = new(aB); + + _rA = Transform.Mul(qA, LocalAnchorA - _localCenterA); + _rB = Transform.Mul(qB, LocalAnchorB - _localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Matrix33 K; + K.EX.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB; + K.EY.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB; + K.EZ.X = -_rA.Y * iA - _rB.Y * iB; + K.EX.Y = K.EY.X; + K.EY.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB; + K.EZ.Y = _rA.X * iA + _rB.X * iB; + K.EX.Z = K.EZ.X; + K.EY.Z = K.EZ.Y; + K.EZ.Z = iA + iB; + + if (Stiffness > 0.0f) + { + K.GetInverse22(ref _mass); + + float invM = iA + iB; + + float C = aB - aA - ReferenceAngle; + + // Damping coefficient + float d = Damping; + + // Spring stiffness + float k = Stiffness; + + // magic formulas + float h = data.FrameTime; + _gamma = h * (d + h * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + Bias = C * h * k * _gamma; + + invM += _gamma; + _mass.EZ.Z = invM != 0.0f ? 1.0f / invM : 0.0f; + } + else if (K.EZ.Z == 0.0f) + { + K.GetInverse22(ref _mass); + _gamma = 0.0f; + Bias = 0.0f; + } + else + { + K.GetSymInverse33(ref _mass); + _gamma = 0.0f; + Bias = 0.0f; + } + + if (data.WarmStarting) + { + // Scale impulses to support a variable time step. + _impulse *= data.DtRatio; + + var P = new Vector2(_impulse.X, _impulse.Y); + + vA -= P * mA; + wA -= iA * (Vector2.Cross(_rA, P) + _impulse.Z); + + vB += P * mB; + wB += iB * (Vector2.Cross(_rB, P) + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + } + + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; + } + + internal override void SolveVelocityConstraints( + in SolverData data, + in SharedPhysicsSystem.IslandData island, + Vector2[] linearVelocities, + float[] angularVelocities) + { + 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; + + if (Stiffness > 0.0f) + { + float Cdot2 = wB - wA; + + float impulse2 = -_mass.EZ.Z * (Cdot2 + Bias + _gamma * _impulse.Z); + _impulse.Z += impulse2; + + wA -= iA * impulse2; + wB += iB * impulse2; + + var Cdot1 = vB + Vector2.Cross(wB, _rB) - vA - Vector2.Cross(wA, _rA); + + var impulse1 = -_mass.Mul22(Cdot1); + _impulse.X += impulse1.X; + _impulse.Y += impulse1.Y; + + var P = impulse1; + + vA -= P * mA; + wA -= iA * Vector2.Cross(_rA, P); + + vB += P * mB; + wB += iB * Vector2.Cross(_rB, P); + } + else + { + var Cdot1 = vB + Vector2.Cross(wB, _rB) - vA - Vector2.Cross(wA, _rA); + float Cdot2 = wB - wA; + var Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + var impulse = -_mass.Mul(Cdot); + _impulse += impulse; + + var P = new Vector2(impulse.X, impulse.Y); + + vA -= P * mA; + wA -= iA * (Vector2.Cross(_rA, P) + impulse.Z); + + vB += P * mB; + wB += iB * (Vector2.Cross(_rB, P) + impulse.Z); + } + + linearVelocities[offset + _indexA] = vA; + angularVelocities[offset + _indexA] = wA; + linearVelocities[offset + _indexB] = vB; + angularVelocities[offset + _indexB] = wB; + } + + internal override bool SolvePositionConstraints( + in SolverData data, + Vector2[] positions, + float[] angles) + { + var cA = positions[_indexA]; + float aA = angles[_indexA]; + var cB = positions[_indexB]; + float aB = angles[_indexB]; + + Quaternion2D qA = new(aA), qB = new(aB); + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + var rA = Transform.Mul(qA, LocalAnchorA - _localCenterA); + var rB = Transform.Mul(qB, LocalAnchorB - _localCenterB); + + float positionError, angularError; + + Matrix33 K; + K.EX.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + K.EY.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + K.EZ.X = -rA.Y * iA - rB.Y * iB; + K.EX.Y = K.EY.X; + K.EY.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + K.EZ.Y = rA.X * iA + rB.X * iB; + K.EX.Z = K.EZ.X; + K.EY.Z = K.EZ.Y; + K.EZ.Z = iA + iB; + + if (Stiffness > 0.0f) + { + var C1 = cB + rB - cA - rA; + + positionError = C1.Length; + angularError = 0.0f; + + var P = -K.Solve22(C1); + + cA -= P * mA; + aA -= iA * Vector2.Cross(rA, P); + + cB += P * mB; + aB += iB * Vector2.Cross(rB, P); + } + else + { + var C1 = cB + rB - cA - rA; + float C2 = aB - aA - ReferenceAngle; + + positionError = C1.Length; + angularError = Math.Abs(C2); + + Vector3 C = new(C1.X, C1.Y, C2); + + Vector3 impulse; + if (K.EZ.Z > 0.0f) + { + impulse = -K.Solve33(C); + } + else + { + var impulse2 = -K.Solve22(C1); + impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); + } + + var P = new Vector2(impulse.X, impulse.Y); + + cA -= P * mA; + aA -= iA * (Vector2.Cross(rA, P) + impulse.Z); + + cB += P * mB; + aB += iB * (Vector2.Cross(rB, P) + impulse.Z); + } + + positions[_indexA] = cA; + angles[_indexA]= aA; + positions[_indexB] = cB; + angles[_indexB] = aB; + + return positionError <= PhysicsConstants.LinearSlop && angularError <= PhysicsConstants.AngularSlop; + } + + public override Joint Clone(EntityUid uidA, EntityUid uidB) + { + var weld = new WeldJoint(uidA, uidB, LocalAnchorA, LocalAnchorB, ReferenceAngle) + { + Enabled = Enabled, + Bias = Bias, + Damping = Damping, + Stiffness = Stiffness, + _impulse = _impulse, + Breakpoint = Breakpoint + }; + return weld; + } + + public override void CopyTo(Joint original) + { + if (original is not WeldJoint weld) + return; + + weld.Enabled = Enabled; + weld.Bias = Bias; + weld.Damping = Damping; + weld.Stiffness = Stiffness; + weld._impulse = _impulse; + weld.Breakpoint = Breakpoint; + } + + public bool Equals(WeldJoint? other) + { + if (ReferenceEquals(null, other)) return false; + if (ReferenceEquals(this, other)) return true; + if (!base.Equals(other)) return false; + + return Stiffness.Equals(other.Stiffness) && + Damping.Equals(other.Damping) && + Bias.Equals(other.Bias); } } diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs index b461de246..4fa20d2de 100644 --- a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs @@ -153,7 +153,8 @@ namespace Robust.Shared.Physics.Systems internal void FindNewContacts(PhysicsMapComponent component, MapId mapId) { var moveBuffer = component.MoveBuffer; - var movedGrids = _mapManager.GetMovedGrids(mapId); + var mapUid = _mapManager.GetMapEntityId(mapId); + var movedGrids = Comp(mapUid).MovedGrids; var gridMoveBuffer = new Dictionary(); var broadphaseQuery = GetEntityQuery(); @@ -265,7 +266,7 @@ namespace Robust.Shared.Physics.Systems ArrayPool>.Shared.Return(contactBuffer); ArrayPool<(FixtureProxy Proxy, Box2 AABB)>.Shared.Return(pMoveBuffer); moveBuffer.Clear(); - _mapManager.ClearMovedGrids(mapId); + movedGrids.Clear(); } private void HandleGridCollisions( diff --git a/Robust.UnitTesting/RobustUnitTest.cs b/Robust.UnitTesting/RobustUnitTest.cs index b7bdd8640..b14b7c21a 100644 --- a/Robust.UnitTesting/RobustUnitTest.cs +++ b/Robust.UnitTesting/RobustUnitTest.cs @@ -177,6 +177,16 @@ namespace Robust.UnitTesting compFactory.RegisterClass(); } + if (!compFactory.AllRegisteredTypes.Contains(typeof(GridTreeComponent))) + { + compFactory.RegisterClass(); + } + + if (!compFactory.AllRegisteredTypes.Contains(typeof(MovedGridsComponent))) + { + compFactory.RegisterClass(); + } + if (!compFactory.AllRegisteredTypes.Contains(typeof(JointRelayTargetComponent))) { compFactory.RegisterClass(); diff --git a/Robust.UnitTesting/Server/RobustServerSimulation.cs b/Robust.UnitTesting/Server/RobustServerSimulation.cs index c0e5de467..0acfcf0ec 100644 --- a/Robust.UnitTesting/Server/RobustServerSimulation.cs +++ b/Robust.UnitTesting/Server/RobustServerSimulation.cs @@ -270,6 +270,8 @@ namespace Robust.UnitTesting.Server compFactory.RegisterClass(); compFactory.RegisterClass(); compFactory.RegisterClass(); + compFactory.RegisterClass(); + compFactory.RegisterClass(); compFactory.RegisterClass(); compFactory.RegisterClass(); compFactory.RegisterClass();