space-wizards/RobustToolbox
1
0
Fork 0
mirror of https://github.com/space-wizards/RobustToolbox.git synced 2026-02-14 19:29:36 +01:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Physics (#1602)"

Browse Source 
This reverts commit fefcc7cba3.
This commit is contained in:
Pieter-Jan Briers
2021-02-28 18:45:18 +01:00
parent 2ace0e9e5a
commit d751c0b3ab
106 changed files with 3170 additions and 13103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Resources/Prototypes/dummy_entity.yml
View File

@@ -1,4 +0,0 @@
- type: entity
name: blank entity
id: BlankEntity
abstract: true

6
Robust.Client/Audio/Midi/MidiManager.cs
View File

@@ -13,7 +13,6 @@ using Robust.Shared.Log;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Utility;
using Logger = Robust.Shared.Log.Logger;
@@ -72,8 +71,6 @@ namespace Robust.Client.Audio.Midi
[Dependency] private readonly IResourceManagerInternal _resourceManager = default!;
[Dependency] private readonly IEntityManager _entityManager = default!;
private SharedBroadPhaseSystem _broadPhaseSystem = default!;
public bool IsAvailable
{
get
@@ -157,7 +154,6 @@ namespace Robust.Client.Audio.Midi
_midiThread = new Thread(ThreadUpdate);
_midiThread.Start();
_broadPhaseSystem = EntitySystem.Get<SharedBroadPhaseSystem>();
FluidsynthInitialized = true;
}
@@ -302,7 +298,7 @@ namespace Robust.Client.Audio.Midi
var occlusion = 0f;
if (sourceRelative.Length > 0)
{
occlusion = _broadPhaseSystem.IntersectRayPenetration(
occlusion = IoCManager.Resolve<IPhysicsManager>().IntersectRayPenetration(
pos.MapId,
new CollisionRay(
pos.Position,

41
Robust.Client/Console/Commands/PhysicsOverlayCommands.cs
View File

@@ -1,41 +0,0 @@
using Robust.Client.Debugging;
using Robust.Shared.Console;
using Robust.Shared.GameObjects;
namespace Robust.Client.Console.Commands
{
public sealed class PhysicsOverlayCommands : IConsoleCommand
{
public string Command => "physics";
public string Description => $"{Command} <contactnormals / contactpoints / shapes>";
public string Help => $"{Command} <overlay>";
public void Execute(IConsoleShell shell, string argStr, string[] args)
{
if (args.Length != 1)
{
shell.WriteLine($"Invalid number of args supplied");
return;
}
var system = EntitySystem.Get<DebugPhysicsSystem>();
switch (args[0])
{
case "contactnormals":
system.Flags ^= PhysicsDebugFlags.ContactNormals;
break;
case "contactpoints":
system.Flags ^= PhysicsDebugFlags.ContactPoints;
break;
case "shapes":
system.Flags ^= PhysicsDebugFlags.Shapes;
break;
default:
shell.WriteLine($"{args[0]} is not a recognised overlay");
return;
}
return;
}
}
}

17
Robust.Client/Console/Commands/VelocitiesCommand.cs
View File

@@ -1,17 +0,0 @@
using Robust.Client.GameObjects;
using Robust.Shared.Console;
using Robust.Shared.GameObjects;
namespace Robust.Client.Console.Commands
{
public class VelocitiesCommand : IConsoleCommand
{
public string Command => "showvelocities";
public string Description => "Displays your angular and linear velocities";
public string Help => $"{Command}";
public void Execute(IConsoleShell shell, string argStr, string[] args)
{
EntitySystem.Get<VelocityDebugSystem>().Enabled ^= true;
}
}
}

26
Robust.Client/Debugging/DebugDrawing.cs
View File

@@ -3,13 +3,10 @@ using System.Collections.Generic;
using Robust.Client.Graphics;
using Robust.Client.Input;
using Robust.Client.ResourceManagement;
using Robust.Shared;
using Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Prototypes;
namespace Robust.Client.Debugging
@@ -139,7 +136,7 @@ namespace Robust.Client.Debugging
row++;
DrawString(screenHandle, _font, drawPos + new Vector2(0, row * lineHeight), $"Mask: {Convert.ToString(body.CollisionMask, 2)}");
row++;
DrawString(screenHandle, _font, drawPos + new Vector2(0, row * lineHeight), $"Enabled: {body.CanCollide}, Hard: {body.Hard}, Anchored: {(body).BodyType == BodyType.Static}");
DrawString(screenHandle, _font, drawPos + new Vector2(0, row * lineHeight), $"Enabled: {body.CanCollide}, Hard: {body.Hard}, Anchored: {((IPhysicsComponent)body).Anchored}");
row++;
}
@@ -161,22 +158,19 @@ namespace Robust.Client.Debugging
var mapId = _eyeManager.CurrentMap;
foreach (var physBody in EntitySystem.Get<SharedBroadPhaseSystem>().GetCollidingEntities(mapId, viewport))
foreach (var physBody in _physicsManager.GetCollidingEntities(mapId, viewport))
{
// all entities have a TransformComponent
var transform = physBody.Entity.Transform;
var worldBox = physBody.GetWorldAABB();
var worldBox = physBody.WorldAABB;
if (worldBox.IsEmpty()) continue;
var colorEdge = Color.Red.WithAlpha(0.33f);
var sleepThreshold = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.TimeToSleep);
foreach (var fixture in physBody.Fixtures)
foreach (var shape in physBody.PhysicsShapes)
{
var shape = fixture.Shape;
var sleepPercent = physBody.Awake ? physBody.SleepTime / sleepThreshold : 1.0f;
shape.DebugDraw(drawing, transform.WorldMatrix, in viewport, sleepPercent);
shape.DebugDraw(drawing, transform.WorldMatrix, in viewport, physBody.SleepAccumulator / (float) physBody.SleepThreshold);
}
if (worldBox.Contains(mouseWorldPos))
@@ -239,16 +233,6 @@ namespace Robust.Client.Debugging
_handle.DrawCircle(origin, radius, color);
}
public override void DrawPolygonShape(Vector2[] vertices, in Color color)
{
_handle.DrawPrimitives(DrawPrimitiveTopology.TriangleFan, vertices, color);
}
public override void DrawLine(Vector2 start, Vector2 end, in Color color)
{
_handle.DrawLine(start, end, color);
}
public override void SetTransform(in Matrix3 transform)
{
_handle.SetTransform(transform);

149
Robust.Client/Debugging/DebugPhysicsSystem.cs
View File

@@ -1,149 +0,0 @@
using System;
using Robust.Client.Graphics;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Collision;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Dynamics.Contacts;
// TODO: Copy farseer licence here coz it's heavily inspired by it.
namespace Robust.Client.Debugging
{
internal sealed class DebugPhysicsSystem : EntitySystem
{
/*
* Used for debugging shapes, controllers, joints, contacts
*/
private const int MaxContactPoints = 2048;
internal int PointCount;
internal ContactPoint[] _points = new ContactPoint[MaxContactPoints];
public PhysicsDebugFlags Flags
{
get => _flags;
set
{
if (value == _flags) return;
if (_flags == PhysicsDebugFlags.None)
IoCManager.Resolve<IOverlayManager>().AddOverlay(new PhysicsDebugOverlay(this));
if (value == PhysicsDebugFlags.None)
IoCManager.Resolve<IOverlayManager>().RemoveOverlay(nameof(PhysicsDebugOverlay));
_flags = value;
}
}
private PhysicsDebugFlags _flags;
public override void Initialize()
{
base.Initialize();
SubscribeLocalEvent<PreSolveMessage>(HandlePreSolve);
}
private void HandlePreSolve(PreSolveMessage message)
{
Contact contact = message.Contact;
Manifold oldManifold = message.OldManifold;
if ((Flags & PhysicsDebugFlags.ContactPoints) != 0)
{
Manifold manifold = contact.Manifold;
if (manifold.PointCount == 0)
return;
Fixture fixtureA = contact.FixtureA!;
PointState[] state1, state2;
CollisionManager.GetPointStates(out state1, out state2, oldManifold, manifold);
Vector2[] points;
Vector2 normal;
contact.GetWorldManifold(out normal, out points);
for (int i = 0; i < manifold.PointCount && PointCount < MaxContactPoints; ++i)
{
if (fixtureA == null)
_points[i] = new ContactPoint();
ContactPoint cp = _points[PointCount];
cp.Position = points[i];
cp.Normal = normal;
cp.State = state2[i];
_points[PointCount] = cp;
++PointCount;
}
}
}
internal struct ContactPoint
{
public Vector2 Normal;
public Vector2 Position;
public PointState State;
}
}
[Flags]
internal enum PhysicsDebugFlags : byte
{
None = 0,
ContactPoints = 1 << 0,
ContactNormals = 1 << 1,
Shapes = 1 << 2,
}
internal sealed class PhysicsDebugOverlay : Overlay
{
private DebugPhysicsSystem _physics = default!;
public override OverlaySpace Space => OverlaySpace.WorldSpace;
public PhysicsDebugOverlay(DebugPhysicsSystem system) : base(nameof(PhysicsDebugOverlay))
{
_physics = system;
}
protected override void Draw(DrawingHandleBase handle, OverlaySpace currentSpace)
{
if (_physics.Flags == PhysicsDebugFlags.None) return;
var worldHandle = (DrawingHandleWorld) handle;
if ((_physics.Flags & PhysicsDebugFlags.Shapes) != 0)
{
// Port DebugDrawing over.
}
if ((_physics.Flags & PhysicsDebugFlags.ContactPoints) != 0)
{
const float axisScale = 0.3f;
for (int i = 0; i < _physics.PointCount; ++i)
{
DebugPhysicsSystem.ContactPoint point = _physics._points[i];
if (point.State == PointState.Add)
worldHandle.DrawCircle(point.Position, 0.5f, new Color(255, 77, 243, 77));
else if (point.State == PointState.Persist)
worldHandle.DrawCircle(point.Position, 0.5f, new Color(255, 77, 77, 77));
if ((_physics.Flags & PhysicsDebugFlags.ContactNormals) != 0)
{
Vector2 p1 = point.Position;
Vector2 p2 = p1 + point.Normal * axisScale;
worldHandle.DrawLine(p1, p2, new Color(255, 102, 230, 102));
}
}
_physics.PointCount = 0;
}
}
}
}

2
Robust.Client/GameObjects/ClientComponentFactory.cs
View File

@@ -23,7 +23,7 @@ namespace Robust.Client.GameObjects
Register<PhysicsComponent>();
RegisterReference<PhysicsComponent, IPhysBody>();
RegisterReference<PhysicsComponent, IPhysicsComponent>();
RegisterIgnore("KeyBindingInput");
Register<InputComponent>();

5
Robust.Client/GameObjects/Components/Containers/ContainerManagerComponent.ClientContainer.cs
View File

@@ -1,7 +1,6 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using Robust.Server.GameObjects;
using Robust.Shared.GameObjects;
using Robust.Shared.ViewVariables;
@@ -63,14 +62,14 @@ namespace Robust.Client.GameObjects
public void DoInsert(IEntity entity)
{
Entities.Add(entity);
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new EntInsertedIntoContainerMessage(entity, this));
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new UpdateContainerOcclusionMessage(entity));
}
public void DoRemove(IEntity entity)
{
Entities.Remove(entity);
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new EntRemovedFromContainerMessage(entity, this));
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new UpdateContainerOcclusionMessage(entity));
}

8
Robust.Client/GameObjects/EntitySystems/AudioSystem.cs
View File

@@ -11,7 +11,6 @@ using Robust.Shared.Log;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Utility;
namespace Robust.Client.GameObjects
@@ -25,8 +24,6 @@ namespace Robust.Client.GameObjects
[Dependency] private readonly IEyeManager _eyeManager = default!;
[Dependency] private readonly IEntityManager _entityManager = default!;
private SharedBroadPhaseSystem _broadPhaseSystem = default!;
private readonly List<PlayingStream> _playingClydeStreams = new();
public int OcclusionCollisionMask;
@@ -34,13 +31,10 @@ namespace Robust.Client.GameObjects
/// <inheritdoc />
public override void Initialize()
{
base.Initialize();
SubscribeNetworkEvent<PlayAudioEntityMessage>(PlayAudioEntityHandler);
SubscribeNetworkEvent<PlayAudioGlobalMessage>(PlayAudioGlobalHandler);
SubscribeNetworkEvent<PlayAudioPositionalMessage>(PlayAudioPositionalHandler);
SubscribeNetworkEvent<StopAudioMessageClient>(StopAudioMessageHandler);
_broadPhaseSystem = Get<SharedBroadPhaseSystem>();
}
private void StopAudioMessageHandler(StopAudioMessageClient ev)
@@ -147,7 +141,7 @@ namespace Robust.Client.GameObjects
var occlusion = 0f;
if (sourceRelative.Length > 0)
{
occlusion = _broadPhaseSystem.IntersectRayPenetration(
occlusion = IoCManager.Resolve<IPhysicsManager>().IntersectRayPenetration(
pos.MapId,
new CollisionRay(
pos.Position,

51
Robust.Client/GameObjects/EntitySystems/VelocityDebugSystem.cs
View File

@@ -1,51 +0,0 @@
using Robust.Client.Graphics;
using Robust.Client.Player;
using Robust.Client.UserInterface;
using Robust.Client.UserInterface.Controls;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
namespace Robust.Client.GameObjects
{
public class VelocityDebugSystem : EntitySystem
{
[Dependency] private readonly IEyeManager _eyeManager = default!;
[Dependency] private readonly IPlayerManager _playerManager = default!;
internal bool Enabled { get; set; }
private Label _label = default!;
public override void Initialize()
{
base.Initialize();
_label = new Label();
IoCManager.Resolve<IUserInterfaceManager>().StateRoot.AddChild(_label);
}
public override void FrameUpdate(float frameTime)
{
base.FrameUpdate(frameTime);
if (!Enabled)
{
_label.Visible = false;
return;
}
var player = _playerManager.LocalPlayer?.ControlledEntity;
if (player == null || !player.TryGetComponent(out PhysicsComponent? body))
{
_label.Visible = false;
return;
}
var screenPos = _eyeManager.WorldToScreen(player.Transform.WorldPosition);
LayoutContainer.SetPosition(_label, screenPos + new Vector2(0, 50));
_label.Visible = true;
_label.Text = $"Speed: {body.LinearVelocity.Length}\nLinear: {body.LinearVelocity.X:0.00}, {body.LinearVelocity.Y:0.00}\nAngular:{body.AngularVelocity}";
}
}
}

4
Robust.Client/GameStates/NetInterpOverlay.cs
View File

@@ -29,7 +29,7 @@ namespace Robust.Client.GameStates
handle.UseShader(_shader);
var worldHandle = (DrawingHandleWorld) handle;
var viewport = _eyeManager.GetWorldViewport();
foreach (var boundingBox in _componentManager.EntityQuery<IPhysBody>(true))
foreach (var boundingBox in _componentManager.EntityQuery<IPhysicsComponent>(true))
{
// all entities have a TransformComponent
var transform = ((IComponent)boundingBox).Owner.Transform;
@@ -42,7 +42,7 @@ namespace Robust.Client.GameStates
if(transform.LerpDestination == null)
continue;
var aabb = boundingBox.GetWorldAABB();
var aabb = ((IPhysBody)boundingBox).AABB;
// if not on screen, or too small, continue
if (!aabb.Translated(transform.WorldPosition).Intersects(viewport) || aabb.IsEmpty())

13
Robust.Client/Physics/BroadPhaseSystem.cs
View File

@@ -1,13 +0,0 @@
using Robust.Shared.Physics.Broadphase;
namespace Robust.Client.Physics
{
internal sealed class BroadPhaseSystem : SharedBroadPhaseSystem
{
public override void Initialize()
{
base.Initialize();
UpdatesBefore.Add(typeof(PhysicsSystem));
}
}
}

135
Robust.Client/Physics/DebugPhysicsIslandSystem.cs
View File

@@ -1,135 +0,0 @@
using System;
using System.Collections.Generic;
using Robust.Client.Graphics;
using Robust.Shared.Console;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Timing;
namespace Robust.Client.Physics
{
internal sealed class PhysicsIslandCommand : IConsoleCommand
{
public string Command => "showislands";
public string Description => "Shows the current physics bodies involved in each physics island.";
public string Help => "showislands";
public void Execute(IConsoleShell shell, string argStr, string[] args)
{
if (args.Length != 0)
{
shell.WriteLine("This command doesn't take args!");
return;
}
EntitySystem.Get<DebugPhysicsIslandSystem>().Mode ^= DebugPhysicsIslandMode.Solve;
}
}
internal sealed class DebugPhysicsIslandSystem : EntitySystem
{
[Dependency] private readonly IGameTiming _gameTiming = default!;
public DebugPhysicsIslandMode Mode { get; set; } = DebugPhysicsIslandMode.None;
/*
* Island solve debug:
* This will draw above every body involved in a particular island solve.
*/
public readonly Queue<(TimeSpan Time, List<IPhysBody> Bodies)> IslandSolve = new();
public const float SolveDuration = 0.1f;
public override void Initialize()
{
base.Initialize();
SubscribeLocalEvent<IslandSolveMessage>(HandleIslandSolveMessage);
IoCManager.Resolve<IOverlayManager>().AddOverlay(new PhysicsIslandOverlay());
}
public override void FrameUpdate(float frameTime)
{
base.FrameUpdate(frameTime);
while (IslandSolve.TryPeek(out var solve))
{
if (solve.Time.TotalSeconds + SolveDuration > _gameTiming.CurTime.TotalSeconds)
{
IslandSolve.Dequeue();
}
else
{
break;
}
}
}
public override void Shutdown()
{
base.Shutdown();
IoCManager.Resolve<IOverlayManager>().RemoveOverlay(nameof(PhysicsIslandOverlay));
}
private void HandleIslandSolveMessage(IslandSolveMessage message)
{
if ((Mode & DebugPhysicsIslandMode.Solve) == 0x0) return;
IslandSolve.Enqueue((_gameTiming.CurTime, message.Bodies));
}
}
[Flags]
internal enum DebugPhysicsIslandMode : ushort
{
None = 0,
Solve = 1 << 0,
Contacts = 1 << 1,
}
internal sealed class PhysicsIslandOverlay : Overlay
{
[Dependency] private readonly IEyeManager _eyeManager = default!;
[Dependency] private readonly IGameTiming _gameTiming = default!;
private DebugPhysicsIslandSystem _islandSystem = default!;
public override OverlaySpace Space => OverlaySpace.WorldSpace;
public PhysicsIslandOverlay() : base(nameof(PhysicsIslandOverlay))
{
_islandSystem = EntitySystem.Get<DebugPhysicsIslandSystem>();
_eyeManager = IoCManager.Resolve<IEyeManager>();
_gameTiming = IoCManager.Resolve<IGameTiming>();
}
protected override void Draw(DrawingHandleBase handle, OverlaySpace currentSpace)
{
var worldHandle = (DrawingHandleWorld) handle;
DrawIslandSolve(worldHandle);
}
private void DrawIslandSolve(DrawingHandleWorld handle)
{
if ((_islandSystem.Mode & DebugPhysicsIslandMode.Solve) == 0x0) return;
var viewport = _eyeManager.GetWorldViewport();
foreach (var solve in _islandSystem.IslandSolve)
{
var ratio = (float) Math.Max(
(solve.Time.TotalSeconds + DebugPhysicsIslandSystem.SolveDuration -
_gameTiming.CurTime.TotalSeconds) / DebugPhysicsIslandSystem.SolveDuration, 0.0f);
if (ratio <= 0.0f) continue;
foreach (var body in solve.Bodies)
{
var worldAABB = body.GetWorldAABB();
if (!viewport.Intersects(worldAABB)) continue;
handle.DrawRect(worldAABB, Color.Green.WithAlpha(ratio * 0.5f));
}
}
}
}
}

4
Robust.Client/Placement/PlacementMode.cs
View File

@@ -4,10 +4,8 @@ using System.Diagnostics.CodeAnalysis;
using System.Linq;
using Robust.Client.Graphics;
using Robust.Client.ResourceManagement;
using Robust.Shared.GameObjects;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Utility;
namespace Robust.Client.Placement
@@ -232,7 +230,7 @@ namespace Robust.Client.Placement
bounds.Width,
bounds.Height);
return EntitySystem.Get<SharedBroadPhaseSystem>().TryCollideRect(collisionBox, mapCoords.MapId);
return pManager.PhysicsManager.TryCollideRect(collisionBox, mapCoords.MapId);
}
protected Vector2 ScreenToWorld(Vector2 point)

4
Robust.Server/Debugging/DebugDrawingManager.cs
View File

@@ -1,4 +1,4 @@
using System.Diagnostics;
using System.Diagnostics;
using Robust.Shared.IoC;
using Robust.Shared.Network;
using Robust.Shared.Network.Messages;
@@ -14,7 +14,7 @@ namespace Robust.Server.Debugging
public void Initialize()
{
_net.RegisterNetMessage<MsgRay>(MsgRay.NAME);
// TODO _physics.DebugDrawRay += data => PhysicsOnDebugDrawRay(data);
_physics.DebugDrawRay += data => PhysicsOnDebugDrawRay(data);
}
[Conditional("DEBUG")]

0
Robust.Shared/GameObjects/EntitySystemMessages/ContainerModifiedMessages.cs → Robust.Server/GameObjects/EntitySystemMessages/EntRemovedFromContainerMessage.cs
View File

5
Robust.Server/GameObjects/EntitySystems/TileLookup/GridTileLookupSystem.cs
View File

@@ -6,7 +6,6 @@ using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
namespace Robust.Server.GameObjects
{
@@ -199,8 +198,8 @@ namespace Robust.Server.GameObjects
private Box2 GetEntityBox(IEntity entity)
{
// Need to clip the aabb as anything with an edge intersecting another tile might be picked up, such as walls.
if (entity.TryGetComponent(out IPhysBody? physics))
return new Box2(physics.GetWorldAABB().BottomLeft + 0.01f, physics.GetWorldAABB().TopRight - 0.01f);
if (entity.TryGetComponent(out IPhysicsComponent? physics))
return new Box2(physics.WorldAABB.BottomLeft + 0.01f, physics.WorldAABB.TopRight - 0.01f);
// Don't want to accidentally get neighboring tiles unless we're near an edge
return Box2.CenteredAround(entity.Transform.Coordinates.ToMapPos(EntityManager), Vector2.One / 2);

6
Robust.Server/GameObjects/ServerComponentFactory.cs
View File

@@ -1,5 +1,4 @@
using Robust.Shared.GameObjects;
using Robust.Shared.Physics;
using Robust.Shared.GameObjects;
namespace Robust.Server.GameObjects
{
@@ -28,8 +27,7 @@ namespace Robust.Server.GameObjects
RegisterReference<BasicActorComponent, IActorComponent>();
Register<PhysicsComponent>();
RegisterReference<PhysicsComponent, IPhysBody>();
RegisterReference<PhysicsComponent, IPhysicsComponent>();
Register<OccluderComponent>();
RegisterIgnore("Input");

7
Robust.Server/GameObjects/ServerEntityManager.cs
View File

@@ -9,7 +9,6 @@ using Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Prototypes;
using Robust.Shared.Timing;
using Robust.Shared.Utility;
@@ -368,7 +367,7 @@ namespace Robust.Server.GameObjects
continue;
}
if (entity.TryGetComponent(out IPhysBody? body))
if (entity.TryGetComponent(out IPhysicsComponent? body))
{
if (body.LinearVelocity.EqualsApprox(Vector2.Zero, MinimumMotionForMovers))
{
@@ -535,7 +534,7 @@ namespace Robust.Server.GameObjects
continue;
}
if (!entity.TryGetComponent(out IPhysBody? body))
if (!entity.TryGetComponent(out IPhysicsComponent? body))
{
// can't be a mover w/o physics
continue;
@@ -789,7 +788,7 @@ namespace Robust.Server.GameObjects
{
addToMovers = true;
}
else if (entity.TryGetComponent(out IPhysBody? physics)
else if (entity.TryGetComponent(out IPhysicsComponent? physics)
&& physics.LastModifiedTick >= currentTick)
{
addToMovers = true;

14
Robust.Server/Physics/BroadPhaseSystem.cs
View File

@@ -1,14 +0,0 @@
using Robust.Server.GameObjects;
using Robust.Shared.Physics.Broadphase;
namespace Robust.Server.Physics
{
internal sealed class BroadPhaseSystem : SharedBroadPhaseSystem
{
public override void Initialize()
{
base.Initialize();
UpdatesBefore.Add(typeof(PhysicsSystem));
}
}
}

7
Robust.Shared.Maths/Vector2.cs
View File

@@ -43,11 +43,6 @@ namespace Robust.Shared.Maths
public static readonly Vector2 Infinity = new(float.PositiveInfinity, float.PositiveInfinity);
/// <summary>
/// A vector with NaN X and Y.
/// </summary>
public static readonly Vector2 NaN = new(float.NaN, float.NaN);
/// <summary>
/// Construct a vector from its coordinates.
/// </summary>
@@ -95,7 +90,7 @@ namespace Robust.Shared.Maths
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public readonly Vector2 Rounded()
{
return new(MathF.Round(X), MathF.Round(Y));
return new((float) MathF.Round(X), (float) MathF.Round(Y));
}
/// <summary>

99
Robust.Shared/CVars.cs
View File

@@ -280,105 +280,6 @@ namespace Robust.Shared
public static readonly CVarDef<string> PlayerName =
CVarDef.Create("player.name", "JoeGenero", CVar.ARCHIVE | CVar.CLIENTONLY);
/*
* PHYSICS
*/
// - Sleep
public static readonly CVarDef<float> AngularSleepTolerance =
CVarDef.Create("physics.angsleeptol", 2.0f / 180.0f * MathF.PI);
public static readonly CVarDef<float> LinearSleepTolerance =
CVarDef.Create("physics.linsleeptol", 0.001f);
public static readonly CVarDef<bool> SleepAllowed =
CVarDef.Create("physics.sleepallowed", true);
// Box2D default is 0.5f
public static readonly CVarDef<float> TimeToSleep =
CVarDef.Create("physics.timetosleep", 0.50f);
// - Solver
// 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.
public static readonly CVarDef<int> PositionIterations =
CVarDef.Create("physics.positer", 3);
public static readonly CVarDef<int> VelocityIterations =
CVarDef.Create("physics.veliter", 8);
public static readonly CVarDef<bool> WarmStarting =
CVarDef.Create("physics.warmstart", true);
public static readonly CVarDef<bool> AutoClearForces =
CVarDef.Create("physics.autoclearforces", true);
/// <summary>
/// A velocity threshold for elastic collisions. Any collision with a relative linear
/// velocity below this threshold will be treated as inelastic.
/// </summary>
public static readonly CVarDef<float> VelocityThreshold =
CVarDef.Create("physics.velocitythreshold", 0.5f);
// TODO: Copy Box2D's comments on baumgarte I think it's on the solver class.
/// <summary>
/// How much overlap is resolved per tick.
/// </summary>
public static readonly CVarDef<float> Baumgarte =
CVarDef.Create("physics.baumgarte", 0.2f);
/// <summary>
/// A small length used as a collision and constraint tolerance. Usually it is
/// chosen to be numerically significant, but visually insignificant.
/// </summary>
public static readonly CVarDef<float> LinearSlop =
CVarDef.Create("physics.linearslop", 0.005f);
/// <summary>
/// A small angle used as a collision and constraint tolerance. Usually it is
/// chosen to be numerically significant, but visually insignificant.
/// </summary>
public static readonly CVarDef<float> AngularSlop =
CVarDef.Create("physics.angularslop", 2.0f / 180.0f * MathF.PI);
/// <summary>
/// The radius of the polygon/edge shape skin. This should not be modified. Making
/// this smaller means polygons will have an insufficient buffer for continuous collision.
/// Making it larger may create artifacts for vertex collision.
/// </summary>
/// <remarks>
/// Default is set to be 2 x linearslop. TODO Should we listen to linearslop changes?
/// </remarks>
public static readonly CVarDef<float> PolygonRadius =
CVarDef.Create("physics.polygonradius", 2 * 0.005f);
/// <summary>
/// If true, it will run a GiftWrap convex hull on all polygon inputs.
/// This makes for a more stable engine when given random input,
/// but if speed of the creation of polygons are more important,
/// you might want to set this to false.
/// </summary>
public static readonly CVarDef<bool> ConvexHullPolygons =
CVarDef.Create("physics.convexhullpolygons", true);
public static readonly CVarDef<int> MaxPolygonVertices =
CVarDef.Create("physics.maxpolygonvertices", 8);
public static readonly CVarDef<float> MaxLinearCorrection =
CVarDef.Create("physics.maxlinearcorrection", 0.2f);
public static readonly CVarDef<float> MaxAngularCorrection =
CVarDef.Create("physics.maxangularcorrection", 8.0f / 180.0f * MathF.PI);
// - Maximums
// Squared
public static readonly CVarDef<float> MaxLinVelocity =
CVarDef.Create("physics.maxlinvelocity", 4.0f);
// Squared
public static readonly CVarDef<float> MaxAngVelocity =
CVarDef.Create("physics.maxangvelocity", 0.5f * MathF.PI);
/*
* DISCORD
*/

3
Robust.Shared/GameObjects/ComponentManager.cs
View File

@@ -4,7 +4,6 @@ using System.Diagnostics.CodeAnalysis;
using System.Linq;
using System.Runtime.CompilerServices;
using Robust.Shared.Exceptions;
using Robust.Shared.Physics;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;
using DependencyAttribute = Robust.Shared.IoC.DependencyAttribute;
@@ -194,7 +193,7 @@ namespace Robust.Shared.GameObjects
{
ITransformComponent _ => 0,
IMetaDataComponent _ => 1,
IPhysBody _ => 2,
IPhysicsComponent _ => 2,
_ => int.MaxValue
};

7
Robust.Shared/GameObjects/Components/Collidable/CollisionChangeMessage.cs
View File

@@ -2,16 +2,13 @@ namespace Robust.Shared.GameObjects
{
public class CollisionChangeMessage : EntitySystemMessage
{
public PhysicsComponent Body { get; }
public EntityUid Owner { get; }
public bool CanCollide { get; }
public CollisionChangeMessage(PhysicsComponent body, EntityUid owner, bool canCollide)
public CollisionChangeMessage(EntityUid owner, bool canCollide)
{
Body = body;
Owner = owner;
CanCollide = canCollide;
}
}
}
}

520
Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Collision.cs
View File

@@ -1,37 +1,25 @@
using System;
using System.Collections;
using System.Collections.Generic;
using Robust.Shared.Containers;
using Robust.Shared.IoC;
using Robust.Shared.Log;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Dynamics.Contacts;
using Robust.Shared.Physics.Dynamics.Joints;
using Robust.Shared.Players;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.GameObjects
{
public interface ICollideBehavior
{
/// <summary>
/// We'll pass in both our body and the other body to save the behaviors having to get these components themselves.
/// </summary>
void CollideWith(IPhysBody ourBody, IPhysBody otherBody);
}
void CollideWith(IEntity collidedWith);
public interface IPostCollide
{
/// <summary>
/// Run behaviour after all other collision behaviors have run.
/// Called after all collisions have been processed, as well as how many collisions occured
/// </summary>
/// <param name="ourBody"></param>
/// <param name="otherBody"></param>
void PostCollide(IPhysBody ourBody, IPhysBody otherBody);
/// <param name="collisionCount"></param>
void PostCollide(int collisionCount) { }
}
public interface ICollideSpecial
@@ -39,6 +27,483 @@ namespace Robust.Shared.GameObjects
bool PreventCollide(IPhysBody collidedwith);
}
public partial interface IPhysicsComponent : IComponent, IPhysBody
{
public new bool Hard { get; set; }
bool IsColliding(Vector2 offset, bool approximate = true);
IEnumerable<IEntity> GetCollidingEntities(Vector2 offset, bool approximate = true);
bool UpdatePhysicsTree();
void RemovedFromPhysicsTree(MapId mapId);
void AddedToPhysicsTree(MapId mapId);
}
public partial class PhysicsComponent : Component, IPhysicsComponent
{
[Dependency] private readonly IPhysicsManager _physicsManager = default!;
private bool _canCollide;
private bool _isHard;
private BodyStatus _status;
private BodyType _bodyType;
private List<IPhysShape> _physShapes = new();
/// <inheritdoc />
public override string Name => "Physics";
/// <inheritdoc />
public override uint? NetID => NetIDs.PHYSICS;
public IEntity Entity => Owner;
/// <inheritdoc />
public MapId MapID => Owner.Transform.MapID;
/// <inheritdoc />
public int ProxyId { get; set; }
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
public BodyType BodyType { get; set; } = BodyType.Static;
/// <inheritdoc />
public int SleepAccumulator
{
get => _sleepAccumulator;
set
{
if (_sleepAccumulator == value)
return;
_sleepAccumulator = value;
Awake = _physicsManager.SleepTimeThreshold > SleepAccumulator;
}
}
private int _sleepAccumulator;
// TODO: When SleepTimeThreshold updates we need to update Awake
public int SleepThreshold
{
get => _physicsManager.SleepTimeThreshold;
set => _physicsManager.SleepTimeThreshold = value;
}
/// <inheritdoc />
[ViewVariables]
public bool Awake
{
get => _awake;
private set
{
if (_awake == value)
return;
_awake = value;
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new PhysicsUpdateMessage(this));
}
}
private bool _awake = true;
/// <inheritdoc />
public void WakeBody()
{
if (CanMove())
SleepAccumulator = 0;
}
public PhysicsComponent()
{
PhysicsShapes = new PhysShapeList(this);
}
/// <inheritdoc />
public override void ExposeData(ObjectSerializer serializer)
{
base.ExposeData(serializer);
serializer.DataField(ref _canCollide, "on", true);
serializer.DataField(ref _isHard, "hard", true);
serializer.DataField(ref _status, "status", BodyStatus.OnGround);
serializer.DataField(ref _bodyType, "bodyType", BodyType.Static);
serializer.DataField(ref _physShapes, "shapes", new List<IPhysShape> {new PhysShapeAabb()});
serializer.DataField(ref _anchored, "anchored", true);
serializer.DataField(ref _mass, "mass", 1.0f);
}
/// <param name="player"></param>
/// <inheritdoc />
public override ComponentState GetComponentState(ICommonSession player)
{
return new PhysicsComponentState(_canCollide, _status, _physShapes, _isHard, _mass, LinearVelocity, AngularVelocity, Anchored);
}
/// <inheritdoc />
public override void HandleComponentState(ComponentState? curState, ComponentState? nextState)
{
if (curState == null)
return;
var newState = (PhysicsComponentState) curState;
_canCollide = newState.CanCollide;
_status = newState.Status;
_isHard = newState.Hard;
_physShapes = newState.PhysShapes;
foreach (var shape in _physShapes)
{
shape.ApplyState();
}
Dirty();
UpdateEntityTree();
Mass = newState.Mass / 1000f; // gram to kilogram
LinearVelocity = newState.LinearVelocity;
// Logger.Debug($"{IGameTiming.TickStampStatic}: [{Owner}] {LinearVelocity}");
AngularVelocity = newState.AngularVelocity;
Anchored = newState.Anchored;
// TODO: Does it make sense to reset controllers here?
// This caused space movement to break in content and I'm not 100% sure this is a good fix.
// Look man the CM test is in 5 hours cut me some slack.
//_controllers = null;
// Reset predict flag to false to avoid predicting stuff too long.
// Another possibly bad hack for content at the moment.
Predict = false;
}
/// <inheritdoc />
[ViewVariables]
Box2 IPhysBody.WorldAABB
{
get
{
var pos = Owner.Transform.WorldPosition;
return ((IPhysBody) this).AABB.Translated(pos);
}
}
/// <inheritdoc />
[ViewVariables]
Box2 IPhysBody.AABB
{
get
{
var angle = Owner.Transform.WorldRotation;
var bounds = new Box2();
foreach (var shape in _physShapes)
{
var shapeBounds = shape.CalculateLocalBounds(angle);
bounds = bounds.IsEmpty() ? shapeBounds : bounds.Union(shapeBounds);
}
return bounds;
}
}
/// <inheritdoc />
[ViewVariables]
public IList<IPhysShape> PhysicsShapes { get; }
/// <summary>
/// Enables or disabled collision processing of this component.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public bool CanCollide
{
get => _canCollide;
set
{
if (_canCollide == value)
return;
_canCollide = value;
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local,
new CollisionChangeMessage(Owner.Uid, _canCollide));
Dirty();
}
}
/// <summary>
/// Non-hard physics bodies will not cause action collision (e.g. blocking of movement)
/// while still raising collision events.
/// </summary>
/// <remarks>
/// This is useful for triggers or such to detect collision without actually causing a blockage.
/// </remarks>
[ViewVariables(VVAccess.ReadWrite)]
public bool Hard
{
get => _isHard;
set
{
if (_isHard == value)
return;
_isHard = value;
Dirty();
}
}
/// <summary>
/// Bitmask of the collision layers this component is a part of.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionLayer
{
get
{
var layers = 0x0;
foreach (var shape in _physShapes)
layers = layers | shape.CollisionLayer;
return layers;
}
}
/// <summary>
/// Bitmask of the layers this component collides with.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionMask
{
get
{
var mask = 0x0;
foreach (var shape in _physShapes)
mask = mask | shape.CollisionMask;
return mask;
}
}
/// <inheritdoc />
public override void Initialize()
{
base.Initialize();
// normally ExposeData would create this
if (_physShapes == null)
{
_physShapes = new List<IPhysShape> {new PhysShapeAabb()};
}
else
{
foreach (var shape in _physShapes)
{
ShapeAdded(shape);
}
}
foreach (var controller in _controllers.Values)
{
controller.ControlledComponent = this;
}
Dirty();
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local,
new CollisionChangeMessage(Owner.Uid, _canCollide));
}
public override void OnAdd()
{
base.OnAdd();
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new PhysicsUpdateMessage(this));
}
public override void OnRemove()
{
base.OnRemove();
// In case somebody starts sharing shapes across multiple components I guess?
foreach (var shape in _physShapes)
{
ShapeRemoved(shape);
}
// Should we not call this if !_canCollide? PathfindingSystem doesn't care at least.
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new CollisionChangeMessage(Owner.Uid, false));
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new PhysicsUpdateMessage(this));
}
private void ShapeAdded(IPhysShape shape)
{
shape.OnDataChanged += ShapeDataChanged;
}
private void ShapeRemoved(IPhysShape item)
{
item.OnDataChanged -= ShapeDataChanged;
}
/// <inheritdoc />
protected override void Startup()
{
base.Startup();
_physicsManager.AddBody(this);
}
/// <inheritdoc />
protected override void Shutdown()
{
RemoveControllers();
_physicsManager.RemoveBody(this);
base.Shutdown();
}
public bool IsColliding(Vector2 offset, bool approx = true)
{
return _physicsManager.IsColliding(this, offset, approx);
}
public IEnumerable<IEntity> GetCollidingEntities(Vector2 offset, bool approx = true)
{
return _physicsManager.GetCollidingEntities(this, offset, approx);
}
public bool UpdatePhysicsTree()
=> _physicsManager.Update(this);
public void RemovedFromPhysicsTree(MapId mapId)
{
_physicsManager.RemovedFromMap(this, mapId);
}
public void AddedToPhysicsTree(MapId mapId)
{
_physicsManager.AddedToMap(this, mapId);
}
private bool UpdateEntityTree() => Owner.EntityManager.UpdateEntityTree(Owner);
public bool IsOnGround()
{
return Status == BodyStatus.OnGround;
}
public bool IsInAir()
{
return Status == BodyStatus.InAir;
}
private void ShapeDataChanged()
{
Dirty();
UpdatePhysicsTree();
}
// Custom IList<> implementation so that we can hook addition/removal of shapes.
// To hook into their OnDataChanged event correctly.
private sealed class PhysShapeList : IList<IPhysShape>
{
private readonly PhysicsComponent _owner;
public PhysShapeList(PhysicsComponent owner)
{
_owner = owner;
}
public IEnumerator<IPhysShape> GetEnumerator()
{
return _owner._physShapes.GetEnumerator();
}
IEnumerator IEnumerable.GetEnumerator()
{
return GetEnumerator();
}
public void Add(IPhysShape item)
{
_owner._physShapes.Add(item);
ItemAdded(item);
}
public void Clear()
{
foreach (var item in _owner._physShapes)
{
ItemRemoved(item);
}
_owner._physShapes.Clear();
}
public bool Contains(IPhysShape item)
{
return _owner._physShapes.Contains(item);
}
public void CopyTo(IPhysShape[] array, int arrayIndex)
{
_owner._physShapes.CopyTo(array, arrayIndex);
}
public bool Remove(IPhysShape item)
{
var found = _owner._physShapes.Remove(item);
if (found)
{
ItemRemoved(item);
}
return found;
}
public int Count => _owner._physShapes.Count;
public bool IsReadOnly => false;
public int IndexOf(IPhysShape item)
{
return _owner._physShapes.IndexOf(item);
}
public void Insert(int index, IPhysShape item)
{
_owner._physShapes.Insert(index, item);
ItemAdded(item);
}
public void RemoveAt(int index)
{
var item = _owner._physShapes[index];
ItemRemoved(item);
_owner._physShapes.RemoveAt(index);
}
public IPhysShape this[int index]
{
get => _owner._physShapes[index];
set
{
var oldItem = _owner._physShapes[index];
ItemRemoved(oldItem);
_owner._physShapes[index] = value;
ItemAdded(value);
}
}
private void ItemAdded(IPhysShape item)
{
_owner.ShapeAdded(item);
}
public void ItemRemoved(IPhysShape item)
{
_owner.ShapeRemoved(item);
}
}
}
[Serializable, NetSerializable]
public enum BodyStatus: byte
{
@@ -47,28 +512,15 @@ namespace Robust.Shared.GameObjects
}
/// <summary>
/// Sent whenever a <see cref="IPhysBody"/> is changed.
/// Sent whenever a <see cref="IPhysicsComponent"/> is changed.
/// </summary>
public sealed class PhysicsUpdateMessage : EntitySystemMessage
{
public PhysicsComponent Component { get; }
public IPhysicsComponent Component { get; }
public PhysicsUpdateMessage(PhysicsComponent component)
public PhysicsUpdateMessage(IPhysicsComponent component)
{
Component = component;
}
}
public sealed class FixtureUpdateMessage : EntitySystemMessage
{
public PhysicsComponent Body { get; }
public Fixture Fixture { get; }
public FixtureUpdateMessage(PhysicsComponent body, Fixture fixture)
{
Body = body;
Fixture = fixture;
}
}
}

1349
Robust.Shared/GameObjects/Components/Collidable/PhysicsComponent.Physics.cs
View File

File diff suppressed because it is too large Load Diff

41
Robust.Shared/GameObjects/Components/Collidable/PhysicsComponentState.cs
View File

@@ -2,8 +2,6 @@
using System.Collections.Generic;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Dynamics.Joints;
using Robust.Shared.Serialization;
namespace Robust.Shared.GameObjects
@@ -12,11 +10,10 @@ namespace Robust.Shared.GameObjects
public class PhysicsComponentState : ComponentState
{
public readonly bool CanCollide;
public readonly bool SleepingAllowed;
public readonly bool FixedRotation;
public readonly BodyStatus Status;
public readonly List<Fixture> Fixtures;
public readonly List<Joint> Joints;
public readonly List<IPhysShape> PhysShapes;
public readonly bool Hard;
/// <summary>
/// Current mass of the entity, stored in grams.
@@ -24,45 +21,31 @@ namespace Robust.Shared.GameObjects
public readonly int Mass;
public readonly Vector2 LinearVelocity;
public readonly float AngularVelocity;
public readonly BodyType BodyType;
public readonly bool Anchored;
/// <summary>
///
/// </summary>
/// <param name="canCollide"></param>
/// <param name="sleepingAllowed"></param>
/// <param name="fixedRotation"></param>
/// <param name="status"></param>
/// <param name="fixtures"></param>
/// <param name="joints"></param>
/// <param name="physShapes"></param>
/// <param name="hard"></param>
/// <param name="mass">Current Mass of the entity.</param>
/// <param name="linearVelocity">Current linear velocity of the entity in meters per second.</param>
/// <param name="angularVelocity">Current angular velocity of the entity in radians per sec.</param>
/// <param name="bodyType"></param>
public PhysicsComponentState(
bool canCollide,
bool sleepingAllowed,
bool fixedRotation,
BodyStatus status,
List<Fixture> fixtures,
List<Joint> joints,
float mass,
Vector2 linearVelocity,
float angularVelocity,
BodyType bodyType)
/// <param name="anchored">Whether or not the entity is anchored in place.</param>
public PhysicsComponentState(bool canCollide, BodyStatus status, List<IPhysShape> physShapes, bool hard, float mass, Vector2 linearVelocity, float angularVelocity, bool anchored)
: base(NetIDs.PHYSICS)
{
CanCollide = canCollide;
SleepingAllowed = sleepingAllowed;
FixedRotation = fixedRotation;
Status = status;
Fixtures = fixtures;
Joints = joints;
PhysShapes = physShapes;
Hard = hard;
LinearVelocity = linearVelocity;
AngularVelocity = angularVelocity;
Mass = (int) Math.Round(mass * 1000); // rounds kg to nearest gram
BodyType = bodyType;
Mass = (int)Math.Round(mass * 1000); // rounds kg to nearest gram
Anchored = anchored;
}
}
}

27
Robust.Shared/GameObjects/Components/CollisionWakeComponent.cs
View File

@@ -1,27 +0,0 @@
using Robust.Shared.Physics;
namespace Robust.Shared.GameObjects
{
/// <summary>
/// An optimisation component for stuff that should be set as collidable when its awake and non-collidable when asleep.
/// </summary>
[RegisterComponent]
public sealed class CollisionWakeComponent : Component
{
public override string Name => "CollisionWake";
public override void HandleMessage(ComponentMessage message, IComponent? component)
{
base.HandleMessage(message, component);
switch (message)
{
case PhysicsWakeCompMessage msg:
msg.Body.CanCollide = true;
break;
case PhysicsSleepCompMessage msg:
msg.Body.CanCollide = false;
break;
}
}
}
}

32
Robust.Shared/GameObjects/Components/ITransformComponent.cs
View File

@@ -13,22 +13,7 @@ namespace Robust.Shared.GameObjects
public interface ITransformComponent : IComponent
{
/// <summary>
/// Defer updates to the EntityTree and MoveEvent calls if toggled.
/// </summary>
bool DeferUpdates { get; set; }
/// <summary>
/// While updating did we actually defer anything?
/// </summary>
bool UpdatesDeferred { get; }
/// <summary>
/// Run MoveEvent, RotateEvent, and UpdateEntityTree updates.
/// </summary>
void RunDeferred();
/// <summary>
/// Disables or enables to ability to locally rotate the entity. When set it removes any local rotation.
/// Disables or enables to ability to locally rotate the entity. When set it removes any local rotation.
/// </summary>
bool NoLocalRotation { get; set; }
@@ -118,12 +103,25 @@ namespace Robust.Shared.GameObjects
/// </summary>
GridId GridID { get; }
bool UpdateEntityTree();
/// <summary>
/// Whether external system updates should run or not (e.g. EntityTree, Matrices, PhysicsTree).
/// These should be manually run later.
/// </summary>
bool DeferUpdates { get; set; }
void AttachToGridOrMap();
void AttachParent(ITransformComponent parent);
void AttachParent(IEntity parent);
/// <summary>
/// Run the updates marked as deferred (UpdateEntityTree and movement events).
/// Don't call this unless you REALLY need to.
/// </summary>
/// <remarks>
/// Physics optimisation so these aren't spammed during physics updates.
/// </remarks>
void RunPhysicsDeferred();
IEnumerable<ITransformComponent> Children { get; }
int ChildCount { get; }
IEnumerable<EntityUid> ChildEntityUids { get; }

108
Robust.Shared/GameObjects/Components/Transform/TransformComponent.cs
View File

@@ -29,18 +29,13 @@ namespace Robust.Shared.GameObjects
private Vector2 _prevPosition;
private Angle _prevRotation;
// Cache changes so we can distribute them after physics is done (better cache)
private EntityCoordinates? _oldCoords;
private Angle? _oldLocalRotation;
public bool UpdatesDeferred => _oldCoords != null || _oldLocalRotation != null;
[ViewVariables(VVAccess.ReadWrite)]
public bool ActivelyLerping { get; set; }
[ViewVariables] private readonly SortedSet<EntityUid> _children = new();
[Dependency] private readonly IMapManager _mapManager = default!;
[Dependency] private readonly IEntityManager _entityManager = default!;
/// <inheritdoc />
public override string Name => "Transform";
@@ -54,7 +49,6 @@ namespace Robust.Shared.GameObjects
private bool _mapIdInitialized;
public bool DeferUpdates { get; set; }
/// <inheritdoc />
[ViewVariables]
@@ -79,6 +73,13 @@ namespace Robust.Shared.GameObjects
}
}
/// <inheritdoc />
public bool DeferUpdates { get; set; }
// Deferred fields
private Angle? _oldLocalRotation;
private EntityCoordinates? _oldCoords;
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
public bool NoLocalRotation
@@ -119,6 +120,7 @@ namespace Robust.Shared.GameObjects
{
RebuildMatrices();
UpdateEntityTree();
UpdatePhysicsTree();
Owner.EntityManager.EventBus.RaiseEvent(
EventSource.Local, new RotateEvent(Owner, oldRotation, _localRotation));
}
@@ -174,7 +176,7 @@ namespace Robust.Shared.GameObjects
public EntityUid ParentUid
{
get => _parent;
set => Parent = Owner.EntityManager.GetEntity(value).Transform;
set => Parent = _entityManager.GetEntity(value).Transform;
}
/// <inheritdoc />
@@ -263,7 +265,7 @@ namespace Robust.Shared.GameObjects
if (value.EntityId != _parent)
{
var newEntity = Owner.EntityManager.GetEntity(value.EntityId);
var newEntity = _entityManager.GetEntity(value.EntityId);
AttachParent(newEntity);
}
@@ -281,6 +283,7 @@ namespace Robust.Shared.GameObjects
}
UpdateEntityTree();
UpdatePhysicsTree();
}
else
{
@@ -313,6 +316,7 @@ namespace Robust.Shared.GameObjects
{
RebuildMatrices();
UpdateEntityTree();
UpdatePhysicsTree();
Owner.EntityManager.EventBus.RaiseEvent(
EventSource.Local, new MoveEvent(Owner, oldGridPos, Coordinates));
}
@@ -323,6 +327,35 @@ namespace Robust.Shared.GameObjects
}
}
/// <inheritdoc />
public void RunPhysicsDeferred()
{
// if we resolved to (close enough) to the OG position then no update.
if ((_oldCoords == null || _oldCoords.Equals(Coordinates)) &&
(_oldLocalRotation == null || _oldLocalRotation.Equals(_localRotation)))
{
return;
}
RebuildMatrices();
UpdateEntityTree();
UpdatePhysicsTree();
if (_oldCoords != null)
{
Owner.EntityManager.EventBus.RaiseEvent(
EventSource.Local, new MoveEvent(Owner, _oldCoords.Value, Coordinates));
_oldCoords = null;
}
if (_oldLocalRotation != null)
{
Owner.EntityManager.EventBus.RaiseEvent(
EventSource.Local, new RotateEvent(Owner, _oldLocalRotation.Value, _localRotation));
_oldLocalRotation = null;
}
}
[ViewVariables]
public IEnumerable<ITransformComponent> Children =>
_children.Select(u => Owner.EntityManager.GetEntity(u).Transform);
@@ -445,33 +478,6 @@ namespace Robust.Shared.GameObjects
base.OnRemove();
}
public void RunDeferred()
{
// if we resolved to (close enough) to the OG position then no update.
if ((_oldCoords == null || _oldCoords.Equals(Coordinates)) &&
(_oldLocalRotation == null || _oldLocalRotation.Equals(_localRotation)))
{
return;
}
RebuildMatrices();
UpdateEntityTree();
if (_oldCoords != null)
{
Owner.EntityManager.EventBus.RaiseEvent(
EventSource.Local, new MoveEvent(Owner, _oldCoords.Value, Coordinates));
_oldCoords = null;
}
if (_oldLocalRotation != null)
{
Owner.EntityManager.EventBus.RaiseEvent(
EventSource.Local, new RotateEvent(Owner, _oldLocalRotation.Value, _localRotation));
_oldLocalRotation = null;
}
}
/// <summary>
/// Detaches this entity from its parent.
/// </summary>
@@ -489,7 +495,7 @@ namespace Robust.Shared.GameObjects
IEntity newMapEntity;
if (_mapManager.TryFindGridAt(mapPos, out var mapGrid))
{
newMapEntity = Owner.EntityManager.GetEntity(mapGrid.GridEntityId);
newMapEntity = _entityManager.GetEntity(mapGrid.GridEntityId);
}
else if (_mapManager.HasMapEntity(mapPos.MapId))
{
@@ -510,10 +516,7 @@ namespace Robust.Shared.GameObjects
AttachParent(newMapEntity);
// Technically we're not moving, just changing parent.
DeferUpdates = true;
WorldPosition = mapPos.Position;
DeferUpdates = false;
Dirty();
}
@@ -624,12 +627,24 @@ namespace Robust.Shared.GameObjects
private void MapIdChanged(MapId oldId)
{
IPhysicsComponent? collider;
if (oldId != MapId.Nullspace)
{
Owner.EntityManager.RemoveFromEntityTree(Owner, oldId);
_entityManager.RemoveFromEntityTree(Owner, oldId);
if (Initialized && Owner.TryGetComponent(out collider))
{
collider.RemovedFromPhysicsTree(oldId);
}
}
Owner.EntityManager.EventBus.RaiseEvent(EventSource.Local, new EntMapIdChangedMessage(Owner, oldId));
if (MapID != MapId.Nullspace && Initialized && Owner.TryGetComponent(out collider))
{
collider.AddedToPhysicsTree(MapID);
}
_entityManager.EventBus.RaiseEvent(EventSource.Local, new EntMapIdChangedMessage(Owner, oldId));
}
public void AttachParent(IEntity parent)
@@ -745,6 +760,7 @@ namespace Robust.Shared.GameObjects
Dirty();
UpdateEntityTree();
TryUpdatePhysicsTree();
}
if (nextState is TransformComponentState nextTransform)
@@ -766,7 +782,6 @@ namespace Robust.Shared.GameObjects
// Hooks for GodotTransformComponent go here.
protected virtual void SetPosition(Vector2 position)
{
// DebugTools.Assert(!float.IsNaN(position.X) && !float.IsNaN(position.Y));
_localPosition = position;
}
@@ -809,7 +824,12 @@ namespace Robust.Shared.GameObjects
_invLocalMatrix = itransMat;
}
public bool UpdateEntityTree() => Owner.EntityManager.UpdateEntityTree(Owner);
private bool TryUpdatePhysicsTree() => Initialized && UpdatePhysicsTree();
private bool UpdatePhysicsTree() =>
Owner.TryGetComponent(out IPhysicsComponent? collider) && collider.UpdatePhysicsTree();
private bool UpdateEntityTree() => _entityManager.UpdateEntityTree(Owner);
public string GetDebugString()
{

5
Robust.Shared/GameObjects/Entity.cs
View File

@@ -3,7 +3,6 @@ using System.Collections.Generic;
using System.Diagnostics.CodeAnalysis;
using System.Linq;
using Robust.Shared.Network;
using Robust.Shared.Physics;
using Robust.Shared.Prototypes;
using Robust.Shared.Timing;
using Robust.Shared.Utility;
@@ -130,7 +129,7 @@ namespace Robust.Shared.GameObjects
.OrderBy(x => x switch
{
ITransformComponent _ => 0,
IPhysBody _ => 1,
IPhysicsComponent _ => 1,
_ => int.MaxValue
});
@@ -169,7 +168,7 @@ namespace Robust.Shared.GameObjects
.OrderBy(x => x switch
{
ITransformComponent _ => 0,
IPhysBody _ => 1,
IPhysicsComponent _ => 1,
_ => int.MaxValue
});

31
Robust.Shared/GameObjects/EntityManager.cs
View File

@@ -476,9 +476,9 @@ namespace Robust.Shared.GameObjects
/// <inheritdoc />
public IEnumerable<IEntity> GetEntitiesIntersecting(IEntity entity, bool approximate = false)
{
if (entity.TryGetComponent<IPhysBody>(out var component))
if (entity.TryGetComponent<IPhysicsComponent>(out var component))
{
return GetEntitiesIntersecting(entity.Transform.MapID, component.GetWorldAABB(), approximate);
return GetEntitiesIntersecting(entity.Transform.MapID, component.WorldAABB, approximate);
}
return GetEntitiesIntersecting(entity.Transform.Coordinates, approximate);
@@ -493,9 +493,9 @@ namespace Robust.Shared.GameObjects
private static bool Intersecting(IEntity entity, Vector2 mapPosition)
{
if (entity.TryGetComponent(out IPhysBody? component))
if (entity.TryGetComponent(out IPhysicsComponent? component))
{
if (component.GetWorldAABB().Contains(mapPosition))
if (component.WorldAABB.Contains(mapPosition))
return true;
}
else
@@ -539,9 +539,9 @@ namespace Robust.Shared.GameObjects
/// <inheritdoc />
public IEnumerable<IEntity> GetEntitiesInRange(IEntity entity, float range, bool approximate = false)
{
if (entity.TryGetComponent<IPhysBody>(out var component))
if (entity.TryGetComponent<IPhysicsComponent>(out var component))
{
return GetEntitiesInRange(entity.Transform.MapID, component.GetWorldAABB(), range, approximate);
return GetEntitiesInRange(entity.Transform.MapID, component.WorldAABB, range, approximate);
}
return GetEntitiesInRange(entity.Transform.Coordinates, range, approximate);
@@ -591,11 +591,7 @@ namespace Robust.Shared.GameObjects
if (!_entityTreesPerMap.TryGetValue(mapId, out var entTree))
{
entTree = new DynamicTree<IEntity>(
GetWorldAabbFromEntity,
capacity: 16,
growthFunc: x => x == 16 ? 3840 : x + 256
);
entTree = EntityTreeFactory();
_entityTreesPerMap.Add(mapId, entTree);
}
@@ -639,13 +635,20 @@ namespace Robust.Shared.GameObjects
}
}
protected Box2 GetWorldAabbFromEntity(in IEntity ent)
private static DynamicTree<IEntity> EntityTreeFactory() =>
new(
GetWorldAabbFromEntity,
capacity: 16,
growthFunc: x => x == 16 ? 3840 : x + 256
);
protected static Box2 GetWorldAabbFromEntity(in IEntity ent)
{
if (ent.Deleted)
return new Box2(0, 0, 0, 0);
if (ent.TryGetComponent(out IPhysBody? collider))
return collider.GetWorldAABB(_mapManager);
if (ent.TryGetComponent(out IPhysicsComponent? collider))
return collider.WorldAABB;
var pos = ent.Transform.WorldPosition;
return new Box2(pos, pos);

5
Robust.Shared/GameObjects/EntityQuery.cs
View File

@@ -2,7 +2,6 @@
using System.Collections.Generic;
using System.Linq;
using JetBrains.Annotations;
using Robust.Shared.Physics;
using Robust.Shared.Utility;
namespace Robust.Shared.GameObjects
@@ -121,9 +120,9 @@ namespace Robust.Shared.GameObjects
/// <inheritdoc />
public bool Match(IEntity entity)
{
if(Entity.TryGetComponent<IPhysBody>(out var physics))
if(Entity.TryGetComponent<IPhysicsComponent>(out var physics))
{
return physics.MapID == entity.Transform.MapID && physics.GetWorldAABB().Contains(entity.Transform.WorldPosition);
return physics.MapID == entity.Transform.MapID && physics.WorldAABB.Contains(entity.Transform.WorldPosition);
}
return false;
}

18
Robust.Shared/GameObjects/EntitySystemManager.cs
View File

@@ -158,12 +158,12 @@ namespace Robust.Shared.GameObjects
private static (IEnumerable<IEntitySystem> frameUpd, IEnumerable<IEntitySystem> upd)
CalculateUpdateOrder(Dictionary<Type, IEntitySystem>.ValueCollection systems)
{
var allNodes = new List<GraphNode<IEntitySystem>>();
var typeToNode = new Dictionary<Type, GraphNode<IEntitySystem>>();
var allNodes = new List<GraphNode>();
var typeToNode = new Dictionary<Type, GraphNode>();
foreach (var system in systems)
{
var node = new GraphNode<IEntitySystem>(system);
var node = new GraphNode(system);
allNodes.Add(node);
typeToNode.Add(system.GetType(), node);
@@ -193,10 +193,10 @@ namespace Robust.Shared.GameObjects
return (frameUpdate, update);
}
internal static IEnumerable<GraphNode<T>> TopologicalSort<T>(IEnumerable<GraphNode<T>> nodes)
private static IEnumerable<GraphNode> TopologicalSort(IEnumerable<GraphNode> nodes)
{
var elems = nodes.ToDictionary(node => node,
node => new HashSet<GraphNode<T>>(node.DependsOn));
node => new HashSet<GraphNode>(node.DependsOn));
while (elems.Count > 0)
{
var elem =
@@ -331,12 +331,12 @@ namespace Robust.Shared.GameObjects
}
[DebuggerDisplay("GraphNode: {" + nameof(System) + "}")]
internal sealed class GraphNode<T>
private sealed class GraphNode
{
public readonly T System;
public readonly List<GraphNode<T>> DependsOn = new();
public readonly IEntitySystem System;
public readonly List<GraphNode> DependsOn = new();
public GraphNode(T system)
public GraphNode(IEntitySystem system)
{
System = system;
}

66
Robust.Shared/GameObjects/Systems/SharedGridTraversalSystem.cs
View File

@@ -1,66 +0,0 @@
using System.Collections.Generic;
using Robust.Shared.Containers;
using Robust.Shared.IoC;
using Robust.Shared.Map;
namespace Robust.Shared.GameObjects
{
/// <summary>
/// Handles moving entities between grids as they move around.
/// </summary>
internal sealed class SharedGridTraversalSystem : EntitySystem
{
[Dependency] private readonly IMapManager _mapManager = default!;
private Queue<MoveEvent> _queuedMoveEvents = new();
public override void Initialize()
{
base.Initialize();
SubscribeLocalEvent<MoveEvent>(QueueMoveEvent);
}
public override void Shutdown()
{
base.Shutdown();
UnsubscribeLocalEvent<MoveEvent>();
}
public override void Update(float frameTime)
{
base.Update(frameTime);
while (_queuedMoveEvents.Count > 0)
{
var moveEvent = _queuedMoveEvents.Dequeue();
var entity = moveEvent.Sender;
if (entity.Deleted || !entity.HasComponent<PhysicsComponent>() || entity.IsInContainer()) continue;
var transform = entity.Transform;
// Change parent if necessary
// Given islands will probably have a bunch of static bodies in them then we'll verify velocities first as it's way cheaper
// This shoouullddnnn'''tt de-parent anything in a container because none of that should have physics applied to it.
if (_mapManager.TryFindGridAt(transform.MapID, moveEvent.NewPosition.ToMapPos(EntityManager), out var grid) &&
grid.GridEntityId.IsValid() &&
grid.GridEntityId != entity.Uid)
{
// Also this may deparent if 2 entities are parented but not using containers so fix that
if (grid.GridEntityId != transform.ParentUid)
{
transform.AttachParent(EntityManager.GetEntity(grid.GridEntityId));
}
}
else
{
transform.AttachParent(_mapManager.GetMapEntity(transform.MapID));
}
}
}
private void QueueMoveEvent(MoveEvent moveEvent)
{
_queuedMoveEvents.Enqueue(moveEvent);
}
}
}

620
Robust.Shared/GameObjects/Systems/SharedPhysicsSystem.cs
View File

@@ -1,241 +1,106 @@
using System;
using System.Collections.Generic;
using System.Linq;
using Robust.Server.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Containers;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Controllers;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Reflection;
using Robust.Shared.Random;
using Robust.Shared.Timing;
using DependencyAttribute = Robust.Shared.IoC.DependencyAttribute;
using Logger = Robust.Shared.Log.Logger;
namespace Robust.Shared.GameObjects
{
public abstract class SharedPhysicsSystem : EntitySystem
{
/*
* TODO:
* Port acruid's box solver in to reduce allocs for building manifolds (this one is important for perf to remove the disgusting ctors and casts)
* Raycasts for non-box shapes.
* SetTransformIgnoreContacts for teleports (and anything else left on the physics body in Farseer)
* Actual center of mass for shapes (currently just assumes center coordinate)
* Circle offsets to entity.
* TOI Solver (continuous collision detection)
* Poly cutting
* Chain shape
* (Content) grenade launcher grenades that explode after time rather than impact.
*/
/*
* Multi-threading notes:
* Sources:
* https://github.com/VelcroPhysics/VelcroPhysics/issues/29
* Aether2D
* Rapier
* https://www.slideshare.net/takahiroharada/solver-34909157
*
* SO essentially what we should look at doing from what I can discern:
* Build islands sequentially and then solve them all in parallel (as static bodies are the only thing shared
* it should be okay given they're never written to)
* After this, we can then look at doing narrowphase in parallel maybe (at least Aether2D does it) +
* position constraints in parallel + velocity constraints in parallel
*
* The main issue to tackle is graph colouring; Aether2D just seems to use locks for the parallel constraints solver
* though rapier has a graph colouring implementation (and because of this we should be able to avoid using locks) which we could try using.
*
* Given the kind of game SS14 is (our target game I guess) parallelising the islands will probably be the biggest benefit.
*/
[Dependency] private readonly ITileDefinitionManager _tileDefinitionManager = default!;
[Dependency] private readonly IMapManager _mapManager = default!;
[Dependency] private readonly IPhysicsManager _physicsManager = default!;
[Dependency] private readonly IRobustRandom _random = default!;
[Dependency] private readonly IGameTiming _timing = default!;
public IReadOnlyDictionary<MapId, PhysicsMap> Maps => _maps;
private Dictionary<MapId, PhysicsMap> _maps = new();
private const float Epsilon = 1.0e-6f;
internal IReadOnlyList<VirtualController> Controllers => _controllers;
private List<VirtualController> _controllers = new();
private readonly List<Manifold> _collisionCache = new();
// TODO: Stoer all the controllers here akshully
/// <summary>
/// Physics objects that are awake and usable for world simulation.
/// </summary>
private readonly HashSet<IPhysicsComponent> _awakeBodies = new();
/// <summary>
/// Physics objects that are awake and predicted and usable for world simulation.
/// </summary>
private readonly HashSet<IPhysicsComponent> _predictedAwakeBodies = new();
/// <summary>
/// VirtualControllers on applicable <see cref="IPhysicsComponent"/>s
/// </summary>
private Dictionary<IPhysicsComponent, IEnumerable<VirtualController>> _controllers =
new();
// We'll defer changes to IPhysicsComponent until each step is done.
private readonly List<IPhysicsComponent> _queuedDeletions = new();
private readonly List<IPhysicsComponent> _queuedUpdates = new();
/// <summary>
/// Updates to EntityTree etc. that are deferred until the end of physics.
/// </summary>
private readonly HashSet<IPhysicsComponent> _deferredUpdates = new();
// CVars aren't replicated to client (yet) so not using a cvar server-side for this.
private float _speedLimit = 30.0f;
public override void Initialize()
{
base.Initialize();
// Having a nullspace map just makes a bunch of code easier, we just don't iterate on it.
var nullMap = new PhysicsMap(MapId.Nullspace);
_maps[MapId.Nullspace] = nullMap;
nullMap.Initialize();
_mapManager.MapCreated += HandleMapCreated;
_mapManager.MapDestroyed += HandleMapDestroyed;
SubscribeLocalEvent<PhysicsUpdateMessage>(HandlePhysicsUpdateMessage);
SubscribeLocalEvent<PhysicsWakeMessage>(HandleWakeMessage);
SubscribeLocalEvent<PhysicsSleepMessage>(HandleSleepMessage);
SubscribeLocalEvent<EntMapIdChangedMessage>(HandleMapChange);
SubscribeLocalEvent<EntInsertedIntoContainerMessage>(HandleContainerInserted);
SubscribeLocalEvent<EntRemovedFromContainerMessage>(HandleContainerRemoved);
BuildControllers();
Logger.DebugS("physics", $"Found {_controllers.Count} physics controllers.");
}
private void BuildControllers()
{
var reflectionManager = IoCManager.Resolve<IReflectionManager>();
var typeFactory = IoCManager.Resolve<IDynamicTypeFactory>();
var allControllerTypes = new List<Type>();
foreach (var type in reflectionManager.GetAllChildren(typeof(VirtualController)))
{
if (type.IsAbstract) continue;
allControllerTypes.Add(type);
}
var instantiated = new Dictionary<Type, VirtualController>();
foreach (var type in allControllerTypes)
{
instantiated.Add(type, (VirtualController) typeFactory.CreateInstance(type));
}
// Build dependency graph, copied from EntitySystemManager *COUGH
var nodes = new Dictionary<Type, EntitySystemManager.GraphNode<VirtualController>>();
foreach (var (_, controller) in instantiated)
{
var node = new EntitySystemManager.GraphNode<VirtualController>(controller);
nodes[controller.GetType()] = node;
}
foreach (var (type, node) in nodes)
{
foreach (var before in instantiated[type].UpdatesBefore)
{
nodes[before].DependsOn.Add(node);
}
foreach (var after in instantiated[type].UpdatesAfter)
{
node.DependsOn.Add(nodes[after]);
}
}
_controllers = GameObjects.EntitySystemManager.TopologicalSort(nodes.Values).Select(c => c.System).ToList();
foreach (var controller in _controllers)
{
controller.Initialize();
}
}
public override void Shutdown()
{
base.Shutdown();
_mapManager.MapCreated -= HandleMapCreated;
_mapManager.MapDestroyed -= HandleMapDestroyed;
UnsubscribeLocalEvent<PhysicsUpdateMessage>();
UnsubscribeLocalEvent<PhysicsWakeMessage>();
UnsubscribeLocalEvent<PhysicsSleepMessage>();
UnsubscribeLocalEvent<EntMapIdChangedMessage>();
UnsubscribeLocalEvent<EntInsertedIntoContainerMessage>();
UnsubscribeLocalEvent<EntRemovedFromContainerMessage>();
}
private void HandleMapCreated(object? sender, MapEventArgs eventArgs)
{
// Server just creates nullspace map on its own but sends it to client hence we will just ignore it.
if (_maps.ContainsKey(eventArgs.Map)) return;
var map = new PhysicsMap(eventArgs.Map);
_maps.Add(eventArgs.Map, map);
map.Initialize();
Logger.DebugS("physics", $"Created physics map for {eventArgs.Map}");
}
private void HandleMapDestroyed(object? sender, MapEventArgs eventArgs)
{
_maps.Remove(eventArgs.Map);
Logger.DebugS("physics", $"Destroyed physics map for {eventArgs.Map}");
}
private void HandleMapChange(EntMapIdChangedMessage message)
{
if (!message.Entity.TryGetComponent(out PhysicsComponent? physicsComponent))
return;
var oldMapId = message.OldMapId;
if (oldMapId != MapId.Nullspace)
{
_maps[oldMapId].RemoveBody(physicsComponent);
physicsComponent.ClearJoints();
}
var newMapId = message.Entity.Transform.MapID;
if (newMapId != MapId.Nullspace)
{
_maps[newMapId].AddBody(physicsComponent);
}
}
private void HandlePhysicsUpdateMessage(PhysicsUpdateMessage message)
{
var mapId = message.Component.Owner.Transform.MapID;
if (mapId == MapId.Nullspace)
return;
if (message.Component.Deleted || !message.Component.CanCollide)
if (message.Component.Deleted || !message.Component.Awake)
{
_maps[mapId].RemoveBody(message.Component);
_queuedDeletions.Add(message.Component);
}
else
{
_maps[mapId].AddBody(message.Component);
_queuedUpdates.Add(message.Component);
}
}
private void HandleWakeMessage(PhysicsWakeMessage message)
/// <summary>
/// Process the changes to cached <see cref="IPhysicsComponent"/>s
/// </summary>
private void ProcessQueue()
{
var mapId = message.Body.Owner.Transform.MapID;
// At this stage only the dynamictree cares about asleep bodies
// Implicitly awake bodies so don't need to check .Awake again
// Controllers should wake their body up (inside)
foreach (var physics in _queuedUpdates)
{
if (physics.Predict)
_predictedAwakeBodies.Add(physics);
if (mapId == MapId.Nullspace)
return;
_awakeBodies.Add(physics);
_maps[mapId].AddAwakeBody(message.Body);
}
if (physics.Controllers.Count > 0 && !_controllers.ContainsKey(physics))
_controllers.Add(physics, physics.Controllers.Values);
private void HandleSleepMessage(PhysicsSleepMessage message)
{
var mapId = message.Body.Owner.Transform.MapID;
}
if (mapId == MapId.Nullspace)
return;
_queuedUpdates.Clear();
_maps[mapId].RemoveSleepBody(message.Body);
}
foreach (var physics in _queuedDeletions)
{
// If an entity was swapped from awake -> sleep -> awake then it's still relevant.
if (!physics.Deleted && physics.Awake) continue;
_awakeBodies.Remove(physics);
_predictedAwakeBodies.Remove(physics);
_controllers.Remove(physics);
}
private void HandleContainerInserted(EntInsertedIntoContainerMessage message)
{
if (!message.Entity.TryGetComponent(out PhysicsComponent? physicsComponent)) return;
var mapId = message.Container.Owner.Transform.MapID;
_maps[mapId].RemoveBody(physicsComponent);
}
private void HandleContainerRemoved(EntRemovedFromContainerMessage message)
{
if (!message.Entity.TryGetComponent(out PhysicsComponent? physicsComponent)) return;
var mapId = message.Container.Owner.Transform.MapID;
_maps[mapId].AddBody(physicsComponent);
_queuedDeletions.Clear();
}
/// <summary>
@@ -245,28 +110,361 @@ namespace Robust.Shared.GameObjects
/// <param name="prediction">Should only predicted entities be considered in this simulation step?</param>
protected void SimulateWorld(float deltaTime, bool prediction)
{
foreach (var controller in _controllers)
var simulatedBodies = prediction ? _predictedAwakeBodies : _awakeBodies;
ProcessQueue();
foreach (var body in simulatedBodies)
{
controller.UpdateBeforeSolve(prediction, deltaTime);
// running prediction updates will not cause a body to go to sleep.
if(!prediction)
body.SleepAccumulator++;
// if the body cannot move, nothing to do here
if(!body.CanMove())
continue;
var linearVelocity = Vector2.Zero;
foreach (var controller in body.Controllers.Values)
{
controller.UpdateBeforeProcessing();
linearVelocity += controller.LinearVelocity;
}
// i'm not sure if this is the proper way to solve this, but
// these are not kinematic bodies, so we need to preserve the previous
// velocity.
//if (body.LinearVelocity.LengthSquared < linearVelocity.LengthSquared)
body.LinearVelocity = linearVelocity;
// Integrate forces
body.LinearVelocity += body.Force * body.InvMass * deltaTime;
body.AngularVelocity += body.Torque * body.InvI * deltaTime;
// forces are instantaneous, so these properties are cleared
// once integrated. If you want to apply a continuous force,
// it has to be re-applied every tick.
body.Force = Vector2.Zero;
body.Torque = 0f;
}
foreach (var (mapId, map) in _maps)
// Calculate collisions and store them in the cache
ProcessCollisions(_awakeBodies);
// Remove all entities that were deleted during collision handling
ProcessQueue();
// Process frictional forces
foreach (var physics in _awakeBodies)
{
if (mapId == MapId.Nullspace) continue;
map.Step(deltaTime, prediction);
ProcessFriction(physics, deltaTime);
}
foreach (var controller in _controllers)
foreach (var (_, controllers) in _controllers)
{
controller.UpdateAfterSolve(prediction, deltaTime);
foreach (var controller in controllers)
{
controller.UpdateAfterProcessing();
}
}
// Go through and run all of the deferred events now
foreach (var (mapId, map) in _maps)
// Remove all entities that were deleted due to the controller
ProcessQueue();
const int solveIterationsAt60 = 4;
var multiplier = deltaTime / (1f / 60);
var divisions = MathHelper.Clamp(
MathF.Round(solveIterationsAt60 * multiplier, MidpointRounding.AwayFromZero),
1,
20
);
if (_timing.InSimulation) divisions = 1;
for (var i = 0; i < divisions; i++)
{
if (mapId == MapId.Nullspace) continue;
map.ProcessQueue();
foreach (var physics in simulatedBodies)
{
if (physics.CanMove())
{
UpdatePosition(physics, deltaTime / divisions);
}
}
for (var j = 0; j < divisions; ++j)
{
if (FixClipping(_collisionCache, divisions))
{
break;
}
}
}
// As we also defer the updates for the _collisionCache we need to update all entities
foreach (var physics in _deferredUpdates)
{
var transform = physics.Owner.Transform;
transform.DeferUpdates = false;
transform.RunPhysicsDeferred();
}
_deferredUpdates.Clear();
}
// Runs collision behavior and updates cache
private void ProcessCollisions(IEnumerable<IPhysicsComponent> awakeBodies)
{
_collisionCache.Clear();
var combinations = new HashSet<(EntityUid, EntityUid)>();
foreach (var aPhysics in awakeBodies)
{
foreach (var b in _physicsManager.GetCollidingEntities(aPhysics, Vector2.Zero, false))
{
var aUid = aPhysics.Entity.Uid;
var bUid = b.Uid;
if (bUid.CompareTo(aUid) > 0)
{
var tmpUid = bUid;
bUid = aUid;
aUid = tmpUid;
}
if (!combinations.Add((aUid, bUid)))
{
continue;
}
var bPhysics = b.GetComponent<IPhysicsComponent>();
_collisionCache.Add(new Manifold(aPhysics, bPhysics, aPhysics.Hard && bPhysics.Hard));
}
}
var counter = 0;
if (_collisionCache.Count > 0)
{
while(GetNextCollision(_collisionCache, counter, out var collision))
{
collision.A.WakeBody();
collision.B.WakeBody();
counter++;
var impulse = _physicsManager.SolveCollisionImpulse(collision);
if (collision.A.CanMove())
{
collision.A.ApplyImpulse(-impulse);
}
if (collision.B.CanMove())
{
collision.B.ApplyImpulse(impulse);
}
}
}
var collisionsWith = new Dictionary<ICollideBehavior, int>();
foreach (var collision in _collisionCache)
{
// Apply onCollide behavior
foreach (var behavior in collision.A.Entity.GetAllComponents<ICollideBehavior>().ToArray())
{
var entity = collision.B.Entity;
if (entity.Deleted) break;
behavior.CollideWith(entity);
if (collisionsWith.ContainsKey(behavior))
{
collisionsWith[behavior] += 1;
}
else
{
collisionsWith[behavior] = 1;
}
}
foreach (var behavior in collision.B.Entity.GetAllComponents<ICollideBehavior>().ToArray())
{
var entity = collision.A.Entity;
if (entity.Deleted) break;
behavior.CollideWith(entity);
if (collisionsWith.ContainsKey(behavior))
{
collisionsWith[behavior] += 1;
}
else
{
collisionsWith[behavior] = 1;
}
}
}
foreach (var behavior in collisionsWith.Keys)
{
behavior.PostCollide(collisionsWith[behavior]);
}
}
private bool GetNextCollision(IReadOnlyList<Manifold> collisions, int counter, out Manifold collision)
{
// The *4 is completely arbitrary
if (counter > collisions.Count * 4)
{
collision = default;
return false;
}
var offset = _random.Next(collisions.Count - 1);
for (var i = 0; i < collisions.Count; i++)
{
var index = (i + offset) % collisions.Count;
if (collisions[index].Unresolved)
{
collision = collisions[index];
return true;
}
}
collision = default;
return false;
}
private void ProcessFriction(IPhysicsComponent body, float deltaTime)
{
if (body.LinearVelocity == Vector2.Zero) return;
// sliding friction coefficient, and current gravity at current location
var (friction, gravity) = GetFriction(body);
// friction between the two objects
var effectiveFriction = friction * body.Friction;
// current acceleration due to friction
var fAcceleration = effectiveFriction * gravity;
// integrate acceleration
var fVelocity = fAcceleration * deltaTime;
// Clamp friction because friction can't make you accelerate backwards
friction = Math.Min(fVelocity, body.LinearVelocity.Length);
if (friction == 0.0f)
{
return;
}
// No multiplication/division by mass here since that would be redundant.
var frictionVelocityChange = body.LinearVelocity.Normalized * -friction;
body.LinearVelocity += frictionVelocityChange;
}
private void UpdatePosition(IPhysicsComponent physics, float frameTime)
{
var ent = physics.Entity;
if (!physics.CanMove() || (physics.LinearVelocity.LengthSquared < Epsilon && MathF.Abs(physics.AngularVelocity) < Epsilon))
return;
if (physics.LinearVelocity != Vector2.Zero)
{
if (ent.IsInContainer())
{
var relayEntityMoveMessage = new RelayMovementEntityMessage(ent);
ent.Transform.Parent!.Owner.SendMessage(ent.Transform, relayEntityMoveMessage);
// This prevents redundant messages from being sent if solveIterations > 1 and also simulates the entity "colliding" against the locker door when it opens.
physics.LinearVelocity = Vector2.Zero;
}
}
physics.Owner.Transform.DeferUpdates = true;
_deferredUpdates.Add(physics);
// Slow zone up in here
if (physics.LinearVelocity.Length > _speedLimit)
physics.LinearVelocity = physics.LinearVelocity.Normalized * _speedLimit;
var newPosition = physics.WorldPosition + physics.LinearVelocity * frameTime;
var owner = physics.Owner;
var transform = owner.Transform;
// Change parent if necessary
if (!owner.IsInContainer())
{
// This shoouullddnnn'''tt de-parent anything in a container because none of that should have physics applied to it.
if (_mapManager.TryFindGridAt(owner.Transform.MapID, newPosition, out var grid) &&
grid.GridEntityId.IsValid() &&
grid.GridEntityId != owner.Uid)
{
if (grid.GridEntityId != transform.ParentUid)
transform.AttachParent(owner.EntityManager.GetEntity(grid.GridEntityId));
}
else
{
transform.AttachParent(_mapManager.GetMapEntity(transform.MapID));
}
}
physics.WorldRotation += physics.AngularVelocity * frameTime;
physics.WorldPosition = newPosition;
}
// Based off of Randy Gaul's ImpulseEngine code
// https://github.com/RandyGaul/ImpulseEngine/blob/5181fee1648acc4a889b9beec8e13cbe7dac9288/Manifold.cpp#L123a
private bool FixClipping(List<Manifold> collisions, float divisions)
{
const float allowance = 1 / 128.0f;
var percent = MathHelper.Clamp(0.4f / divisions, 0.01f, 1f);
var done = true;
foreach (var collision in collisions)
{
if (!collision.Hard)
{
continue;
}
if (collision.A.Owner.Deleted || collision.B.Owner.Deleted)
continue;
var penetration = _physicsManager.CalculatePenetration(collision.A, collision.B);
if (penetration <= allowance)
continue;
done = false;
//var correction = collision.Normal * Math.Abs(penetration) * percent;
var correction = collision.Normal * Math.Max(penetration - allowance, 0.0f) / (collision.A.InvMass + collision.B.InvMass) * percent;
if (collision.A.CanMove())
{
collision.A.Owner.Transform.DeferUpdates = true;
_deferredUpdates.Add(collision.A);
collision.A.Owner.Transform.WorldPosition -= correction * collision.A.InvMass;
}
if (collision.B.CanMove())
{
collision.B.Owner.Transform.DeferUpdates = true;
_deferredUpdates.Add(collision.B);
collision.B.Owner.Transform.WorldPosition += correction * collision.B.InvMass;
}
}
return done;
}
private (float friction, float gravity) GetFriction(IPhysicsComponent body)
{
if (!body.OnGround)
return (0f, 0f);
var location = body.Owner.Transform;
var grid = _mapManager.GetGrid(location.Coordinates.GetGridId(EntityManager));
var tile = grid.GetTileRef(location.Coordinates);
var tileDef = _tileDefinitionManager[tile.Tile.TypeId];
return (tileDef.Friction, grid.HasGravity ? 9.8f : 0f);
}
}
}

4
Robust.Shared/Map/MapManager.cs
View File

@@ -6,8 +6,6 @@ using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Log;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Timing;
using Robust.Shared.Utility;
@@ -448,7 +446,7 @@ namespace Robust.Shared.Map
var collideComp = newEnt.AddComponent<PhysicsComponent>();
collideComp.CanCollide = true;
collideComp.AddFixture(new Fixture(collideComp, new PhysShapeGrid(grid)) {CollisionMask = MapGridHelpers.CollisionGroup, CollisionLayer = MapGridHelpers.CollisionGroup});
collideComp.PhysicsShapes.Add(new PhysShapeGrid(grid));
newEnt.Transform.AttachParent(_entityManager.GetEntity(_mapEntities[currentMapID]));

55
Robust.Shared/Map/PhysShapeGrid.cs
View File

@@ -1,5 +1,4 @@
using System;
using Robust.Shared.Configuration;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
@@ -14,32 +13,33 @@ namespace Robust.Shared.Map
[Serializable, NetSerializable]
public class PhysShapeGrid : IPhysShape
{
public int ChildCount => 1;
public Box2 LocalBounds => _mapGrid.LocalBounds;
/// <summary>
/// The radius of this AABB
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float Radius
{
get => _radius;
set
{
if (MathHelper.CloseTo(_radius, value)) return;
_radius = value;
}
}
private float _radius;
public ShapeType ShapeType => ShapeType.Polygon;
private GridId _gridId;
[NonSerialized]
private IMapGridInternal _mapGrid = default!;
/// <inheritdoc />
/// <remarks>
/// The collision layer of a grid physics shape cannot be changed.
/// </remarks>
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionLayer
{
get => MapGridHelpers.CollisionGroup;
set { }
}
/// <inheritdoc />
/// <remarks>
/// The collision mask of a grid physics shape cannot be changed.
/// </remarks>
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionMask
{
get => MapGridHelpers.CollisionGroup;
set { }
}
/// <inheritdoc />
public void ApplyState()
{
@@ -91,8 +91,6 @@ namespace Robust.Shared.Map
var mapMan = IoCManager.Resolve<IMapManager>();
_mapGrid = (IMapGridInternal)mapMan.GetGrid(_gridId);
}
_radius = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.PolygonRadius);
}
public event Action? OnDataChanged { add { } remove { } }
@@ -100,14 +98,7 @@ namespace Robust.Shared.Map
/// <inheritdoc />
public Box2 CalculateLocalBounds(Angle rotation)
{
return new Box2Rotated(_mapGrid.LocalBounds, rotation).CalcBoundingBox().Scale(1 + Radius);
}
public bool Equals(IPhysShape? other)
{
if (other is not PhysShapeGrid otherGrid) return false;
return MathHelper.CloseTo(_radius, otherGrid._radius) &&
_gridId == otherGrid._gridId;
return new Box2Rotated(_mapGrid.LocalBounds, rotation).CalcBoundingBox();
}
}
}

12
Robust.Shared/Physics/B2DynamicTree.cs
View File

@@ -26,7 +26,6 @@ using System.Diagnostics;
using System.Diagnostics.CodeAnalysis;
using System.Runtime.CompilerServices;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Utility;
using Vector2 = Robust.Shared.Maths.Vector2;
@@ -369,17 +368,6 @@ namespace Robust.Shared.Physics
return _nodes[proxy].UserData;
}
/// <summary>
/// Get the fat AABB for a proxy.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <param name="fatAABB">The fat AABB.</param>
public void GetFatAABB(Proxy proxy, out Box2 fatAABB)
{
DebugTools.Assert(0 <= proxy && proxy < Capacity);
fatAABB = _nodes[proxy].Aabb;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public bool WasMoved(Proxy proxy)
{

15
Robust.Shared/Physics/BodyType.cs
View File

@@ -10,20 +10,27 @@ namespace Robust.Shared.Physics
public enum BodyType : byte
{
/// <summary>
/// Kinematic objects have to be moved manually and have their forces reset every tick.
/// Will not be processed by the collision system. Basically "out of phase" with the world.
/// They will not raise collision events. Forces are still applied to the body, and it can move.
/// </summary>
Kinematic = 0,
None,
/// <summary>
/// Static objects have infinite mass and cannot be moved by forces or collisions. They are solid,
/// will collide with other objects, and raise collision events. This is what you use for immovable level geometry.
/// </summary>
Static = 1 << 0,
Static,
/// <summary>
/// Dynamic objects will respond to collisions and forces. They will raise collision events. This is what
/// you use for movable objects in the game.
/// </summary>
Dynamic = 1 << 1,
Dynamic,
/// <summary>
/// Trigger objects cannot be moved by collisions or forces. They are not solid and won't block objects.
/// Collision events will still be raised.
/// </summary>
Trigger,
}
}

94
Robust.Shared/Physics/BroadPhase.cs Normal file
View File

@@ -0,0 +1,94 @@
using System.Collections;
using System.Collections.Generic;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics {
public sealed class BroadPhase : IBroadPhase {
private readonly DynamicTree<IPhysBody> _tree;
public BroadPhase() =>
_tree = new DynamicTree<IPhysBody>(
(in IPhysBody body) => body.WorldAABB,
capacity: 3840,
growthFunc: x => x + 256
);
public IEnumerator<IPhysBody> GetEnumerator() => _tree.GetEnumerator();
IEnumerator IEnumerable.GetEnumerator() => ((IEnumerable) _tree).GetEnumerator();
void ICollection<IPhysBody>.Add(IPhysBody item) => _tree.Add(item);
public void Clear() => _tree.Clear();
public bool Contains(IPhysBody item) => _tree.Contains(item);
public void CopyTo(IPhysBody[] array, int arrayIndex) => _tree.CopyTo(array, arrayIndex);
public bool Remove(IPhysBody item) => _tree.Remove(item);
public int Capacity => _tree.Capacity;
public int Height => _tree.Height;
public int MaxBalance => _tree.MaxBalance;
public float AreaRatio => _tree.AreaRatio;
public int Count => _tree.Count;
public bool Add(in IPhysBody item) => _tree.Add(in item);
public bool Remove(in IPhysBody item) => _tree.Remove(in item);
public bool Update(in IPhysBody item) => _tree.Update(in item);
public void QueryAabb(DynamicTree<IPhysBody>.QueryCallbackDelegate callback, Box2 aabb, bool approx = false)
{
_tree.QueryAabb(callback, aabb, approx);
}
public void QueryAabb<TState>(ref TState state, DynamicTree<IPhysBody>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx = false)
{
_tree.QueryAabb(ref state, callback, aabb, approx);
}
public IEnumerable<IPhysBody> QueryAabb(Box2 aabb, bool approx = false)
{
return _tree.QueryAabb(aabb, approx);
}
public void QueryPoint(DynamicTree<IPhysBody>.QueryCallbackDelegate callback, Vector2 point,
bool approx = false)
{
_tree.QueryPoint(callback, point, approx);
}
public void QueryPoint<TState>(ref TState state, DynamicTree<IPhysBody>.QueryCallbackDelegate<TState> callback,
Vector2 point, bool approx = false)
{
_tree.QueryPoint(ref state, callback, point, approx);
}
public IEnumerable<IPhysBody> QueryPoint(Vector2 point, bool approx = false)
{
return _tree.QueryPoint(point, approx);
}
public void QueryRay(DynamicTree<IPhysBody>.RayQueryCallbackDelegate callback, in Ray ray, bool approx = false) =>
_tree.QueryRay(callback, ray, approx);
public void QueryRay<TState>(ref TState state, DynamicTree<IPhysBody>.RayQueryCallbackDelegate<TState> callback, in Ray ray,
bool approx = false)
{
_tree.QueryRay(ref state, callback, ray, approx);
}
public bool IsReadOnly => _tree.IsReadOnly;
}
}

352
Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs
View File

@@ -1,352 +0,0 @@
using System;
using System.Collections.Generic;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics;
namespace Robust.Shared.Physics.Broadphase
{
public class DynamicTreeBroadPhase : IBroadPhase
{
public MapId MapId { get; set; }
public GridId GridId { get; set; }
// TODO: DynamicTree seems slow at updates when we have large entity counts so when we have boxstation
// need to suss out whether chunking it might be useful.
private B2DynamicTree<FixtureProxy> _tree = new(capacity: 256);
private readonly DynamicTree<FixtureProxy>.ExtractAabbDelegate _extractAabb = ExtractAabbFunc;
private DynamicTree.Proxy[] _moveBuffer;
private int _moveCapacity;
private int _moveCount;
private (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[] _pairBuffer;
private int _pairCapacity;
private int _pairCount;
private int _proxyCount;
private B2DynamicTree<FixtureProxy>.QueryCallback _queryCallback;
private DynamicTree.Proxy _queryProxyId;
public DynamicTreeBroadPhase(MapId mapId, GridId gridId)
{
MapId = mapId;
GridId = gridId;
_queryCallback = QueryCallback;
_proxyCount = 0;
_pairCapacity = 16;
_pairCount = 0;
_pairBuffer = new (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[_pairCapacity];
_moveCapacity = 16;
_moveCount = 0;
_moveBuffer = new DynamicTree.Proxy[_moveCapacity];
}
private static Box2 ExtractAabbFunc(in FixtureProxy proxy)
{
return proxy.AABB;
}
public void UpdatePairs(BroadPhaseDelegate callback)
{
// Reset pair buffer
_pairCount = 0;
// Perform tree queries for all moving proxies.
for (int j = 0; j < _moveCount; ++j)
{
_queryProxyId = _moveBuffer[j];
if (_queryProxyId == DynamicTree.Proxy.Free)
{
continue;
}
// We have to query the tree with the fat AABB so that
// we don't fail to create a pair that may touch later.
Box2 fatAABB;
_tree.GetFatAABB(_queryProxyId, out fatAABB);
// Query tree, create pairs and add them pair buffer.
_tree.Query(_queryCallback, in fatAABB);
}
// Reset move buffer
_moveCount = 0;
// Sort the pair buffer to expose duplicates.
Array.Sort(_pairBuffer, 0, _pairCount);
// Send the pairs back to the client.
int i = 0;
while (i < _pairCount)
{
var primaryPair = _pairBuffer[i];
FixtureProxy userDataA = _tree.GetUserData(primaryPair.ProxyA)!;
FixtureProxy userDataB = _tree.GetUserData(primaryPair.ProxyB)!;
callback(GridId, in userDataA, in userDataB);
++i;
// Skip any duplicate pairs.
while (i < _pairCount)
{
(DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB) pair = _pairBuffer[i];
if (pair.ProxyA != primaryPair.ProxyA || pair.ProxyB != primaryPair.ProxyB)
{
break;
}
++i;
}
}
// Try to keep the tree balanced.
//_tree.Rebalance(4);
}
/// <summary>
/// This is called from DynamicTree.Query when we are gathering pairs.
/// </summary>
/// <param name="proxyId"></param>
/// <returns></returns>
private bool QueryCallback(DynamicTree.Proxy proxyId)
{
// A proxy cannot form a pair with itself.
if (proxyId == _queryProxyId)
{
return true;
}
// Grow the pair buffer as needed.
if (_pairCount == _pairCapacity)
{
(DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[] oldBuffer = _pairBuffer;
_pairCapacity *= 2;
_pairBuffer = new (DynamicTree.Proxy ProxyA, DynamicTree.Proxy ProxyB)[_pairCapacity];
Array.Copy(oldBuffer, _pairBuffer, _pairCount);
}
_pairBuffer[_pairCount].ProxyA = new DynamicTree.Proxy(Math.Min(proxyId, _queryProxyId));
_pairBuffer[_pairCount].ProxyB = new DynamicTree.Proxy(Math.Max(proxyId, _queryProxyId));
_pairCount++;
return true;
}
// TODO: Refactor to use fatAABB
/// <summary>
/// Already assumed to be within the same broadphase.
/// </summary>
/// <param name="proxyIdA"></param>
/// <param name="proxyIdB"></param>
/// <returns></returns>
public bool TestOverlap(DynamicTree.Proxy proxyIdA, DynamicTree.Proxy proxyIdB)
{
var proxyA = _tree.GetUserData(proxyIdA);
var proxyB = _tree.GetUserData(proxyIdB);
if (proxyA == null || proxyB == null) return false;
return proxyB.AABB.Intersects(proxyA.AABB);
}
public DynamicTree.Proxy AddProxy(ref FixtureProxy proxy)
{
var proxyID = _tree.CreateProxy(proxy.AABB, proxy);
_proxyCount++;
BufferMove(proxyID);
return proxyID;
}
public void MoveProxy(DynamicTree.Proxy proxy, in Box2 aabb, Vector2 displacement)
{
var buffer = _tree.MoveProxy(proxy, in aabb, displacement);
if (buffer)
{
BufferMove(proxy);
}
}
public void TouchProxy(DynamicTree.Proxy proxy)
{
BufferMove(proxy);
}
private void BufferMove(DynamicTree.Proxy proxyId)
{
if (_moveCount == _moveCapacity)
{
DynamicTree.Proxy[] oldBuffer = _moveBuffer;
_moveCapacity *= 2;
_moveBuffer = new DynamicTree.Proxy[_moveCapacity];
Array.Copy(oldBuffer, _moveBuffer, _moveCount);
}
_moveBuffer[_moveCount] = proxyId;
_moveCount++;
}
private void UnBufferMove(int proxyId)
{
for (int i = 0; i < _moveCount; ++i)
{
if (_moveBuffer[i] == proxyId)
{
_moveBuffer[i] = DynamicTree.Proxy.Free;
}
}
}
public void RemoveProxy(DynamicTree.Proxy proxy)
{
UnBufferMove(proxy);
_proxyCount--;
_tree.DestroyProxy(proxy);
}
public void QueryAABB(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback, Box2 aabb, bool approx = false)
{
QueryAabb(ref callback, EasyQueryCallback, aabb, approx);
}
public FixtureProxy? GetProxy(DynamicTree.Proxy proxy)
{
return _tree.GetUserData(proxy);
}
public void QueryAabb(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback, Box2 aabb, bool approx = false)
{
QueryAabb(ref callback, EasyQueryCallback, aabb, approx);
}
public void QueryAabb<TState>(ref TState state, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx = false)
{
var tuple = (state, _tree, callback, aabb, approx, _extractAabb);
_tree.Query(ref tuple, DelegateCache<TState>.AabbQueryState, aabb);
state = tuple.state;
}
public IEnumerable<FixtureProxy> QueryAabb(Box2 aabb, bool approx = false)
{
var list = new List<FixtureProxy>();
QueryAabb(ref list, (ref List<FixtureProxy> lst, in FixtureProxy i) =>
{
lst.Add(i);
return true;
}, aabb, approx);
return list;
}
public void QueryPoint(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback, Vector2 point, bool approx = false)
{
QueryPoint(ref callback, EasyQueryCallback, point, approx);
}
public void QueryPoint<TState>(ref TState state, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Vector2 point, bool approx = false)
{
var tuple = (state, _tree, callback, point, approx, _extractAabb);
_tree.Query(ref tuple,
(ref (TState state, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Vector2 point, bool approx, DynamicTree<FixtureProxy>.ExtractAabbDelegate extract) tuple,
DynamicTree.Proxy proxy) =>
{
var item = tuple.tree.GetUserData(proxy)!;
if (!tuple.approx)
{
var precise = tuple.extract(item);
if (!precise.Contains(tuple.point))
{
return true;
}
}
return tuple.callback(ref tuple.state, item);
}, Box2.CenteredAround(point, new Vector2(0.1f, 0.1f)));
state = tuple.state;
}
public IEnumerable<FixtureProxy> QueryPoint(Vector2 point, bool approx = false)
{
var list = new List<FixtureProxy>();
QueryPoint(ref list, (ref List<FixtureProxy> list, in FixtureProxy i) =>
{
list.Add(i);
return true;
}, point, approx);
return list;
}
private static readonly DynamicTree<FixtureProxy>.QueryCallbackDelegate<DynamicTree<FixtureProxy>.QueryCallbackDelegate> EasyQueryCallback =
(ref DynamicTree<FixtureProxy>.QueryCallbackDelegate s, in FixtureProxy v) => s(v);
public void QueryRay<TState>(ref TState state, DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<TState> callback, in Ray ray, bool approx = false)
{
var tuple = (state, callback, _tree, approx ? null : _extractAabb, ray);
_tree.RayCast(ref tuple, DelegateCache<TState>.RayQueryState, ray);
state = tuple.state;
}
private static bool AabbQueryStateCallback<TState>(ref (TState state, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx, DynamicTree<FixtureProxy>.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<TState>(ref (TState state, DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<TState> callback, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.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))
{
return true;
}
}
return tuple.callback(ref tuple.state, item, hit, distance);
}
public void QueryRay(DynamicTree<FixtureProxy>.RayQueryCallbackDelegate callback, in Ray ray, bool approx = false)
{
QueryRay(ref callback, RayQueryDelegateCallbackInst, ray, approx);
}
private static readonly DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<DynamicTree<FixtureProxy>.RayQueryCallbackDelegate> RayQueryDelegateCallbackInst = RayQueryDelegateCallback;
private static bool RayQueryDelegateCallback(ref DynamicTree<FixtureProxy>.RayQueryCallbackDelegate state, in FixtureProxy value, in Vector2 point, float distFromOrigin)
{
return state(value, point, distFromOrigin);
}
private static class DelegateCache<TState>
{
public static readonly
B2DynamicTree<FixtureProxy>.QueryCallback<(TState state, B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback, Box2 aabb, bool approx, DynamicTree<FixtureProxy>.ExtractAabbDelegate extract)> AabbQueryState =
AabbQueryStateCallback;
public static readonly
B2DynamicTree<FixtureProxy>.RayQueryCallback<(TState state, DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<TState> callback,
B2DynamicTree<FixtureProxy> tree, DynamicTree<FixtureProxy>.ExtractAabbDelegate? extract, Ray srcRay)> RayQueryState =
RayQueryStateCallback;
}
}
}

935
Robust.Shared/Physics/BroadPhase/SharedBroadPhaseSystem.cs
View File

@@ -1,935 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using Robust.Server.GameObjects;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Broadphase
{
public abstract class SharedBroadPhaseSystem : EntitySystem
{
/*
* That's right both the system implements IBroadPhase and also each grid has its own as well.
* The reason for this is other stuff should just be able to check for broadphase with no regard
* for the concept of grids, whereas internally this needs to worry about it.
*/
// Anything in a container is removed from the graph and anything removed from a container is added to the graph.
/*
* So the Box2D derivatives just use a generic "SynchronizeFixtures" method but it's kinda obtuse so
* I just made other methods (AddFixture, RefreshFixtures, etc.) that are clearer on what they're doing.
*/
[Dependency] private readonly IMapManager _mapManager = default!;
private readonly Dictionary<MapId, Dictionary<GridId, IBroadPhase>> _graph = new();
private Dictionary<IPhysBody, List<IBroadPhase>> _lastBroadPhases = new();
/// <summary>
/// Given MoveEvent and RotateEvent do the same thing we won't double up on work.
/// </summary>
private HashSet<IEntity> _handledThisTick = new();
private Queue<MoveEvent> _queuedMoveEvents = new();
private Queue<RotateEvent> _queuedRotateEvent = new();
private Queue<EntMapIdChangedMessage> _queuedMapChanges = new();
private Queue<FixtureUpdateMessage> _queuedFixtureUpdates = new();
private Queue<CollisionChangeMessage> _queuedCollisionChanges = new();
private Queue<EntInsertedIntoContainerMessage> _queuedContainerInsert = new();
private Queue<EntRemovedFromContainerMessage> _queuedContainerRemove = new();
private IEnumerable<IBroadPhase> BroadPhases()
{
foreach (var (_, grids) in _graph)
{
foreach (var (_, broad) in grids)
{
yield return broad;
}
}
}
/// <summary>
/// Gets the corresponding broadphase for this grid.
/// </summary>
/// <param name="mapId"></param>
/// <param name="gridId"></param>
/// <returns>null if broadphase already destroyed or none exists</returns>
public IBroadPhase? GetBroadPhase(MapId mapId, GridId gridId)
{
// Might be null if the grid is being instantiated.
if (mapId == MapId.Nullspace || !_graph[mapId].TryGetValue(gridId, out var grid))
return null;
return grid;
}
public ICollection<IBroadPhase> GetBroadPhases(MapId mapId)
{
return _graph[mapId].Values;
}
public IEnumerable<IBroadPhase> GetBroadPhases(PhysicsComponent body)
{
if (!_lastBroadPhases.TryGetValue(body, out var broadPhases)) return Array.Empty<IBroadPhase>();
return broadPhases;
}
// Look for now I've hardcoded grids
public IEnumerable<(IBroadPhase Broadphase, GridId GridId)> GetBroadphases(PhysicsComponent body)
{
if (!_lastBroadPhases.TryGetValue(body, out var broadPhases)) yield break;
foreach (var (_, grids) in _graph)
{
foreach (var broad in broadPhases)
{
foreach (var (gridId, broadPhase) in grids)
{
if (broad == broadPhase)
{
yield return (broadPhase, gridId);
break;
}
}
}
}
}
public bool TestOverlap(FixtureProxy proxyA, FixtureProxy proxyB)
{
// TODO: This should only ever be called on the same grid I think so maybe just assert
var mapA = proxyA.Fixture.Body.Owner.Transform.MapID;
var mapB = proxyB.Fixture.Body.Owner.Transform.MapID;
if (mapA != mapB)
return false;
return proxyA.AABB.Intersects(proxyB.AABB);
}
/// <summary>
/// Get the percentage that 2 bodies overlap. Ignores whether collision is turned on for either body.
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <returns> 0 -> 1.0f based on WorldAABB overlap</returns>
public float IntersectionPercent(PhysicsComponent bodyA, PhysicsComponent bodyB)
{
// TODO: Use actual shapes and not just the AABB?
return bodyA.GetWorldAABB(_mapManager).IntersectPercentage(bodyB.GetWorldAABB(_mapManager));
}
public override void Initialize()
{
base.Initialize();
SubscribeLocalEvent<CollisionChangeMessage>(QueueCollisionChange);
SubscribeLocalEvent<MoveEvent>(QueuePhysicsMove);
SubscribeLocalEvent<RotateEvent>(QueuePhysicsRotate);
SubscribeLocalEvent<EntMapIdChangedMessage>(QueueMapChange);
SubscribeLocalEvent<EntInsertedIntoContainerMessage>(QueueContainerInsertMessage);
SubscribeLocalEvent<EntRemovedFromContainerMessage>(QueueContainerRemoveMessage);
SubscribeLocalEvent<FixtureUpdateMessage>(QueueFixtureUpdate);
_mapManager.OnGridCreated += HandleGridCreated;
_mapManager.OnGridRemoved += HandleGridRemoval;
_mapManager.MapCreated += HandleMapCreated;
}
public override void Update(float frameTime)
{
base.Update(frameTime);
while (_queuedMoveEvents.Count > 0)
{
var moveEvent = _queuedMoveEvents.Dequeue();
// Doing this seems to fuck with tp so leave off for now I guess, it's mainly to avoid the rotate duplication
if (_handledThisTick.Contains(moveEvent.Sender)) continue;
_handledThisTick.Add(moveEvent.Sender);
if (moveEvent.Sender.Deleted || !moveEvent.Sender.TryGetComponent(out PhysicsComponent? physicsComponent)) continue;
SynchronizeFixtures(physicsComponent, moveEvent.NewPosition.ToMapPos(EntityManager) - moveEvent.OldPosition.ToMapPos(EntityManager));
}
while (_queuedRotateEvent.Count > 0)
{
var rotateEvent = _queuedRotateEvent.Dequeue();
if (_handledThisTick.Contains(rotateEvent.Sender)) continue;
_handledThisTick.Add(rotateEvent.Sender);
if (rotateEvent.Sender.Deleted || !rotateEvent.Sender.TryGetComponent(out PhysicsComponent? physicsComponent))
return;
SynchronizeFixtures(physicsComponent, Vector2.Zero);
}
_handledThisTick.Clear();
// TODO: Just call ProcessEventQueue directly?
// Manually manage queued stuff ourself given EventBus.QueueEvent happens at the same time every time
while (_queuedMapChanges.Count > 0)
{
HandleMapChange(_queuedMapChanges.Dequeue());
}
while (_queuedContainerInsert.Count > 0)
{
HandleContainerInsert(_queuedContainerInsert.Dequeue());
}
while (_queuedContainerRemove.Count > 0)
{
HandleContainerRemove(_queuedContainerRemove.Dequeue());
}
while (_queuedCollisionChanges.Count > 0)
{
var message = _queuedCollisionChanges.Dequeue();
if (message.CanCollide && !message.Body.Deleted)
{
AddBody(message.Body);
}
else
{
RemoveBody(message.Body);
}
}
while (_queuedFixtureUpdates.Count > 0)
{
var message = _queuedFixtureUpdates.Dequeue();
RefreshFixture(message.Body, message.Fixture);
}
}
public override void Shutdown()
{
base.Shutdown();
UnsubscribeLocalEvent<CollisionChangeMessage>();
UnsubscribeLocalEvent<MoveEvent>();
UnsubscribeLocalEvent<RotateEvent>();
UnsubscribeLocalEvent<EntMapIdChangedMessage>();
UnsubscribeLocalEvent<EntInsertedIntoContainerMessage>();
UnsubscribeLocalEvent<EntRemovedFromContainerMessage>();
UnsubscribeLocalEvent<FixtureUpdateMessage>();
_mapManager.OnGridCreated -= HandleGridCreated;
_mapManager.OnGridRemoved -= HandleGridRemoval;
_mapManager.MapCreated -= HandleMapCreated;
}
private void QueuePhysicsMove(MoveEvent moveEvent)
{
_queuedMoveEvents.Enqueue(moveEvent);
}
private void QueuePhysicsRotate(RotateEvent rotateEvent)
{
if (!rotateEvent.Sender.TryGetComponent(out PhysicsComponent? physicsComponent))
return;
SynchronizeFixtures(physicsComponent, Vector2.Zero);
}
private void QueueCollisionChange(CollisionChangeMessage message)
{
_queuedCollisionChanges.Enqueue(message);
}
private void QueueMapChange(EntMapIdChangedMessage message)
{
_queuedMapChanges.Enqueue(message);
}
private void QueueContainerInsertMessage(EntInsertedIntoContainerMessage message)
{
_queuedContainerInsert.Enqueue(message);
}
private void QueueContainerRemoveMessage(EntRemovedFromContainerMessage message)
{
_queuedContainerRemove.Enqueue(message);
}
private void HandleContainerInsert(EntInsertedIntoContainerMessage message)
{
if (!message.Entity.Deleted && message.Entity.TryGetComponent(out IPhysBody? physicsComponent))
{
physicsComponent.CanCollide = false;
physicsComponent.Awake = false;
}
}
private void HandleContainerRemove(EntRemovedFromContainerMessage message)
{
if (!message.Entity.Deleted && message.Entity.TryGetComponent(out IPhysBody? physicsComponent))
{
physicsComponent.CanCollide = true;
physicsComponent.Awake = true;
}
}
private void QueueFixtureUpdate(FixtureUpdateMessage message)
{
_queuedFixtureUpdates.Enqueue(message);
}
public void AddBroadPhase(PhysicsComponent body, IBroadPhase broadPhase)
{
if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
{
return;
}
if (broadPhases.Contains(broadPhase)) return;
broadPhases.Add(broadPhase);
}
/// <summary>
/// Handles map changes for bodies completely
/// </summary>
/// <param name="message"></param>
private void HandleMapChange(EntMapIdChangedMessage message)
{
if (message.Entity.Deleted ||
!message.Entity.TryGetComponent(out PhysicsComponent? body) ||
!_lastBroadPhases.TryGetValue(body, out var broadPhases))
{
return;
}
if (Get<SharedPhysicsSystem>().Maps.TryGetValue(message.OldMapId, out var oldMap))
{
oldMap.RemoveBody(body);
}
body.ClearProxies();
if (Get<SharedPhysicsSystem>().Maps.TryGetValue(message.Entity.Transform.MapID, out var newMap))
{
newMap.AddBody(body);
body.CreateProxies();
SetBroadPhases(body);
}
}
private void SetBroadPhases(IPhysBody body)
{
if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
{
broadPhases = new List<IBroadPhase>();
_lastBroadPhases[body] = broadPhases;
}
broadPhases.Clear();
foreach (var fixture in body.Fixtures)
{
foreach (var (gridId, _) in fixture.Proxies)
{
var broadPhase = GetBroadPhase(body.Owner.Transform.MapID, gridId);
if (broadPhase == null) continue;
broadPhases.Add(broadPhase);
}
}
}
private void HandleGridCreated(GridId gridId)
{
var mapId = _mapManager.GetGrid(gridId).ParentMapId;
if (!_graph.TryGetValue(mapId, out var grids))
{
grids = new Dictionary<GridId, IBroadPhase>();
_graph[mapId] = grids;
}
grids[gridId] = new DynamicTreeBroadPhase(mapId, gridId);
}
private void HandleMapCreated(object? sender, MapEventArgs eventArgs)
{
_graph[eventArgs.Map] = new Dictionary<GridId, IBroadPhase>()
{
{
GridId.Invalid,
new DynamicTreeBroadPhase(eventArgs.Map, GridId.Invalid)
}
};
}
private void HandleGridRemoval(GridId gridId)
{
foreach (var (_, grids) in _graph)
{
if (!grids.TryGetValue(gridId, out var broadPhase)) continue;
var bodyCleanup = false;
var toCleanup = new List<IPhysBody>();
// Need to cleanup every body that was touching this grid.
foreach (var (body, broadPhases) in _lastBroadPhases)
{
if (broadPhases.Contains(broadPhase))
{
toCleanup.Add(body);
}
}
foreach (var body in toCleanup)
{
RemoveBody(body);
}
grids.Remove(gridId);
foreach (var body in toCleanup)
{
AddBody(body);
}
}
}
public void AddBody(IPhysBody body)
{
if (_lastBroadPhases.ContainsKey(body))
{
return;
}
var mapId = body.Owner.Transform.MapID;
if (mapId == MapId.Nullspace)
{
_lastBroadPhases[body] = new List<IBroadPhase>();
return;
}
foreach (var fixture in body.Fixtures)
{
DebugTools.Assert(fixture.Proxies.Count == 0, "Can't add a body to broadphase when it already has proxies!");
}
var broadPhases = new List<IBroadPhase>();
_lastBroadPhases[body] = broadPhases;
body.CreateProxies(_mapManager, this);
SetBroadPhases(body);
// TODO: Remove this garbage
EntityManager.UpdateEntityTree(body.Owner);
}
public void RemoveBody(IPhysBody body)
{
if (!_lastBroadPhases.ContainsKey(body))
{
return;
}
body.ClearProxies();
// TODO: Remove after pvs refactor
if (!body.Owner.Deleted)
{
EntityManager.UpdateEntityTree(body.Owner);
}
_lastBroadPhases.Remove(body);
}
/// <summary>
/// Recreates this fixture in the relevant broadphases.
/// </summary>
/// <param name="body"></param>
/// <param name="fixture"></param>
public void RefreshFixture(PhysicsComponent body, Fixture fixture)
{
if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
{
return;
}
var mapId = body.Owner.Transform.MapID;
if (mapId == MapId.Nullspace || body.Deleted)
{
return;
}
fixture.ClearProxies(mapId, this);
fixture.CreateProxies(_mapManager, this);
// Need to update what broadphases are relevant.
SetBroadPhases(body);
}
internal void AddFixture(PhysicsComponent body, Fixture fixture)
{
// If the entity's still being initialized it might have MoveEvent called (might change in future?)
if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
{
return;
}
var mapId = body.Owner.Transform.MapID;
DebugTools.Assert(fixture.Proxies.Count == 0);
if (mapId == MapId.Nullspace || body.Deleted)
{
body.ClearProxies();
return;
}
broadPhases.Clear();
fixture.CreateProxies(_mapManager, this);
foreach (var fix in body.Fixtures)
{
foreach (var (gridId, _) in fix.Proxies)
{
var broadPhase = GetBroadPhase(mapId, gridId);
if (broadPhase == null) continue;
broadPhases.Add(broadPhase);
}
}
}
internal void RemoveFixture(PhysicsComponent body, Fixture fixture)
{
// If the entity's still being initialized it might have MoveEvent called (might change in future?)
if (!_lastBroadPhases.TryGetValue(body, out var broadPhases))
{
return;
}
var mapId = body.Owner.Transform.MapID;
fixture.ClearProxies(mapId, this);
if (mapId == MapId.Nullspace || body.Deleted)
{
body.ClearProxies();
return;
}
// Need to re-build the broadphases.
broadPhases.Clear();
foreach (var fix in body.Fixtures)
{
foreach (var (gridId, _) in fix.Proxies)
{
var broadPhase = GetBroadPhase(mapId, gridId);
if (broadPhase == null) continue;
broadPhases.Add(broadPhase);
}
}
}
/// <summary>
/// Move all of the fixtures on this body.
/// </summary>
/// <param name="body"></param>
/// <param name="displacement"></param>
private void SynchronizeFixtures(PhysicsComponent body, Vector2 displacement)
{
// If the entity's still being initialized it might have MoveEvent called (might change in future?)
if (!_lastBroadPhases.TryGetValue(body, out var oldBroadPhases))
{
return;
}
var mapId = body.Owner.Transform.MapID;
var worldAABB = body.GetWorldAABB(_mapManager);
var newBroadPhases = _mapManager
.FindGridIdsIntersecting(mapId, worldAABB, true)
.Select(gridId => GetBroadPhase(mapId, gridId))
.ToArray();
// Remove from old broadphases
foreach (var broadPhase in oldBroadPhases.ToArray())
{
if (newBroadPhases.Contains(broadPhase)) continue;
var gridId = GetGridId(broadPhase);
foreach (var fixture in body.Fixtures)
{
fixture.ClearProxies(mapId, this, gridId);
}
oldBroadPhases.Remove(broadPhase);
}
// Update retained broadphases
// TODO: These will need swept broadPhases
var offset = body.Owner.Transform.WorldPosition;
var worldRotation = body.Owner.Transform.WorldRotation;
foreach (var broadPhase in oldBroadPhases)
{
if (!newBroadPhases.Contains(broadPhase)) continue;
var gridId = GetGridId(broadPhase);
foreach (var fixture in body.Fixtures)
{
if (!fixture.Proxies.TryGetValue(gridId, out var proxies)) continue;
foreach (var proxy in proxies)
{
double gridRotation = worldRotation;
if (gridId != GridId.Invalid)
{
var grid = _mapManager.GetGrid(gridId);
offset -= grid.WorldPosition;
// TODO: Should probably have a helper for this
gridRotation = worldRotation - body.Owner.EntityManager.GetEntity(grid.GridEntityId).Transform.WorldRotation;
}
var aabb = fixture.Shape.CalculateLocalBounds(gridRotation).Translated(offset);
proxy.AABB = aabb;
broadPhase.MoveProxy(proxy.ProxyId, in aabb, displacement);
}
}
}
// Add to new broadphases
foreach (var broadPhase in newBroadPhases)
{
if (broadPhase == null || oldBroadPhases.Contains(broadPhase)) continue;
var gridId = GetGridId(broadPhase);
oldBroadPhases.Add(broadPhase);
foreach (var fixture in body.Fixtures)
{
DebugTools.Assert(!fixture.Proxies.ContainsKey(gridId));
fixture.CreateProxies(broadPhase, _mapManager, this);
}
}
}
public GridId GetGridId(IBroadPhase broadPhase)
{
foreach (var (_, grids) in _graph)
{
foreach (var (gridId, broad) in grids)
{
if (broadPhase == broad)
{
return gridId;
}
}
}
throw new InvalidOperationException("Unable to find GridId for broadPhase");
}
// This is dirty but so is a lot of other shit so it'll get refactored at some stage tm
public List<PhysicsComponent> GetAwakeBodies(MapId mapId, GridId gridId)
{
var bodies = new List<PhysicsComponent>();
var map = Get<SharedPhysicsSystem>().Maps[mapId];
foreach (var body in map.AwakeBodies)
{
if (body.Owner.Transform.GridID == gridId)
bodies.Add(body);
}
return bodies;
}
#region Queries
/// <summary>
/// Checks to see if the specified collision rectangle collides with any of the physBodies under management.
/// Also fires the OnCollide event of the first managed physBody to intersect with the collider.
/// </summary>
/// <param name="collider">Collision rectangle to check</param>
/// <param name="mapId">Map to check on</param>
/// <param name="approximate"></param>
/// <returns>true if collides, false if not</returns>
public bool TryCollideRect(Box2 collider, MapId mapId, bool approximate = true)
{
var state = (collider, mapId, found: false);
foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, collider, true))
{
var gridCollider = _mapManager.GetGrid(gridId).WorldToLocal(collider.Center);
var gridBox = collider.Translated(gridCollider);
_graph[mapId][gridId].QueryAabb(ref state, (ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) =>
{
if (proxy.Fixture.CollisionLayer == 0x0)
return true;
if (proxy.AABB.Intersects(gridBox))
{
state.found = true;
return false;
}
return true;
}, gridBox, approximate);
}
return state.found;
}
public IEnumerable<PhysicsComponent> GetCollidingEntities(PhysicsComponent body, Vector2 offset, bool approximate = true)
{
// If the body has just had its collision enabled or disabled it may not be ready yet so we'll wait a tick.
if (!body.CanCollide || body.Owner.Transform.MapID == MapId.Nullspace)
{
return Array.Empty<PhysicsComponent>();
}
// Unfortunately due to the way grids are currently created we have to queue CanCollide event changes, hence we need to do this here.
if (!_lastBroadPhases.ContainsKey(body))
{
AddBody(body);
}
var modifiers = body.Entity.GetAllComponents<ICollideSpecial>();
var entities = new List<PhysicsComponent>();
var state = (body, modifiers, entities);
foreach (var broadPhase in _lastBroadPhases[body])
{
foreach (var fixture in body.Fixtures)
{
foreach (var proxy in fixture.Proxies[GetGridId(broadPhase)])
{
broadPhase.QueryAabb(ref state,
(ref (PhysicsComponent body, IEnumerable<ICollideSpecial> modifiers, List<PhysicsComponent> entities) state,
in FixtureProxy other) =>
{
if (other.Fixture.Body.Deleted || other.Fixture.Body == body) return true;
if ((proxy.Fixture.CollisionMask & other.Fixture.CollisionLayer) == 0x0) return true;
var preventCollision = false;
var otherModifiers = other.Fixture.Body.Owner.GetAllComponents<ICollideSpecial>();
foreach (var modifier in state.modifiers)
{
preventCollision |= modifier.PreventCollide(other.Fixture.Body);
}
foreach (var modifier in otherModifiers)
{
preventCollision |= modifier.PreventCollide(body);
}
if (preventCollision)
return true;
state.entities.Add(other.Fixture.Body);
return true;
}, proxy.AABB, approximate);
}
}
}
return entities;
}
/// <summary>
/// Get all entities colliding with a certain body.
/// </summary>
/// <param name="body"></param>
/// <returns></returns>
public IEnumerable<PhysicsComponent> GetCollidingEntities(MapId mapId, in Box2 worldAABB)
{
if (mapId == MapId.Nullspace) return Array.Empty<PhysicsComponent>();
var bodies = new HashSet<PhysicsComponent>();
foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, worldAABB, true))
{
Vector2 offset;
if (gridId == GridId.Invalid)
{
offset = Vector2.Zero;
}
else
{
offset = -_mapManager.GetGrid(gridId).WorldPosition;
}
var gridBox = worldAABB.Translated(offset);
foreach (var proxy in _graph[mapId][gridId].QueryAabb(gridBox, false))
{
if (bodies.Contains(proxy.Fixture.Body)) continue;
bodies.Add(proxy.Fixture.Body);
}
}
return bodies;
}
// TODO: This will get every body but we don't need to do that
/// <summary>
/// Checks whether a body is colliding
/// </summary>
/// <param name="body"></param>
/// <param name="offset"></param>
/// <returns></returns>
public bool IsColliding(PhysicsComponent body, Vector2 offset, bool approximate)
{
return GetCollidingEntities(body, offset, approximate).Any();
}
#endregion
#region RayCast
/// <summary>
/// Casts a ray in the world, returning the first entity it hits (or all entities it hits, if so specified)
/// </summary>
/// <param name="mapId"></param>
/// <param name="ray">Ray to cast in the world.</param>
/// <param name="maxLength">Maximum length of the ray in meters.</param>
/// <param name="predicate">A predicate to check whether to ignore an entity or not. If it returns true, it will be ignored.</param>
/// <param name="returnOnFirstHit">If true, will only include the first hit entity in results. Otherwise, returns all of them.</param>
/// <returns>A result object describing the hit, if any.</returns>
public IEnumerable<RayCastResults> IntersectRayWithPredicate(MapId mapId, CollisionRay ray,
float maxLength = 50F,
Func<IEntity, bool>? predicate = null, bool returnOnFirstHit = true)
{
List<RayCastResults> results = new();
var endPoint = ray.Position + ray.Direction.Normalized * maxLength;
var rayBox = new Box2(Vector2.ComponentMin(ray.Position, endPoint),
Vector2.ComponentMax(ray.Position, endPoint));
foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, rayBox, true))
{
Vector2 offset;
if (gridId == GridId.Invalid)
{
offset = Vector2.Zero;
}
else
{
offset = _mapManager.GetGrid(gridId).WorldPosition;
}
var broadPhase = GetBroadPhase(mapId, gridId);
var gridRay = new CollisionRay(ray.Position - offset, ray.Direction, ray.CollisionMask);
// TODO: Probably need rotation when we get rotatable grids
broadPhase?.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
{
if (returnOnFirstHit && results.Count > 0) return true;
if (distFromOrigin > maxLength)
{
return true;
}
if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
{
return true;
}
if (predicate?.Invoke(proxy.Fixture.Body.Entity) == true)
{
return true;
}
// TODO: Shape raycast here
// Need to convert it back to world-space.
var result = new RayCastResults(distFromOrigin, point + offset, proxy.Fixture.Body.Entity);
results.Add(result);
EntityManager.EventBus.QueueEvent(EventSource.Local,
new DebugDrawRayMessage(
new DebugRayData(ray, maxLength, result)));
return true;
}, gridRay);
}
if (results.Count == 0)
{
EntityManager.EventBus.QueueEvent(EventSource.Local,
new DebugDrawRayMessage(
new DebugRayData(ray, maxLength, null)));
}
results.Sort((a, b) => a.Distance.CompareTo(b.Distance));
return results;
}
/// <summary>
/// Casts a ray in the world and returns the first entity it hits, or a list of all entities it hits.
/// </summary>
/// <param name="mapId"></param>
/// <param name="ray">Ray to cast in the world.</param>
/// <param name="maxLength">Maximum length of the ray in meters.</param>
/// <param name="ignoredEnt">A single entity that can be ignored by the RayCast. Useful if the ray starts inside the body of an entity.</param>
/// <param name="returnOnFirstHit">If false, will return a list of everything it hits, otherwise will just return a list of the first entity hit</param>
/// <returns>An enumerable of either the first entity hit or everything hit</returns>
public IEnumerable<RayCastResults> IntersectRay(MapId mapId, CollisionRay ray, float maxLength = 50, IEntity? ignoredEnt = null, bool returnOnFirstHit = true)
=> IntersectRayWithPredicate(mapId, ray, maxLength, entity => entity == ignoredEnt, returnOnFirstHit);
/// <summary>
/// Casts a ray in the world and returns the distance the ray traveled while colliding with entities
/// </summary>
/// <param name="mapId"></param>
/// <param name="ray">Ray to cast in the world.</param>
/// <param name="maxLength">Maximum length of the ray in meters.</param>
/// <param name="ignoredEnt">A single entity that can be ignored by the RayCast. Useful if the ray starts inside the body of an entity.</param>
/// <returns>The distance the ray traveled while colliding with entities</returns>
public float IntersectRayPenetration(MapId mapId, CollisionRay ray, float maxLength, IEntity? ignoredEnt = null)
{
var penetration = 0f;
// TODO: Just make an actual box
var rayBox = new Box2(ray.Position - maxLength, ray.Position + maxLength);
foreach (var gridId in _mapManager.FindGridIdsIntersecting(mapId, rayBox, true))
{
var offset = gridId == GridId.Invalid ? Vector2.Zero : _mapManager.GetGrid(gridId).WorldPosition;
var broadPhase = GetBroadPhase(mapId, gridId);
if (broadPhase == null) continue;
var gridRay = new CollisionRay(ray.Position - offset, ray.Direction, ray.CollisionMask);
// TODO: Probably need rotation when we get rotatable grids
broadPhase.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
{
if (distFromOrigin > maxLength || proxy.Fixture.Body.Entity == ignoredEnt) return true;
if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
{
return true;
}
if (new Ray(point + ray.Direction * proxy.AABB.Size.Length * 2, -ray.Direction).Intersects(
proxy.AABB, out _, out var exitPoint))
{
penetration += (point - exitPoint).Length;
}
return true;
}, gridRay);
}
return penetration;
}
#endregion
}
}

115
Robust.Shared/Physics/Collision/AlignedRectangle.cs
View File

@@ -1,115 +0,0 @@
using System;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// A rectangle that is always axis-aligned.
/// </summary>
[Serializable]
internal readonly struct AlignedRectangle : IEquatable<AlignedRectangle>
{
/// <summary>
/// Center point of the rectangle in world space.
/// </summary>
public readonly Vector2 Center;
/// <summary>
/// Half of the total width and height of the rectangle.
/// </summary>
public readonly Vector2 HalfExtents;
/// <summary>
/// A 1x1 unit rectangle with the origin centered on the world origin.
/// </summary>
public static readonly AlignedRectangle UnitCentered = new(new Vector2(0.5f, 0.5f));
/// <summary>
/// The lower X coordinate of the left edge of the box.
/// </summary>
public float Left => Center.X - HalfExtents.X;
/// <summary>
/// The higher X coordinate of the right edge of the box.
/// </summary>
public float Right => Center.X + HalfExtents.X;
/// <summary>
/// The lower Y coordinate of the top edge of the box.
/// </summary>
public float Bottom => Center.Y + HalfExtents.Y;
/// <summary>
/// The higher Y coordinate of the bottom of the box.
/// </summary>
public float Top => Center.Y + HalfExtents.Y;
public AlignedRectangle(Box2 box)
{
var halfWidth = box.Width / 2;
var halfHeight = box.Height / 2;
HalfExtents = new Vector2(halfWidth, halfHeight);
Center = new Vector2(box.Left + halfWidth, box.Height + halfHeight);
}
public AlignedRectangle(Vector2 halfExtents)
{
Center = default;
HalfExtents = halfExtents;
}
public AlignedRectangle(Vector2 center, Vector2 halfExtents)
{
Center = center;
HalfExtents = halfExtents;
}
/// <summary>
/// Given a point, returns the closest point to it inside the box.
/// </summary>
public Vector2 ClosestPoint(in Vector2 position)
{
// clamp the point to the border of the box
var cx = MathHelper.Clamp(position.X, Left, Right);
var cy = MathHelper.Clamp(position.Y, Bottom, Top);
return new Vector2(cx, cy);
}
#region Equality members
public bool Equals(AlignedRectangle other)
{
return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents);
}
public override bool Equals(object? obj)
{
return obj is AlignedRectangle other && Equals(other);
}
public override int GetHashCode()
{
return HashCode.Combine(Center, HalfExtents);
}
public static bool operator ==(AlignedRectangle left, AlignedRectangle right) {
return left.Equals(right);
}
public static bool operator !=(AlignedRectangle left, AlignedRectangle right) {
return !left.Equals(right);
}
#endregion
/// <summary>
/// Returns the string representation of this object.
/// </summary>
public override string ToString()
{
return $"({Left}, {Bottom}, {Right}, {Top})";
}
}
}

35
Robust.Shared/Physics/Collision/ClipVertex.cs
View File

@@ -1,35 +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 Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// Used for computing contact manifolds.
/// </summary>
internal struct ClipVertex
{
public ContactID ID;
public Vector2 V;
}
}

1496
Robust.Shared/Physics/Collision/Collision.cs
View File

File diff suppressed because it is too large Load Diff

37
Robust.Shared/Physics/Collision/DistanceInput.cs
View File

@@ -1,37 +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.
*/
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// Input for Distance.ComputeDistance().
/// You have to option to use the shape radii in the computation.
/// </summary>
internal sealed class DistanceInput
{
public DistanceProxy ProxyA = new();
public DistanceProxy ProxyB = new();
public Transform TransformA;
public Transform TransformB;
public bool UseRadii;
}
}

187
Robust.Shared/Physics/Collision/DistanceManager.cs
View File

@@ -1,187 +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 Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
{
internal static class DistanceManager
{
private const byte MaxGJKIterations = 20;
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = new SimplexCache();
/*
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKCalls;
*/
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
var saveA = new int[3];
var saveB = new int[3];
//float distanceSqr1 = Settings.MaxFloat;
// Main iteration loop.
int iter = 0;
while (iter < MaxGJKIterations)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (var i = 0; i < saveCount; ++i)
{
saveA[i] = simplex.V[i].IndexA;
saveB[i] = simplex.V[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
throw new ArgumentOutOfRangeException();
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//float distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared < float.Epsilon * float.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = simplex.V[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(Transform.MulT(input.TransformA.Quaternion2D, -d));
vertex.WA = Transform.Mul(input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexB = input.ProxyB.GetSupport(Transform.MulT(input.TransformB.Quaternion2D, d));
vertex.WB = Transform.Mul(input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
vertex.W = vertex.WB - vertex.WA;
simplex.V[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
/*
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKIters;
*/
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length;
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float rB = input.ProxyB.Radius;
if (output.Distance > rA + rB && output.Distance > float.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 normal = output.PointB - output.PointA;
normal = normal.Normalized;
output.PointA += normal * rA;
output.PointB -= normal * rB;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = (output.PointA + output.PointB) * 0.5f;
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
}
}

608
Robust.Shared/Physics/Collision/DistanceProxy.cs
View File

@@ -1,608 +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.Collections.Generic;
using System.Diagnostics;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics.Shapes;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// A distance proxy is used by the GJK algorithm.
/// It encapsulates any shape.
/// </summary>
internal sealed class DistanceProxy
{
internal float Radius;
internal List<Vector2> Vertices = new();
// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
/// <summary>
/// Initialize the proxy using the given shape. The shape
/// must remain in scope while the proxy is in use.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="index">The index.</param>
public void Set(IPhysShape shape, int index)
{
Vertices.Clear();
switch (shape.ShapeType)
{
case ShapeType.Aabb:
var aabb = (PhysShapeAabb) shape;
var bounds = aabb.LocalBounds;
Vertices.Add(bounds.BottomRight);
Vertices.Add(bounds.TopRight);
Vertices.Add(bounds.TopLeft);
Vertices.Add(bounds.BottomLeft);
Radius = aabb.Radius;
break;
case ShapeType.Circle:
PhysShapeCircle circle = (PhysShapeCircle) shape;
// TODO: Circle's position offset to entity, someday.
Vertices.Add(Vector2.Zero);
Radius = circle.Radius;
break;
case ShapeType.Polygon:
// TODO: When manifold building gets fixed replace it back with a cast.
var polygon = new PolygonShape(shape);
foreach (var vert in polygon.Vertices)
{
Vertices.Add(vert);
}
Radius = polygon.Radius;
break;
case ShapeType.Chain:
throw new NotImplementedException();
/*
ChainShape chain = (ChainShape) shape;
Debug.Assert(0 <= index && index < chain.Vertices.Count);
Vertices.Clear();
Vertices.Add(chain.Vertices[index]);
Vertices.Add(index + 1 < chain.Vertices.Count ? chain.Vertices[index + 1] : chain.Vertices[0]);
Radius = chain.Radius;
*/
break;
case ShapeType.Edge:
EdgeShape edge = (EdgeShape) shape;
Vertices.Add(edge.Vertex1);
Vertices.Add(edge.Vertex2);
Radius = edge.Radius;
break;
case ShapeType.Rectangle:
var rectangle = (PhysShapeRect) shape;
var calcedBounds = rectangle.CachedBounds;
Vertices.Add(calcedBounds.BottomRight);
Vertices.Add(calcedBounds.TopRight);
Vertices.Add(calcedBounds.TopLeft);
Vertices.Add(calcedBounds.BottomLeft);
Radius = rectangle.Radius;
break;
default:
throw new InvalidOperationException($"Invalid shapetype specified {shape.ShapeType}");
}
}
/// <summary>
/// Get the supporting vertex index in the given direction.
/// </summary>
/// <param name="direction">The direction.</param>
/// <returns></returns>
public int GetSupport(Vector2 direction)
{
int bestIndex = 0;
float bestValue = Vector2.Dot(Vertices[0], direction);
for (int i = 1; i < Vertices.Count; ++i)
{
float value = Vector2.Dot(Vertices[i], direction);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return bestIndex;
}
/// <summary>
/// Get the supporting vertex in the given direction.
/// </summary>
/// <param name="direction">The direction.</param>
/// <returns></returns>
public Vector2 GetSupportVertex(Vector2 direction)
{
int bestIndex = 0;
float bestValue = Vector2.Dot(Vertices[0], direction);
for (int i = 1; i < Vertices.Count; ++i)
{
float value = Vector2.Dot(Vertices[i], direction);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return Vertices[bestIndex];
}
}
/// <summary>
/// Output for Distance.ComputeDistance().
/// </summary>
public struct DistanceOutput
{
public float Distance;
/// <summary>
/// Number of GJK iterations used
/// </summary>
public int Iterations;
/// <summary>
/// Closest point on shapeA
/// </summary>
public Vector2 PointA;
/// <summary>
/// Closest point on shapeB
/// </summary>
public Vector2 PointB;
}
internal struct SimplexVertex
{
/// <summary>
/// Barycentric coordinate for closest point
/// </summary>
public float A;
/// <summary>
/// wA index
/// </summary>
public int IndexA;
/// <summary>
/// wB index
/// </summary>
public int IndexB;
/// <summary>
/// wB - wA
/// </summary>
public Vector2 W;
/// <summary>
/// Support point in proxyA
/// </summary>
public Vector2 WA;
/// <summary>
/// Support point in proxyB
/// </summary>
public Vector2 WB;
}
internal class Simplex
{
// Made it a class from a struct as it seemed silly to be a struct considering it's being mutated constantly.
internal int Count;
internal readonly SimplexVertex[] V = new SimplexVertex[3];
internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB)
{
DebugTools.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = V[i];
unsafe
{
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
}
Vector2 wALocal = proxyA.Vertices[v.IndexA];
Vector2 wBLocal = proxyB.Vertices[v.IndexB];
v.WA = Transform.Mul(transformA, wALocal);
v.WB = Transform.Mul(transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 0.0f;
V[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
float metric1 = cache.Metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = V[0];
v.IndexA = 0;
v.IndexB = 0;
Vector2 wALocal = proxyA.Vertices[0];
Vector2 wBLocal = proxyB.Vertices[0];
v.WA = Transform.Mul(transformA, wALocal);
v.WB = Transform.Mul(transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 1.0f;
V[0] = v;
Count = 1;
}
}
internal void WriteCache(ref SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (UInt16)Count;
for (var i = 0; i < Count; ++i)
{
unsafe
{
cache.IndexA[i] = (byte) (V[i].IndexA);
cache.IndexB[i] = (byte) (V[i].IndexB);
}
}
}
internal Vector2 GetSearchDirection()
{
switch (Count)
{
case 1:
return -V[0].W;
case 2:
{
Vector2 e12 = V[1].W - V[0].W;
float sgn = Vector2.Cross(e12, -V[0].W);
if (sgn > 0.0f)
{
// Origin is left of e12.
return new Vector2(-e12.Y, e12.X);
}
else
{
// Origin is right of e12.
return new Vector2(e12.Y, -e12.X);
}
}
default:
Debug.Assert(false);
return Vector2.Zero;
}
}
internal Vector2 GetClosestPoint()
{
switch (Count)
{
case 0:
Debug.Assert(false);
return Vector2.Zero;
case 1:
return V[0].W;
case 2:
return V[0].W * V[0].A + V[1].W * V[1].A;
case 3:
return Vector2.Zero;
default:
Debug.Assert(false);
return Vector2.Zero;
}
}
internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB)
{
switch (Count)
{
case 0:
pA = Vector2.Zero;
pB = Vector2.Zero;
Debug.Assert(false);
break;
case 1:
pA = V[0].WA;
pB = V[0].WB;
break;
case 2:
pA = V[0].WA * V[0].A + V[1].WA * V[1].A;
pB = V[0].WB * V[0].A + V[1].WB * V[1].A;
break;
case 3:
pA = V[0].WA * V[0].A + V[1].WA * V[1].A + V[2].WA * V[2].A;
pB = pA;
break;
default:
throw new Exception();
}
}
internal float GetMetric()
{
switch (Count)
{
case 0:
Debug.Assert(false);
return 0.0f;
case 1:
return 0.0f;
case 2:
return (V[0].W - V[1].W).Length;
case 3:
return Vector2.Cross(V[1].W - V[0].W, V[2].W - V[0].W);
default:
Debug.Assert(false);
return 0.0f;
}
}
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
internal void Solve2()
{
Vector2 w1 = V[0].W;
Vector2 w2 = V[1].W;
Vector2 e12 = w2 - w1;
// w1 region
float d12_2 = -Vector2.Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
SimplexVertex v0 = V[0];
v0.A = 1.0f;
V[0] = v0;
Count = 1;
return;
}
// w2 region
float d12_1 = Vector2.Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
SimplexVertex v1 = V[1];
v1.A = 1.0f;
V[1] = v1;
Count = 1;
V[0] = V[1];
return;
}
// Must be in e12 region.
float inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
internal void Solve3()
{
Vector2 w1 = V[0].W;
Vector2 w2 = V[1].W;
Vector2 w3 = V[2].W;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
Vector2 e12 = w2 - w1;
float w1e12 = Vector2.Dot(w1, e12);
float w2e12 = Vector2.Dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
Vector2 e13 = w3 - w1;
float w1e13 = Vector2.Dot(w1, e13);
float w3e13 = Vector2.Dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
Vector2 e23 = w3 - w2;
float w2e23 = Vector2.Dot(w2, e23);
float w3e23 = Vector2.Dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;
// Triangle123
float n123 = Vector2.Cross(e12, e13);
float d123_1 = n123 * Vector2.Cross(w2, w3);
float d123_2 = n123 * Vector2.Cross(w3, w1);
float d123_3 = n123 * Vector2.Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
SimplexVertex v0_1 = V[0];
v0_1.A = 1.0f;
V[0] = v0_1;
Count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
float inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
float inv_d13 = 1.0f / (d13_1 + d13_2);
SimplexVertex v0_3 = V[0];
SimplexVertex v2_3 = V[2];
v0_3.A = d13_1 * inv_d13;
v2_3.A = d13_2 * inv_d13;
V[0] = v0_3;
V[2] = v2_3;
Count = 2;
V[1] = V[2];
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
SimplexVertex v1_4 = V[1];
v1_4.A = 1.0f;
V[1] = v1_4;
Count = 1;
V[0] = V[1];
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
SimplexVertex v2_5 = V[2];
v2_5.A = 1.0f;
V[2] = v2_5;
Count = 1;
V[0] = V[2];
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
float inv_d23 = 1.0f / (d23_1 + d23_2);
SimplexVertex v1_6 = V[1];
SimplexVertex v2_6 = V[2];
v1_6.A = d23_1 * inv_d23;
v2_6.A = d23_2 * inv_d23;
V[1] = v1_6;
V[2] = v2_6;
Count = 2;
V[0] = V[2];
return;
}
// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
SimplexVertex v0_7 = V[0];
SimplexVertex v1_7 = V[1];
SimplexVertex v2_7 = V[2];
v0_7.A = d123_1 * inv_d123;
v1_7.A = d123_2 * inv_d123;
v2_7.A = d123_3 * inv_d123;
V[0] = v0_7;
V[1] = v1_7;
V[2] = v2_7;
Count = 3;
}
}
}

172
Robust.Shared/Physics/Collision/Manifold.cs
View File

@@ -1,172 +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.Runtime.InteropServices;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
{
public enum ManifoldType : byte
{
Invalid = 0,
Circles,
FaceA,
FaceB,
}
internal enum ContactFeatureType : byte
{
Vertex = 0,
Face = 1,
}
/// <summary>
/// The features that intersect to form the contact point
/// This must be 4 bytes or less.
/// </summary>
public struct ContactFeature
{
/// <summary>
/// Feature index on ShapeA
/// </summary>
public byte IndexA;
/// <summary>
/// Feature index on ShapeB
/// </summary>
public byte IndexB;
/// <summary>
/// The feature type on ShapeA
/// </summary>
public byte TypeA;
/// <summary>
/// The feature type on ShapeB
/// </summary>
public byte TypeB;
}
/// <summary>
/// Contact ids to facilitate warm starting.
/// </summary>
[StructLayout(LayoutKind.Explicit)]
public struct ContactID
{
/// <summary>
/// The features that intersect to form the contact point
/// </summary>
[FieldOffset(0)]
public ContactFeature Features;
/// <summary>
/// Used to quickly compare contact ids.
/// </summary>
[FieldOffset(0)]
public uint Key;
public static bool operator ==(ContactID id, ContactID other)
{
return id.Key == other.Key;
}
public static bool operator !=(ContactID id, ContactID other)
{
return !(id == other);
}
}
// Made a class because A) It gets mutated bloody everywhere and B) unless you're careful you'll get solver issues (yay!)
// which I really could not be fucked dealing with
internal sealed class Manifold
{
public Vector2 LocalNormal;
/// <summary>
/// Usage depends on manifold type.
/// </summary>
public Vector2 LocalPoint;
public int PointCount;
/// <summary>
/// Points of contact, can only be 0 -> 2.
/// </summary>
public ManifoldPoint[] Points = new ManifoldPoint[2];
public ManifoldType Type;
public Manifold() {}
public Manifold(Vector2 localNormal, Vector2 localPoint, int pointCount, ManifoldPoint[] points, ManifoldType type)
{
LocalNormal = localNormal;
LocalPoint = localPoint;
PointCount = pointCount;
// Do not remove this copy or shit BREAKS
Array.Copy(points, Points, PointCount);
Type = type;
}
internal Manifold Clone()
{
return new(LocalNormal, LocalPoint, PointCount, Points, Type);
}
}
public struct ManifoldPoint
{
/// <summary>
/// Unique identifier for the contact point between 2 shapes.
/// </summary>
public ContactID Id;
/// <summary>
/// Usage depends on manifold type.
/// </summary>
public Vector2 LocalPoint;
/// <summary>
/// The non-penetration impulse.
/// </summary>
public float NormalImpulse;
/// <summary>
/// Friction impulse.
/// </summary>
public float TangentImpulse;
public static bool operator ==(ManifoldPoint point, ManifoldPoint other)
{
return point.Id == other.Id &&
point.LocalPoint.Equals(other.LocalPoint) &&
point.NormalImpulse.Equals(other.NormalImpulse) &&
point.TangentImpulse.Equals(other.TangentImpulse);
}
public static bool operator !=(ManifoldPoint point, ManifoldPoint other)
{
return !(point == other);
}
}
}

213
Robust.Shared/Physics/Collision/OrientedRectangle.cs
View File

@@ -1,213 +0,0 @@
using System;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// A rectangle that can be rotated.
/// </summary>
[Serializable]
internal readonly struct OrientedRectangle : IEquatable<OrientedRectangle>
{
/// <summary>
/// Center point of the rectangle in world space.
/// </summary>
public readonly Vector2 Center;
/// <summary>
/// Half of the total width and height of the rectangle.
/// </summary>
public readonly Vector2 HalfExtents;
/// <summary>
/// World rotation of the rectangle in radians.
/// </summary>
public readonly float Rotation;
/// <summary>
/// A 1x1 unit box with the origin centered and identity rotation.
/// </summary>
public static readonly OrientedRectangle UnitCentered = new(Vector2.Zero, Vector2.One, 0);
public OrientedRectangle(Box2 worldBox)
{
Center = worldBox.Center;
var hWidth = MathF.Abs(worldBox.Right - worldBox.Left) * 0.5f;
var hHeight = MathF.Abs(worldBox.Bottom - worldBox.Top) * 0.5f;
HalfExtents = new Vector2(hWidth, hHeight);
Rotation = 0;
}
public OrientedRectangle(Vector2 halfExtents)
{
Center = default;
HalfExtents = halfExtents;
Rotation = default;
}
public OrientedRectangle(Vector2 center, Vector2 halfExtents)
{
Center = center;
HalfExtents = halfExtents;
Rotation = default;
}
public OrientedRectangle(Vector2 center, Vector2 halfExtents, float rotation)
{
Center = center;
HalfExtents = halfExtents;
Rotation = rotation;
}
public OrientedRectangle(in Vector2 center, in Box2 localBox, float rotation)
{
Center = center;
HalfExtents = new Vector2(localBox.Width / 2, localBox.Height / 2);
Rotation = rotation;
}
/// <summary>
/// calculates the smallest AABB that will encompass this rectangle. The AABB is in local space.
/// </summary>
public Box2 CalcBoundingBox()
{
var Fi = Rotation;
var CX = Center.X;
var CY = Center.Y;
var WX = HalfExtents.X;
var WY = HalfExtents.Y;
var SF = MathF.Sin(Fi);
var CF = MathF.Cos(Fi);
var NH = MathF.Abs(WX * SF) + MathF.Abs(WY * CF); //boundrect half-height
var NW = MathF.Abs(WX * CF) + MathF.Abs(WY * SF); //boundrect half-width
return new Box2((float)(CX - NW), (float)(CY - NH), (float)(CX + NW), (float)(CY + NH)); //draw bound rectangle
}
/// <summary>
/// Tests if a point is contained inside this rectangle.
/// </summary>
/// <param name="point">Point to test.</param>
/// <returns>True if the point is contained inside this rectangle.</returns>
public bool Contains(Vector2 point)
{
// rotate around rectangle center by -rectAngle
var s = MathF.Sin(-Rotation);
var c = MathF.Cos(-Rotation);
// set origin to rect center
var newPoint = point - Center;
// rotate
newPoint = new Vector2(newPoint.X * c - newPoint.Y * s, newPoint.X * s + newPoint.Y * c);
// put origin back
newPoint += Center;
// check if our transformed point is in the rectangle, which is no longer
// rotated relative to the point
var xMin = -HalfExtents.X;
var xMax = HalfExtents.X;
var yMin = -HalfExtents.Y;
var yMax = HalfExtents.Y;
return newPoint.X >= xMin && newPoint.X <= xMax && newPoint.Y >= yMin && newPoint.Y <= yMax;
}
/// <summary>
/// Returns the closest point inside the rectangle to the given point in world space.
/// </summary>
public Vector2 ClosestPointWorld(Vector2 worldPoint)
{
// inverse-transform the sphere's center into the box's local space.
var localPoint = InverseTransformPoint(worldPoint);
var xMin = -HalfExtents.X;
var xMax = HalfExtents.X;
var yMin = -HalfExtents.Y;
var yMax = HalfExtents.Y;
// clamp the point to the border of the box
var cx = MathHelper.Clamp(localPoint.X, xMin, xMax);
var cy = MathHelper.Clamp(localPoint.Y, yMin, yMax);
return TransformPoint(new Vector2(cx, cy));
}
/// <summary>
/// Transforms a point from the rectangle's local space to world space.
/// </summary>
public Vector2 TransformPoint(Vector2 localPoint)
{
var theta = Rotation;
var (x, y) = localPoint;
var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
return new Vector2(dx, dy) + Center;
}
/// <summary>
/// Transforms a point from world space to the rectangle's local space.
/// </summary>
public Vector2 InverseTransformPoint(Vector2 worldPoint)
{
var theta = -Rotation;
var (x, y) = worldPoint + -Center;
var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
return new Vector2(dx, dy);
}
#region Equality Members
/// <inheritdoc />
public bool Equals(OrientedRectangle other)
{
return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents) && Rotation.Equals(other.Rotation);
}
/// <inheritdoc />
public override bool Equals(object? obj)
{
return obj is OrientedRectangle other && Equals(other);
}
/// <inheritdoc />
public override int GetHashCode()
{
return HashCode.Combine(Center, HalfExtents, Rotation);
}
/// <summary>
/// Check for equality by value between two <see cref="OrientedRectangle"/>.
/// </summary>
public static bool operator ==(OrientedRectangle left, OrientedRectangle right) {
return left.Equals(right);
}
/// <summary>
/// Check for inequality by value between two <see cref="OrientedRectangle"/>.
/// </summary>
public static bool operator !=(OrientedRectangle left, OrientedRectangle right) {
return !left.Equals(right);
}
#endregion
/// <summary>
/// Returns the string representation of this object.
/// </summary>
public override string ToString()
{
var box = new Box2(-HalfExtents.X, -HalfExtents.Y, HalfExtents.X, HalfExtents.Y).Translated(Center);
return $"{box}, {Rotation}";
}
}
}

50
Robust.Shared/Physics/Collision/PointState.cs
View File

@@ -1,50 +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.
*/
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// This is used for determining the state of contact points.
/// </summary>
public enum PointState : byte
{
/// <summary>
/// Point does not exist
/// </summary>
Null,
/// <summary>
/// Point was added in the update
/// </summary>
Add,
/// <summary>
/// Point persisted across the update
/// </summary>
Persist,
/// <summary>
/// Point was removed in the update
/// </summary>
Remove,
}
}

48
Robust.Shared/Physics/Collision/SimplexCache.cs
View File

@@ -1,48 +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.
*/
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// Used to warm start ComputeDistance.
/// Set count to zero on first call.
/// </summary>
internal struct SimplexCache
{
/// <summary>
/// Length or area
/// </summary>
public ushort Count;
/// <summary>
/// Vertices on shape A
/// </summary>
public unsafe fixed byte IndexA[3];
/// <summary>
/// Vertices on shape B
/// </summary>
public unsafe fixed byte IndexB[3];
public float Metric;
}
}

688
Robust.Shared/Physics/CollisionSolver.cs Normal file
View File

@@ -0,0 +1,688 @@
using System;
using Robust.Shared.Map;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics
{
internal static class CollisionSolver
{
public static void CalculateFeatures(Manifold manifold, IPhysShape a, IPhysShape b, out CollisionFeatures features)
{
// 2D table of all possible PhysShape combinations
switch (a)
{
case PhysShapeCircle aCircle:
switch (b)
{
case PhysShapeCircle bCircle:
CircleCircle(manifold, aCircle, bCircle, 1, out features);
return;
case PhysShapeAabb bAabb:
CircleBox(manifold, aCircle, bAabb, 1, out features);
return;
case PhysShapeRect bRect:
RectCircle(manifold, bRect, aCircle, -1, out features);
return;
case PhysShapeGrid bGrid:
DummyBoundsFeatures(manifold, out features);
return;
}
break;
case PhysShapeAabb aAabb:
switch (b)
{
case PhysShapeCircle bCircle:
CircleBox(manifold, bCircle, aAabb, -1, out features);
return;
case PhysShapeAabb bAabb:
BoxBox(manifold, aAabb, bAabb, 1, out features);
return;
case PhysShapeRect bRect:
DummyBoundsFeatures(manifold, out features);
return;
case PhysShapeGrid bGrid:
DummyBoundsFeatures(manifold, out features);
return;
}
break;
case PhysShapeRect aRect:
switch (b)
{
case PhysShapeCircle bCircle:
RectCircle(manifold, aRect, bCircle, 1, out features);
return;
case PhysShapeAabb bAabb:
DummyBoundsFeatures(manifold, out features);
return;
case PhysShapeRect bRect:
DummyBoundsFeatures(manifold, out features);
return;
case PhysShapeGrid bGrid:
DummyBoundsFeatures(manifold, out features);
return;
}
break;
case PhysShapeGrid aGrid:
switch (b)
{
case PhysShapeCircle bCircle:
DummyBoundsFeatures(manifold, out features);
return;
case PhysShapeAabb bAabb:
DummyBoundsFeatures(manifold, out features);
return;
case PhysShapeRect bRect:
DummyBoundsFeatures(manifold, out features);
return;
case PhysShapeGrid bGrid:
DummyBoundsFeatures(manifold, out features);
return;
}
break;
}
features = default;
}
private static void DummyBoundsFeatures(Manifold manifold, out CollisionFeatures features)
{
var aRect = new AlignedRectangle(manifold.A.Entity.Transform.WorldPosition, manifold.A.AABB.Size / 2);
var bRect = new AlignedRectangle(manifold.B.Entity.Transform.WorldPosition, manifold.B.AABB.Size / 2);
CalculateCollisionFeatures(aRect, bRect, 1, out features);
}
private static void CircleCircle(Manifold manifold, PhysShapeCircle a, PhysShapeCircle b, float flip,
out CollisionFeatures features)
{
var aRad = a.Radius;
var bRad = b.Radius;
var aPos = manifold.A.Entity.Transform.WorldPosition;
var bPos = manifold.B.Entity.Transform.WorldPosition;
CalculateCollisionFeatures(new Circle(aPos, aRad), new Circle(bPos, bRad), (float) flip, out features);
}
private static void CircleBox(Manifold manifold, PhysShapeCircle a, PhysShapeAabb b, float flip,
out CollisionFeatures features)
{
var aRad = a.Radius;
var aPos = manifold.A.Entity.Transform.WorldPosition;
var bRect = new AlignedRectangle(manifold.B.Entity.Transform.WorldPosition, b.LocalBounds.Size / 2);
CalculateCollisionFeatures(bRect, new Circle(aPos, aRad), (float) flip * -1, out features);
}
private static void RectCircle(Manifold manifold, PhysShapeRect a, PhysShapeCircle b, float flip,
out CollisionFeatures features)
{
var aPos = manifold.A.Entity.Transform.WorldPosition;
var bPos = manifold.B.Entity.Transform.WorldPosition;
var aRot = (float)manifold.A.Entity.Transform.WorldRotation.Theta;
CalculateCollisionFeatures(new OrientedRectangle(aPos, a.Rectangle, aRot), new Circle(bPos, b.Radius), (float) flip, out features);
}
private static void BoxBox(Manifold manifold, PhysShapeAabb a, PhysShapeAabb b, float flip,
out CollisionFeatures features)
{
var aRect = new AlignedRectangle(manifold.A.Entity.Transform.WorldPosition, a.LocalBounds.Size / 2);
var bRect = new AlignedRectangle(manifold.B.Entity.Transform.WorldPosition, b.LocalBounds.Size / 2);
CalculateCollisionFeatures(in aRect, in bRect, flip, out features);
}
public static void CalculateCollisionFeatures(in Circle A, in Circle B, float flip, out CollisionFeatures features)
{
var aRad = A.Radius;
var bRad = B.Radius;
var aPos = A.Position;
var bPos = B.Position;
// combined radius
var radiiSum = aRad + bRad;
// distance between circles
var dist = bPos - aPos;
// if the distance between two circles is larger than their combined radii,
// they are not colliding, otherwise they are
if (dist.LengthSquared > radiiSum * radiiSum)
{
features = default;
return;
}
// if dist between circles is zero, the circles are concentric, this collision cannot be resolved
if (dist.LengthSquared.Equals(0f))
{
features = default;
return;
}
// generate collision normal
var normal = dist.Normalized;
// half of the total
var penetraction = (radiiSum - dist.Length) * 0.5f;
var contacts = new Vector2[1];
// dtp - Distance to intersection point
var dtp = aRad - penetraction;
var contact = aPos + normal * dtp;
contacts[0] = contact;
features = new CollisionFeatures(true, normal, penetraction, contacts);
}
public static void CalculateCollisionFeatures(in AlignedRectangle A, in Circle B, float flip, out CollisionFeatures features)
{
// closest point inside the rectangle to the center of the sphere.
var closestPoint = A.ClosestPoint(in B.Position);
// If the point is outside the sphere, the sphere and OBB do not intersect.
var distanceSq = (closestPoint - B.Position).LengthSquared;
if (distanceSq > B.Radius * B.Radius)
{
features = default;
return;
}
Vector2 normal;
if (distanceSq.Equals(0.0f))
{
var mSq = (closestPoint - A.Center).LengthSquared;
if (mSq.Equals(0.0f))
{
features = default;
return;
}
// Closest point is at the center of the sphere
normal = (closestPoint - A.Center).Normalized;
}
else
normal = (B.Position - closestPoint).Normalized;
var outsidePoint = B.Position - normal * B.Radius;
var distance = (closestPoint - outsidePoint).Length;
var contacts = new Vector2[1];
contacts[0] = closestPoint + (outsidePoint - closestPoint) * 0.5f;
var depth = distance * 0.5f;
features = new CollisionFeatures(true, normal, depth, contacts);
}
public static void CalculateCollisionFeatures(in OrientedRectangle A, in Circle B, float flip, out CollisionFeatures features)
{
// closest point inside the rectangle to the center of the sphere.
var closestPoint = A.ClosestPointWorld(B.Position);
// If the point is outside the sphere, the sphere and OBB do not intersect.
var distanceSq = (closestPoint - B.Position).LengthSquared;
if (distanceSq > B.Radius * B.Radius)
{
features = default;
return;
}
Vector2 normal;
if (distanceSq.Equals(0.0f))
{
var mSq = (closestPoint - A.Center).LengthSquared;
if (mSq.Equals(0.0f))
{
features = default;
return;
}
// Closest point is at the center of the sphere
normal = (closestPoint - A.Center).Normalized;
}
else
normal = (B.Position - closestPoint).Normalized;
var outsidePoint = B.Position - normal * B.Radius;
var distance = (closestPoint - outsidePoint).Length;
var contacts = new Vector2[1];
contacts[0] = closestPoint + (outsidePoint - closestPoint) * 0.5f;
var depth = distance * 0.5f;
features = new CollisionFeatures(true, normal, depth, contacts);
}
public static void CalculateCollisionFeatures(in AlignedRectangle A, in AlignedRectangle B, float flip, out CollisionFeatures features)
{
// Vector from A to B
Vector2 n = B.Center - A.Center;
// Calculate half extents along x axis for each object
float a_extent_x = A.HalfExtents.X;
float b_extent_x = B.HalfExtents.X;
// Calculate overlap on x axis
float x_overlap = a_extent_x + b_extent_x - MathF.Abs(n.X);
// SAT test on x axis
if (!(x_overlap > 0))
{
features = default;
return;
}
// Calculate half extents along y axis for each object
float a_extent_y = A.HalfExtents.Y;
float b_extent_y = B.HalfExtents.Y;
// Calculate overlap on y axis
float y_overlap = a_extent_y + b_extent_y - MathF.Abs(n.Y);
// SAT test on y axis
if (!(y_overlap > 0))
{
features = default;
return;
}
Vector2 normal;
float penetration;
Vector2 contact;
// Find out which axis is axis of least penetration
if (x_overlap < y_overlap)
{
// Point towards B knowing that n points from A to B
if (n.X < 0)
normal = new Vector2(-1, 0);
else
normal = new Vector2(1, 0);
penetration = x_overlap / 2;
var hitx = A.Center.X + (a_extent_x * normal.X);
var hity = B.Center.Y;
contact = new Vector2(hitx, hity);
}
else
{
// Point toward B knowing that n points from A to B
if (n.Y < 0)
normal = new Vector2(0, -1);
else
normal = new Vector2(0, 1);
penetration = y_overlap / 2;
var hitx = B.Center.X;
var hity = A.Center.Y + (a_extent_y * normal.Y);
contact = new Vector2(hitx, hity);
}
features = new CollisionFeatures(true, normal, penetration, new[] {contact});
}
}
/// <summary>
/// Features of the collision.
/// </summary>
internal readonly struct CollisionFeatures
{
/// <summary>
/// Are the two shapes *actually* colliding? If this is false, the rest of the
/// values in this struct are default.
/// </summary>
public readonly bool Collided;
/// <summary>
/// Collision normal. If A moves in the negative direction of the normal and
/// B moves in the positive direction, the objects will no longer intersect.
/// </summary>
public readonly Vector2 Normal;
/// <summary>
/// Half of the total depth of penetration. Each object needs to move
/// by the penetration distance along the normal to resolve the collision.
/// </summary>
/// <remarks>
/// The penetration depth is the length of the minimum translation vector (MTV), which
/// is the smallest vector along which we can translate an intersecting shape to
/// separate it from the other shape.
/// </remarks>
public readonly float Penetration;
/// <summary>
/// all the points at which the two objects collide, projected onto a plane.The plane
/// these points are projected onto has the normal of the collision normal and is
/// located halfway between the colliding objects.
/// </summary>
public readonly Vector2[] Contacts;
/// <summary>
/// Constructs a new instance of <see cref="CollisionFeatures"/>.
/// </summary>
public CollisionFeatures(bool collided, Vector2 normal, float penetration, Vector2[] contacts)
{
Collided = collided;
Normal = normal;
Penetration = penetration;
Contacts = contacts;
}
}
/// <summary>
/// A rectangle that is always axis-aligned.
/// </summary>
[Serializable]
internal readonly struct AlignedRectangle : IEquatable<AlignedRectangle>
{
/// <summary>
/// Center point of the rectangle in world space.
/// </summary>
public readonly Vector2 Center;
/// <summary>
/// Half of the total width and height of the rectangle.
/// </summary>
public readonly Vector2 HalfExtents;
/// <summary>
/// A 1x1 unit rectangle with the origin centered on the world origin.
/// </summary>
public static readonly AlignedRectangle UnitCentered = new(new Vector2(0.5f, 0.5f));
/// <summary>
/// The lower X coordinate of the left edge of the box.
/// </summary>
public float Left => Center.X - HalfExtents.X;
/// <summary>
/// The higher X coordinate of the right edge of the box.
/// </summary>
public float Right => Center.X + HalfExtents.X;
/// <summary>
/// The lower Y coordinate of the top edge of the box.
/// </summary>
public float Bottom => Center.Y + HalfExtents.Y;
/// <summary>
/// The higher Y coordinate of the bottom of the box.
/// </summary>
public float Top => Center.Y + HalfExtents.Y;
public AlignedRectangle(Box2 box)
{
var halfWidth = box.Width / 2;
var halfHeight = box.Height / 2;
HalfExtents = new Vector2(halfWidth, halfHeight);
Center = new Vector2(box.Left + halfWidth, box.Height + halfHeight);
}
public AlignedRectangle(Vector2 halfExtents)
{
Center = default;
HalfExtents = halfExtents;
}
public AlignedRectangle(Vector2 center, Vector2 halfExtents)
{
Center = center;
HalfExtents = halfExtents;
}
/// <summary>
/// Given a point, returns the closest point to it inside the box.
/// </summary>
public Vector2 ClosestPoint(in Vector2 position)
{
// clamp the point to the border of the box
var cx = MathHelper.Clamp(position.X, Left, Right);
var cy = MathHelper.Clamp(position.Y, Bottom, Top);
return new Vector2(cx, cy);
}
#region Equality members
public bool Equals(AlignedRectangle other)
{
return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents);
}
public override bool Equals(object? obj)
{
return obj is AlignedRectangle other && Equals(other);
}
public override int GetHashCode()
{
return HashCode.Combine(Center, HalfExtents);
}
public static bool operator ==(AlignedRectangle left, AlignedRectangle right) {
return left.Equals(right);
}
public static bool operator !=(AlignedRectangle left, AlignedRectangle right) {
return !left.Equals(right);
}
#endregion
/// <summary>
/// Returns the string representation of this object.
/// </summary>
public override string ToString()
{
return $"({Left}, {Bottom}, {Right}, {Top})";
}
}
/// <summary>
/// A rectangle that can be rotated.
/// </summary>
[Serializable]
internal readonly struct OrientedRectangle : IEquatable<OrientedRectangle>
{
/// <summary>
/// Center point of the rectangle in world space.
/// </summary>
public readonly Vector2 Center;
/// <summary>
/// Half of the total width and height of the rectangle.
/// </summary>
public readonly Vector2 HalfExtents;
/// <summary>
/// World rotation of the rectangle in radians.
/// </summary>
public readonly float Rotation;
/// <summary>
/// A 1x1 unit box with the origin centered and identity rotation.
/// </summary>
public static readonly OrientedRectangle UnitCentered = new(Vector2.Zero, Vector2.One, 0);
public OrientedRectangle(Box2 worldBox)
{
Center = worldBox.Center;
var hWidth = MathF.Abs(worldBox.Right - worldBox.Left) * 0.5f;
var hHeight = MathF.Abs(worldBox.Bottom - worldBox.Top) * 0.5f;
HalfExtents = new Vector2(hWidth, hHeight);
Rotation = 0;
}
public OrientedRectangle(Vector2 halfExtents)
{
Center = default;
HalfExtents = halfExtents;
Rotation = default;
}
public OrientedRectangle(Vector2 center, Vector2 halfExtents)
{
Center = center;
HalfExtents = halfExtents;
Rotation = default;
}
public OrientedRectangle(Vector2 center, Vector2 halfExtents, float rotation)
{
Center = center;
HalfExtents = halfExtents;
Rotation = rotation;
}
public OrientedRectangle(in Vector2 center, in Box2 localBox, float rotation)
{
Center = center;
HalfExtents = new Vector2(localBox.Width / 2, localBox.Height / 2);
Rotation = rotation;
}
/// <summary>
/// calculates the smallest AABB that will encompass this rectangle. The AABB is in local space.
/// </summary>
public Box2 CalcBoundingBox()
{
var Fi = Rotation;
var CX = Center.X;
var CY = Center.Y;
var WX = HalfExtents.X;
var WY = HalfExtents.Y;
var SF = MathF.Sin(Fi);
var CF = MathF.Cos(Fi);
var NH = MathF.Abs(WX * SF) + MathF.Abs(WY * CF); //boundrect half-height
var NW = MathF.Abs(WX * CF) + MathF.Abs(WY * SF); //boundrect half-width
return new Box2((float)(CX - NW), (float)(CY - NH), (float)(CX + NW), (float)(CY + NH)); //draw bound rectangle
}
/// <summary>
/// Tests if a point is contained inside this rectangle.
/// </summary>
/// <param name="point">Point to test.</param>
/// <returns>True if the point is contained inside this rectangle.</returns>
public bool Contains(Vector2 point)
{
// rotate around rectangle center by -rectAngle
var s = MathF.Sin(-Rotation);
var c = MathF.Cos(-Rotation);
// set origin to rect center
var newPoint = point - Center;
// rotate
newPoint = new Vector2(newPoint.X * c - newPoint.Y * s, newPoint.X * s + newPoint.Y * c);
// put origin back
newPoint += Center;
// check if our transformed point is in the rectangle, which is no longer
// rotated relative to the point
var xMin = -HalfExtents.X;
var xMax = HalfExtents.X;
var yMin = -HalfExtents.Y;
var yMax = HalfExtents.Y;
return newPoint.X >= xMin && newPoint.X <= xMax && newPoint.Y >= yMin && newPoint.Y <= yMax;
}
/// <summary>
/// Returns the closest point inside the rectangle to the given point in world space.
/// </summary>
public Vector2 ClosestPointWorld(Vector2 worldPoint)
{
// inverse-transform the sphere's center into the box's local space.
var localPoint = InverseTransformPoint(worldPoint);
var xMin = -HalfExtents.X;
var xMax = HalfExtents.X;
var yMin = -HalfExtents.Y;
var yMax = HalfExtents.Y;
// clamp the point to the border of the box
var cx = MathHelper.Clamp(localPoint.X, xMin, xMax);
var cy = MathHelper.Clamp(localPoint.Y, yMin, yMax);
return TransformPoint(new Vector2(cx, cy));
}
/// <summary>
/// Transforms a point from the rectangle's local space to world space.
/// </summary>
public Vector2 TransformPoint(Vector2 localPoint)
{
var theta = Rotation;
var (x, y) = localPoint;
var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
return new Vector2(dx, dy) + Center;
}
/// <summary>
/// Transforms a point from world space to the rectangle's local space.
/// </summary>
public Vector2 InverseTransformPoint(Vector2 worldPoint)
{
var theta = -Rotation;
var (x, y) = worldPoint + -Center;
var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
return new Vector2(dx, dy);
}
#region Equality Members
/// <inheritdoc />
public bool Equals(OrientedRectangle other)
{
return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents) && Rotation.Equals(other.Rotation);
}
/// <inheritdoc />
public override bool Equals(object? obj)
{
return obj is OrientedRectangle other && Equals(other);
}
/// <inheritdoc />
public override int GetHashCode()
{
return HashCode.Combine(Center, HalfExtents, Rotation);
}
/// <summary>
/// Check for equality by value between two <see cref="OrientedRectangle"/>.
/// </summary>
public static bool operator ==(OrientedRectangle left, OrientedRectangle right) {
return left.Equals(right);
}
/// <summary>
/// Check for inequality by value between two <see cref="OrientedRectangle"/>.
/// </summary>
public static bool operator !=(OrientedRectangle left, OrientedRectangle right) {
return !left.Equals(right);
}
#endregion
/// <summary>
/// Returns the string representation of this object.
/// </summary>
public override string ToString()
{
var box = new Box2(-HalfExtents.X, -HalfExtents.Y, HalfExtents.X, HalfExtents.Y).Translated(Center);
return $"{box}, {Rotation}";
}
}
}

55
Robust.Shared/Physics/Controllers/VirtualController.cs
View File

@@ -1,55 +0,0 @@
using System;
using System.Collections.Generic;
using JetBrains.Annotations;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Physics.Dynamics;
namespace Robust.Shared.Physics.Controllers
{
[MeansImplicitUse]
public abstract class VirtualController
{
[Dependency] protected readonly IComponentManager ComponentManager = default!;
[Dependency] protected readonly IEntityManager EntityManager = default!;
public virtual List<Type> UpdatesBefore => new();
public virtual List<Type> UpdatesAfter => new();
public virtual void Initialize()
{
IoCManager.InjectDependencies(this);
}
/// <summary>
/// Run before any map processing starts.
/// </summary>
/// <param name="prediction"></param>
/// <param name="frameTime"></param>
public virtual void UpdateBeforeSolve(bool prediction, float frameTime) {}
/// <summary>
/// Run after all map processing has finished.
/// </summary>
/// <param name="prediction"></param>
/// <param name="frameTime"></param>
public virtual void UpdateAfterSolve(bool prediction, float frameTime) {}
/// <summary>
/// Run before a particular map starts.
/// </summary>
/// <param name="prediction"></param>
/// <param name="map"></param>
/// <param name="frameTime"></param>
public virtual void UpdateBeforeMapSolve(bool prediction, PhysicsMap map, float frameTime) {}
/// <summary>
/// Run after a particular map finishes.
/// </summary>
/// <param name="prediction"></param>
/// <param name="map"></param>
/// <param name="frameTime"></param>
public virtual void UpdateAfterMapSolve(bool prediction, PhysicsMap map, float frameTime) {}
}
}

14
Robust.Shared/Physics/DebugDrawRayMessage.cs
View File

@@ -1,14 +0,0 @@
using Robust.Shared.GameObjects;
namespace Robust.Shared.Physics
{
public sealed class DebugDrawRayMessage : EntitySystemMessage
{
public DebugRayData Data { get; }
public DebugDrawRayMessage(DebugRayData data)
{
Data = data;
}
}
}

2
Robust.Shared/Physics/DebugDrawingHandle.cs
View File

@@ -11,8 +11,6 @@ namespace Robust.Shared.Physics
public abstract void DrawRect(in Box2 box, in Color color);
public abstract void DrawRect(in Box2Rotated box, in Color color);
public abstract void DrawCircle(Vector2 origin, float radius, in Color color);
public abstract void DrawPolygonShape(Vector2[] vertices, in Color color);
public abstract void DrawLine(Vector2 start, Vector2 end, in Color color);
public abstract void SetTransform(in Matrix3 transform);
public abstract Color CalcWakeColor(Color color, float wakePercent);

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

@@ -1,488 +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.Collections.Generic;
using System.Linq;
using Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Physics.Dynamics.Contacts;
namespace Robust.Shared.Physics.Dynamics
{
internal sealed class ContactManager
{
// TODO: When a static body has no contacts left need to set it to sleep as otherwise it'll just show as awake
// for debug drawing (map never adds static bodies as awake so should be no problem there).
[Dependency] private readonly IConfigurationManager _configManager = default!;
internal MapId MapId { get; set; }
private SharedBroadPhaseSystem _broadPhaseSystem = default!;
/// <summary>
/// Called when the broadphase finds two fixtures close to each other.
/// </summary>
public BroadPhaseDelegate OnBroadPhaseCollision;
/// <summary>
/// The set of active contacts.
/// </summary>
internal HashSet<Contact> ActiveContacts = new(128);
/// <summary>
/// A temporary copy of active contacts that is used during updates so
/// the hash set can have members added/removed during the update.
/// This list is cleared after every update.
/// </summary>
private List<Contact> ActiveList = new(128);
private List<ICollideBehavior> _collisionBehaviors = new();
private List<IPostCollide> _postCollideBehaviors = new();
public ContactManager()
{
OnBroadPhaseCollision = AddPair;
}
public void Initialize()
{
IoCManager.InjectDependencies(this);
_broadPhaseSystem = EntitySystem.Get<SharedBroadPhaseSystem>();
}
public void FindNewContacts(MapId mapId)
{
foreach (var broadPhase in _broadPhaseSystem.GetBroadPhases(mapId))
{
broadPhase.UpdatePairs(OnBroadPhaseCollision);
}
}
internal void UpdateContacts(ContactEdge? contactEdge, bool value)
{
if (value)
{
while (contactEdge != null)
{
var contact = contactEdge.Contact!;
if (!ActiveContacts.Contains(contact))
{
ActiveContacts.Add(contact);
}
contactEdge = contactEdge.Next;
}
}
else
{
while (contactEdge != null)
{
var contact = contactEdge.Contact!;
if (!contactEdge.Other!.Awake)
{
if (ActiveContacts.Contains(contact))
{
ActiveContacts.Remove(contact);
}
}
contactEdge = contactEdge.Next;
}
}
}
internal void RemoveActiveContact(Contact contact)
{
if (!ActiveContacts.Contains(contact))
{
ActiveContacts.Remove(contact);
}
}
/// <summary>
/// Go through the cached broadphase movement and update contacts.
/// </summary>
/// <param name="gridId"></param>
/// <param name="proxyA"></param>
/// <param name="proxyB"></param>
private void AddPair(GridId gridId, in FixtureProxy proxyA, in FixtureProxy proxyB)
{
Fixture fixtureA = proxyA.Fixture;
Fixture fixtureB = proxyB.Fixture;
int indexA = proxyA.ChildIndex;
int indexB = proxyB.ChildIndex;
PhysicsComponent bodyA = fixtureA.Body;
PhysicsComponent bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA.Owner.Uid.Equals(bodyB.Owner.Uid)) return;
// Box2D checks the mask / layer below but IMO doing it before contact is better.
// Check default filter
if (!ShouldCollide(fixtureA, fixtureB))
return;
// Does a contact already exist?
var edge = bodyB.ContactEdges;
while (edge != null)
{
if (edge.Other == bodyA)
{
Fixture fA = edge.Contact?.FixtureA!;
Fixture fB = edge.Contact?.FixtureB!;
var iA = edge.Contact!.ChildIndexA;
var iB = edge.Contact!.ChildIndexB;
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.Next;
}
// Does a joint override collision? Is at least one body dynamic?
if (!bodyB.ShouldCollide(bodyA))
return;
//FPE feature: BeforeCollision delegate
/*
if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false)
return;
if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false)
return;
*/
// Call the factory.
Contact c = Contact.Create(gridId, fixtureA, indexA, fixtureB, indexB);
// Contact creation may swap fixtures.
fixtureA = c.FixtureA!;
fixtureB = c.FixtureB!;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
ActiveContacts.Add(c);
// Connect to island graph.
// Connect to body A
c.NodeA.Contact = c;
c.NodeA.Other = bodyB;
c.NodeA.Previous = null;
c.NodeA.Next = bodyA.ContactEdges;
if (bodyA.ContactEdges != null)
{
bodyA.ContactEdges.Previous = c.NodeA;
}
bodyA.ContactEdges = c.NodeA;
// Connect to body B
c.NodeB.Contact = c;
c.NodeB.Other = bodyA;
c.NodeB.Previous = null;
c.NodeB.Next = bodyB.ContactEdges;
if (bodyB.ContactEdges != null)
{
bodyB.ContactEdges.Previous = c.NodeB;
}
bodyB.ContactEdges = c.NodeB;
// Wake up the bodies
if (fixtureA.Hard && fixtureB.Hard)
{
bodyA.Awake = true;
bodyB.Awake = true;
}
}
private bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
{
// TODO: Should we only be checking one side's mask? I think maybe fixtureB? IDK
return !((fixtureA.CollisionMask & fixtureB.CollisionLayer) == 0x0 &&
(fixtureB.CollisionMask & fixtureA.CollisionLayer) == 0x0);
}
public void Destroy(Contact contact)
{
Fixture fixtureA = contact.FixtureA!;
Fixture fixtureB = contact.FixtureB!;
PhysicsComponent bodyA = fixtureA.Body;
PhysicsComponent bodyB = fixtureB.Body;
if (contact.IsTouching)
{
//Report the separation to both participants:
// TODO: Needs to do like a comp message and system message
// fixtureA?.OnSeparation(fixtureA, fixtureB);
//Reverse the order of the reported fixtures. The first fixture is always the one that the
//user subscribed to.
// fixtureB.OnSeparation(fixtureB, fixtureA);
// EndContact(contact);
}
// Remove from body 1
if (contact.NodeA.Previous != null)
{
contact.NodeA.Previous.Next = contact.NodeA.Next;
}
if (contact.NodeA.Next != null)
{
contact.NodeA.Next.Previous = contact.NodeA.Previous;
}
if (contact.NodeA == bodyA.ContactEdges)
{
bodyA.ContactEdges = contact.NodeA.Next;
}
// Remove from body 2
if (contact.NodeB.Previous != null)
{
contact.NodeB.Previous.Next = contact.NodeB.Next;
}
if (contact.NodeB.Next != null)
{
contact.NodeB.Next.Previous = contact.NodeB.Previous;
}
if (contact.NodeB == bodyB.ContactEdges)
{
bodyB.ContactEdges = contact.NodeB.Next;
}
ActiveContacts.Remove(contact);
contact.Destroy();
}
internal void Collide()
{
// TODO: We need to handle collisions during prediction but also handle the start / stop colliding shit during sim ONLY
// Update awake contacts
ActiveList.AddRange(ActiveContacts);
// Can be changed while enumerating
foreach (var contact in ActiveList)
{
Fixture fixtureA = contact.FixtureA!;
Fixture fixtureB = contact.FixtureB!;
int indexA = contact.ChildIndexA;
int indexB = contact.ChildIndexB;
PhysicsComponent bodyA = fixtureA.Body;
PhysicsComponent bodyB = fixtureB.Body;
// Do not try to collide disabled bodies
// FPE just continues here but in our case I think it's better to also destroy the contact.
if (!bodyA.CanCollide || !bodyB.CanCollide)
{
Contact cNuke = contact;
Destroy(cNuke);
continue;
}
// Is this contact flagged for filtering?
if (contact.FilterFlag)
{
// Should these bodies collide?
if (!bodyB.ShouldCollide(bodyA))
{
Contact cNuke = contact;
Destroy(cNuke);
continue;
}
// Check default filtering
if (!ShouldCollide(fixtureA, fixtureB))
{
Contact cNuke = contact;
Destroy(cNuke);
continue;
}
// Check user filtering.
/*
if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
Destroy(cNuke);
continue;
}
*/
// Clear the filtering flag.
contact.FilterFlag = false;
}
var activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static;
var activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static;
// At least one body must be awake and it must be dynamic or kinematic.
if (!activeA && !activeB)
{
ActiveContacts.Remove(contact);
continue;
}
// TODO: Need to handle moving grids
bool? overlap = false;
// Sloth addition: Kind of hacky and might need to be removed at some point.
// One of the bodies was probably put into nullspace so we need to remove I think.
if (fixtureA.Proxies.ContainsKey(contact.GridId) && fixtureB.Proxies.ContainsKey(contact.GridId))
{
var proxyIdA = fixtureA.Proxies[contact.GridId][indexA].ProxyId;
var proxyIdB = fixtureB.Proxies[contact.GridId][indexB].ProxyId;
var broadPhase = _broadPhaseSystem.GetBroadPhase(MapId, contact.GridId);
overlap = broadPhase?.TestOverlap(proxyIdA, proxyIdB);
}
// Here we destroy contacts that cease to overlap in the broad-phase.
if (overlap == false)
{
Contact cNuke = contact;
Destroy(cNuke);
continue;
}
// The contact persists.
contact.Update(this);
}
ActiveList.Clear();
}
public void PreSolve()
{
// We'll do pre and post-solve around all islands rather than each specific island as it seems cleaner with race conditions.
foreach (var contact in ActiveContacts)
{
if (!contact.IsTouching) continue;
// God this area's hard to read but tl;dr run ICollideBehavior and IPostCollide and try to optimise it a little.
var bodyA = contact.FixtureA!.Body;
var bodyB = contact.FixtureB!.Body;
if (!bodyA.Entity.Deleted)
{
foreach (var behavior in bodyA.Owner.GetAllComponents<ICollideBehavior>())
{
_collisionBehaviors.Add(behavior);
}
foreach (var behavior in _collisionBehaviors)
{
if (bodyB.Deleted) break;
behavior.CollideWith(bodyA, bodyB);
}
_collisionBehaviors.Clear();
}
if (!bodyB.Entity.Deleted)
{
foreach (var behavior in bodyB.Owner.GetAllComponents<ICollideBehavior>())
{
_collisionBehaviors.Add(behavior);
}
foreach (var behavior in _collisionBehaviors)
{
if (bodyA.Deleted) break;
behavior.CollideWith(bodyB, bodyA);
}
_collisionBehaviors.Clear();
}
if (!bodyA.Entity.Deleted)
{
foreach (var behavior in bodyA.Owner.GetAllComponents<IPostCollide>())
{
_postCollideBehaviors.Add(behavior);
}
foreach (var behavior in _postCollideBehaviors)
{
behavior.PostCollide(bodyA, bodyB);
if (bodyB.Deleted) break;
}
_postCollideBehaviors.Clear();
}
if (!bodyB.Entity.Deleted)
{
foreach (var behavior in bodyB.Owner.GetAllComponents<IPostCollide>())
{
_postCollideBehaviors.Add(behavior);
}
foreach (var behavior in _postCollideBehaviors)
{
behavior.PostCollide(bodyB, bodyA);
if (bodyA.Deleted) break;
}
_postCollideBehaviors.Clear();
}
}
}
public void PostSolve()
{
}
}
public delegate void BroadPhaseDelegate(GridId gridId, in FixtureProxy proxyA, in FixtureProxy proxyB);
}

505
Robust.Shared/Physics/Dynamics/Contacts/Contact.cs
View File

@@ -1,505 +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.Collections.Generic;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Collision;
using Robust.Shared.Physics.Dynamics.Shapes;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Dynamics.Contacts
{
internal sealed class Contact
{
[Dependency] private readonly ICollisionManager _collisionManager = default!;
[Dependency] private readonly IEntityManager _entityManager = default!;
public ContactEdge NodeA = new();
public ContactEdge NodeB = new();
public Fixture? FixtureA;
public Fixture? FixtureB;
public Manifold Manifold = new();
private ContactType _type;
/// <summary>
/// Ordering is under <see cref="ShapeType"/>
/// uses enum to work out which collision evaluation to use.
/// </summary>
private static ContactType[,] _registers = {
{
// Circle register
ContactType.Circle,
ContactType.EdgeAndCircle,
ContactType.PolygonAndCircle,
ContactType.ChainAndCircle,
ContactType.AabbAndCircle,
ContactType.RectAndCircle,
},
{
// Edge register
ContactType.EdgeAndCircle,
ContactType.NotSupported, // Edge
ContactType.EdgeAndPolygon,
ContactType.NotSupported, // Chain
ContactType.NotSupported, // Aabb
ContactType.NotSupported, // Rect
},
{
// Polygon register
ContactType.PolygonAndCircle,
ContactType.EdgeAndPolygon,
ContactType.Polygon,
ContactType.ChainAndPolygon,
ContactType.AabbAndPolygon,
ContactType.RectAndPolygon,
},
{
// Chain register
ContactType.ChainAndCircle,
ContactType.NotSupported, // Edge
ContactType.ChainAndPolygon,
ContactType.NotSupported, // Chain
ContactType.NotSupported, // Aabb - TODO Just cast to poly
ContactType.NotSupported, // Rect - TODO Just cast to poly
},
{
// Aabb register
ContactType.AabbAndCircle,
ContactType.NotSupported, // Edge - TODO Just cast to poly
ContactType.AabbAndPolygon,
ContactType.NotSupported, // Chain - TODO Just cast to poly
ContactType.Aabb,
ContactType.AabbAndRect,
},
{
// Rectangle register
ContactType.RectAndCircle,
ContactType.NotSupported, // Edge - TODO Just cast to poly
ContactType.RectAndPolygon,
ContactType.NotSupported, // Chain - TODO Just cast to poly
ContactType.AabbAndRect,
ContactType.Rect,
}
};
/// <summary>
/// Has this contact already been added to an island?
/// </summary>
public bool IslandFlag { get; set; }
public bool FilterFlag { get; set; }
/// <summary>
/// Determines whether the contact is touching.
/// </summary>
public bool IsTouching { get; set; }
// Some day we'll refactor it to be more like EntityCoordinates
public GridId GridId { get; set; } = GridId.Invalid;
/// Enable/disable this contact. This can be used inside the pre-solve
/// contact listener. The contact is only disabled for the current
/// time step (or sub-step in continuous collisions).
/// NOTE: If you are setting Enabled to a constant true or false,
/// use the explicit Enable() or Disable() functions instead to
/// save the CPU from doing a branch operation.
public bool Enabled { get; set; }
/// <summary>
/// Get the child primitive index for fixture A.
/// </summary>
public int ChildIndexA { get; internal set; }
/// <summary>
/// Get the child primitive index for fixture B.
/// </summary>
public int ChildIndexB { get; internal set; }
/// <summary>
/// The mixed friction of the 2 fixtures.
/// </summary>
public float Friction { get; set; }
/// <summary>
/// The mixed restitution of the 2 fixtures.
/// </summary>
public float Restitution { get; set; }
/// <summary>
/// Used for conveyor belt behavior in m/s.
/// </summary>
public float TangentSpeed { get; set; }
public Contact(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
{
IoCManager.InjectDependencies(this);
Reset(fixtureA, indexA, fixtureB, indexB);
}
/// <summary>
/// Gets a new contact to use, using the contact pool if relevant.
/// </summary>
/// <param name="fixtureA"></param>
/// <param name="indexA"></param>
/// <param name="fixtureB"></param>
/// <param name="indexB"></param>
/// <returns></returns>
internal static Contact Create(GridId gridId, Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
{
var type1 = fixtureA.Shape.ShapeType;
var type2 = fixtureB.Shape.ShapeType;
DebugTools.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount);
DebugTools.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount);
Queue<Contact> pool = fixtureA.Body.PhysicsMap.ContactPool;
if (pool.TryDequeue(out var contact))
{
if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
{
contact.Reset(fixtureA, indexA, fixtureB, indexB);
}
else
{
contact.Reset(fixtureB, indexB, fixtureA, indexA);
}
}
else
{
// Edge+Polygon is non-symmetrical due to the way Erin handles collision type registration.
if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
{
contact = new Contact(fixtureA, indexA, fixtureB, indexB);
}
else
{
contact = new Contact(fixtureB, indexB, fixtureA, indexA);
}
}
contact.GridId = gridId;
contact._type = _registers[(int) type1, (int) type2];
return contact;
}
public void ResetRestitution()
{
Restitution = MathF.Max(FixtureA?.Restitution ?? 0.0f, FixtureB?.Restitution ?? 0.0f);
}
public void ResetFriction()
{
Friction = MathF.Sqrt(FixtureA?.Friction ?? 0.0f * FixtureB?.Friction ?? 0.0f);
}
private void Reset(Fixture? fixtureA, int indexA, Fixture? fixtureB, int indexB)
{
Enabled = true;
IsTouching = false;
IslandFlag = false;
FilterFlag = false;
// TOIFlag = false;
FixtureA = fixtureA;
FixtureB = fixtureB;
ChildIndexA = indexA;
ChildIndexB = indexB;
Manifold.PointCount = 0;
NodeA.Contact = null;
NodeA.Previous = null;
NodeA.Next = null;
NodeA.Other = null;
NodeB.Contact = null;
NodeB.Previous = null;
NodeB.Next = null;
NodeB.Other = null;
// _toiCount = 0;
//FPE: We only set the friction and restitution if we are not destroying the contact
if (FixtureA != null && FixtureB != null)
{
Friction = MathF.Sqrt(FixtureA.Friction * FixtureB.Friction);
Restitution = MathF.Max(FixtureA.Restitution, FixtureB.Restitution);
}
TangentSpeed = 0;
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out Vector2[] points)
{
PhysicsComponent bodyA = FixtureA?.Body!;
PhysicsComponent bodyB = FixtureB?.Body!;
IPhysShape shapeA = FixtureA?.Shape!;
IPhysShape shapeB = FixtureB?.Shape!;
ContactSolver.InitializeManifold(Manifold, bodyA.GetTransform(), bodyB.GetTransform(), shapeA.Radius, shapeB.Radius, out normal, out points);
}
/// <summary>
/// Update the contact manifold and touching status.
/// Note: do not assume the fixture AABBs are overlapping or are valid.
/// </summary>
/// <param name="contactManager">The contact manager.</param>
internal void Update(ContactManager contactManager)
{
PhysicsComponent bodyA = FixtureA!.Body;
PhysicsComponent bodyB = FixtureB!.Body;
if (FixtureA == null || FixtureB == null)
return;
Manifold oldManifold = Manifold.Clone();
// Re-enable this contact.
Enabled = true;
bool touching;
bool wasTouching = IsTouching;
bool sensor = !FixtureA.Hard || !FixtureB.Hard;
// Is this contact a sensor?
if (sensor)
{
IPhysShape shapeA = FixtureA.Shape;
IPhysShape shapeB = FixtureB.Shape;
touching = _collisionManager.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, bodyA.GetTransform(), bodyB.GetTransform());
// Sensors don't generate manifolds.
Manifold.PointCount = 0;
}
else
{
Evaluate(Manifold, bodyA.GetTransform(), bodyB.GetTransform());
touching = Manifold.PointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < Manifold.PointCount; ++i)
{
ManifoldPoint mp2 = Manifold.Points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
for (int j = 0; j < oldManifold.PointCount; ++j)
{
ManifoldPoint mp1 = oldManifold.Points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
break;
}
}
Manifold.Points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.Awake = true;
bodyB.Awake = true;
}
}
IsTouching = touching;
// TODO: Need to do collision behaviors around here.
if (!wasTouching)
{
if (touching)
{
var enabledA = true;
var enabledB = true;
/*
// Report the collision to both participants. Track which ones returned true so we can
// later call OnSeparation if the contact is disabled for a different reason.
if (FixtureA.OnCollision != null)
foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
enabledA = handler(FixtureA, FixtureB, this) && enabledA;
// Reverse the order of the reported fixtures. The first fixture is always the one that the
// user subscribed to.
if (FixtureB.OnCollision != null)
foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
enabledB = handler(FixtureB, FixtureA, this) && enabledB;
*/
Enabled = enabledA && enabledB;
// BeginContact can also return false and disable the contact
/*
if (enabledA && enabledB && contactManager.BeginContact != null)
Enabled = contactManager.BeginContact(this);
*/
}
}
else
{
if (!touching)
{
/*
//Report the separation to both participants:
if (FixtureA != null && FixtureA.OnSeparation != null)
FixtureA.OnSeparation(FixtureA, FixtureB);
//Reverse the order of the reported fixtures. The first fixture is always the one that the
//user subscribed to.
if (FixtureB != null && FixtureB.OnSeparation != null)
FixtureB.OnSeparation(FixtureB, FixtureA);
if (contactManager.EndContact != null)
contactManager.EndContact(this);
*/
}
}
if (sensor)
return;
_entityManager.EventBus.RaiseEvent(EventSource.Local, new PreSolveMessage(this, oldManifold));
}
/// <summary>
/// Evaluate this contact with your own manifold and transforms.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="transformA">The first transform.</param>
/// <param name="transformB">The second transform.</param>
private void Evaluate(Manifold manifold, in Transform transformA, in Transform transformB)
{
// This is expensive and shitcodey, see below.
switch (_type)
{
// TODO: Need a unit test for these.
case ContactType.Polygon:
_collisionManager.CollidePolygons(manifold, new PolygonShape(FixtureA!.Shape), transformA, new PolygonShape(FixtureB!.Shape), transformB);
break;
case ContactType.PolygonAndCircle:
_collisionManager.CollidePolygonAndCircle(manifold, new PolygonShape(FixtureA!.Shape), transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
break;
case ContactType.EdgeAndCircle:
_collisionManager.CollideEdgeAndCircle(manifold, (EdgeShape) FixtureA!.Shape, transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
break;
case ContactType.EdgeAndPolygon:
_collisionManager.CollideEdgeAndPolygon(manifold, (EdgeShape) FixtureA!.Shape, transformA, new PolygonShape(FixtureB!.Shape), transformB);
break;
case ContactType.ChainAndCircle:
throw new NotImplementedException();
/*
ChainShape chain = (ChainShape)FixtureA.Shape;
chain.GetChildEdge(_edge, ChildIndexA);
Collision.CollisionManager.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
*/
break;
case ContactType.ChainAndPolygon:
throw new NotImplementedException();
/*
ChainShape loop2 = (ChainShape)FixtureA.Shape;
loop2.GetChildEdge(_edge, ChildIndexA);
Collision.CollisionManager.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB);
*/
break;
case ContactType.Circle:
_collisionManager.CollideCircles(manifold, (PhysShapeCircle) FixtureA!.Shape, in transformA, (PhysShapeCircle) FixtureB!.Shape, in transformB);
break;
// Custom ones
// This is kind of shitcodey and originally I just had the poly version but if we get an AABB -> whatever version directly you'll get good optimisations over a cast.
case ContactType.Aabb:
_collisionManager.CollideAabbs(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PhysShapeAabb) FixtureB!.Shape, transformB);
break;
case ContactType.AabbAndCircle:
_collisionManager.CollideAabbAndCircle(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
break;
case ContactType.AabbAndPolygon:
_collisionManager.CollideAabbAndPolygon(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PolygonShape) FixtureB!.Shape, transformB);
break;
case ContactType.AabbAndRect:
_collisionManager.CollideAabbAndRect(manifold, (PhysShapeAabb) FixtureA!.Shape, transformA, (PhysShapeRect) FixtureB!.Shape, transformB);
break;
case ContactType.Rect:
_collisionManager.CollideRects(manifold, (PhysShapeRect) FixtureA!.Shape, transformA, (PhysShapeRect) FixtureB!.Shape, transformB);
break;
case ContactType.RectAndCircle:
_collisionManager.CollideRectAndCircle(manifold, (PhysShapeRect) FixtureA!.Shape, transformA, (PhysShapeCircle) FixtureB!.Shape, transformB);
break;
case ContactType.RectAndPolygon:
_collisionManager.CollideRectAndPolygon(manifold, (PhysShapeRect) FixtureA!.Shape, transformA, (PolygonShape) FixtureB!.Shape, transformB);
break;
}
}
internal void Destroy()
{
// Seems like active contacts were never used in farseer anyway
// FixtureA?.Body.PhysicsMap.ContactManager.RemoveActiveContact(this);
FixtureA?.Body.PhysicsMap.ContactPool.Enqueue(this);
if (Manifold.PointCount > 0 && FixtureA?.Hard == true && FixtureB?.Hard == true)
{
FixtureA.Body.Awake = true;
FixtureB.Body.Awake = true;
}
Reset(null, 0, null, 0);
}
private enum ContactType : byte
{
NotSupported,
Polygon,
PolygonAndCircle,
Circle,
EdgeAndPolygon,
EdgeAndCircle,
ChainAndPolygon,
ChainAndCircle,
// Custom
Aabb,
AabbAndPolygon,
AabbAndCircle,
AabbAndRect,
Rect,
RectAndCircle,
RectAndPolygon,
}
}
}

50
Robust.Shared/Physics/Dynamics/Contacts/ContactEdge.cs
View File

@@ -1,50 +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 Robust.Shared.GameObjects;
namespace Robust.Shared.Physics.Dynamics.Contacts
{
internal sealed class ContactEdge
{
/// <summary>
/// This contact in the chain.
/// </summary>
public Contact? Contact { get; set; } = default!;
public ContactEdge? Next { get; set; }
public ContactEdge? Previous { get; set; } = default!;
// Subject to change
public PhysicsComponent? Other { get; set; } = default!;
public ContactEdge() {}
public ContactEdge(Contact contact, ContactEdge previous, PhysicsComponent other)
{
Contact = contact;
Previous = previous;
Other = other;
}
}
}

66
Robust.Shared/Physics/Dynamics/Contacts/ContactPositionConstraint.cs
View File

@@ -1,66 +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 Robust.Shared.Maths;
using Robust.Shared.Physics.Collision;
namespace Robust.Shared.Physics.Dynamics.Contacts
{
internal sealed class ContactPositionConstraint
{
/// <summary>
/// Index of BodyA in the island.
/// </summary>
public int IndexA { get; set; }
/// <summary>
/// Index of BodyB in the island.
/// </summary>
public int IndexB { get; set; }
public Vector2[] LocalPoints = new Vector2[2];
public Vector2 LocalNormal;
public Vector2 LocalPoint;
public float InvMassA;
public float InvMassB;
public Vector2 LocalCenterA;
public Vector2 LocalCenterB;
public float InvIA;
public float InvIB;
public ManifoldType Type;
public float RadiusA;
public float RadiusB;
public int PointCount;
}
}

861
Robust.Shared/Physics/Dynamics/Contacts/ContactSolver.cs
View File

@@ -1,861 +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 Robust.Shared.Configuration;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Collision;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Dynamics.Contacts
{
internal sealed class ContactSolver
{
[Dependency] private readonly IConfigurationManager _configManager = default!;
private bool _warmStarting;
private float _velocityThreshold;
private float _baumgarte;
private float _linearSlop;
private float _maxLinearCorrection;
private Vector2[] _linearVelocities = Array.Empty<Vector2>();
private float[] _angularVelocities = Array.Empty<float>();
private Vector2[] _positions = Array.Empty<Vector2>();
private float[] _angles = Array.Empty<float>();
private Contact[] _contacts = Array.Empty<Contact>();
private int _contactCount;
private ContactVelocityConstraint[] _velocityConstraints = Array.Empty<ContactVelocityConstraint>();
private ContactPositionConstraint[] _positionConstraints = Array.Empty<ContactPositionConstraint>();
public void Initialize()
{
IoCManager.InjectDependencies(this);
_warmStarting = _configManager.GetCVar(CVars.WarmStarting);
_configManager.OnValueChanged(CVars.WarmStarting, value => _warmStarting = value);
_velocityThreshold = _configManager.GetCVar(CVars.VelocityThreshold);
_configManager.OnValueChanged(CVars.VelocityThreshold, value => _velocityThreshold = value);
_baumgarte = _configManager.GetCVar(CVars.Baumgarte);
_configManager.OnValueChanged(CVars.Baumgarte, value => _baumgarte = value);
_linearSlop = _configManager.GetCVar(CVars.LinearSlop);
_configManager.OnValueChanged(CVars.LinearSlop, value => _linearSlop = value);
_maxLinearCorrection = _configManager.GetCVar(CVars.MaxLinearCorrection);
_configManager.OnValueChanged(CVars.MaxLinearCorrection, value => _maxLinearCorrection = value);
}
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++)
{
_velocityConstraints[i] = new ContactVelocityConstraint();
_positionConstraints[i] = new ContactPositionConstraint();
}
}
// 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);
var velocityConstraint = _velocityConstraints[i];
velocityConstraint.Friction = contact.Friction;
velocityConstraint.Restitution = contact.Restitution;
velocityConstraint.TangentSpeed = contact.TangentSpeed;
velocityConstraint.IndexA = bodyA.IslandIndex;
velocityConstraint.IndexB = bodyB.IslandIndex;
velocityConstraint.InvMassA = bodyA.InvMass;
velocityConstraint.InvMassB = bodyB.InvMass;
velocityConstraint.InvIA = bodyA.InvI;
velocityConstraint.InvIB = bodyB.InvI;
velocityConstraint.ContactIndex = i;
velocityConstraint.PointCount = pointCount;
for (var x = 0; x < 2; x++)
{
velocityConstraint.K[x] = Vector2.Zero;
velocityConstraint.NormalMass[x] = Vector2.Zero;
}
var positionConstraint = _positionConstraints[i];
positionConstraint.IndexA = bodyA.IslandIndex;
positionConstraint.IndexB = bodyB.IslandIndex;
positionConstraint.InvMassA = bodyA.InvMass;
positionConstraint.InvMassB = bodyB.InvMass;
// TODO: Dis
// positionConstraint.LocalCenterA = bodyA._sweep.LocalCenter;
// positionConstraint.LocalCenterB = bodyB._sweep.LocalCenter;
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 (int j = 0; j < pointCount; ++j)
{
var contactPoint = manifold.Points[j];
var constraintPoint = 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;
}
}
}
public void InitializeVelocityConstraints()
{
for (var i = 0; i < _contactCount; ++i)
{
var velocityConstraint = _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);
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;
var points = new Vector2[2];
InitializeManifold(manifold, xfA, xfB, radiusA, radiusB, out normal, out points);
velocityConstraint.Normal = normal;
int pointCount = velocityConstraint.PointCount;
for (int j = 0; j < pointCount; ++j)
{
VelocityConstraintPoint vcp = 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[0] = new Vector2(k11, k12);
velocityConstraint.K[1] = new Vector2(k12, k22);
velocityConstraint.NormalMass = velocityConstraint.K.Inverse();
}
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;
var linVelocityA = _linearVelocities[indexA];
var angVelocityA = _angularVelocities[indexA];
var linVelocityB = _linearVelocities[indexB];
var angVelocityB = _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;
}
_linearVelocities[indexA] = linVelocityA;
_angularVelocities[indexA] = angVelocityA;
_linearVelocities[indexB] = linVelocityB;
_angularVelocities[indexB] = angVelocityB;
}
}
public void SolveVelocityConstraints()
{
// Here be dragons
for (var i = 0; i < _contactCount; ++i)
{
var velocityConstraint = _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;
var vA = _linearVelocities[indexA];
var wA = _angularVelocities[indexA];
var vB = _linearVelocities[indexB];
var wB = _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)
{
VelocityConstraintPoint velConstraintPoint = 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)
{
VelocityConstraintPoint vcp = 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;
VelocityConstraintPoint cp1 = velocityConstraint.Points[0];
VelocityConstraintPoint cp2 = 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[0].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[1].X * 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;
}
}
_linearVelocities[indexA] = vA;
_angularVelocities[indexA] = wA;
_linearVelocities[indexB] = vB;
_angularVelocities[indexB] = wB;
}
}
public void StoreImpulses()
{
for (int i = 0; i < _contactCount; ++i)
{
ContactVelocityConstraint velocityConstraint = _velocityConstraints[i];
Collision.Manifold manifold = _contacts[velocityConstraint.ContactIndex].Manifold;
for (int j = 0; j < velocityConstraint.PointCount; ++j)
{
ManifoldPoint point = manifold.Points[j];
point.NormalImpulse = velocityConstraint.Points[j].NormalImpulse;
point.TangentImpulse = velocityConstraint.Points[j].TangentImpulse;
manifold.Points[j] = point;
}
_contacts[velocityConstraint.ContactIndex].Manifold = manifold;
}
}
/// <summary>
/// Tries to solve positions for all contacts specified.
/// </summary>
/// <returns>true if all positions solved</returns>
public bool SolvePositionConstraints()
{
float minSeparation = 0.0f;
for (int i = 0; i < _contactCount; ++i)
{
ContactPositionConstraint 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;
Vector2 centerA = _positions[indexA];
float angleA = _angles[indexA];
Vector2 centerB = _positions[indexB];
float angleB = _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 + _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);
}
_positions[indexA] = centerA;
_angles[indexA] = angleA;
_positions[indexB] = centerB;
_angles[indexB] = angleB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -3.0f * _linearSlop;
}
/// <summary>
/// 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.
/// </summary>
internal static void InitializeManifold(
in Collision.Manifold manifold,
in Transform xfA,
in Transform xfB,
float radiusA,
float radiusB,
out Vector2 normal,
out Vector2[] points)
{
normal = Vector2.Zero;
points = new Vector2[2];
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;
}
}
}
}

91
Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs
View File

@@ -1,91 +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 Robust.Shared.Maths;
namespace Robust.Shared.Physics.Dynamics.Contacts
{
internal sealed class ContactVelocityConstraint
{
public int ContactIndex { get; set; }
/// <summary>
/// Index of BodyA in the island.
/// </summary>
public int IndexA { get; set; }
/// <summary>
/// Index of BodyB in the island.
/// </summary>
public int IndexB { get; set; }
// Use 2 as its the max number of manifold points.
public VelocityConstraintPoint[] Points = new VelocityConstraintPoint[2];
public Vector2 Normal;
public Vector2[] NormalMass = new Vector2[2];
public Vector2[] K = new Vector2[2];
public float InvMassA;
public float InvMassB;
public float InvIA;
public float InvIB;
public float Friction;
public float Restitution;
public float TangentSpeed;
public int PointCount;
public ContactVelocityConstraint()
{
for (var i = 0; i < 2; i++)
{
Points[i] = new VelocityConstraintPoint();
}
}
}
internal sealed class VelocityConstraintPoint
{
public Vector2 RelativeVelocityA;
public Vector2 RelativeVelocityB;
public float NormalImpulse;
public float TangentImpulse;
public float NormalMass;
public float TangentMass;
public float VelocityBias;
}
}

18
Robust.Shared/Physics/Dynamics/Contacts/PreSolveMessage.cs
View File

@@ -1,18 +0,0 @@
using Robust.Shared.GameObjects;
using Robust.Shared.Physics.Collision;
namespace Robust.Shared.Physics.Dynamics.Contacts
{
internal sealed class PreSolveMessage : EntitySystemMessage
{
public Contact Contact { get; }
public Collision.Manifold OldManifold { get; }
public PreSolveMessage(Contact contact, Collision.Manifold oldManifold)
{
Contact = contact;
OldManifold = oldManifold;
}
}
}

372
Robust.Shared/Physics/Dynamics/Fixture.cs
View File

@@ -1,372 +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.Collections.Generic;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Physics.Dynamics.Shapes;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics
{
public interface IFixture
{
// TODO
}
[Serializable, NetSerializable]
public sealed class Fixture : IFixture, IExposeData, IEquatable<Fixture>
{
public IReadOnlyDictionary<GridId, FixtureProxy[]> Proxies => _proxies;
[NonSerialized]
private readonly Dictionary<GridId, FixtureProxy[]> _proxies = new();
[ViewVariables]
[NonSerialized]
public int ProxyCount = 0;
[ViewVariables] public IPhysShape Shape { get; private set; } = default!;
[ViewVariables]
[field:NonSerialized]
public PhysicsComponent Body { get; internal set; } = default!;
[ViewVariables(VVAccess.ReadWrite)]
public float Friction
{
get => _friction;
set
{
if (MathHelper.CloseTo(value, _friction)) return;
_friction = value;
Body.FixtureChanged(this);
}
}
private float _friction;
[ViewVariables(VVAccess.ReadWrite)]
public float Restitution
{
get => _restitution;
set
{
if (MathHelper.CloseTo(value, _restitution)) return;
_restitution = value;
Body.FixtureChanged(this);
}
}
private float _restitution;
/// <summary>
/// Non-hard <see cref="IPhysBody"/>s will not cause action collision (e.g. blocking of movement)
/// while still raising collision events.
/// </summary>
/// <remarks>
/// This is useful for triggers or such to detect collision without actually causing a blockage.
/// </remarks>
[ViewVariables(VVAccess.ReadWrite)]
public bool Hard
{
get => _hard;
set
{
if (_hard == value)
return;
Body.RegenerateContacts();
_hard = value;
Body.FixtureChanged(this);
}
}
private bool _hard;
/// <summary>
/// Bitmask of the collision layers the component is a part of.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionLayer
{
get => _collisionLayer;
set
{
if (_collisionLayer == value)
return;
Body.RegenerateContacts();
_collisionLayer = value;
Body.FixtureChanged(this);
}
}
private int _collisionLayer;
/// <summary>
/// Bitmask of the layers this component collides with.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionMask
{
get => _collisionMask;
set
{
if (_collisionMask == value)
return;
Body.RegenerateContacts();
_collisionMask = value;
Body.FixtureChanged(this);
}
}
private int _collisionMask;
public Fixture(PhysicsComponent body, IPhysShape shape)
{
Body = body;
Shape = shape;
}
public Fixture(IPhysShape shape, int collisionLayer, int collisionMask, bool hard)
{
Shape = shape;
_collisionLayer = collisionLayer;
_collisionMask = collisionMask;
_hard = hard;
}
public Fixture() {}
public void ExposeData(ObjectSerializer serializer)
{
serializer.DataField(this, x => x.Shape, "shape", new PhysShapeAabb());
serializer.DataField(ref _friction, "friction", 0.4f);
serializer.DataField(ref _restitution, "restitution", 0f);
serializer.DataField(ref _hard, "hard", true);
serializer.DataField(ref _collisionLayer, "layer", 0, WithFormat.Flags<CollisionLayer>());
serializer.DataField(ref _collisionMask, "mask", 0, WithFormat.Flags<CollisionMask>());
}
/// <summary>
/// As a bunch of things aren't serialized we need to instantiate Fixture from an empty ctor and then copy values across.
/// </summary>
/// <param name="fixture"></param>
internal void CopyTo(Fixture fixture)
{
fixture.Shape = Shape;
fixture._friction = _friction;
fixture._restitution = _restitution;
fixture._hard = _hard;
fixture._collisionLayer = _collisionLayer;
fixture._collisionMask = _collisionMask;
}
internal void SetProxies(GridId gridId, FixtureProxy[] proxies)
{
DebugTools.Assert(!_proxies.ContainsKey(gridId));
_proxies[gridId] = proxies;
}
/// <summary>
/// Clear this fixture's proxies from the broadphase.
/// If doing this for every fixture at once consider using the method on PhysicsComponent instead.
/// </summary>
/// <remarks>
/// Broadphase system will also need cleaning up for the cached broadphases for the body.
/// </remarks>
/// <param name="mapId"></param>
/// <param name="broadPhaseSystem"></param>
public void ClearProxies(MapId? mapId = null, SharedBroadPhaseSystem? broadPhaseSystem = null)
{
mapId ??= Body.Owner.Transform.MapID;
broadPhaseSystem ??= EntitySystem.Get<SharedBroadPhaseSystem>();
foreach (var (gridId, proxies) in _proxies)
{
var broadPhase = broadPhaseSystem.GetBroadPhase(mapId.Value, gridId);
if (broadPhase == null) continue;
foreach (var proxy in proxies)
{
broadPhase.RemoveProxy(proxy.ProxyId);
}
}
_proxies.Clear();
}
/// <summary>
/// Clears the particular grid's proxies for this fixture.
/// </summary>
/// <param name="mapId"></param>
/// <param name="broadPhaseSystem"></param>
/// <param name="gridId"></param>
public void ClearProxies(MapId mapId, SharedBroadPhaseSystem broadPhaseSystem, GridId gridId)
{
if (!Proxies.TryGetValue(gridId, out var proxies)) return;
var broadPhase = broadPhaseSystem.GetBroadPhase(mapId, gridId);
if (broadPhase != null)
{
foreach (var proxy in proxies)
{
broadPhase.RemoveProxy(proxy.ProxyId);
}
}
_proxies.Remove(gridId);
}
/// <summary>
/// Creates FixtureProxies on the relevant broadphases.
/// If doing this for every fixture at once consider using the method on PhysicsComponent instead.
/// </summary>
/// <remarks>
/// You will need to manually add this to the body's broadphases.
/// </remarks>
public void CreateProxies(IMapManager? mapManager = null, SharedBroadPhaseSystem? broadPhaseSystem = null)
{
DebugTools.Assert(_proxies.Count == 0);
ProxyCount = Shape.ChildCount;
var mapId = Body.Owner.Transform.MapID;
mapManager ??= IoCManager.Resolve<IMapManager>();
broadPhaseSystem ??= EntitySystem.Get<SharedBroadPhaseSystem>();
var worldAABB = Body.GetWorldAABB(mapManager);
var worldPosition = Body.Owner.Transform.WorldPosition;
var worldRotation = Body.Owner.Transform.WorldRotation;
foreach (var gridId in mapManager.FindGridIdsIntersecting(mapId, worldAABB, true))
{
var broadPhase = broadPhaseSystem.GetBroadPhase(mapId, gridId);
if (broadPhase == null) continue;
Vector2 offset = worldPosition;
double gridRotation = worldRotation;
if (gridId != GridId.Invalid)
{
var grid = mapManager.GetGrid(gridId);
offset -= grid.WorldPosition;
// TODO: Should probably have a helper for this
gridRotation = worldRotation - Body.Owner.EntityManager.GetEntity(grid.GridEntityId).Transform.WorldRotation;
}
var proxies = new FixtureProxy[Shape.ChildCount];
_proxies[gridId] = proxies;
for (var i = 0; i < ProxyCount; i++)
{
// TODO: Will need to pass in childIndex to this as well
var aabb = Shape.CalculateLocalBounds(gridRotation).Translated(offset);
var proxy = new FixtureProxy(aabb, this, i);
proxy.ProxyId = broadPhase.AddProxy(ref proxy);
proxies[i] = proxy;
DebugTools.Assert(proxies[i].ProxyId != DynamicTree.Proxy.Free);
}
}
}
/// <summary>
/// Creates FixtureProxies on the relevant broadphase.
/// If doing this for every fixture at once consider using the method on PhysicsComponent instead.
/// </summary>
public void CreateProxies(IBroadPhase broadPhase, IMapManager? mapManager = null, SharedBroadPhaseSystem? broadPhaseSystem = null)
{
// TODO: Combine with the above method to be less DRY.
mapManager ??= IoCManager.Resolve<IMapManager>();
broadPhaseSystem ??= EntitySystem.Get<SharedBroadPhaseSystem>();
var gridId = broadPhaseSystem.GetGridId(broadPhase);
Vector2 offset = Body.Owner.Transform.WorldPosition;
var worldRotation = Body.Owner.Transform.WorldRotation;
double gridRotation = worldRotation;
if (gridId != GridId.Invalid)
{
var grid = mapManager.GetGrid(gridId);
offset -= grid.WorldPosition;
// TODO: Should probably have a helper for this
gridRotation = worldRotation - Body.Owner.EntityManager.GetEntity(grid.GridEntityId).Transform.WorldRotation;
}
var proxies = new FixtureProxy[Shape.ChildCount];
_proxies[gridId] = proxies;
for (var i = 0; i < ProxyCount; i++)
{
// TODO: Will need to pass in childIndex to this as well
var aabb = Shape.CalculateLocalBounds(gridRotation).Translated(offset);
var proxy = new FixtureProxy(aabb, this, i);
proxy.ProxyId = broadPhase.AddProxy(ref proxy);
proxies[i] = proxy;
DebugTools.Assert(proxies[i].ProxyId != DynamicTree.Proxy.Free);
}
broadPhaseSystem.AddBroadPhase(Body, broadPhase);
}
// This is a crude equals mainly to avoid having to re-create the fixtures every time a state comes in.
public bool Equals(Fixture? other)
{
if (other == null) return false;
return _hard == other.Hard &&
_collisionLayer == other.CollisionLayer &&
_collisionMask == other.CollisionMask &&
Shape.Equals(other.Shape) &&
Body == other.Body;
}
}
/// <summary>
/// Tag type for defining the representation of the collision layer bitmask
/// in terms of readable names in the content. To understand more about the
/// point of this type, see the <see cref="FlagsForAttribute"/>.
/// </summary>
public sealed class CollisionLayer {}
/// <summary>
/// Tag type for defining the representation of the collision mask bitmask
/// in terms of readable names in the content. To understand more about the
/// point of this type, see the <see cref="FlagsForAttribute"/>.
/// </summary>
public sealed class CollisionMask {}
}

53
Robust.Shared/Physics/Dynamics/FixtureProxy.cs
View File

@@ -1,53 +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 Robust.Shared.Maths;
namespace Robust.Shared.Physics.Dynamics
{
public class FixtureProxy
{
/// <summary>
/// Grid-based AABB of this proxy.
/// </summary>
public Box2 AABB;
public int ChildIndex;
/// <summary>
/// Our parent fixture
/// </summary>
public Fixture Fixture;
/// <summary>
/// ID of this proxy in the broadphase dynamictree.
/// </summary>
public DynamicTree.Proxy ProxyId = DynamicTree.Proxy.Free;
public FixtureProxy(Box2 aabb, Fixture fixture, int childIndex)
{
AABB = aabb;
Fixture = fixture;
ChildIndex = childIndex;
}
}
}

574
Robust.Shared/Physics/Dynamics/Joints/DistanceJoint.cs
View File

@@ -1,574 +0,0 @@
// 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.
/*
* Farseer DistanceJoint but with some recent Box2D additions
*/
using System;
using Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Joints
{
// TODO: FOR THIS LICENCE ALSO PUT BOX2D ON IT.
// 1-D rained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
/// <summary>
/// A distance joint rains two points on two bodies
/// to remain at a fixed distance from each other. You can view
/// this as a massless, rigid rod.
/// </summary>
[Serializable, NetSerializable]
public sealed class DistanceJoint : Joint, IEquatable<DistanceJoint>
{
// Sloth note:
// Box2D is replacing rope with distance hence this is also a partial port of Box2D
// Solver shared
[NonSerialized] private float _bias;
[NonSerialized] private float _gamma;
[NonSerialized] private float _impulse;
[NonSerialized] private float _lowerImpulse;
[NonSerialized] private float _upperImpulse;
// Solver temp
[NonSerialized] private int _indexA;
[NonSerialized] private int _indexB;
[NonSerialized] private Vector2 _u;
[NonSerialized] private Vector2 _rA;
[NonSerialized] private Vector2 _rB;
[NonSerialized] private Vector2 _localCenterA;
[NonSerialized] private Vector2 _localCenterB;
[NonSerialized] private float _invMassA;
[NonSerialized] private float _invMassB;
[NonSerialized] private float _invIA;
[NonSerialized] private float _invIB;
[NonSerialized] private float _mass;
[NonSerialized] private float _currentLength;
[NonSerialized] private float _softMass;
public override JointType JointType => JointType.Distance;
/// <summary>
/// This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. If you don't supply a length, the local anchor points
/// is used so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// Warning Do not use a zero or short length.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="anchorA">The first body anchor</param>
/// <param name="anchorB">The second body anchor</param>
public DistanceJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, Vector2 anchorA, Vector2 anchorB)
: base(bodyA, bodyB)
{
LocalAnchorA = anchorA;
LocalAnchorB = anchorB;
// TODO: Just pass this into the ctor.
var configManager = IoCManager.Resolve<IConfigurationManager>();
Length = MathF.Max(configManager.GetCVar(CVars.LinearSlop), (BodyB.GetWorldPoint(anchorB) - BodyA.GetWorldPoint(anchorA)).Length);
WarmStarting = configManager.GetCVar(CVars.WarmStarting);
_minLength = _length;
_maxLength = _length;
}
/// <summary>
/// Does the DistanceJoint warmstart? Can be overridden from the cvar default.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public bool WarmStarting
{
get => _warmStarting;
set
{
if (_warmStarting == value) return;
_warmStarting = value;
Dirty();
}
}
private bool _warmStarting;
/// <summary>
/// The local anchor point relative to bodyA's origin.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public Vector2 LocalAnchorA
{
get => _localAnchorA;
set
{
if (_localAnchorA.EqualsApprox(value)) return;
_localAnchorA = value;
Dirty();
}
}
private Vector2 _localAnchorA;
/// <summary>
/// The local anchor point relative to bodyB's origin.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public Vector2 LocalAnchorB
{
get => _localAnchorB;
set
{
if (_localAnchorB.EqualsApprox(value)) return;
_localAnchorB = value;
Dirty();
}
}
private Vector2 _localAnchorB;
public override Vector2 WorldAnchorA
{
get => BodyA.GetWorldPoint(LocalAnchorA);
set => DebugTools.Assert(false, "You can't set the world anchor on this joint type.");
}
public override Vector2 WorldAnchorB
{
get => BodyB.GetWorldPoint(LocalAnchorB);
set => DebugTools.Assert(false, "You can't set the world anchor on this joint type.");
}
/// <summary>
/// The natural length between the anchor points.
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float Length
{
get => _length;
set
{
if (MathHelper.CloseTo(value, _length)) return;
_impulse = 0.0f;
_length = MathF.Max(IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.LinearSlop), _length);
Dirty();
}
}
private float _length;
/// <summary>
/// The upper limit allowed between the 2 bodies.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float MaxLength
{
get => _maxLength;
set
{
if (MathHelper.CloseTo(value, _maxLength)) return;
_upperImpulse = 0.0f;
_maxLength = MathF.Max(value, _minLength);
Dirty();
}
}
private float _maxLength;
/// <summary>
/// The lower limit allowed between the 2 bodies.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float MinLength
{
get => _minLength;
set
{
if (MathHelper.CloseTo(value, _minLength)) return;
_lowerImpulse = 0.0f;
_minLength = Math.Clamp(_minLength, IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.LinearSlop), MaxLength);
Dirty();
}
}
private float _minLength;
[ViewVariables(VVAccess.ReadWrite)]
public float Stiffness
{
get => _stiffness;
set
{
if (MathHelper.CloseTo(_stiffness, value)) return;
_stiffness = value;
Dirty();
}
}
private float _stiffness;
[ViewVariables(VVAccess.ReadWrite)]
public float Damping
{
get => _damping;
set
{
if (MathHelper.CloseTo(_damping, value)) return;
_damping = value;
Dirty();
}
}
private float _damping;
/// <summary>
/// Get the reaction force given the inverse time step. Unit is N.
/// </summary>
/// <param name="invDt"></param>
/// <returns></returns>
public override Vector2 GetReactionForce(float invDt)
{
Vector2 F = _u * invDt * (_impulse + _lowerImpulse - _upperImpulse);
return F;
}
public void LinearStiffness(float frequency, float dampingRatio)
{
var massA = BodyA.Mass;
var massB = BodyB.Mass;
float mass;
if (massA > 0.0f && massB > 0.0f)
{
mass = massA * massB / (massA + massB);
}
else if (massA > 0.0f)
{
mass = massA;
}
else
{
mass = massB;
}
var omega = 2.0f * MathF.PI * frequency;
Stiffness = mass * omega * omega;
Damping = 2.0f * mass * dampingRatio * omega;
}
/// <summary>
/// Get the reaction torque given the inverse time step.
/// Unit is N*m. This is always zero for a distance joint.
/// </summary>
/// <param name="invDt"></param>
/// <returns></returns>
public override float GetReactionTorque(float invDt)
{
return 0.0f;
}
internal override void InitVelocityConstraints(SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = Vector2.Zero; //BodyA->m_sweep.localCenter;
_localCenterB = Vector2.Zero; //BodyB->m_sweep.localCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_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 cB = data.Positions[_indexB];
float aB = data.Angles[_indexB];
var vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
Quaternion2D qA = new(aA), qB = new(aB);
_rA = Transform.Mul(qA, LocalAnchorA - _localCenterA);
_rB = Transform.Mul(qB, LocalAnchorB - _localCenterB);
_u = cB + _rB - cA - _rA;
var configManager = IoCManager.Resolve<IConfigurationManager>();
var linearSlop = configManager.GetCVar(CVars.LinearSlop);
// Handle singularity.
_currentLength = _u.Length;
if (_currentLength > linearSlop)
{
_u *= 1.0f / _currentLength;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
}
float crAu = Vector2.Cross(_rA, _u);
float crBu = Vector2.Cross(_rB, _u);
float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Stiffness > 0.0f && _minLength < _maxLength)
{
// soft
float C = _currentLength - _length;
float d = Damping;
float k = Stiffness;
// magic formulas
float h = data.FrameTime;
// gamma = 1 / (h * (d + h * k))
// the extra factor of h in the denominator is since the lambda is an impulse, not a force
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invMass += _gamma;
_softMass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
}
else
{
// rigid
_gamma = 0.0f;
_bias = 0.0f;
_softMass = _mass;
}
if (_warmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.DtRatio;
_lowerImpulse *= data.DtRatio;
_upperImpulse *= data.DtRatio;
var P = _u * (_impulse + _lowerImpulse - _upperImpulse);
vA -= P * _invMassA;
wA -= _invIA * Vector2.Cross(_rA, P);
vB += P * _invMassB;
wB += _invIB * Vector2.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
data.LinearVelocities[_indexA] = vA;
data.AngularVelocities[_indexA] = wA;
data.LinearVelocities[_indexB] = vB;
data.AngularVelocities[_indexB] = wB;
}
internal override void SolveVelocityConstraints(SolverData data)
{
var vA = data.LinearVelocities[_indexA];
float wA = data.AngularVelocities[_indexA];
var vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
if (_minLength < _maxLength)
{
if (Stiffness > 0.0f)
{
// Cdot = dot(u, v + cross(w, r))
var vpA = vA + Vector2.Cross(wA, _rA);
var vpB = vB + Vector2.Cross(wB, _rB);
float Cdot = Vector2.Dot(_u, vpB - vpA);
float impulse = -_softMass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
// TODO: Ability to make this one-sided.
var P = _u * impulse;
vA -= P * _invMassA;
wA -= _invIA * Vector2.Cross(_rA, P);
vB += P * _invMassB;
wB += _invIB * Vector2.Cross(_rB, P);
}
// lower
{
float C = _currentLength - _minLength;
float bias = MathF.Max(0.0f, C) * data.InvDt;
var vpA = vA + Vector2.Cross(wA, _rA);
var vpB = vB + Vector2.Cross(wB, _rB);
float Cdot = Vector2.Dot(_u, vpB - vpA);
float impulse = -_mass * (Cdot + bias);
float oldImpulse = _lowerImpulse;
_lowerImpulse = MathF.Max(0.0f, _lowerImpulse + impulse);
impulse = _lowerImpulse - oldImpulse;
var P = _u * impulse;
vA -= P * _invMassA;
wA -= _invIA * Vector2.Cross(_rA, P);
vB += P * _invMassB;
wB += _invIB * Vector2.Cross(_rB, P);
}
// upper
{
float C = _maxLength - _currentLength;
float bias = MathF.Max(0.0f, C) * data.InvDt;
var vpA = vA + Vector2.Cross(wA, _rA);
var vpB = vB + Vector2.Cross(wB, _rB);
float Cdot = Vector2.Dot(_u, vpA - vpB);
float impulse = -_mass * (Cdot + bias);
float oldImpulse = _upperImpulse;
_upperImpulse = MathF.Max(0.0f, _upperImpulse + impulse);
impulse = _upperImpulse - oldImpulse;
var P = _u * -impulse;
vA -= P * _invMassA;
wA -= _invIA * Vector2.Cross(_rA, P);
vB += P * _invMassB;
wB += _invIB * Vector2.Cross(_rB, P);
}
}
else
{
// Equal limits
// Cdot = dot(u, v + cross(w, r))
var vpA = vA + Vector2.Cross(wA, _rA);
var vpB = vB + Vector2.Cross(wB, _rB);
float Cdot = Vector2.Dot(_u, vpB - vpA);
float impulse = -_mass * Cdot;
_impulse += impulse;
var P = _u * impulse;
vA -= P * _invMassA;
wA -= _invIA * Vector2.Cross(_rA, P);
vB += P * _invMassB;
wB += _invIB * Vector2.Cross(_rB, P);
}
data.LinearVelocities[_indexA] = vA;
data.AngularVelocities[_indexA] = wA;
data.LinearVelocities[_indexB] = vB;
data.AngularVelocities[_indexB] = wB;
}
internal override bool SolvePositionConstraints(SolverData data)
{
var cA = data.Positions[_indexA];
float aA = data.Angles[_indexA];
var cB = data.Positions[_indexB];
float aB = data.Angles[_indexB];
Quaternion2D qA = new(aA), qB = new(aB);
var rA = Transform.Mul(qA, LocalAnchorA - _localCenterA);
var rB = Transform.Mul(qB, LocalAnchorB - _localCenterB);
var u = cB + rB - cA - rA;
float length = u.Length;
u = u.Normalized;
float C;
if (MathHelper.CloseTo(_minLength, _maxLength))
{
C = length - _minLength;
}
else if (length < _minLength)
{
C = length - _minLength;
}
else if (_maxLength < length)
{
C = length - _maxLength;
}
else
{
return true;
}
float impulse = -_mass * C;
var P = u * impulse;
cA -= P * _invMassA;
aA -= _invIA * Vector2.Cross(rA, P);
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;
return MathF.Abs(C) < IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.LinearSlop);
}
public bool Equals(DistanceJoint? other)
{
if (ReferenceEquals(null, other)) return false;
if (ReferenceEquals(this, other)) return true;
return LocalAnchorA.EqualsApprox(other.LocalAnchorA) &&
LocalAnchorB.EqualsApprox(other.LocalAnchorB) &&
MathHelper.CloseTo(Length, other.Length) &&
MathHelper.CloseTo(Stiffness, other.Stiffness) &&
MathHelper.CloseTo(Damping, other.Damping) &&
MathHelper.CloseTo(MaxLength, other.MaxLength) &&
MathHelper.CloseTo(MinLength, other.MinLength);
}
}
}

312
Robust.Shared/Physics/Dynamics/Joints/FrictionJoint.cs
View File

@@ -1,312 +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 Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Joints
{
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
/// <summary>
/// Friction joint. This is used for top-down friction.
/// It provides 2D translational friction and angular friction.
/// </summary>
[Serializable, NetSerializable]
public sealed class FrictionJoint : Joint, IEquatable<FrictionJoint>
{
// Solver shared
[NonSerialized] private Vector2 _linearImpulse;
[NonSerialized] private float _angularImpulse;
// Solver temp
[NonSerialized] private int _indexA;
[NonSerialized] private int _indexB;
[NonSerialized] private Vector2 _rA;
[NonSerialized] private Vector2 _rB;
[NonSerialized] private Vector2 _localCenterA;
[NonSerialized] private Vector2 _localCenterB;
[NonSerialized] private float _invMassA;
[NonSerialized] private float _invMassB;
[NonSerialized] private float _invIA;
[NonSerialized] private float _invIB;
[NonSerialized] private float _angularMass;
[NonSerialized] private Vector2[] _linearMass = new Vector2[2];
public override JointType JointType => JointType.Friction;
/// <summary>
/// The maximum friction force in N.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float MaxForce { get; set; }
/// <summary>
/// The maximum friction torque in N-m.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float MaxTorque { get; set; }
/// <summary>
/// The local anchor point on BodyA
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public Vector2 LocalAnchorA { get; set; }
/// <summary>
/// The local anchor point on BodyB
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public Vector2 LocalAnchorB { get; set; }
public override Vector2 WorldAnchorA
{
get => BodyA.GetWorldPoint(LocalAnchorA);
set => LocalAnchorA = BodyA.GetLocalPoint(value);
}
public override Vector2 WorldAnchorB
{
get => BodyB.GetWorldPoint(LocalAnchorB);
set => LocalAnchorB = BodyB.GetLocalPoint(value);
}
/// <summary>
/// Constructor for FrictionJoint.
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="anchor"></param>
/// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param>
public FrictionJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, Vector2 anchor, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(anchor);
LocalAnchorB = BodyB.GetLocalPoint(anchor);
}
else
{
LocalAnchorA = anchor;
LocalAnchorB = anchor;
}
}
public FrictionJoint(PhysicsComponent bodyA, PhysicsComponent bodyB, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(Vector2.Zero);
LocalAnchorB = BodyB.GetLocalPoint(Vector2.Zero);
}
else
{
LocalAnchorA = Vector2.Zero;
LocalAnchorB = Vector2.Zero;
}
}
public override void ExposeData(ObjectSerializer serializer)
{
base.ExposeData(serializer);
serializer.DataField(this, x => x.MaxForce, "maxForce", 0f);
serializer.DataField(this, x => x.MaxTorque, "maxTorque", 0f);
}
public override Vector2 GetReactionForce(float invDt)
{
return _linearImpulse * invDt;
}
public override float GetReactionTorque(float invDt)
{
return invDt * _angularImpulse;
}
internal override void InitVelocityConstraints(SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = Vector2.Zero; // BodyA._sweep.LocalCenter;
_localCenterB = Vector2.Zero; //BodyB._sweep.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;
_invIB = BodyB.InvI;
float aA = data.Angles[_indexA];
Vector2 vA = data.LinearVelocities[_indexA];
float wA = data.AngularVelocities[_indexA];
float aB = data.Angles[_indexB];
Vector2 vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
Quaternion2D qA = new(aA), qB = new(aB);
// Compute the effective mass matrix.
_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;
var K = new Vector2[2];
K[0].X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
K[0].Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
K[1].X = K[0].Y;
K[1].Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
_linearMass = K.Inverse();
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
// ReSharper disable once ConditionIsAlwaysTrueOrFalse
if (IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.WarmStarting))
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.DtRatio;
_angularImpulse *= data.DtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= P * mA;
wA -= iA * (Vector2.Cross(_rA, P) + _angularImpulse);
vB += P * mB;
wB += iB * (Vector2.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
data.LinearVelocities[_indexA] = vA;
data.AngularVelocities[_indexA] = wA;
data.LinearVelocities[_indexB] = vB;
data.AngularVelocities[_indexB] = wB;
}
internal override void SolveVelocityConstraints(SolverData data)
{
Vector2 vA = data.LinearVelocities[_indexA];
float wA = data.AngularVelocities[_indexA];
Vector2 vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
float h = data.FrameTime;
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = h * MaxTorque;
_angularImpulse = Math.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + Vector2.Cross(wB, _rB) - vA - Vector2.Cross(wA, _rA);
Vector2 impulse = -Transform.Mul(_linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = h * MaxForce;
if (_linearImpulse.LengthSquared > maxImpulse * maxImpulse)
{
_linearImpulse = _linearImpulse.Normalized;
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= impulse * mA;
wA -= iA * Vector2.Cross(_rA, impulse);
vB += impulse * mB;
wB += iB * Vector2.Cross(_rB, impulse);
}
data.LinearVelocities[_indexA] = vA;
data.AngularVelocities[_indexA] = wA;
data.LinearVelocities[_indexB] = vB;
data.AngularVelocities[_indexB] = wB;
}
internal override bool SolvePositionConstraints(SolverData data)
{
return true;
}
public bool Equals(FrictionJoint? other)
{
if (ReferenceEquals(null, other)) return false;
if (ReferenceEquals(this, other)) return true;
return base.Equals(other) &&
MathHelper.CloseTo(MaxForce, other.MaxForce) &&
MathHelper.CloseTo(MaxTorque, other.MaxTorque) &&
LocalAnchorA.EqualsApprox(other.LocalAnchorA) &&
LocalAnchorB.EqualsApprox(other.LocalAnchorB);
}
}
}

244
Robust.Shared/Physics/Dynamics/Joints/Joint.cs
View File

@@ -1,244 +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.Diagnostics;
using Robust.Shared.GameObjects;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Joints
{
public enum JointType : byte
{
Unknown,
Revolute,
Prismatic,
Distance,
Pulley,
//Mouse, <- We have fixed mouse
Gear,
Wheel,
Weld,
Friction,
Rope,
Motor,
// Sloth note: I Removed FPE's fixed joints
}
public enum LimitState : byte
{
Inactive,
AtLower,
AtUpper,
Equal,
}
[Serializable, NetSerializable]
public abstract class Joint : IExposeData, IEquatable<Joint>
{
/// <summary>
/// Indicate if this join is enabled or not. Disabling a joint
/// means it is still in the simulation, but inactive.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public bool Enabled
{
get => _enabled;
set
{
if (_enabled == value) return;
_enabled = value;
Dirty();
}
}
private bool _enabled = true;
[NonSerialized] internal JointEdge EdgeA = new();
[NonSerialized] internal JointEdge EdgeB = new();
/// <summary>
/// Has this joint already been added to an island.
/// </summary>
[NonSerialized] internal bool IslandFlag;
// For some reason in FPE this is settable?
/// <summary>
/// Gets the type of the joint.
/// </summary>
/// <value>The type of the joint.</value>
public abstract JointType JointType { get; }
/// <summary>
/// Get the first body attached to this joint.
/// </summary>
[field:NonSerialized] public PhysicsComponent BodyA { get; internal set; }
public EntityUid BodyAUid { get; internal set; }
/// <summary>
/// Get the second body attached to this joint.
/// </summary>
[field:NonSerialized] public PhysicsComponent BodyB { get; internal set; }
public EntityUid BodyBUid { get; internal set; }
/// <summary>
/// Get the anchor point on bodyA in world coordinates.
/// On some joints, this value indicate the anchor point within the world.
/// </summary>
public abstract Vector2 WorldAnchorA { get; set; }
/// <summary>
/// Get the anchor point on bodyB in world coordinates.
/// On some joints, this value indicate the anchor point within the world.
/// </summary>
public abstract Vector2 WorldAnchorB { get; set; }
/// <summary>
/// Set this flag to true if the attached bodies should collide.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public bool CollideConnected
{
get => _collideConnected;
set
{
if (_collideConnected == value) return;
_collideConnected = value;
Dirty();
}
}
private bool _collideConnected;
/// <summary>
/// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks.
/// The default value is float.MaxValue, which means it never breaks.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float Breakpoint
{
get => _breakpoint;
set
{
if (MathHelper.CloseTo(_breakpoint, value)) return;
_breakpoint = value;
_breakpointSquared = _breakpoint * _breakpoint;
Dirty();
}
}
private float _breakpoint = float.MaxValue;
private double _breakpointSquared = Double.MaxValue;
public virtual void ExposeData(ObjectSerializer serializer)
{
serializer.DataField(this, x => x.Enabled, "enabled", true);
// TODO: Later nerd
// serializer.DataField(this, x => x.BodyA, "bodyA", EntityUid.Invalid);
// serializer.DataField(this, x => x.BodyB, "bodyB", Ent);
serializer.DataField(this, x => x.CollideConnected, "collideConnected", false);
}
protected void Dirty()
{
BodyA.Dirty();
BodyB.Dirty();
}
protected Joint(PhysicsComponent bodyA, PhysicsComponent bodyB)
{
//Can't connect a joint to the same body twice.
Debug.Assert(bodyA != bodyB);
BodyA = bodyA;
BodyB = bodyB;
}
/// <summary>
/// Get the reaction force on body at the joint anchor in Newtons.
/// </summary>
/// <param name="invDt">The inverse delta time.</param>
public abstract Vector2 GetReactionForce(float invDt);
/// <summary>
/// Get the reaction torque on the body at the joint anchor in N*m.
/// </summary>
/// <param name="invDt">The inverse delta time.</param>
public abstract float GetReactionTorque(float invDt);
protected void WakeBodies()
{
if (BodyA != null)
BodyA.Awake = true;
if (BodyB != null)
BodyB.Awake = true;
}
internal abstract void InitVelocityConstraints(SolverData data);
internal void Validate(float invDt)
{
if (!Enabled)
return;
float jointErrorSquared = GetReactionForce(invDt).LengthSquared;
if (MathF.Abs(jointErrorSquared) <= _breakpointSquared)
return;
Enabled = false;
// BodyA
/* TODO: Dis, just use comp messages and a system message
if (Broke != null)
Broke(this, MathF.Sqrt(jointErrorSquared));
*/
}
internal abstract void SolveVelocityConstraints(SolverData data);
/// <summary>
/// Solves the position constraints.
/// </summary>
/// <returns>returns true if the position errors are within tolerance.</returns>
internal abstract bool SolvePositionConstraints(SolverData data);
public bool Equals(Joint? other)
{
if (ReferenceEquals(null, other)) return false;
if (ReferenceEquals(this, other)) return true;
return Enabled == other.Enabled &&
JointType == other.JointType &&
BodyAUid.Equals(other.BodyAUid) &&
BodyBUid.Equals(other.BodyBUid) &&
CollideConnected == other.CollideConnected &&
MathHelper.CloseTo(_breakpoint, other._breakpoint);
}
}
}

56
Robust.Shared/Physics/Dynamics/Joints/JointEdge.cs
View File

@@ -1,56 +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 Robust.Shared.GameObjects;
namespace Robust.Shared.Physics.Dynamics.Joints
{
/// <summary>
/// A joint edge is used to connect bodies and joints together
/// in a joint graph where each body is a node and each joint
/// is an edge. A joint edge belongs to a doubly linked list
/// maintained in each attached body. Each joint has two joint
/// nodes, one for each attached body.
/// </summary>
public sealed class JointEdge
{
/// <summary>
/// The joint.
/// </summary>
public Joint Joint { get; set; } = default!;
/// <summary>
/// The next joint edge in the body's joint list.
/// </summary>
public JointEdge? Next { get; set; }
/// <summary>
/// Provides quick access to the other body attached.
/// </summary>
public PhysicsComponent Other { get; set; } = default!;
/// <summary>
/// The previous joint edge in the body's joint list.
/// </summary>
public JointEdge? Prev { get; set; }
}
}

23
Robust.Shared/Physics/Dynamics/Joints/JointHelpers.cs
View File

@@ -1,23 +0,0 @@
using Robust.Shared.GameObjects;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Dynamics.Joints
{
// better than calcium
public static class JointHelpers
{
public static DistanceJoint CreateDistanceJoint(this PhysicsComponent bodyA, PhysicsComponent bodyB)
{
var joint = new DistanceJoint(bodyA, bodyB, Vector2.Zero, Vector2.Zero);
bodyA.AddJoint(joint);
return joint;
}
public static SlothJoint CreateSlothJoint(this PhysicsComponent bodyA, PhysicsComponent bodyB)
{
var joint = new SlothJoint(bodyA, bodyB);
bodyA.AddJoint(joint);
return joint;
}
}
}

126
Robust.Shared/Physics/Dynamics/Joints/SlothJoint.cs
View File

@@ -1,126 +0,0 @@
using System;
using JetBrains.Annotations;
using Robust.Shared.GameObjects;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Joints
{
[Serializable, NetSerializable]
public class SlothJoint : Joint
{
// Solver temp
[NonSerialized] private int _indexA;
[NonSerialized] private int _indexB;
[NonSerialized] private Vector2 _rA;
[NonSerialized] private Vector2 _rB;
[NonSerialized] private Vector2 _localCenterA;
[NonSerialized] private Vector2 _localCenterB;
[NonSerialized] private float _invMassA;
[NonSerialized] private float _invMassB;
[NonSerialized] private float _invIA;
[NonSerialized] private float _invIB;
[NonSerialized] private float _mass;
[NonSerialized] private float _currentLength;
[NonSerialized] private float _softMass;
public SlothJoint(PhysicsComponent bodyA, PhysicsComponent bodyB) : base(bodyA, bodyB)
{
}
public override JointType JointType => JointType.Distance;
[field:NonSerialized]
public override Vector2 WorldAnchorA { get; set; }
[field:NonSerialized]
public override Vector2 WorldAnchorB { get; set; }
[ViewVariables(VVAccess.ReadWrite)]
public float MaxLength
{
get => _maxLength;
set
{
if (MathHelper.CloseTo(value, _maxLength)) return;
_maxLength = value;
Dirty();
}
}
private float _maxLength;
public override Vector2 GetReactionForce(float invDt)
{
// TODO: Need break force
return Vector2.Zero;
}
public override float GetReactionTorque(float invDt)
{
return 0f;
}
internal override void InitVelocityConstraints(SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = Vector2.Zero; //BodyA->m_sweep.localCenter;
_localCenterB = Vector2.Zero; //BodyB->m_sweep.localCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;
_invIB = BodyB.InvI;
_currentLength = (data.Positions[_indexA] - data.Positions[_indexB]).Length;
_softMass = _mass;
}
internal override void SolveVelocityConstraints(SolverData data)
{
if (_currentLength < _maxLength) return;
var posA = data.Positions[_indexA];
var posB = data.Positions[_indexB];
var vA = data.LinearVelocities[_indexA];
float wA = data.AngularVelocities[_indexA];
var vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
var correctionDistance = _maxLength - _currentLength;
//var P = _u * impulse;
//vA -= P * _invMassA;
//wA -= _invIA * Vector2.Cross(_rA, P);
//vB += P * _invMassB;
//wB += _invIB * Vector2.Cross(_rB, P);
}
internal override bool SolvePositionConstraints(SolverData data)
{
if (_currentLength < _maxLength) return true;
var posA = data.Positions[_indexA];
var posB = data.Positions[_indexB];
var vA = data.LinearVelocities[_indexA];
float wA = data.AngularVelocities[_indexA];
var vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
var correctionDistance = _maxLength - _currentLength;
data.Positions[_indexB] -= correctionDistance;
//var P = _u * impulse;
//vA -= P * _invMassA;
//wA -= _invIA * Vector2.Cross(_rA, P);
//vB += P * _invMassB;
//wB += _invIB * Vector2.Cross(_rB, P);
return true;
}
}
}

544
Robust.Shared/Physics/Dynamics/PhysicsIsland.cs
View File

@@ -1,544 +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.Collections.Generic;
using Robust.Shared.Configuration;
using Robust.Shared.Containers;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics.Contacts;
using Robust.Shared.Physics.Dynamics.Joints;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Dynamics
{
/*
* 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.
*/
internal sealed class PhysicsIsland
{
[Dependency] private readonly IConfigurationManager _configManager = default!;
[Dependency] private readonly IMapManager _mapManager = default!;
private ContactSolver _contactSolver = default!;
private float _angTolSqr;
private float _linTolSqr;
private bool _warmStarting;
private int _velocityIterations;
private float _maxLinearVelocity;
private float _maxAngularVelocity;
private int _positionIterations;
private bool _sleepAllowed; // BONAFIDE MONAFIED
private float _timeToSleep;
public IPhysBody[] Bodies = Array.Empty<IPhysBody>();
private Contact[] _contacts = Array.Empty<Contact>();
private Joint[] _joints = Array.Empty<Joint>();
// These are joint in box2d / derivatives
private Vector2[] _linearVelocities = Array.Empty<Vector2>();
private float[] _angularVelocities = Array.Empty<float>();
private Vector2[] _positions = Array.Empty<Vector2>();
private float[] _angles = Array.Empty<float>();
internal SolverData SolverData = new();
/// <summary>
/// How many bodies we can fit in the island before needing to re-size.
/// </summary>
public int BodyCapacity { get; private set; }
/// <summary>
/// How many bodies are in the island.
/// </summary>
public int BodyCount { get; private set; }
/// <summary>
/// How many contacts we can fit in the island before needing to re-size.
/// </summary>
public int ContactCapacity { get; private set; }
/// <summary>
/// How many contacts are in the island.
/// </summary>
public int ContactCount { get; private set; }
/// <summary>
/// How many joints we can fit in the island before needing to re-size.
/// </summary>
public int JointCapacity { get; private set; }
/// <summary>
/// How many joints are in the island.
/// </summary>
public int JointCount { get; private set; }
public void Initialize()
{
IoCManager.InjectDependencies(this);
_contactSolver = new ContactSolver();
_contactSolver.Initialize();
// Values
_angTolSqr = MathF.Pow(_configManager.GetCVar(CVars.AngularSleepTolerance), 2);
_configManager.OnValueChanged(CVars.AngularSleepTolerance, value => _angTolSqr = MathF.Pow(value, 2));
_linTolSqr = MathF.Pow(_configManager.GetCVar(CVars.LinearSleepTolerance), 2);
_configManager.OnValueChanged(CVars.LinearSleepTolerance, value => _linTolSqr = MathF.Pow(value, 2));
_warmStarting = _configManager.GetCVar(CVars.WarmStarting);
_configManager.OnValueChanged(CVars.WarmStarting, value => _warmStarting = value);
_velocityIterations = _configManager.GetCVar(CVars.VelocityIterations);
_configManager.OnValueChanged(CVars.VelocityIterations, value => _velocityIterations = value);
_maxLinearVelocity = _configManager.GetCVar(CVars.MaxLinVelocity);
_configManager.OnValueChanged(CVars.MaxLinVelocity, value => _maxLinearVelocity = value);
_maxAngularVelocity = _configManager.GetCVar(CVars.MaxAngVelocity);
_configManager.OnValueChanged(CVars.MaxAngVelocity, value => _maxAngularVelocity = value);
_positionIterations = _configManager.GetCVar(CVars.PositionIterations);
_configManager.OnValueChanged(CVars.PositionIterations, value => _positionIterations = value);
_sleepAllowed = _configManager.GetCVar(CVars.SleepAllowed);
_configManager.OnValueChanged(CVars.SleepAllowed, value => _sleepAllowed = value);
_timeToSleep = _configManager.GetCVar(CVars.TimeToSleep);
_configManager.OnValueChanged(CVars.TimeToSleep, value => _timeToSleep = value);
}
public void Add(IPhysBody body)
{
body.IslandIndex = BodyCount;
Bodies[BodyCount++] = body;
}
public void Add(Contact contact)
{
_contacts[ContactCount++] = contact;
}
public void Add(Joint joint)
{
_joints[JointCount++] = joint;
}
public void Clear()
{
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 Reset(int bodyCapacity, int contactCapacity, int jointCapacity)
{
BodyCapacity = bodyCapacity;
BodyCount = 0;
ContactCapacity = contactCapacity;
ContactCount = 0;
JointCapacity = jointCapacity;
JointCount = 0;
if (Bodies.Length < bodyCapacity)
{
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)
{
Array.Resize(ref _contacts, contactCapacity * 2);
}
if (_joints.Length < jointCapacity)
{
Array.Resize(ref _joints, jointCapacity * 2);
}
}
/// <summary>
/// Go through all the bodies in this island and solve.
/// </summary>
/// <param name="gravity"></param>
/// <param name="frameTime"></param>
/// <param name="dtRatio"></param>
/// <param name="invDt"></param>
/// <param name="prediction"></param>
/// <param name="deferredUpdates">Add any transform updates to a deferred list</param>
public void Solve(Vector2 gravity, float frameTime, float dtRatio, float invDt, bool prediction, List<ITransformComponent> deferredUpdates)
{
#if DEBUG
var debugBodies = new List<IPhysBody>();
for (var i = 0; i < BodyCount; i++)
{
debugBodies.Add(Bodies[i]);
}
IoCManager.Resolve<IEntityManager>().EventBus.RaiseEvent(EventSource.Local, new IslandSolveMessage(debugBodies));
#endif
for (var i = 0; i < BodyCount; i++)
{
var body = Bodies[i];
// In future we'll set these to existing
// Didn't use the old variable names because they're hard to read
var position = body.Owner.Transform.WorldPosition;
// DebugTools.Assert(!float.IsNaN(position.X) && !float.IsNaN(position.Y));
var angle = (float) body.Owner.Transform.WorldRotation.Theta;
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.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 = _joints[i];
if (!joint.Enabled) continue;
joint.InitVelocityConstraints(SolverData);
}
// Velocity solver
for (var i = 0; i < _velocityIterations; i++)
{
for (var j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
continue;
joint.SolveVelocityConstraints(SolverData);
joint.Validate(invDt);
}
_contactSolver.SolveVelocityConstraints();
}
// Store for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (var i = 0; i < BodyCount; i++)
{
var linearVelocity = _linearVelocities[i];
var angularVelocity = _angularVelocities[i];
var position = _positions[i];
var angle = _angles[i];
var translation = linearVelocity * frameTime;
if (Vector2.Dot(translation, translation) > _maxLinearVelocity)
{
var ratio = _maxLinearVelocity / translation.Length;
linearVelocity *= ratio;
}
var rotation = angularVelocity * frameTime;
if (rotation * rotation > _maxAngularVelocity)
{
var ratio = _maxAngularVelocity / MathF.Abs(rotation);
angularVelocity *= ratio;
}
// Integrate
position += linearVelocity * frameTime;
angle += angularVelocity * frameTime;
_linearVelocities[i] = linearVelocity;
_angularVelocities[i] = angularVelocity;
_positions[i] = position;
_angles[i] = angle;
}
var positionSolved = false;
for (var i = 0; i < _positionIterations; i++)
{
var contactsOkay = _contactSolver.SolvePositionConstraints();
var jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
continue;
bool jointOkay = joint.SolvePositionConstraints(SolverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
positionSolved = true;
break;
}
}
// 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];
// body.Sweep.Center = bodyPos;
// body.Sweep.Angle = angle;
// DebugTools.Assert(!float.IsNaN(bodyPos.X) && !float.IsNaN(bodyPos.Y));
var transform = body.Owner.Transform;
// Defer MoveEvent / RotateEvent until the end of the physics step so cache can be better.
transform.DeferUpdates = true;
transform.WorldPosition = bodyPos;
transform.WorldRotation = angle;
transform.DeferUpdates = false;
if (transform.UpdatesDeferred)
{
deferredUpdates.Add(transform);
}
body.LinearVelocity = _linearVelocities[i];
body.AngularVelocity = _angularVelocities[i];
}
// 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)
{
body.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
body.SleepTime += frameTime;
minSleepTime = MathF.Min(minSleepTime, body.SleepTime);
}
}
if (minSleepTime >= _timeToSleep && positionSolved)
{
for (var i = 0; i < BodyCount; i++)
{
var body = Bodies[i];
body.Awake = false;
}
}
}
}
}
/// <summary>
/// Easy way of passing around the data required for the contact solver.
/// </summary>
internal sealed class SolverData
{
public float FrameTime { get; set; }
public float DtRatio { get; set; }
public float InvDt { 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!;
}
}

645
Robust.Shared/Physics/Dynamics/PhysicsMap.cs
View File

@@ -1,645 +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.Collections.Generic;
using Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics.Contacts;
using Robust.Shared.Physics.Dynamics.Joints;
using Robust.Shared.Utility;
using PhysicsComponent = Robust.Shared.GameObjects.PhysicsComponent;
namespace Robust.Shared.Physics.Dynamics
{
public sealed class PhysicsMap
{
[Dependency] private readonly IConfigurationManager _configManager = default!;
[Dependency] private readonly IEntityManager _entityManager = default!;
// TODO: Pausing but need to merge in shared IPauseManager
// AKA world.
private SharedPhysicsSystem _physicsSystem = default!;
internal ContactManager ContactManager = new();
private bool _autoClearForces;
/// <summary>
/// Change the global gravity vector.
/// </summary>
public Vector2 Gravity
{
get => _gravity;
set
{
if (_gravity.EqualsApprox(value)) return;
// Force every body awake just in case.
foreach (var body in Bodies)
{
if (body.BodyType != BodyType.Dynamic) continue;
body.Awake = true;
}
_gravity = value;
}
}
private Vector2 _gravity;
// TODO: Given physics bodies are a common thing to be listening for on moveevents it's probably beneficial to have 2 versions; one that includes the entity
// and one that includes the body
private List<ITransformComponent> _deferredUpdates = new();
/// <summary>
/// All bodies present on this map.
/// </summary>
public HashSet<PhysicsComponent> Bodies = new();
/// <summary>
/// All awake bodies on this map.
/// </summary>
public HashSet<PhysicsComponent> AwakeBodies = new();
/// <summary>
/// Temporary body storage during solving.
/// </summary>
private List<PhysicsComponent> _awakeBodyList = new();
/// <summary>
/// Get all the joints on this map
/// </summary>
public List<Joint> Joints { get; private set; } = new();
/// <summary>
/// Temporarily store island-bodies for easier iteration.
/// </summary>
private HashSet<PhysicsComponent> _islandSet = new();
private HashSet<Joint> _queuedJointAdd = new();
private HashSet<Joint> _queuedJointRemove = new();
private HashSet<PhysicsComponent> _queuedWake = new();
private HashSet<PhysicsComponent> _queuedSleep = new();
private Queue<CollisionChangeMessage> _queuedCollisionMessages = new();
/// <summary>
/// We'll re-use contacts where possible to save on allocations.
/// </summary>
internal Queue<Contact> ContactPool = new(128);
private PhysicsIsland _island = default!;
/// <summary>
/// To build islands we do a depth-first search of all colliding bodies and group them together.
/// This stack is used to store bodies that are colliding.
/// </summary>
private PhysicsComponent[] _stack = new PhysicsComponent[64];
/// <summary>
/// Store last tick's invDT
/// </summary>
private float _invDt0;
public MapId MapId { get; }
public PhysicsMap(MapId mapId)
{
MapId = mapId;
_physicsSystem = EntitySystem.Get<SharedPhysicsSystem>();
}
public void Initialize()
{
IoCManager.InjectDependencies(this);
ContactManager.Initialize();
ContactManager.MapId = MapId;
_island = new PhysicsIsland();
_island.Initialize();
_autoClearForces = _configManager.GetCVar(CVars.AutoClearForces);
_configManager.OnValueChanged(CVars.AutoClearForces, value => _autoClearForces = value);
}
#region AddRemove
public void AddAwakeBody(PhysicsComponent body)
{
_queuedWake.Add(body);
}
public void RemoveBody(PhysicsComponent body)
{
Bodies.Remove(body);
AwakeBodies.Remove(body);
body.DestroyContacts();
}
public void RemoveSleepBody(PhysicsComponent body)
{
_queuedSleep.Add(body);
}
public void AddJoint(Joint joint)
{
// TODO: Need static helper class to easily create Joints
_queuedJointAdd.Add(joint);
}
public void RemoveJoint(Joint joint)
{
_queuedJointRemove.Add(joint);
}
#endregion
#region Queue
private void ProcessChanges()
{
ProcessBodyChanges();
ProcessWakeQueue();
ProcessSleepQueue();
ProcessAddedJoints();
ProcessRemovedJoints();
}
private void ProcessBodyChanges()
{
while (_queuedCollisionMessages.Count > 0)
{
var message = _queuedCollisionMessages.Dequeue();
if (!message.Body.Deleted && message.Body.CanCollide)
{
AddBody(message.Body);
}
else
{
RemoveBody(message.Body);
}
}
}
public void AddBody(PhysicsComponent body)
{
if (Bodies.Contains(body)) return;
// TODO: Kinda dodgy with this and wake shit.
// Look at my note under ProcessWakeQueue
if (body.Awake && body.BodyType != BodyType.Static)
{
_queuedWake.Remove(body);
AwakeBodies.Add(body);
}
Bodies.Add(body);
body.PhysicsMap = this;
}
private void ProcessWakeQueue()
{
foreach (var body in _queuedWake)
{
// Sloth note: So FPE doesn't seem to handle static bodies being woken gracefully as they never sleep
// (No static body's an island so can't increase their min sleep time).
// AFAIK not adding it to woken bodies shouldn't matter for anything tm...
if (!Bodies.Contains(body) || !body.Awake || body.BodyType == BodyType.Static) continue;
AwakeBodies.Add(body);
}
_queuedWake.Clear();
}
private void ProcessSleepQueue()
{
foreach (var body in _queuedSleep)
{
if (body.Awake) continue;
AwakeBodies.Remove(body);
}
_queuedSleep.Clear();
}
private void ProcessAddedJoints()
{
foreach (var joint in _queuedJointAdd)
{
// TODO: Optimise dafuk out of this.
if (Joints.Contains(joint)) continue;
// Just end me, I fucken hate how garbage the physics compstate is.
// because EACH body will have a joint update we needs to check if.
PhysicsComponent? bodyA;
PhysicsComponent? bodyB;
// ReSharper disable once ConditionIsAlwaysTrueOrFalse
if (joint.BodyA == null || joint.BodyB == null)
{
if (!_entityManager.TryGetEntity(joint.BodyAUid, out var bodyAEntity) ||
!_entityManager.TryGetEntity(joint.BodyBUid, out var bodyBEntity))
{
continue;
}
if (!bodyAEntity.TryGetComponent(out bodyA) ||
!bodyBEntity.TryGetComponent(out bodyB))
{
continue;
}
// TODO: Need to mark all this shit as nullable coming in from the state probably.
joint.BodyA = bodyAEntity.GetComponent<PhysicsComponent>();
joint.BodyB = bodyBEntity.GetComponent<PhysicsComponent>();
}
else
{
bodyA = joint.BodyA;
bodyB = joint.BodyB;
}
// BodyA and BodyB should share joints so we can just check if BodyA already has this joint.
for (var je = bodyA.JointEdges; je != null; je = je.Next)
{
if (je.Joint.Equals(joint)) continue;
}
// Connect to the world list.
Joints.Add(joint);
// Connect to the bodies' doubly linked lists.
joint.EdgeA.Joint = joint;
joint.EdgeA.Other = bodyB;
joint.EdgeA.Prev = null;
joint.EdgeA.Next = bodyA.JointEdges;
if (bodyA.JointEdges != null)
bodyA.JointEdges.Prev = joint.EdgeA;
bodyA.JointEdges = joint.EdgeA;
joint.EdgeB.Joint = joint;
joint.EdgeB.Other = bodyA;
joint.EdgeB.Prev = null;
joint.EdgeB.Next = bodyB.JointEdges;
if (bodyB.JointEdges != null)
bodyB.JointEdges.Prev = joint.EdgeB;
bodyB.JointEdges = joint.EdgeB;
joint.BodyAUid = bodyA.Owner.Uid;
joint.BodyBUid = bodyB.Owner.Uid;
// If the joint prevents collisions, then flag any contacts for filtering.
if (!joint.CollideConnected)
{
ContactEdge? edge = bodyB.ContactEdges;
while (edge != null)
{
if (edge.Other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.Contact!.FilterFlag = true;
}
edge = edge.Next;
}
}
bodyA.Dirty();
bodyB.Dirty();
// Note: creating a joint doesn't wake the bodies.
}
_queuedJointAdd.Clear();
}
private void ProcessRemovedJoints()
{
foreach (var joint in _queuedJointRemove)
{
bool collideConnected = joint.CollideConnected;
// Remove from the world list.
Joints.Remove(joint);
// Disconnect from island graph.
PhysicsComponent bodyA = joint.BodyA;
PhysicsComponent bodyB = joint.BodyB;
// Wake up connected bodies.
bodyA.Awake = true;
bodyB.Awake = true;
// Remove from body 1.
if (joint.EdgeA.Prev != null)
{
joint.EdgeA.Prev.Next = joint.EdgeA.Next;
}
if (joint.EdgeA.Next != null)
{
joint.EdgeA.Next.Prev = joint.EdgeA.Prev;
}
if (joint.EdgeA == bodyA.JointEdges)
{
bodyA.JointEdges = joint.EdgeA.Next;
}
joint.EdgeA.Prev = null;
joint.EdgeA.Next = null;
// Remove from body 2
if (joint.EdgeB.Prev != null)
{
joint.EdgeB.Prev.Next = joint.EdgeB.Next;
}
if (joint.EdgeB.Next != null)
{
joint.EdgeB.Next.Prev = joint.EdgeB.Prev;
}
if (joint.EdgeB == bodyB.JointEdges)
{
bodyB.JointEdges = joint.EdgeB.Next;
}
joint.EdgeB.Prev = null;
joint.EdgeB.Next = null;
// If the joint prevents collisions, then flag any contacts for filtering.
if (!collideConnected)
{
ContactEdge? edge = bodyB.ContactEdges;
while (edge != null)
{
if (edge.Other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.Contact!.FilterFlag = true;
}
edge = edge.Next;
}
}
}
_queuedJointRemove.Clear();
}
#endregion
/// <summary>
/// Where the magic happens.
/// </summary>
/// <param name="frameTime"></param>
/// <param name="prediction"></param>
public void Step(float frameTime, bool prediction)
{
// The original doesn't call ProcessChanges quite so much but stuff like collision behaviors
// can edit things during the solver so we'll just handle it as it comes up.
ProcessChanges();
// 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.
ContactManager.FindNewContacts(MapId);
var invDt = frameTime > 0.0f ? 1.0f / frameTime : 0.0f;
var dtRatio = _invDt0 * frameTime;
foreach (var controller in _physicsSystem.Controllers)
{
controller.UpdateBeforeMapSolve(prediction, this, frameTime);
}
ContactManager.Collide();
// TODO: May move this as a PostSolve once we have broadphase collisions where contacts can be generated
// even though the bodies may not technically be colliding
if (!prediction)
ContactManager.PreSolve();
// Remove all deleted entities etc.
ProcessChanges();
// Integrate velocities, solve velocity constraints, and do integration.
Solve(frameTime, dtRatio, invDt, prediction);
// TODO: SolveTOI
foreach (var controller in _physicsSystem.Controllers)
{
controller.UpdateAfterMapSolve(prediction, this, frameTime);
}
// Box2d recommends clearing (if you are) during fixed updates rather than variable if you are using it
if (!prediction && _autoClearForces)
ClearForces();
_invDt0 = invDt;
}
/// <summary>
/// Go through all of the deferred MoveEvents and then run them
/// </summary>
public void ProcessQueue()
{
foreach (var transform in _deferredUpdates)
{
transform.RunDeferred();
}
_deferredUpdates.Clear();
}
private void Solve(float frameTime, float dtRatio, float invDt, bool prediction)
{
// Re-size island for worst-case -> TODO Probably smaller than this given everything's awake at the start?
_island.Reset(Bodies.Count, ContactManager.ActiveContacts.Count, Joints.Count);
DebugTools.Assert(_islandSet.Count == 0);
foreach (var contact in ContactManager.ActiveContacts)
{
contact.IslandFlag = false;
}
foreach (var joint in Joints)
{
joint.IslandFlag = false;
}
// Build and simulated islands from awake bodies.
// Ideally you don't need a stack size for all bodies but we'll optimise it later.
var stackSize = Bodies.Count;
if (stackSize > _stack.Length)
{
Array.Resize(ref _stack, Math.Max(_stack.Length * 2, stackSize));
}
_awakeBodyList.AddRange(AwakeBodies);
// 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.
// prediction && !seed.Predict ||
// AHHH need a way to ignore paused for mapping (seed.Paused && !seed.Owner.TryGetComponent(out IMoverComponent)) ||
if ((seed.Paused && !seed.IgnorePaused) ||
seed.Island ||
!seed.CanCollide ||
seed.BodyType == BodyType.Static) continue;
// Start of a new island
_island.Clear();
var stackCount = 0;
_stack[stackCount++] = seed;
_islandSet.Add(seed);
seed.Island = true;
while (stackCount > 0)
{
var body = _stack[--stackCount];
_island.Add(body);
_islandSet.Add(body);
body.Awake = true;
// Static bodies don't propagate islands
if (body.BodyType == BodyType.Static) continue;
for (var contactEdge = body.ContactEdges; contactEdge != null; contactEdge = contactEdge.Next)
{
var contact = contactEdge.Contact!;
// Has this contact already been added to an island?
if (contact.IslandFlag) 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;
_island.Add(contact);
contact.IslandFlag = true;
var other = contactEdge.Other!;
// Was the other body already added to this island?
if (other.Island) continue;
DebugTools.Assert(stackCount < stackSize);
_stack[stackCount++] = other;
if (!_islandSet.Contains(body))
_islandSet.Add(body);
other.Island = true;
}
for (JointEdge? je = body.JointEdges; je != null; je = je.Next)
{
if (je.Joint.IslandFlag)
{
continue;
}
PhysicsComponent other = je.Other;
// Don't simulate joints connected to inactive bodies.
if (!other.CanCollide) continue;
_island.Add(je.Joint);
je.Joint.IslandFlag = true;
if (other.Island) continue;
DebugTools.Assert(stackCount < stackSize);
_stack[stackCount++] = other;
if (!_islandSet.Contains(body))
_islandSet.Add(body);
other.Island = true;
}
}
_island.Solve(Gravity, frameTime, dtRatio, invDt, prediction, _deferredUpdates);
// Post-solve cleanup for island
for (var i = 0; i < _island.BodyCount; i++)
{
var body = _island.Bodies[i];
// Static bodies can participate in other islands
if (body.BodyType == BodyType.Static)
{
body.Island = false;
}
}
}
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();
ContactManager.PostSolve();
}
private void ClearForces()
{
foreach (var body in AwakeBodies)
{
body.Force = Vector2.Zero;
body.Torque = 0.0f;
}
}
}
}

154
Robust.Shared/Physics/Dynamics/Shapes/EdgeShape.cs
View File

@@ -1,154 +0,0 @@
// 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.
using System;
using Robust.Shared.Configuration;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
namespace Robust.Shared.Physics.Dynamics.Shapes
{
[Serializable, NetSerializable]
public sealed class EdgeShape : IPhysShape
{
/// <summary>
/// Edge start vertex
/// </summary>
internal Vector2 Vertex1;
/// <summary>
/// Edge end vertex
/// </summary>
internal Vector2 Vertex2;
/// <summary>
/// Is true if the edge is connected to an adjacent vertex before vertex 1.
/// </summary>
public bool HasVertex0 { get; set; }
/// <summary>
/// Is true if the edge is connected to an adjacent vertex after vertex2.
/// </summary>
public bool HasVertex3 { get; set; }
/// <summary>
/// Optional adjacent vertices. These are used for smooth collision.
/// </summary>
public Vector2 Vertex0 { get; set; }
/// <summary>
/// Optional adjacent vertices. These are used for smooth collision.
/// </summary>
public Vector2 Vertex3 { get; set; }
public int ChildCount => 1;
public bool OneSided => !(HasVertex0 && HasVertex3);
public float Radius
{
get => _radius;
set
{
if (MathHelper.CloseTo(_radius, value)) return;
_radius = value;
}
}
private float _radius;
public ShapeType ShapeType => ShapeType.Edge;
public void ExposeData(ObjectSerializer serializer)
{
}
/// <summary>
/// Create a 1-sided edge.
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
public EdgeShape(Vector2 start, Vector2 end)
{
Set(start, end);
_radius = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.PolygonRadius);
}
/// <summary>
/// Set this as an isolated edge.
/// </summary>
/// <param name="start">The start.</param>
/// <param name="end">The end.</param>
public void Set(Vector2 start, Vector2 end)
{
Vertex1 = start;
Vertex2 = end;
HasVertex0 = false;
HasVertex3 = false;
// TODO ComputeProperties();
}
public bool Equals(IPhysShape? other)
{
if (other is not EdgeShape edge) return false;
return (HasVertex0 == edge.HasVertex0 &&
HasVertex3 == edge.HasVertex3 &&
Vertex0 == edge.Vertex0 &&
Vertex1 == edge.Vertex1 &&
Vertex2 == edge.Vertex2 &&
Vertex3 == edge.Vertex3);
}
public Box2 CalculateLocalBounds(Angle rotation)
{
Vector2 lower = Vector2.ComponentMin(Vertex1, Vertex2);
Vector2 upper = Vector2.ComponentMax(Vertex1, Vertex2);
Vector2 r = new Vector2(Radius, Radius);
var aabb = new Box2
{
BottomLeft = lower - r,
TopRight = upper + r
};
return aabb;
}
public void ApplyState()
{
return;
}
public void DebugDraw(DebugDrawingHandle handle, in Matrix3 modelMatrix, in Box2 worldViewport, float sleepPercent)
{
var m = Matrix3.Identity;
m.R0C2 = modelMatrix.R0C2;
m.R1C2 = modelMatrix.R1C2;
handle.SetTransform(m);
handle.DrawLine(Vertex1, Vertex2, handle.CalcWakeColor(handle.RectFillColor, sleepPercent));
handle.SetTransform(Matrix3.Identity);
}
}
}

45
Robust.Shared/Physics/Dynamics/Shapes/IPhysShape.cs
View File

@@ -1,45 +0,0 @@
using System;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics.Shapes;
using Robust.Shared.Serialization;
namespace Robust.Shared.Physics
{
public enum ShapeType : sbyte
{
Unknown = -1,
Circle = 0,
Edge = 1,
Polygon = 2,
Chain = 3,
Aabb = 4,
Rectangle = 5, // Look you might be able to replace this with polys but for now I have done the thing
TypeCount = 6, // Obviously increment this if you add something
}
/// <summary>
/// A primitive physical shape that is used by a <see cref="IPhysBody"/>.
/// </summary>
public interface IPhysShape : IExposeData, IEquatable<IPhysShape>
{
/// <summary>
/// Get the number of child primitives. Only relevant for chain shape.
/// </summary>
int ChildCount { get; }
float Radius { get; set; }
ShapeType ShapeType { get; }
/// <summary>
/// Calculates the AABB of the shape.
/// </summary>
/// <param name="rotation"></param>
/// <returns></returns>
Box2 CalculateLocalBounds(Angle rotation);
void ApplyState();
void DebugDraw(DebugDrawingHandle handle, in Matrix3 modelMatrix, in Box2 worldViewport, float sleepPercent);
}
}

291
Robust.Shared/Physics/Dynamics/Shapes/PolygonShape.cs
View File

@@ -1,291 +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.Collections.Generic;
using Robust.Shared.Configuration;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Shapes
{
[Serializable, NetSerializable]
public class PolygonShape : IPhysShape
{
/// <summary>
/// Counter-clockwise (CCW) order.
/// </summary>
[ViewVariables]
public List<Vector2> Vertices
{
get => _vertices;
set
{
_vertices = value;
var configManager = IoCManager.Resolve<IConfigurationManager>();
DebugTools.Assert(_vertices.Count >= 3 && _vertices.Count <= configManager.GetCVar(CVars.MaxPolygonVertices));
if (configManager.GetCVar(CVars.ConvexHullPolygons))
{
//FPE note: This check is required as the GiftWrap algorithm early exits on triangles
//So instead of giftwrapping a triangle, we just force it to be clock wise.
if (_vertices.Count <= 3)
_vertices.ForceCounterClockwise();
else
_vertices = GiftWrap.GetConvexHull(_vertices);
}
_normals = new List<Vector2>(_vertices.Count);
// Compute normals. Ensure the edges have non-zero length.
for (int i = 0; i < _vertices.Count; ++i)
{
int next = i + 1 < _vertices.Count ? i + 1 : 0;
Vector2 edge = _vertices[next] - _vertices[i];
DebugTools.Assert(edge.LengthSquared > float.Epsilon * float.Epsilon);
//FPE optimization: Normals.Add(MathHelper.Cross(edge, 1.0f));
Vector2 temp = new Vector2(edge.Y, -edge.X);
_normals.Add(temp.Normalized);
}
// Compute the polygon mass data
// ComputeProperties();
}
}
private List<Vector2> _vertices = new();
[ViewVariables]
public List<Vector2> Normals => _normals;
private List<Vector2> _normals = new();
public int ChildCount => 1;
/// <summary>
/// The radius of this polygon.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float Radius
{
get => _radius;
set
{
if (MathHelper.CloseTo(_radius, value)) return;
_radius = value;
}
}
private float _radius;
public ShapeType ShapeType => ShapeType.Polygon;
public PolygonShape(IPhysShape shape)
{
// Look it's fucking shitcode but go look at Collision to explain why (tl;dr until Acruid's manifold generation is ported this is faster than doing 2 casts)
switch (shape)
{
case PhysShapeAabb aabb:
_radius = aabb.Radius;
var bounds = aabb.LocalBounds;
// Set normals directly to avoid giftwrap
_vertices = new List<Vector2>
{
bounds.BottomRight,
bounds.TopRight,
bounds.TopLeft,
bounds.BottomLeft,
};
_normals = new List<Vector2>
{
new(1, -0),
new(0, 1),
new(-1, -0),
new(0, -1),
};
break;
case PhysShapeRect rect:
_radius = rect.Radius;
var rectBounds = rect.CachedBounds;
Vertices = new List<Vector2>
{
rectBounds.BottomRight,
rectBounds.TopRight,
rectBounds.TopLeft,
rectBounds.BottomLeft,
};
break;
case PolygonShape poly:
_radius = poly.Radius;
_vertices = poly._vertices;
_normals = poly._normals;
break;
default:
throw new InvalidOperationException();
}
}
public PolygonShape()
{
_radius = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.PolygonRadius);
}
public PolygonShape(float radius)
{
_radius = radius;
}
public void SetAsBox(float width, float height)
{
Vertices = new List<Vector2>()
{
new(-width, -height),
new(width, -height),
new(width, height),
new(-width, height),
};
}
public void ExposeData(ObjectSerializer serializer)
{
serializer.DataField(this, x => x.Vertices, "vertices", new List<Vector2>());
// ComputeProperties();
}
/// <summary>
/// A temporary optimisation that bypasses GiftWrapping until we get proper AABB and Rect collisions
/// </summary>
internal void SetVertices(List<Vector2> vertices)
{
DebugTools.Assert(vertices.Count == 4, "Vertices optimisation only usable on rectangles");
// TODO: Get what the normals should be
Vertices = vertices;
// _normals = new List<Vector2>()
// Verify on debug that the vertices skip was actually USEFUL
DebugTools.Assert(Vertices == vertices);
}
public bool Equals(IPhysShape? other)
{
// TODO: Could use casts for AABB and Rect
if (other is not PolygonShape poly) return false;
if (_vertices.Count != poly.Vertices.Count) return false;
for (var i = 0; i < _vertices.Count; i++)
{
var vert = _vertices[i];
if (!vert.EqualsApprox(poly.Vertices[i])) return false;
}
return true;
}
public Box2 CalculateLocalBounds(Angle rotation)
{
if (Vertices.Count == 0) return new Box2();
var aabb = new Box2();
Vector2 lower = Vertices[0];
Vector2 upper = lower;
for (int i = 1; i < Vertices.Count; ++i)
{
Vector2 v = Vertices[i];
lower = Vector2.ComponentMin(lower, v);
upper = Vector2.ComponentMax(upper, v);
}
Vector2 r = new Vector2(Radius, Radius);
aabb.BottomLeft = lower - r;
aabb.TopRight = upper + r;
return aabb;
}
public void ApplyState()
{
return;
}
public void DebugDraw(DebugDrawingHandle handle, in Matrix3 modelMatrix, in Box2 worldViewport, float sleepPercent)
{
var m = Matrix3.Identity;
m.R0C2 = modelMatrix.R0C2;
m.R1C2 = modelMatrix.R1C2;
handle.SetTransform(m);
handle.DrawPolygonShape(_vertices.ToArray(), handle.CalcWakeColor(handle.RectFillColor, sleepPercent));
handle.SetTransform(Matrix3.Identity);
}
public static explicit operator PolygonShape(PhysShapeAabb aabb)
{
// TODO: Need a test for this probably, if there is no AABB manifold generator done at least.
var bounds = aabb.LocalBounds;
// Don't use Vertices property given we can just unwind it ourselves faster.
// Ideal world we don't need this but for now.
return new PolygonShape(aabb.Radius)
{
// Giftwrap seems to use bottom-right first.
_vertices = new List<Vector2>
{
bounds.BottomRight,
bounds.TopRight,
bounds.TopLeft,
bounds.BottomLeft,
},
_normals = new List<Vector2>
{
new(1, -0),
new (0, 1),
new (-1, -0),
new (0, -1),
}
};
}
public static explicit operator PolygonShape(PhysShapeRect rect)
{
// Ideal world we don't even need PhysShapeRect?
var bounds = rect.CachedBounds;
return new PolygonShape(rect.Radius)
{
Vertices = new List<Vector2>
{
bounds.BottomRight,
bounds.TopRight,
bounds.TopLeft,
bounds.BottomLeft,
},
};
}
}
}

45
Robust.Shared/Physics/Dynamics/Sweep.cs
View File

@@ -1,45 +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 Robust.Shared.Maths;
namespace Robust.Shared.Physics.Dynamics
{
internal struct Sweep
{
public float Angle;
public float Angle0;
public float Alpha0;
/// <summary>
/// Generally reflects the body's worldposition but not always.
/// Can be used to temporarily store it during CCD.
/// </summary>
public Vector2 Center;
public float Center0;
// I Didn't copy LocalCenter because it's also on the Body and it will normally be rarely set sooooo
}
}

112
Robust.Shared/Physics/GiftWrap.cs
View File

@@ -1,112 +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.Collections.Generic;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics
{
/// <summary>
/// Giftwrap convex hull algorithm.
/// O(nh) time complexity, where n is the number of points and h is the number of points on the convex hull.
///
/// See http://en.wikipedia.org/wiki/Gift_wrapping_algorithm for more details.
/// </summary>
public static class GiftWrap
{
//Extracted from Box2D
/// <summary>
/// Returns the convex hull from the given vertices.
/// </summary>
/// <param name="vertices">The vertices.</param>
public static List<Vector2> GetConvexHull(List<Vector2> vertices)
{
if (vertices.Count <= 3)
return vertices;
// Find the right most point on the hull
int i0 = 0;
float x0 = vertices[0].X;
for (int i = 1; i < vertices.Count; ++i)
{
float x = vertices[i].X;
if (x > x0 || (MathHelper.CloseTo(x, x0) && vertices[i].Y < vertices[i0].Y))
{
i0 = i;
x0 = x;
}
}
int[] hull = new int[vertices.Count];
int m = 0;
int ih = i0;
for (; ; )
{
hull[m] = ih;
int ie = 0;
for (int j = 1; j < vertices.Count; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
Vector2 r = vertices[ie] - vertices[hull[m]];
Vector2 v = vertices[j] - vertices[hull[m]];
float c = Vector2.Cross(r, v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared > r.LengthSquared)
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
var result = new List<Vector2>(m);
// Copy vertices.
for (var i = 0; i < m; ++i)
{
result.Add(vertices[hull[i]]);
}
return result;
}
}
}

49
Robust.Shared/Physics/IBroadPhase.cs
View File

@@ -1,58 +1,11 @@
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Dynamics;
namespace Robust.Shared.Physics {
public interface IBroadPhase
{
void UpdatePairs(BroadPhaseDelegate callback);
public interface IBroadPhase : IBroadPhase<IPhysBody> {
bool TestOverlap(DynamicTree.Proxy proxyA, DynamicTree.Proxy proxyB);
DynamicTree.Proxy AddProxy(ref FixtureProxy proxy);
void MoveProxy(DynamicTree.Proxy proxyId, in Box2 aabb, Vector2 displacement);
void RemoveProxy(DynamicTree.Proxy proxy);
void TouchProxy(DynamicTree.Proxy proxy);
void QueryAABB(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback,
Box2 aabb,
bool approx = false);
void QueryAabb<TState>(
ref TState state,
DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback,
Box2 aabb,
bool approx = false);
IEnumerable<FixtureProxy> QueryAabb(Box2 aabb, bool approx = false);
void QueryPoint(DynamicTree<FixtureProxy>.QueryCallbackDelegate callback,
Vector2 point,
bool approx = false);
void QueryPoint<TState>(
ref TState state,
DynamicTree<FixtureProxy>.QueryCallbackDelegate<TState> callback,
Vector2 point,
bool approx = false);
IEnumerable<FixtureProxy> QueryPoint(Vector2 point, bool approx = false);
void QueryRay(
DynamicTree<FixtureProxy>.RayQueryCallbackDelegate callback,
in Ray ray,
bool approx = false);
void QueryRay<TState>(
ref TState state,
DynamicTree<FixtureProxy>.RayQueryCallbackDelegate<TState> callback,
in Ray ray,
bool approx = false);
}
public interface IBroadPhase<T> : ICollection<T> where T : notnull {

69
Robust.Shared/Physics/IPhysBody.cs
View File

@@ -2,31 +2,14 @@
using Robust.Shared.GameObjects;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Dynamics.Contacts;
namespace Robust.Shared.Physics
{
/// <summary>
///
/// </summary>
public interface IPhysBody : IComponent
public interface IPhysBody
{
bool IgnoreGravity { get; set; }
int IslandIndex { get; set; }
/// <summary>
/// Has this body already been added to a physics island
/// </summary>
bool Island { get; set; }
/// <summary>
/// Should the body still have physics updates applied even if paused
/// </summary>
bool IgnorePaused { get; set; }
/// <summary>
/// Entity that this physBody represents.
/// </summary>
@@ -35,7 +18,14 @@ namespace Robust.Shared.Physics
/// <summary>
/// AABB of this entity in world space.
/// </summary>
Box2 GetWorldAABB(IMapManager? mapManager = null);
Box2 WorldAABB { get; }
/// <summary>
/// AABB of this entity in local space.
/// </summary>
Box2 AABB { get; }
IList<IPhysShape> PhysicsShapes { get; }
/// <summary>
/// Whether or not this body can collide.
@@ -54,39 +44,29 @@ namespace Robust.Shared.Physics
/// </summary>
int CollisionMask { get; }
void CreateProxies(IMapManager? mapManager = null, SharedBroadPhaseSystem? broadPhaseSystem = null);
void ClearProxies();
IReadOnlyList<Fixture> Fixtures { get; }
/// <summary>
/// The map index this physBody is located upon
/// </summary>
MapId MapID { get; }
/// <summary>
/// Broad Phase proxy ID.
/// </summary>
int ProxyId { get; set; }
/// <summary>
/// The type of the body, which determines how collisions effect this object.
/// </summary>
BodyType BodyType { get; set; }
/// <summary>
/// Whether the body is affected by tile friction or not.
/// </summary>
BodyStatus BodyStatus { get; set; }
int SleepAccumulator { get; set; }
bool Awake { get; set; }
int SleepThreshold { get; set; }
bool SleepingAllowed { get; set; }
float SleepTime { get; set; }
float LinearDamping { get; set; }
float AngularDamping { get; set; }
bool Awake { get; }
/// <summary>
/// Non-hard <see cref="IPhysBody"/>s will not cause action collision (e.g. blocking of movement)
/// Non-hard <see cref="IPhysicsComponent"/>s will not cause action collision (e.g. blocking of movement)
/// while still raising collision events.
/// </summary>
/// <remarks>
@@ -95,19 +75,14 @@ namespace Robust.Shared.Physics
bool Hard { get; set; }
/// <summary>
/// Inverse mass of the entity in kilograms (1 / Mass).
/// Inverse mass of the entity in kilograms (1 / Mass).
/// </summary>
float InvMass { get; }
/// <summary>
/// Mass of the entity in kilograms
/// Inverse moment of inertia, in
/// </summary>
float Mass { get; set; }
/// <summary>
/// Inverse inertia
/// </summary>
float InvI { get; set; }
float InvI { get; }
/// <summary>
/// Current Force being applied to this entity in Newtons.
@@ -172,7 +147,5 @@ namespace Robust.Shared.Physics
/// </summary>
/// <returns>True if this body can move, false if it is static.</returns>
bool CanMove();
IEnumerable<IPhysBody> GetCollidingEntities(Vector2 offset, bool approx = true);
}
}

52
Robust.Shared/Physics/IPhysShape.cs Normal file
View File

@@ -0,0 +1,52 @@
using System;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
namespace Robust.Shared.Physics
{
/// <summary>
/// A primitive physical shape that is used by a <see cref="IPhysBody"/>.
/// </summary>
public interface IPhysShape : IExposeData
{
/// <summary>
/// Raised when any of the parameters on this physics shape change.
/// </summary>
event Action OnDataChanged;
/// <summary>
/// Calculates the AABB of the shape.
/// </summary>
/// <param name="rotation"></param>
/// <returns></returns>
Box2 CalculateLocalBounds(Angle rotation);
/// <summary>
/// Bitmask of the collision layers the component is a part of.
/// </summary>
int CollisionLayer { get; set; }
/// <summary>
/// Bitmask of the layers this component collides with.
/// </summary>
int CollisionMask { get; set; }
void ApplyState();
void DebugDraw(DebugDrawingHandle handle, in Matrix3 modelMatrix, in Box2 worldViewport, float sleepPercent);
}
/// <summary>
/// Tag type for defining the representation of the collision layer bitmask
/// in terms of readable names in the content. To understand more about the
/// point of this type, see the <see cref="FlagsForAttribute"/>.
/// </summary>
public sealed class CollisionLayer {}
/// <summary>
/// Tag type for defining the representation of the collision mask bitmask
/// in terms of readable names in the content. To understand more about the
/// point of this type, see the <see cref="FlagsForAttribute"/>.
/// </summary>
public sealed class CollisionMask {}
}

80
Robust.Shared/Physics/IPhysicsManager.cs
View File

@@ -1,4 +1,8 @@
using Robust.Shared.Map;
using System;
using System.Collections.Generic;
using Robust.Shared.GameObjects;
using Robust.Shared.Map;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics
{
@@ -7,6 +11,15 @@ namespace Robust.Shared.Physics
/// </summary>
public interface IPhysicsManager
{
/// <summary>
/// Checks to see if the specified collision rectangle collides with any of the physBodies under management.
/// Also fires the OnCollide event of the first managed physBody to intersect with the collider.
/// </summary>
/// <param name="collider">Collision rectangle to check</param>
/// <param name="map">Map to check on</param>
/// <returns>true if collides, false if not</returns>
bool TryCollideRect(Box2 collider, MapId map);
/// <summary>
/// Checks whether a certain grid position is weightless or not
/// </summary>
@@ -14,6 +27,48 @@ namespace Robust.Shared.Physics
/// <returns></returns>
bool IsWeightless(EntityCoordinates coordinates);
/// <summary>
/// Get all entities colliding with a certain body.
/// </summary>
/// <param name="body"></param>
/// <returns></returns>
IEnumerable<IEntity> GetCollidingEntities(IPhysBody body, Vector2 offset, bool approximate = true);
IEnumerable<IPhysBody> GetCollidingEntities(MapId mapId, in Box2 worldBox);
/// <summary>
/// Checks whether a body is colliding
/// </summary>
/// <param name="body"></param>
/// <param name="offset"></param>
/// <returns></returns>
bool IsColliding(IPhysBody body, Vector2 offset, bool approx);
void AddBody(IPhysBody physBody);
void RemoveBody(IPhysBody physBody);
/// <summary>
/// Casts a ray in the world and returns the first entity it hits, or a list of all entities it hits.
/// </summary>
/// <param name="mapId"></param>
/// <param name="ray">Ray to cast in the world.</param>
/// <param name="maxLength">Maximum length of the ray in meters.</param>
/// <param name="ignoredEnt">A single entity that can be ignored by the RayCast. Useful if the ray starts inside the body of an entity.</param>
/// <param name="returnOnFirstHit">If false, will return a list of everything it hits, otherwise will just return a list of the first entity hit</param>
/// <returns>An enumerable of either the first entity hit or everything hit</returns>
IEnumerable<RayCastResults> IntersectRay(MapId mapId, CollisionRay ray, float maxLength = 50, IEntity? ignoredEnt = null, bool returnOnFirstHit = true);
/// <summary>
/// Casts a ray in the world and returns the distance the ray traveled while colliding with entities
/// </summary>
/// <param name="mapId"></param>
/// <param name="ray">Ray to cast in the world.</param>
/// <param name="maxLength">Maximum length of the ray in meters.</param>
/// <param name="ignoredEnt">A single entity that can be ignored by the RayCast. Useful if the ray starts inside the body of an entity.</param>
/// <returns>The distance the ray traveled while colliding with entities</returns>
public float IntersectRayPenetration(MapId mapId, CollisionRay ray, float maxLength, IEntity? ignoredEnt = null);
/// <summary>
/// Calculates the penetration depth of the axis-of-least-penetration for a
/// </summary>
@@ -21,5 +76,26 @@ namespace Robust.Shared.Physics
/// <param name="source"></param>
/// <returns></returns>
float CalculatePenetration(IPhysBody target, IPhysBody source);
Vector2 SolveCollisionImpulse(Manifold manifold);
/// <summary>
/// Casts a ray in the world, returning the first entity it hits (or all entities it hits, if so specified)
/// </summary>
/// <param name="mapId"></param>
/// <param name="ray">Ray to cast in the world.</param>
/// <param name="maxLength">Maximum length of the ray in meters.</param>
/// <param name="predicate">A predicate to check whether to ignore an entity or not. If it returns true, it will be ignored.</param>
/// <param name="returnOnFirstHit">If true, will only include the first hit entity in results. Otherwise, returns all of them.</param>
/// <returns>A result object describing the hit, if any.</returns>
IEnumerable<RayCastResults> IntersectRayWithPredicate(MapId mapId, CollisionRay ray, float maxLength = 50, Func<IEntity, bool>? predicate = null, bool returnOnFirstHit = true);
event Action<DebugRayData> DebugDrawRay;
bool Update(IPhysBody collider);
void RemovedFromMap(IPhysBody body, MapId mapId);
void AddedToMap(IPhysBody body, MapId mapId);
int SleepTimeThreshold { get; set; }
}
}
}

15
Robust.Shared/Physics/IslandSolveMessage.cs
View File

@@ -1,15 +0,0 @@
using System.Collections.Generic;
using Robust.Shared.GameObjects;
namespace Robust.Shared.Physics
{
internal class IslandSolveMessage : EntitySystemMessage
{
public List<IPhysBody> Bodies { get; }
public IslandSolveMessage(List<IPhysBody> bodies)
{
Bodies = bodies;
}
}
}

26
Robust.Shared/Physics/Manifold.cs Normal file
View File

@@ -0,0 +1,26 @@
using Robust.Shared.GameObjects;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics
{
public readonly struct Manifold
{
public readonly IPhysicsComponent A;
public readonly IPhysicsComponent B;
public readonly Vector2 Normal;
public readonly bool Hard;
public Vector2 RelativeVelocity => B.LinearVelocity - A.LinearVelocity;
public bool Unresolved => Vector2.Dot(RelativeVelocity, Normal) < 0 && Hard;
public Manifold(IPhysicsComponent a, IPhysicsComponent b, bool hard)
{
A = a;
B = b;
Normal = PhysicsManager.CalculateNormal(a, b);
Hard = hard;
}
}
}

82
Robust.Shared/Physics/Dynamics/Shapes/PhysShapeAabb.cs → Robust.Shared/Physics/PhysShapeAabb.cs
View File

@@ -1,13 +1,9 @@
using System;
using System.Collections.Generic;
using JetBrains.Annotations;
using Robust.Shared.Configuration;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Shapes
namespace Robust.Shared.Physics
{
/// <summary>
/// A physics shape that represents an Axis-Aligned Bounding Box.
@@ -17,27 +13,8 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
[Serializable, NetSerializable]
public class PhysShapeAabb : IPhysShape
{
public int ChildCount => 1;
/// <summary>
/// The radius of this AABB
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public float Radius
{
get => _radius;
set
{
if (MathHelper.CloseTo(_radius, value)) return;
_radius = value;
OnDataChanged?.Invoke();
}
}
private float _radius;
public ShapeType ShapeType => ShapeType.Polygon;
private int _collisionLayer;
private int _collisionMask;
private Box2 _localBounds = Box2.UnitCentered;
/// <summary>
@@ -49,14 +26,35 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
get => _localBounds;
set
{
if (_localBounds == value)
return;
_localBounds = value;
OnDataChanged?.Invoke();
}
}
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionLayer
{
get => _collisionLayer;
set
{
_collisionLayer = value;
OnDataChanged?.Invoke();
}
}
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionMask
{
get => _collisionMask;
set
{
_collisionMask = value;
OnDataChanged?.Invoke();
}
}
/// <inheritdoc />
public void ApplyState() { }
@@ -72,41 +70,21 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
handle.SetTransform(Matrix3.Identity);
}
// TODO
[field: NonSerialized]
public event Action? OnDataChanged;
/// <inheritdoc />
public Box2 CalculateLocalBounds(Angle rotation)
{
// TODO: Make a new ComputeAABB func or just wrap ComputeAABB into the existing methods?
return _localBounds.Scale(1 + Radius);
return _localBounds;
}
/// <inheritdoc />
void IExposeData.ExposeData(ObjectSerializer serializer)
{
serializer.DataField(ref _collisionLayer, "layer", 0, WithFormat.Flags<CollisionLayer>());
serializer.DataField(ref _collisionMask, "mask", 0, WithFormat.Flags<CollisionMask>());
serializer.DataField(ref _localBounds, "bounds", Box2.UnitCentered);
_radius = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.PolygonRadius);
}
[Pure]
internal List<Vector2> GetVertices()
{
return new()
{
_localBounds.BottomRight,
_localBounds.TopRight,
_localBounds.TopLeft,
_localBounds.BottomLeft,
};
}
public bool Equals(IPhysShape? other)
{
if (other is not PhysShapeAabb otherAABB) return false;
return _localBounds.EqualsApprox(otherAABB._localBounds);
}
}
}

40
Robust.Shared/Physics/Dynamics/Shapes/PhysShapeCircle.cs → Robust.Shared/Physics/PhysShapeCircle.cs
View File

@@ -3,7 +3,7 @@ using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Shapes
namespace Robust.Shared.Physics
{
/// <summary>
/// A physics shape that represents a circle. The circle cannot be rotated,
@@ -12,17 +12,40 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
[Serializable, NetSerializable]
public class PhysShapeCircle : IPhysShape
{
public int ChildCount => 1;
public ShapeType ShapeType => ShapeType.Circle;
private const float DefaultRadius = 0.5f;
private int _collisionLayer;
private int _collisionMask;
private float _radius = DefaultRadius;
/// <inheritdoc />
[field: NonSerialized]
public event Action? OnDataChanged;
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionLayer
{
get => _collisionLayer;
set
{
_collisionLayer = value;
OnDataChanged?.Invoke();
}
}
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionMask
{
get => _collisionMask;
set
{
_collisionMask = value;
OnDataChanged?.Invoke();
}
}
/// <summary>
/// The radius of this circle.
/// </summary>
@@ -32,7 +55,6 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
get => _radius;
set
{
if (MathHelper.CloseTo(_radius, value)) return;
_radius = value;
OnDataChanged?.Invoke();
}
@@ -41,6 +63,8 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
/// <inheritdoc />
void IExposeData.ExposeData(ObjectSerializer serializer)
{
serializer.DataField(ref _collisionLayer, "layer", 0, WithFormat.Flags<CollisionLayer>());
serializer.DataField(ref _collisionMask, "mask", 0, WithFormat.Flags<CollisionMask>());
serializer.DataField(ref _radius, "radius", DefaultRadius);
}
@@ -61,11 +85,5 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
handle.DrawCircle(Vector2.Zero, _radius, handle.CalcWakeColor(handle.RectFillColor, sleepPercent));
handle.SetTransform(in Matrix3.Identity);
}
public bool Equals(IPhysShape? other)
{
if (other is not PhysShapeCircle otherCircle) return false;
return MathHelper.CloseTo(_radius, otherCircle._radius);
}
}
}

63
Robust.Shared/Physics/Dynamics/Shapes/PhysShapeRect.cs → Robust.Shared/Physics/PhysShapeRect.cs
View File

@@ -1,13 +1,9 @@
using System;
using System.Collections.Generic;
using JetBrains.Annotations;
using Robust.Shared.Configuration;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Serialization;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics.Dynamics.Shapes
namespace Robust.Shared.Physics
{
/// <summary>
/// A physics shape that represents an OBB.
@@ -17,34 +13,44 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
[Serializable, NetSerializable]
public class PhysShapeRect : IPhysShape
{
public int ChildCount => 1;
private int _collisionLayer;
private int _collisionMask;
/// <summary>
/// The radius of this AABB
/// </summary>
private Box2 _rectangle = Box2.UnitCentered;
[ViewVariables(VVAccess.ReadWrite)]
public float Radius
public Box2 Rectangle
{
get => _radius;
get => _rectangle;
set
{
if (MathHelper.CloseTo(_radius, value)) return;
_radius = value;
_rectangle = value;
OnDataChanged?.Invoke();
}
}
private float _radius;
public ShapeType ShapeType => ShapeType.Polygon;
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
private Box2 _rectangle = Box2.UnitCentered;
public int CollisionLayer
{
get => _collisionLayer;
set
{
_collisionLayer = value;
OnDataChanged?.Invoke();
}
}
public Box2 CachedBounds => _cachedBounds;
[ViewVariables]
private Box2 _cachedBounds;
/// <inheritdoc />
[ViewVariables(VVAccess.ReadWrite)]
public int CollisionMask
{
get => _collisionMask;
set
{
_collisionMask = value;
OnDataChanged?.Invoke();
}
}
/// <inheritdoc />
public void ApplyState() { }
@@ -60,9 +66,9 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
void IExposeData.ExposeData(ObjectSerializer serializer)
{
serializer.DataField(ref _collisionLayer, "layer", 0, WithFormat.Flags<CollisionLayer>());
serializer.DataField(ref _collisionMask, "mask", 0, WithFormat.Flags<CollisionMask>());
serializer.DataField(ref _rectangle, "bounds", Box2.UnitCentered);
_radius = IoCManager.Resolve<IConfigurationManager>().GetCVar(CVars.PolygonRadius);
}
[field: NonSerialized]
@@ -70,14 +76,7 @@ namespace Robust.Shared.Physics.Dynamics.Shapes
public Box2 CalculateLocalBounds(Angle rotation)
{
_cachedBounds = new Box2Rotated(_rectangle, rotation.Opposite(), Vector2.Zero).CalcBoundingBox().Scale(1 + Radius);
return _cachedBounds;
}
public bool Equals(IPhysShape? other)
{
if (other is not PhysShapeRect rect) return false;
return _rectangle.EqualsApprox(rect._rectangle);
return new Box2Rotated(_rectangle, rotation.Opposite(), Vector2.Zero).CalcBoundingBox();
}
}
}

304
Robust.Shared/Physics/PhysicsManager.cs
View File

@@ -16,6 +16,37 @@ namespace Robust.Shared.Physics
[Dependency] private readonly IMapManager _mapManager = default!;
[Dependency] private readonly IEntityManager _entityManager = default!;
private readonly ConcurrentDictionary<MapId,BroadPhase> _treesPerMap =
new();
private BroadPhase this[MapId mapId] => _treesPerMap.GetOrAdd(mapId, _ => new BroadPhase());
/// <summary>
/// returns true if collider intersects a physBody under management.
/// </summary>
/// <param name="collider">Rectangle to check for collision</param>
/// <param name="map">Map ID to filter</param>
/// <returns></returns>
public bool TryCollideRect(Box2 collider, MapId map)
{
var state = (collider, map, found: false);
this[map].QueryAabb(ref state, (ref (Box2 collider, MapId map, bool found) state, in IPhysBody body) =>
{
if (!body.CanCollide || body.CollisionLayer == 0x0)
return true;
if (body.MapID == state.map &&
body.WorldAABB.Intersects(state.collider))
{
state.found = true;
return false;
}
return true;
}, collider, true);
return state.found;
}
public bool IsWeightless(EntityCoordinates coordinates)
{
var gridId = coordinates.GetGridId(_entityManager);
@@ -38,32 +69,106 @@ namespace Robust.Shared.Physics
/// <returns></returns>
public static Vector2 CalculateNormal(IPhysBody target, IPhysBody source)
{
var manifold = target.GetWorldAABB().Intersect(source.GetWorldAABB());
var manifold = target.WorldAABB.Intersect(source.WorldAABB);
if (manifold.IsEmpty()) return Vector2.Zero;
if (manifold.Height > manifold.Width)
{
// X is the axis of seperation
var leftDist = source.GetWorldAABB().Right - target.GetWorldAABB().Left;
var rightDist = target.GetWorldAABB().Right - source.GetWorldAABB().Left;
var leftDist = source.WorldAABB.Right - target.WorldAABB.Left;
var rightDist = target.WorldAABB.Right - source.WorldAABB.Left;
return new Vector2(leftDist > rightDist ? 1 : -1, 0);
}
else
{
// Y is the axis of seperation
var bottomDist = source.GetWorldAABB().Top - target.GetWorldAABB().Bottom;
var topDist = target.GetWorldAABB().Top - source.GetWorldAABB().Bottom;
var bottomDist = source.WorldAABB.Top - target.WorldAABB.Bottom;
var topDist = target.WorldAABB.Top - source.WorldAABB.Bottom;
return new Vector2(0, bottomDist > topDist ? 1 : -1);
}
}
public float CalculatePenetration(IPhysBody target, IPhysBody source)
{
var manifold = target.GetWorldAABB().Intersect(source.GetWorldAABB());
var manifold = target.WorldAABB.Intersect(source.WorldAABB);
if (manifold.IsEmpty()) return 0.0f;
return manifold.Height > manifold.Width ? manifold.Width : manifold.Height;
}
[Obsolete("Use fixture masks and layers instead")]
// Impulse resolution algorithm based on Box2D's approach in combination with Randy Gaul's Impulse Engine resolution algorithm.
public Vector2 SolveCollisionImpulse(Manifold manifold)
{
var aP = manifold.A;
var bP = manifold.B;
if (!aP.CanMove() && !bP.CanMove()) return Vector2.Zero;
var restitution = 0.01f;
var normal = manifold.Normal;
var rV = manifold.RelativeVelocity;
var vAlongNormal = Vector2.Dot(rV, normal);
if (vAlongNormal > 0)
{
return Vector2.Zero;
}
var impulse = -(1.0f + restitution) * vAlongNormal;
impulse /= aP.InvMass + bP.InvMass;
return manifold.Normal * impulse;
}
public IEnumerable<IEntity> GetCollidingEntities(IPhysBody physBody, Vector2 offset, bool approximate = true)
{
var modifiers = physBody.Entity.GetAllComponents<ICollideSpecial>();
var entities = new List<IEntity>();
var state = (physBody, modifiers, entities);
this[physBody.MapID].QueryAabb(ref state,
(ref (IPhysBody physBody, IEnumerable<ICollideSpecial> modifiers, List<IEntity> entities) state,
in IPhysBody body) =>
{
if (body.Entity.Deleted) {
return true;
}
if (CollidesOnMask(state.physBody, body))
{
var preventCollision = false;
var otherModifiers = body.Entity.GetAllComponents<ICollideSpecial>();
foreach (var modifier in state.modifiers)
{
preventCollision |= modifier.PreventCollide(body);
}
foreach (var modifier in otherModifiers)
{
preventCollision |= modifier.PreventCollide(state.physBody);
}
if (preventCollision)
{
return true;
}
state.entities.Add(body.Entity);
}
return true;
}, physBody.WorldAABB, approximate);
return entities;
}
/// <inheritdoc />
public IEnumerable<IPhysBody> GetCollidingEntities(MapId mapId, in Box2 worldBox)
{
return this[mapId].QueryAabb(worldBox, false);
}
public bool IsColliding(IPhysBody body, Vector2 offset, bool approximate)
{
return GetCollidingEntities(body, offset, approximate).Any();
}
public static bool CollidesOnMask(IPhysBody a, IPhysBody b)
{
if (a == b)
@@ -78,5 +183,190 @@ namespace Robust.Shared.Physics
return true;
}
/// <summary>
/// Adds a physBody to the manager.
/// </summary>
/// <param name="physBody"></param>
public void AddBody(IPhysBody physBody)
{
if (!this[physBody.MapID].Add(physBody))
{
Logger.WarningS("phys", $"PhysicsBody already registered! {physBody.Entity}");
}
}
/// <summary>
/// Removes a physBody from the manager
/// </summary>
/// <param name="physBody"></param>
public void RemoveBody(IPhysBody physBody)
{
var removed = false;
if (physBody.Entity.Deleted || physBody.Entity.Transform.Deleted)
{
foreach (var mapId in _mapManager.GetAllMapIds())
{
removed = this[mapId].Remove(physBody);
if (removed)
{
break;
}
}
}
if (!removed)
{
try
{
removed = this[physBody.MapID].Remove(physBody);
}
catch (InvalidOperationException)
{
// TODO: TryGetMapId or something
foreach (var mapId in _mapManager.GetAllMapIds())
{
removed = this[mapId].Remove(physBody);
if (removed)
{
break;
}
}
}
}
if (!removed)
{
foreach (var mapId in _mapManager.GetAllMapIds())
{
removed = this[mapId].Remove(physBody);
if (removed)
{
break;
}
}
}
if (!removed)
Logger.WarningS("phys", $"Trying to remove unregistered PhysicsBody! {physBody.Entity.Uid}");
}
/// <inheritdoc />
public IEnumerable<RayCastResults> IntersectRayWithPredicate(MapId mapId, CollisionRay ray,
float maxLength = 50F,
Func<IEntity, bool>? predicate = null, bool returnOnFirstHit = true)
{
List<RayCastResults> results = new();
this[mapId].QueryRay((in IPhysBody body, in Vector2 point, float distFromOrigin) =>
{
if (returnOnFirstHit && results.Count > 0) return true;
if (distFromOrigin > maxLength)
{
return true;
}
if (!body.CanCollide)
{
return true;
}
if ((body.CollisionLayer & ray.CollisionMask) == 0x0)
{
return true;
}
if (predicate != null && predicate.Invoke(body.Entity))
{
return true;
}
var result = new RayCastResults(distFromOrigin, point, body.Entity);
results.Add(result);
DebugDrawRay?.Invoke(new DebugRayData(ray, maxLength, result));
return true;
}, ray);
if (results.Count == 0)
{
DebugDrawRay?.Invoke(new DebugRayData(ray, maxLength, null));
}
results.Sort((a, b) => a.Distance.CompareTo(b.Distance));
return results;
}
/// <inheritdoc />
public IEnumerable<RayCastResults> IntersectRay(MapId mapId, CollisionRay ray, float maxLength = 50, IEntity? ignoredEnt = null, bool returnOnFirstHit = true)
=> IntersectRayWithPredicate(mapId, ray, maxLength, entity => entity == ignoredEnt, returnOnFirstHit);
/// <inheritdoc />
public float IntersectRayPenetration(MapId mapId, CollisionRay ray, float maxLength, IEntity? ignoredEnt = null)
{
var penetration = 0f;
this[mapId].QueryRay((in IPhysBody body, in Vector2 point, float distFromOrigin) =>
{
if (distFromOrigin > maxLength)
{
return true;
}
if (!body.CanCollide)
{
return true;
}
if ((body.CollisionLayer & ray.CollisionMask) == 0x0)
{
return true;
}
if (new Ray(point + ray.Direction * body.WorldAABB.Size.Length * 2, -ray.Direction).Intersects(
body.WorldAABB, out _, out var exitPoint))
{
penetration += (point - exitPoint).Length;
}
return true;
}, ray);
return penetration;
}
public event Action<DebugRayData>? DebugDrawRay;
public bool Update(IPhysBody collider)
{
collider.WakeBody();
return this[collider.MapID].Update(collider);
}
public void RemovedFromMap(IPhysBody body, MapId mapId)
{
body.WakeBody();
this[mapId].Remove(body);
}
public void AddedToMap(IPhysBody body, MapId mapId)
{
body.WakeBody();
this[mapId].Add(body);
}
/// <summary>
/// How many ticks before a physics body will go to sleep. Bodies will only sleep if
/// they have no velocity.
/// </summary>
/// <remarks>
/// This is an arbitrary number greater than zero. To solve "locker stacks" that span multiple ticks,
/// this needs to be greater than one. Every time an entity collides or is moved, the body's <see cref="IPhysBody.SleepAccumulator"/>
/// goes back to zero.
/// </remarks>
public int SleepTimeThreshold { get; set; } = 2;
}
}

45
Robust.Shared/Physics/PhysicsWakeMessage.cs
View File

@@ -1,45 +0,0 @@
using Robust.Shared.GameObjects;
namespace Robust.Shared.Physics
{
// Real pros use the system messages
public sealed class PhysicsWakeMessage : EntitySystemMessage
{
public PhysicsComponent Body { get; }
public PhysicsWakeMessage(PhysicsComponent component)
{
Body = component;
}
}
public sealed class PhysicsSleepMessage : EntitySystemMessage
{
public PhysicsComponent Body { get; }
public PhysicsSleepMessage(PhysicsComponent component)
{
Body = component;
}
}
public sealed class PhysicsWakeCompMessage : ComponentMessage
{
public PhysicsComponent Body { get; }
public PhysicsWakeCompMessage(PhysicsComponent component)
{
Body = component;
}
}
public sealed class PhysicsSleepCompMessage : ComponentMessage
{
public PhysicsComponent Body { get; }
public PhysicsSleepCompMessage(PhysicsComponent component)
{
Body = component;
}
}
}

156
Robust.Shared/Physics/Transform.cs
View File

@@ -1,156 +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 JetBrains.Annotations;
using Robust.Shared.Maths;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics
{
// TODO: Probably replace this internally with just the Vector2 and radians but I'd need to re-learn trig so yeah....
internal struct Transform
{
public Vector2 Position;
public Quaternion2D Quaternion2D;
public Transform(Vector2 position, float angle)
{
Position = position;
Quaternion2D = new Quaternion2D(angle);
}
public Transform(float angle)
{
Position = Vector2.Zero;
Quaternion2D = new Quaternion2D(angle);
}
public static Vector2 Mul(in Transform transform, in Vector2 vector)
{
float x = (transform.Quaternion2D.C * vector.X - transform.Quaternion2D.S * vector.Y) + transform.Position.X;
float y = (transform.Quaternion2D.S * vector.X + transform.Quaternion2D.C * vector.Y) + transform.Position.Y;
return new Vector2(x, y);
}
public static Vector2 MulT(in Vector2[] A, in Vector2 v)
{
DebugTools.Assert(A.Length == 2);
return new Vector2(v.X * A[0].X + v.Y * A[0].Y, v.X * A[1].X + v.Y * A[1].Y);
}
public static Vector2 MulT(in Transform T, in Vector2 v)
{
float px = v.X - T.Position.X;
float py = v.Y - T.Position.Y;
float x = (T.Quaternion2D.C * px + T.Quaternion2D.S * py);
float y = (-T.Quaternion2D.S * px + T.Quaternion2D.C * py);
return new Vector2(x, y);
}
/// Transpose multiply two rotations: qT * r
public static Quaternion2D MulT(in Quaternion2D q, in Quaternion2D r)
{
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
// s = qc * rs - qs * rc
// c = qc * rc + qs * rs
Quaternion2D qr;
qr.S = q.C * r.S - q.S * r.C;
qr.C = q.C * r.C + q.S * r.S;
return qr;
}
// v2 = A.q' * (B.q * v1 + B.p - A.p)
// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
public static Transform MulT(in Transform A, in Transform B)
{
Transform C = new Transform
{
Quaternion2D = MulT(A.Quaternion2D, B.Quaternion2D),
Position = MulT(A.Quaternion2D, B.Position - A.Position)
};
return C;
}
/// <summary>
/// Inverse rotate a vector
/// </summary>
/// <param name="q"></param>
/// <param name="v"></param>
/// <returns></returns>
public static Vector2 MulT(Quaternion2D q, Vector2 v)
{
return new(q.C * v.X + q.S * v.Y, -q.S * v.X + q.C * v.Y);
}
/// <summary>
/// Rotate a vector
/// </summary>
/// <param name="quaternion2D"></param>
/// <param name="vector"></param>
/// <returns></returns>
public static Vector2 Mul(in Quaternion2D quaternion2D, in Vector2 vector)
{
return new(quaternion2D.C * vector.X - quaternion2D.S * vector.Y, quaternion2D.S * vector.X + quaternion2D.C * vector.Y);
}
public static Vector2 Mul(in Vector2[] A, in Vector2 v)
{
// A needing to be a 2 x 2 matrix
DebugTools.Assert(A.Length == 2);
return new Vector2(A[0].X * v.X + A[1].X * v.Y, A[0].Y * v.X + A[1].Y * v.Y);
}
}
public struct Quaternion2D
{
public float C;
public float S;
public Quaternion2D(float cos, float sin)
{
C = cos;
S = sin;
}
public Quaternion2D(float angle)
{
C = MathF.Cos(angle);
S = MathF.Sin(angle);
}
public Quaternion2D Set(float angle)
{
//FPE: Optimization
if (angle == 0.0f)
{
return new Quaternion2D(1.0f, 0.0f);
}
// TODO_ERIN optimize
return new Quaternion2D(MathF.Cos(angle), MathF.Sin(angle));
}
}
}

50
Robust.Shared/Physics/Vector2Helpers.cs
View File

@@ -1,50 +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 Robust.Shared.Maths;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics
{
internal static class Vector2Helpers
{
public static Vector2[] Inverse(this Vector2[] matrix)
{
DebugTools.Assert(matrix.Length == 2);
float a = matrix[0].X, b = matrix[1].X, c = matrix[0].Y, d = matrix[1].Y;
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
Vector2[] result = new Vector2[2];
result[0].X = det * d;
result[0].Y = -det * c;
result[1].X = -det * b;
result[1].Y = det * a;
return result;
}
}
}

73
Robust.Shared/Physics/Vertices.cs
View File

@@ -1,73 +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.Collections.Generic;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics
{
public static class Vertices
{
public static void ForceCounterClockwise(this List<Vector2> vertices)
{
if (vertices.Count < 3) return;
if (!vertices.IsCounterClockwise()) vertices.Reverse();
}
public static bool IsCounterClockwise(this List<Vector2> vertices)
{
if (vertices.Count < 3) return false;
return vertices.GetSignedArea() > 0.0f;
}
/// <summary>
/// Gets the signed area.
/// If the area is less than 0, it indicates that the polygon is clockwise winded.
/// </summary>
/// <returns>The signed area</returns>
public static float GetSignedArea(this List<Vector2> vertices)
{
var count = vertices.Count;
//The simplest polygon which can exist in the Euclidean plane has 3 sides.
if (count < 3)
return 0;
int i;
float area = 0;
for (i = 0; i < count; i++)
{
int j = (i + 1) % count;
Vector2 vi = vertices[i];
Vector2 vj = vertices[j];
area += vi.X * vj.Y;
area -= vi.Y * vj.X;
}
area /= 2.0f;
return area;
}
}
}

56
Robust.Shared/Physics/VirtualController.cs Normal file
View File

@@ -0,0 +1,56 @@
using Robust.Shared.GameObjects;
using Robust.Shared.Maths;
using Robust.Shared.ViewVariables;
namespace Robust.Shared.Physics
{
/// <summary>
/// The VirtualController allows dynamic changes in the properties of a physics component, usually to simulate a complex physical interaction (such as player movement).
/// </summary>
public abstract class VirtualController
{
private Vector2 _linearVelocity;
/// <summary>
/// Current contribution to the linear velocity of the entity in meters per second.
/// </summary>
[ViewVariables(VVAccess.ReadWrite)]
public virtual Vector2 LinearVelocity
{
get => _linearVelocity;
set
{
if (value != Vector2.Zero)
ControlledComponent?.WakeBody();
if (_linearVelocity.EqualsApprox(value, 0.0001))
return;
_linearVelocity = value;
ControlledComponent?.Dirty();
}
}
public virtual IPhysicsComponent? ControlledComponent { protected get; set; }
/// <summary>
/// Tries to set this controller's linear velocity to zero.
/// </summary>
/// <returns>True if successful, false otherwise.</returns>
public virtual bool Stop()
{
LinearVelocity = Vector2.Zero;
return true;
}
/// <summary>
/// Modify a physics component before processing impulses
/// </summary>
public virtual void UpdateBeforeProcessing() { }
/// <summary>
/// Modify a physics component after processing impulses
/// </summary>
public virtual void UpdateAfterProcessing() { }
}
}

2
Robust.Shared/SharedIoC.cs
View File

@@ -9,7 +9,6 @@ using Robust.Shared.Log;
using Robust.Shared.Map;
using Robust.Shared.Network;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Collision;
using Robust.Shared.Random;
using Robust.Shared.Sandboxing;
using Robust.Shared.Serialization;
@@ -48,7 +47,6 @@ namespace Robust.Shared
IoCManager.Register<IRobustMappedStringSerializer, RobustMappedStringSerializer>();
IoCManager.Register<IComponentDependencyManager, ComponentDependencyManager>();
IoCManager.Register<ISandboxHelper, SandboxHelper>();
IoCManager.Register<ICollisionManager, CollisionManager>();
}
}
}

6
Robust.UnitTesting/Client/GameObjects/Components/Transform_Test.cs
View File

@@ -2,12 +2,10 @@ using System.IO;
using Moq;
using NUnit.Framework;
using Robust.Client.GameObjects;
using Robust.Client.Physics;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Prototypes;
namespace Robust.UnitTesting.Client.GameObjects.Components
@@ -40,11 +38,7 @@ namespace Robust.UnitTesting.Client.GameObjects.Components
var mock = new Mock<IEntitySystemManager>();
var system = new SharedTransformSystem();
var broady = new BroadPhaseSystem();
var physics = new PhysicsSystem();
mock.Setup(m => m.GetEntitySystem<SharedTransformSystem>()).Returns(system);
mock.Setup(m => m.GetEntitySystem<SharedBroadPhaseSystem>()).Returns(broady);
mock.Setup(m => m.GetEntitySystem<SharedPhysicsSystem>()).Returns(physics);
IoCManager.RegisterInstance<IEntitySystemManager>(mock.Object, true);
}

15
Robust.UnitTesting/Server/GameObjects/Components/Transform_Test.cs
View File

@@ -1,14 +1,11 @@
using System.IO;
using System.Reflection;
using Moq;
using NUnit.Framework;
using Robust.Server.GameObjects;
using Robust.Server.Physics;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Prototypes;
using Robust.Shared.Timing;
@@ -46,18 +43,6 @@ namespace Robust.UnitTesting.Server.GameObjects.Components
private static readonly EntityCoordinates InitialPos = new(new EntityUid(1), (0, 0));
protected override void OverrideIoC()
{
base.OverrideIoC();
var mock = new Mock<IEntitySystemManager>();
var broady = new BroadPhaseSystem();
var physics = new PhysicsSystem();
mock.Setup(m => m.GetEntitySystem<SharedBroadPhaseSystem>()).Returns(broady);
mock.Setup(m => m.GetEntitySystem<SharedPhysicsSystem>()).Returns(physics);
IoCManager.RegisterInstance<IEntitySystemManager>(mock.Object, true);
}
[OneTimeSetUp]
public void Setup()
{

26
Robust.UnitTesting/Server/Maps/MapLoaderTest.cs
View File

@@ -1,14 +1,10 @@
using System.Linq;
using Moq;
using NUnit.Framework;
using Robust.Server.GameObjects;
using Robust.Server.Maps;
using Robust.Server.Physics;
using Robust.Shared.ContentPack;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Prototypes;
using Robust.Shared.Serialization;
using Robust.Shared.Utility;
@@ -38,10 +34,9 @@ entities:
type: Transform
- index: 0
type: MapGrid
- fixtures:
- shape:
!type:PhysShapeGrid
grid: 0
- shapes:
- !type:PhysShapeGrid
grid: 0
type: Physics
- uid: 1
type: MapDeserializeTest
@@ -62,20 +57,6 @@ entities:
";
protected override void OverrideIoC()
{
base.OverrideIoC();
var mockFormat = new Mock<ICustomFormatManager>();
var mock = new Mock<IEntitySystemManager>();
var broady = new BroadPhaseSystem();
var physics = new PhysicsSystem();
mock.Setup(m => m.GetEntitySystem<SharedBroadPhaseSystem>()).Returns(broady);
mock.Setup(m => m.GetEntitySystem<SharedPhysicsSystem>()).Returns(physics);
IoCManager.RegisterInstance<IEntitySystemManager>(mock.Object, true);
IoCManager.RegisterInstance<ICustomFormatManager>(mockFormat.Object, true);
}
[OneTimeSetUp]
public void Setup()
@@ -99,7 +80,6 @@ entities:
[Test]
public void TestDataLoadPriority()
{
// TODO: Fix after serv3
var map = IoCManager.Resolve<IMapManager>();
var entMan = IoCManager.Resolve<IEntityManager>();

16
Robust.UnitTesting/Shared/Map/EntityCoordinates_Tests.cs
View File

@@ -1,13 +1,9 @@
using System.IO;
using Moq;
using NUnit.Framework;
using Robust.Server.GameObjects;
using Robust.Server.Physics;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Prototypes;
// ReSharper disable InconsistentNaming
@@ -24,18 +20,6 @@ namespace Robust.UnitTesting.Shared.Map
components:
- type: Transform";
protected override void OverrideIoC()
{
base.OverrideIoC();
var mock = new Mock<IEntitySystemManager>();
var broady = new BroadPhaseSystem();
var physics = new PhysicsSystem();
mock.Setup(m => m.GetEntitySystem<SharedBroadPhaseSystem>()).Returns(broady);
mock.Setup(m => m.GetEntitySystem<SharedPhysicsSystem>()).Returns(physics);
IoCManager.RegisterInstance<IEntitySystemManager>(mock.Object, true);
}
[OneTimeSetUp]
public void Setup()
{

16
Robust.UnitTesting/Shared/Map/MapGrid_Tests.cs
View File

@@ -1,13 +1,10 @@
using System.Linq;
using Moq;
using NUnit.Framework;
using Robust.Server.GameObjects;
using Robust.Server.Physics;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Broadphase;
using Robust.Shared.Timing;
using MapGrid = Robust.Shared.Map.MapGrid;
@@ -16,19 +13,6 @@ namespace Robust.UnitTesting.Shared.Map
[TestFixture, TestOf(typeof(MapGrid))]
class MapGrid_Tests : RobustUnitTest
{
protected override void OverrideIoC()
{
base.OverrideIoC();
var mock = new Mock<IEntitySystemManager>();
var broady = new BroadPhaseSystem();
var physics = new PhysicsSystem();
mock.Setup(m => m.GetEntitySystem<SharedBroadPhaseSystem>()).Returns(broady);
mock.Setup(m => m.GetEntitySystem<SharedPhysicsSystem>()).Returns(physics);
IoCManager.RegisterInstance<IEntitySystemManager>(mock.Object, true);
}
[OneTimeSetUp]
public void Setup()
{

Some files were not shown because too many files have changed in this diff Show More