diff --git a/Robust.Shared/GameObjects/Systems/CollisionWakeSystem.cs b/Robust.Shared/GameObjects/Systems/CollisionWakeSystem.cs index e5549ade371..8b181b1fa8c 100644 --- a/Robust.Shared/GameObjects/Systems/CollisionWakeSystem.cs +++ b/Robust.Shared/GameObjects/Systems/CollisionWakeSystem.cs @@ -96,6 +96,7 @@ internal void UpdateCanCollide( // If we're attached to the map we'll also just never disable collision due to how grid movement works. var canCollide = body.Awake || + body.ContactCount > 0 || (TryComp(uid, out JointComponent? jointComponent) && jointComponent.JointCount > 0) || xform.GridUid == null; diff --git a/Robust.Shared/Physics/Collision/CollisionManager.Circles.cs b/Robust.Shared/Physics/Collision/CollisionManager.Circles.cs index 864c3bd103d..ac541b08a8d 100644 --- a/Robust.Shared/Physics/Collision/CollisionManager.Circles.cs +++ b/Robust.Shared/Physics/Collision/CollisionManager.Circles.cs @@ -27,7 +27,7 @@ public void CollideCircles(ref Manifold manifold, PhysShapeCircle circleA, in Tr manifold.LocalNormal = Vector2.Zero; manifold.PointCount = 1; - ref var p0 = ref manifold.Points[0]; + ref var p0 = ref manifold.Points._00; p0.LocalPoint = Vector2.Zero; // Also here p0.Id.Key = 0; diff --git a/Robust.Shared/Physics/Collision/CollisionManager.EdgeCircle.cs b/Robust.Shared/Physics/Collision/CollisionManager.EdgeCircle.cs index 5cd26d5bd31..71952029014 100644 --- a/Robust.Shared/Physics/Collision/CollisionManager.EdgeCircle.cs +++ b/Robust.Shared/Physics/Collision/CollisionManager.EdgeCircle.cs @@ -79,9 +79,9 @@ public void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, in Tran manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; - manifold.Points[0].Id.Key = 0; - manifold.Points[0].Id.Features = cf; - manifold.Points[0].LocalPoint = circleB.Position; + manifold.Points._00.Id.Key = 0; + manifold.Points._00.Id.Features = cf; + manifold.Points._00.LocalPoint = circleB.Position; return; } @@ -114,9 +114,9 @@ public void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, in Tran manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; - manifold.Points[0].Id.Key = 0; - manifold.Points[0].Id.Features = cf; - manifold.Points[0].LocalPoint = circleB.Position; + manifold.Points._00.Id.Key = 0; + manifold.Points._00.Id.Features = cf; + manifold.Points._00.LocalPoint = circleB.Position; return; } @@ -142,8 +142,8 @@ public void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, in Tran manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = n; manifold.LocalPoint = A; - manifold.Points[0].Id.Key = 0; - manifold.Points[0].Id.Features = cf; - manifold.Points[0].LocalPoint = circleB.Position; + manifold.Points._00.Id.Key = 0; + manifold.Points._00.Id.Features = cf; + manifold.Points._00.LocalPoint = circleB.Position; } } diff --git a/Robust.Shared/Physics/Collision/CollisionManager.EdgePolygon.cs b/Robust.Shared/Physics/Collision/CollisionManager.EdgePolygon.cs index cc583beb311..7a3473cb142 100644 --- a/Robust.Shared/Physics/Collision/CollisionManager.EdgePolygon.cs +++ b/Robust.Shared/Physics/Collision/CollisionManager.EdgePolygon.cs @@ -238,15 +238,15 @@ public void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, in Tra } var pointCount = 0; + var points = manifold.Points.AsSpan; + for (var i = 0; i < 2; ++i) { - float separation; - - separation = Vector2.Dot(refFace.normal, clipPoints2[i].V - refFace.v1); + var separation = Vector2.Dot(refFace.normal, clipPoints2[i].V - refFace.v1); if (separation <= radius) { - ref var cp = ref manifold.Points[pointCount]; + ref var cp = ref points[pointCount]; if (primaryAxis.Type == EPAxisType.EdgeA) { diff --git a/Robust.Shared/Physics/Collision/CollisionManager.PolygonCircle.cs b/Robust.Shared/Physics/Collision/CollisionManager.PolygonCircle.cs index efca280c4d0..a032ef5be1b 100644 --- a/Robust.Shared/Physics/Collision/CollisionManager.PolygonCircle.cs +++ b/Robust.Shared/Physics/Collision/CollisionManager.PolygonCircle.cs @@ -64,7 +64,7 @@ public void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA manifold.LocalNormal = normals[normalIndex]; manifold.LocalPoint = (v1 + v2) * 0.5f; - ref var p0 = ref manifold.Points[0]; + ref var p0 = ref manifold.Points._00; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; @@ -88,7 +88,7 @@ public void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA manifold.LocalNormal = (cLocal - v1).Normalized(); manifold.LocalPoint = v1; - ref var p0 = ref manifold.Points[0]; + ref var p0 = ref manifold.Points._00; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; @@ -107,7 +107,7 @@ public void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA manifold.LocalNormal = (cLocal - v2).Normalized(); manifold.LocalPoint = v2; - ref var p0 = ref manifold.Points[0]; + ref var p0 = ref manifold.Points._00; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; @@ -126,7 +126,7 @@ public void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA manifold.LocalNormal = normals[vertIndex1]; manifold.LocalPoint = faceCenter; - ref var p0 = ref manifold.Points[0]; + ref var p0 = ref manifold.Points._00; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; diff --git a/Robust.Shared/Physics/Collision/CollisionManager.Polygons.cs b/Robust.Shared/Physics/Collision/CollisionManager.Polygons.cs index 44b95d99de3..69d9bc1ee9a 100644 --- a/Robust.Shared/Physics/Collision/CollisionManager.Polygons.cs +++ b/Robust.Shared/Physics/Collision/CollisionManager.Polygons.cs @@ -238,6 +238,8 @@ public void CollidePolygons(ref Manifold manifold, PolygonShape polyA, in Transf manifold.LocalPoint = planePoint; int pointCount = 0; + var points = manifold.Points.AsSpan; + for (int i = 0; i < 2; ++i) { Vector2 value = clipPoints2[i].V; @@ -245,7 +247,7 @@ public void CollidePolygons(ref Manifold manifold, PolygonShape polyA, in Transf if (separation <= totalRadius) { - ref var cp = ref manifold.Points[pointCount]; + ref var cp = ref points[pointCount]; cp.LocalPoint = Transform.MulT(xf2, clipPoints2[i].V); cp.Id = clipPoints2[i].ID; diff --git a/Robust.Shared/Physics/Collision/CollisionManager.cs b/Robust.Shared/Physics/Collision/CollisionManager.cs index c5584ed3bf6..7161bc582f9 100644 --- a/Robust.Shared/Physics/Collision/CollisionManager.cs +++ b/Robust.Shared/Physics/Collision/CollisionManager.cs @@ -51,15 +51,18 @@ public static void GetPointStates(ref PointState[] state1, ref PointState[] stat in Manifold manifold2) { // Detect persists and removes. + var points1 = manifold1.Points.AsSpan; + var points2 = manifold2.Points.AsSpan; + for (int i = 0; i < manifold1.PointCount; ++i) { - ContactID id = manifold1.Points[i].Id; + var id = points1[i].Id; state1[i] = PointState.Remove; for (int j = 0; j < manifold2.PointCount; ++j) { - if (manifold2.Points[j].Id.Key == id.Key) + if (points2[j].Id.Key == id.Key) { state1[i] = PointState.Persist; break; @@ -70,13 +73,13 @@ public static void GetPointStates(ref PointState[] state1, ref PointState[] stat // Detect persists and adds. for (int i = 0; i < manifold2.PointCount; ++i) { - ContactID id = manifold2.Points[i].Id; + var id = points2[i].Id; state2[i] = PointState.Add; - for (int j = 0; j < manifold1.PointCount; ++j) + for (var j = 0; j < manifold1.PointCount; ++j) { - if (manifold1.Points[j].Id.Key == id.Key) + if (points1[j].Id.Key == id.Key) { state2[i] = PointState.Persist; break; diff --git a/Robust.Shared/Physics/Collision/Manifold.cs b/Robust.Shared/Physics/Collision/Manifold.cs index 75cd6135fb8..ea6848f7dbe 100644 --- a/Robust.Shared/Physics/Collision/Manifold.cs +++ b/Robust.Shared/Physics/Collision/Manifold.cs @@ -25,6 +25,7 @@ using System.Runtime.InteropServices; using Robust.Shared.Localization; using Robust.Shared.Maths; +using Robust.Shared.Utility; namespace Robust.Shared.Physics.Collision; @@ -146,7 +147,7 @@ public struct Manifold : IEquatable, IApproxEquatable /// /// Points of contact, can only be 0 -> 2. /// - internal ManifoldPoint[] Points; + internal FixedArray2 Points; public ManifoldType Type; @@ -157,9 +158,12 @@ public bool Equals(Manifold other) LocalNormal.Equals(other.LocalNormal) && LocalPoint.Equals(other.LocalPoint))) return false; + var points = Points.AsSpan; + var otherPoints = other.Points.AsSpan; + for (var i = 0; i < PointCount; i++) { - if (!Points[i].Equals(other.Points[i])) return false; + if (!points[i].Equals(otherPoints[i])) return false; } return true; @@ -172,9 +176,12 @@ public bool EqualsApprox(Manifold other) LocalNormal.EqualsApprox(other.LocalNormal) && LocalPoint.EqualsApprox(other.LocalPoint))) return false; + var points = Points.AsSpan; + var otherPoints = other.Points.AsSpan; + for (var i = 0; i < PointCount; i++) { - if (!Points[i].EqualsApprox(other.Points[i])) return false; + if (!points[i].EqualsApprox(otherPoints[i])) return false; } return true; @@ -187,13 +194,26 @@ public bool EqualsApprox(Manifold other, double tolerance) LocalNormal.EqualsApprox(other.LocalNormal, tolerance) && LocalPoint.EqualsApprox(other.LocalPoint, tolerance))) return false; + var points = Points.AsSpan; + var otherPoints = other.Points.AsSpan; + for (var i = 0; i < PointCount; i++) { - if (!Points[i].EqualsApprox(other.Points[i], tolerance)) return false; + if (!points[i].EqualsApprox(otherPoints[i], tolerance)) return false; } return true; } + + public override bool Equals(object? obj) + { + return obj is Manifold manifold && Equals(manifold); + } + + public override int GetHashCode() + { + return HashCode.Combine(LocalNormal, LocalPoint, PointCount, Points, (int)Type); + } } public struct ManifoldPoint : IEquatable, IApproxEquatable diff --git a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs index 4f11831c317..71d9dee98ba 100644 --- a/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs +++ b/Robust.Shared/Physics/Dynamics/Contacts/Contact.cs @@ -206,16 +206,19 @@ internal ContactStatus Update(Transform bodyATransform, Transform bodyBTransform // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. + var points = Manifold.Points.AsSpan; + var oldPoints = oldManifold.Points.AsSpan; + for (var i = 0; i < Manifold.PointCount; ++i) { - var mp2 = Manifold.Points[i]; + var mp2 = points[i]; mp2.NormalImpulse = 0.0f; mp2.TangentImpulse = 0.0f; var id2 = mp2.Id; for (var j = 0; j < oldManifold.PointCount; ++j) { - var mp1 = oldManifold.Points[j]; + var mp1 = oldPoints[j]; if (mp1.Id.Key == id2.Key) { @@ -225,7 +228,7 @@ internal ContactStatus Update(Transform bodyATransform, Transform bodyBTransform } } - Manifold.Points[i] = mp2; + points[i] = mp2; } if (touching != wasTouching) diff --git a/Robust.Shared/Physics/Dynamics/Contacts/ContactPositionConstraint.cs b/Robust.Shared/Physics/Dynamics/Contacts/ContactPositionConstraint.cs index 4d96a81801f..f2058b6579c 100644 --- a/Robust.Shared/Physics/Dynamics/Contacts/ContactPositionConstraint.cs +++ b/Robust.Shared/Physics/Dynamics/Contacts/ContactPositionConstraint.cs @@ -22,6 +22,7 @@ using System.Numerics; using Robust.Shared.Physics.Collision; +using Robust.Shared.Utility; namespace Robust.Shared.Physics.Dynamics.Contacts { @@ -37,7 +38,7 @@ internal struct ContactPositionConstraint /// public int IndexB { get; set; } - public Vector2[] LocalPoints; + internal FixedArray2 LocalPoints; public Vector2 LocalNormal; diff --git a/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs b/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs index a1e876c3bc1..cb6735a248d 100644 --- a/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs +++ b/Robust.Shared/Physics/Dynamics/Contacts/ContactVelocityConstraint.cs @@ -21,6 +21,7 @@ */ using System.Numerics; +using Robust.Shared.Utility; namespace Robust.Shared.Physics.Dynamics.Contacts { @@ -39,13 +40,13 @@ internal struct ContactVelocityConstraint public int IndexB; // Use 2 as its the max number of manifold points. - public VelocityConstraintPoint[] Points; + public FixedArray2 Points; public Vector2 Normal; - public System.Numerics.Vector4 NormalMass; + public Vector4 NormalMass; - public System.Numerics.Vector4 K; + public Vector4 K; public float InvMassA; public float InvMassB; diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs index b2087ae7803..04e6abd4bcf 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Contacts.cs @@ -113,10 +113,7 @@ public Contact Create() #if DEBUG contact._debugPhysics = _debugPhysicsSystem; #endif - contact.Manifold = new Manifold - { - Points = new ManifoldPoint[2] - }; + contact.Manifold = new Manifold(); return contact; } diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs index b3fbc3b344c..3be137287ed 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Island.cs @@ -1026,12 +1026,6 @@ private void UpdateBodies( var angle = angles[offset + i]; var xform = xformQuery.GetComponent(uid); - // Temporary NaN guards until PVS is fixed. - if (!float.IsNaN(position.X) && !float.IsNaN(position.Y)) - { - _transform.SetLocalPositionRotation(xform, xform.LocalPosition + position, xform.LocalRotation + angle); - } - var linVelocity = linearVelocities[offset + i]; var physicsDirtied = false; @@ -1047,6 +1041,13 @@ private void UpdateBodies( physicsDirtied |= SetAngularVelocity(uid, angVelocity, false, body: body); } + // Temporary NaN guards until PVS is fixed. + // May reparent object and change body's velocity. + if (!float.IsNaN(position.X) && !float.IsNaN(position.Y)) + { + _transform.SetLocalPositionRotation(xform, xform.LocalPosition + position, xform.LocalRotation + angle); + } + if (physicsDirtied) Dirty(uid, body); } diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs index d1b8d9b3df2..ff7c1bcd2fa 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Solver.cs @@ -67,7 +67,6 @@ private void ResetSolver( velocityConstraint.TangentSpeed = contact.TangentSpeed; velocityConstraint.IndexA = bodyA.IslandIndex[island.Index]; velocityConstraint.IndexB = bodyB.IslandIndex[island.Index]; - Array.Resize(ref velocityConstraint.Points, 2); // Don't need to reset point data as it all gets set below. var (invMassA, invMassB) = GetInvMass(bodyA, bodyB); @@ -87,7 +86,6 @@ private void ResetSolver( (positionConstraint.InvMassA, positionConstraint.InvMassB) = (invMassA, invMassB); positionConstraint.LocalCenterA = bodyA.LocalCenter; positionConstraint.LocalCenterB = bodyB.LocalCenter; - Array.Resize(ref positionConstraint.LocalPoints, 2); positionConstraint.InvIA = bodyA.InvI; positionConstraint.InvIB = bodyB.InvI; @@ -97,11 +95,14 @@ private void ResetSolver( positionConstraint.RadiusA = radiusA; positionConstraint.RadiusB = radiusB; positionConstraint.Type = manifold.Type; + var points = manifold.Points.AsSpan; + var posPoints = positionConstraint.LocalPoints.AsSpan; + var velPoints = velocityConstraint.Points.AsSpan; for (var j = 0; j < pointCount; ++j) { - var contactPoint = manifold.Points[j]; - ref var constraintPoint = ref velocityConstraint.Points[j]; + var contactPoint = points[j]; + ref var constraintPoint = ref velPoints[j]; if (_warmStarting) { @@ -120,7 +121,7 @@ private void ResetSolver( constraintPoint.TangentMass = 0.0f; constraintPoint.VelocityBias = 0.0f; - positionConstraint.LocalPoints[j] = contactPoint.LocalPoint; + posPoints[j] = contactPoint.LocalPoint; } } } @@ -220,10 +221,11 @@ private void InitializeVelocityConstraints( velocityConstraint.Normal = normal; int pointCount = velocityConstraint.PointCount; + var velPoints = velocityConstraint.Points.AsSpan; for (int j = 0; j < pointCount; ++j) { - ref var vcp = ref velocityConstraint.Points[j]; + ref var vcp = ref velPoints[j]; vcp.RelativeVelocityA = points[j] - centerA; vcp.RelativeVelocityB = points[j] - centerB; @@ -256,8 +258,8 @@ private void InitializeVelocityConstraints( // 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 vcp1 = velocityConstraint.Points._00; + var vcp2 = velocityConstraint.Points._01; var rn1A = Vector2Helpers.Cross(vcp1.RelativeVelocityA, velocityConstraint.Normal); var rn1B = Vector2Helpers.Cross(vcp1.RelativeVelocityB, velocityConstraint.Normal); @@ -299,6 +301,7 @@ private void WarmStart( for (var i = 0; i < island.Contacts.Count; ++i) { var velocityConstraint = velocityConstraints[i]; + var velPoints = velocityConstraint.Points.AsSpan; var indexA = velocityConstraint.IndexA; var indexB = velocityConstraint.IndexB; @@ -318,7 +321,7 @@ private void WarmStart( for (var j = 0; j < pointCount; ++j) { - var constraintPoint = velocityConstraint.Points[j]; + var constraintPoint = velPoints[j]; var P = normal * constraintPoint.NormalImpulse + tangent * constraintPoint.TangentImpulse; angVelocityA -= invIA * Vector2Helpers.Cross(constraintPoint.RelativeVelocityA, P); linVelocityA -= P * invMassA; @@ -386,12 +389,13 @@ private void SolveVelocityConstraints( var friction = velocityConstraint.Friction; DebugTools.Assert(pointCount is 1 or 2); + var velPoints = velocityConstraint.Points.AsSpan; // Solve tangent constraints first because non-penetration is more important // than friction. for (var j = 0; j < pointCount; ++j) { - ref var velConstraintPoint = ref velocityConstraint.Points[j]; + ref var velConstraintPoint = ref velPoints[j]; // Relative velocity at contact var dv = vB + Vector2Helpers.Cross(wB, velConstraintPoint.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, velConstraintPoint.RelativeVelocityA); @@ -419,7 +423,7 @@ private void SolveVelocityConstraints( // Solve normal constraints if (velocityConstraint.PointCount == 1) { - ref var vcp = ref velocityConstraint.Points[0]; + ref var vcp = ref velocityConstraint.Points._00; // Relative velocity at contact Vector2 dv = vB + Vector2Helpers.Cross(wB, vcp.RelativeVelocityB) - vA - Vector2Helpers.Cross(wA, vcp.RelativeVelocityA); @@ -476,8 +480,8 @@ private void SolveVelocityConstraints( // = A * x + b' // b' = b - A * a; - ref var cp1 = ref velocityConstraint.Points[0]; - ref var cp2 = ref velocityConstraint.Points[1]; + ref var cp1 = ref velocityConstraint.Points._00; + ref var cp2 = ref velocityConstraint.Points._01; Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse); DebugTools.Assert(a.X >= 0.0f && a.Y >= 0.0f); @@ -643,14 +647,16 @@ private void StoreImpulses(in IslandData island, ContactVelocityConstraint[] vel { for (var i = 0; i < island.Contacts.Count; ++i) { - ContactVelocityConstraint velocityConstraint = velocityConstraints[i]; + var velocityConstraint = velocityConstraints[i]; ref var manifold = ref island.Contacts[velocityConstraint.ContactIndex].Manifold; + var manPoints = manifold.Points.AsSpan; + var velPoints = velocityConstraint.Points.AsSpan; for (var j = 0; j < velocityConstraint.PointCount; ++j) { - ref var point = ref manifold.Points[j]; - point.NormalImpulse = velocityConstraint.Points[j].NormalImpulse; - point.TangentImpulse = velocityConstraint.Points[j].TangentImpulse; + ref var point = ref manPoints[j]; + point.NormalImpulse = velPoints[j].NormalImpulse; + point.TangentImpulse = velPoints[j].TangentImpulse; } } } @@ -794,7 +800,7 @@ internal static void InitializeManifold( { normal = new Vector2(1.0f, 0.0f); Vector2 pointA = Physics.Transform.Mul(xfA, manifold.LocalPoint); - Vector2 pointB = Physics.Transform.Mul(xfB, manifold.Points[0].LocalPoint); + Vector2 pointB = Physics.Transform.Mul(xfB, manifold.Points._00.LocalPoint); if ((pointA - pointB).LengthSquared() > float.Epsilon * float.Epsilon) { @@ -812,10 +818,11 @@ internal static void InitializeManifold( { normal = Physics.Transform.Mul(xfA.Quaternion2D, manifold.LocalNormal); Vector2 planePoint = Physics.Transform.Mul(xfA, manifold.LocalPoint); + var manPoints = manifold.Points.AsSpan; for (int i = 0; i < manifold.PointCount; ++i) { - Vector2 clipPoint = Physics.Transform.Mul(xfB, manifold.Points[i].LocalPoint); + Vector2 clipPoint = Physics.Transform.Mul(xfB, manPoints[i].LocalPoint); Vector2 cA = clipPoint + normal * (radiusA - Vector2.Dot(clipPoint - planePoint, normal)); Vector2 cB = clipPoint - normal * radiusB; points[i] = (cA + cB) * 0.5f; @@ -827,10 +834,11 @@ internal static void InitializeManifold( { normal = Physics.Transform.Mul(xfB.Quaternion2D, manifold.LocalNormal); Vector2 planePoint = Physics.Transform.Mul(xfB, manifold.LocalPoint); + var manPoints = manifold.Points.AsSpan; for (int i = 0; i < manifold.PointCount; ++i) { - Vector2 clipPoint = Physics.Transform.Mul(xfA, manifold.Points[i].LocalPoint); + Vector2 clipPoint = Physics.Transform.Mul(xfA, manPoints[i].LocalPoint); Vector2 cB = clipPoint + normal * (radiusB - Vector2.Dot(clipPoint - planePoint, normal)); Vector2 cA = clipPoint - normal * radiusA; points[i] = (cA + cB) * 0.5f; @@ -863,7 +871,7 @@ private static void PositionSolverManifoldInitialize( case ManifoldType.Circles: { Vector2 pointA = Physics.Transform.Mul(xfA, pc.LocalPoint); - Vector2 pointB = Physics.Transform.Mul(xfB, pc.LocalPoints[0]); + Vector2 pointB = Physics.Transform.Mul(xfB, pc.LocalPoints._00); normal = pointB - pointA; //FPE: Fix to handle zero normalization @@ -877,10 +885,11 @@ private static void PositionSolverManifoldInitialize( case ManifoldType.FaceA: { + var pcPoints = pc.LocalPoints.AsSpan; normal = Physics.Transform.Mul(xfA.Quaternion2D, pc.LocalNormal); Vector2 planePoint = Physics.Transform.Mul(xfA, pc.LocalPoint); - Vector2 clipPoint = Physics.Transform.Mul(xfB, pc.LocalPoints[index]); + Vector2 clipPoint = Physics.Transform.Mul(xfB, pcPoints[index]); separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB; point = clipPoint; } @@ -888,10 +897,11 @@ private static void PositionSolverManifoldInitialize( case ManifoldType.FaceB: { + var pcPoints = pc.LocalPoints.AsSpan; normal = Physics.Transform.Mul(xfB.Quaternion2D, pc.LocalNormal); Vector2 planePoint = Physics.Transform.Mul(xfB, pc.LocalPoint); - Vector2 clipPoint = Physics.Transform.Mul(xfA, pc.LocalPoints[index]); + Vector2 clipPoint = Physics.Transform.Mul(xfA, pcPoints[index]); separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.RadiusA - pc.RadiusB; point = clipPoint; diff --git a/Robust.Shared/Utility/FixedArray.cs b/Robust.Shared/Utility/FixedArray.cs index 50827d1c565..402ed5f2bda 100644 --- a/Robust.Shared/Utility/FixedArray.cs +++ b/Robust.Shared/Utility/FixedArray.cs @@ -79,12 +79,34 @@ public static Span Alloc128(out FixedArray128 discard) } } - internal struct FixedArray2 + internal struct FixedArray2 : IEquatable> { public T _00; public T _01; public Span AsSpan => MemoryMarshal.CreateSpan(ref _00, 2); + + internal FixedArray2(T x0, T x1) + { + _00 = x0; + _01 = x1; + } + + public bool Equals(FixedArray2 other) + { + return EqualityComparer.Default.Equals(_00, other._00) && + EqualityComparer.Default.Equals(_01, other._01); + } + + public override bool Equals(object? obj) + { + return obj is FixedArray2 other && Equals(other); + } + + public override int GetHashCode() + { + return HashCode.Combine(_00, _01); + } } internal struct FixedArray4 : IEquatable> @@ -96,12 +118,20 @@ internal struct FixedArray4 : IEquatable> public Span AsSpan => MemoryMarshal.CreateSpan(ref _00, 4); + internal FixedArray4(T x0, T x1, T x2, T x3) + { + _00 = x0; + _01 = x1; + _02 = x2; + _03 = x3; + } + public bool Equals(FixedArray4 other) { - return _00?.Equals(other._00) == true && - _01?.Equals(other._01) == true && - _02?.Equals(other._02) == true && - _03?.Equals(other._03) == true; + return EqualityComparer.Default.Equals(_00, other._00) && + EqualityComparer.Default.Equals(_01, other._01) && + EqualityComparer.Default.Equals(_02, other._02) && + EqualityComparer.Default.Equals(_03, other._03); } public override bool Equals(object? obj) @@ -128,16 +158,28 @@ internal struct FixedArray8 : IEquatable> public Span AsSpan => MemoryMarshal.CreateSpan(ref _00, 8); + internal FixedArray8(T x0, T x1, T x2, T x3, T x4, T x5, T x6, T x7) + { + _00 = x0; + _01 = x1; + _02 = x2; + _03 = x3; + _04 = x4; + _05 = x5; + _06 = x6; + _07 = x7; + } + public bool Equals(FixedArray8 other) { - return _00?.Equals(other._00) == true && - _01?.Equals(other._01) == true && - _02?.Equals(other._02) == true && - _03?.Equals(other._03) == true && - _04?.Equals(other._04) == true && - _05?.Equals(other._05) == true && - _06?.Equals(other._06) == true && - _07?.Equals(other._07) == true; + return EqualityComparer.Default.Equals(_00, other._00) && + EqualityComparer.Default.Equals(_01, other._01) && + EqualityComparer.Default.Equals(_02, other._02) && + EqualityComparer.Default.Equals(_03, other._03) && + EqualityComparer.Default.Equals(_04, other._04) && + EqualityComparer.Default.Equals(_05, other._05) && + EqualityComparer.Default.Equals(_06, other._06) && + EqualityComparer.Default.Equals(_07, other._07); } public override bool Equals(object? obj) diff --git a/Robust.UnitTesting/Shared/Physics/GridReparentVelocity_Test.cs b/Robust.UnitTesting/Shared/Physics/GridReparentVelocity_Test.cs new file mode 100644 index 00000000000..fe0038f28b0 --- /dev/null +++ b/Robust.UnitTesting/Shared/Physics/GridReparentVelocity_Test.cs @@ -0,0 +1,223 @@ +using System.Numerics; +using System.Threading.Tasks; +using NUnit.Framework; +using Robust.Shared.GameObjects; +using Robust.Shared.Map; +using Robust.Shared.Maths; +using Robust.Shared.Physics; +using Robust.Shared.Physics.Collision.Shapes; +using Robust.Shared.Physics.Components; +using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Systems; +using Robust.UnitTesting.Server; + +namespace Robust.UnitTesting.Shared.Physics; + +[TestFixture, TestOf(typeof(SharedPhysicsSystem))] +public sealed class GridReparentVelocity_Test +{ + private ISimulation _sim = default!; + private IEntitySystemManager _systems = default!; + private IEntityManager _entManager = default!; + private IMapManager _mapManager = default!; + private FixtureSystem _fixtureSystem = default!; + private SharedMapSystem _mapSystem = default!; + private SharedPhysicsSystem _physSystem = default!; + + // Test objects. + private EntityUid _mapUid = default!; + private MapId _mapId = default!; + private EntityUid _gridUid = default!; + private EntityUid _objUid = default!; + + [OneTimeSetUp] + public void FixtureSetup() + { + _sim = RobustServerSimulation.NewSimulation() + .InitializeInstance(); + + _systems = _sim.Resolve(); + _entManager = _sim.Resolve(); + _mapManager = _sim.Resolve(); + _fixtureSystem = _systems.GetEntitySystem(); + _mapSystem = _systems.GetEntitySystem(); + _physSystem = _systems.GetEntitySystem(); + } + + [SetUp] + public void Setup() + { + _mapUid = _mapSystem.CreateMap(out _mapId); + + // Spawn a 1x1 grid centered at (0.5, 0.5), ensure it's movable and its velocity has no damping. + var gridEnt = _mapManager.CreateGridEntity(_mapId); + var gridPhys = _entManager.GetComponent(gridEnt); + _physSystem.SetSleepingAllowed(gridEnt, gridPhys, false); + _physSystem.SetBodyType(gridEnt, BodyType.Dynamic, body: gridPhys); + _physSystem.SetLinearDamping(gridEnt, gridPhys, 0.0f); + _physSystem.SetAngularDamping(gridEnt, gridPhys, 0.0f); + + _mapSystem.SetTile(gridEnt, Vector2i.Zero, new Tile(1)); + _physSystem.WakeBody(gridEnt, body: gridPhys); + + _gridUid = gridEnt.Owner; + } + + // Spawn a bullet-like test object at the given position. + public EntityUid SetupTestObject(EntityCoordinates coords) + { + var obj = _entManager.SpawnEntity(null, coords); + + var objPhys = _entManager.EnsureComponent(obj); + var objFix = _entManager.EnsureComponent(obj); + + // Set up physics (no velocity damping, dynamic body, physics enabled) + _entManager.GetComponent(obj); + _physSystem.SetSleepingAllowed(obj, objPhys, false); + _physSystem.SetBodyType(obj, BodyType.Dynamic, body: objPhys); + _physSystem.SetLinearDamping(obj, objPhys, 0.0f); + _physSystem.SetAngularDamping(obj, objPhys, 0.0f); + + // Set up fixture. + var poly = new PolygonShape(); + poly.SetAsBox(0.1f, 0.1f); + _fixtureSystem.CreateFixture(obj, "fix1", new Fixture(poly, 0, 0, false), manager: objFix, body: objPhys); + _physSystem.WakeBody(obj, body: objPhys); + + return obj; + } + + [TearDown] + public void Teardown() + { + _entManager.DeleteEntity(_gridUid); + _gridUid = default!; + _entManager.DeleteEntity(_objUid); + _objUid = default!; + _mapSystem.DeleteMap(_mapId); + _mapId = default!; + _entManager.DeleteEntity(_mapUid); + } + + // Moves an object off of a moving grid, checks for conservation of linear velocity. + [Test] + public async Task TestLinearVelocityOnlyMoveOffGrid() + { + // Spawn our test object in the middle of the grid, ensure it has no damping. + _objUid = SetupTestObject(new EntityCoordinates(_gridUid, 0.5f, 0.5f)); + + Assert.Multiple(() => + { + // Our object should start on the grid. + Assert.That(_entManager.GetComponent(_objUid).ParentUid, Is.EqualTo(_gridUid)); + + // Set the velocity of the grid and our object. + Assert.That(_physSystem.SetLinearVelocity(_objUid, new Vector2(3.5f, 4.75f)), Is.True); + Assert.That(_physSystem.SetLinearVelocity(_gridUid, new Vector2(1.0f, 2.0f)), Is.True); + + // Wait a second to clear the grid + _physSystem.Update(1.0f); + + // The object should be parented to the map and maintain its map velocity, the grid should be unchanged. + var objXform = _entManager.GetComponent(_objUid); + var gridXform = _entManager.GetComponent(_gridUid); + Assert.That(objXform.ParentUid, Is.EqualTo(_mapUid), $"Object is not on map - actual position: {objXform.ParentUid} {objXform.LocalPosition}, grid position: {gridXform.ParentUid} {gridXform.LocalPosition}"); + Assert.That(_entManager.GetComponent(_objUid).LinearVelocity, Is.EqualTo(new Vector2(4.5f, 6.75f))); + Assert.That(_entManager.GetComponent(_gridUid).LinearVelocity, Is.EqualTo(new Vector2(1.0f, 2.0f))); + }); + } + + [Test] + // Moves an object onto a moving grid, checks for conservation of linear velocity. + public async Task TestLinearVelocityOnlyMoveOntoGrid() + { + // Spawn our test object 1 m off of the middle of the grid in both directions. + _objUid = SetupTestObject(new EntityCoordinates(_mapUid, 1.5f, 1.5f)); + + Assert.Multiple(() => + { + // Assert that we start off the grid. + Assert.That(_entManager.GetComponent(_objUid).ParentUid, Is.EqualTo(_mapUid)); + + // Set the velocity of the grid and our object. + Assert.That(_physSystem.SetLinearVelocity(_objUid, new Vector2(-2.0f, -3.0f)), Is.True); + Assert.That(_physSystem.SetLinearVelocity(_gridUid, new Vector2(-1.0f, -2.0f)), Is.True); + + // Wait a second to move onto the middle of the grid + _physSystem.Update(1.0f); + + // The object should be parented to the grid and maintain its map velocity (slowing down), the grid should be unchanged. + var objXform = _entManager.GetComponent(_objUid); + var gridXform = _entManager.GetComponent(_gridUid); + Assert.That(objXform.ParentUid, Is.EqualTo(_gridUid), $"Object is not on grid - actual position: {objXform.ParentUid} {objXform.LocalPosition}, grid position: {gridXform.ParentUid} {gridXform.LocalPosition}"); + Assert.That(_entManager.GetComponent(_objUid).LinearVelocity, Is.EqualTo(new Vector2(-1.0f, -1.0f))); + Assert.That(_entManager.GetComponent(_gridUid).LinearVelocity, Is.EqualTo(new Vector2(-1.0f, -2.0f))); + }); + } + + [Test] + // Moves a rotating object off of a rotating grid, checks for conservation of angular velocity. + public async Task TestLinearAndAngularVelocityMoveOffGrid() + { + // Spawn our test object in the middle of the grid. + _objUid = SetupTestObject(new EntityCoordinates(_gridUid, 0.5f, 0.5f)); + + Assert.Multiple(() => + { + // Our object should start on the grid. + Assert.That(_entManager.GetComponent(_objUid).ParentUid, Is.EqualTo(_gridUid)); + + // Set the velocity of the grid and our object. + Assert.That(_physSystem.SetLinearVelocity(_objUid, new Vector2(3.5f, 4.75f)), Is.True); + Assert.That(_physSystem.SetAngularVelocity(_objUid, 1.0f), Is.True); + Assert.That(_physSystem.SetLinearVelocity(_gridUid, new Vector2(1.0f, 2.0f)), Is.True); + Assert.That(_physSystem.SetAngularVelocity(_gridUid, 2.0f), Is.True); + + // Wait a second to clear the grid + _physSystem.Update(1.0f); + + // The object should be parented to the map and maintain its map velocity, the grid should be unchanged. + var objXform = _entManager.GetComponent(_objUid); + var gridXform = _entManager.GetComponent(_gridUid); + Assert.That(objXform.ParentUid, Is.EqualTo(_mapUid), $"Object is not on map - actual position: {objXform.ParentUid} {objXform.LocalPosition}, grid position: {gridXform.ParentUid} {gridXform.LocalPosition}"); + // Not checking object's linear velocity in this case, non-zero contribution from grid angular velocity. + Assert.That(_entManager.GetComponent(_objUid).AngularVelocity, Is.EqualTo(3.0f)); + var gridPhys = _entManager.GetComponent(_gridUid); + Assert.That(gridPhys.LinearVelocity, Is.EqualTo(new Vector2(1.0f, 2.0f))); + Assert.That(gridPhys.AngularVelocity, Is.EqualTo(2.0f)); + }); + } + + [Test] + // Moves a rotating object onto a rotating grid, checks for conservation of angular velocity. + public async Task TestLinearAndAngularVelocityMoveOntoGrid() + { + // Spawn our test object 1 m off of the middle of the grid in both directions. + _objUid = SetupTestObject(new EntityCoordinates(_mapUid, 1.5f, 1.5f)); + + Assert.Multiple(() => + { + // Assert that we start off the grid. + Assert.That(_entManager.GetComponent(_objUid).ParentUid, Is.EqualTo(_mapUid)); + + // Set the velocity of the grid and our object. + Assert.That(_physSystem.SetLinearVelocity(_objUid, new Vector2(-2.0f, -3.0f)), Is.True); + Assert.That(_physSystem.SetAngularVelocity(_objUid, 1.0f), Is.True); + Assert.That(_physSystem.SetLinearVelocity(_gridUid, new Vector2(-1.0f, -2.0f)), Is.True); + Assert.That(_physSystem.SetAngularVelocity(_gridUid, 2.0f), Is.True); + + // Wait a second to move onto the middle of the grid + _physSystem.Update(1.0f); + + // The object should be parented to the grid and maintain its map velocity (slowing down), the grid should be unchanged. + var objXform = _entManager.GetComponent(_objUid); + var gridXform = _entManager.GetComponent(_gridUid); + Assert.That(objXform.ParentUid, Is.EqualTo(_gridUid), $"Object is not on grid - actual position: {objXform.ParentUid} {objXform.LocalPosition}, grid position: {gridXform.ParentUid} {gridXform.LocalPosition}"); + // Not checking object's linear velocity in this case, non-zero contribution from grid angular velocity. + Assert.That(_entManager.GetComponent(_objUid).AngularVelocity, Is.EqualTo(-1.0f)); + var gridPhys = _entManager.GetComponent(_gridUid); + Assert.That(gridPhys.LinearVelocity, Is.EqualTo(new Vector2(-1.0f, -2.0f))); + Assert.That(gridPhys.AngularVelocity, Is.EqualTo(2.0f)); + }); + } +} diff --git a/Robust.UnitTesting/Shared/Physics/ManifoldManager_Test.cs b/Robust.UnitTesting/Shared/Physics/ManifoldManager_Test.cs index e4adecbf6d1..f0fc4d027a3 100644 --- a/Robust.UnitTesting/Shared/Physics/ManifoldManager_Test.cs +++ b/Robust.UnitTesting/Shared/Physics/ManifoldManager_Test.cs @@ -5,6 +5,7 @@ using Robust.Shared.Physics; using Robust.Shared.Physics.Collision; using Robust.Shared.Physics.Collision.Shapes; +using Robust.Shared.Utility; namespace Robust.UnitTesting.Shared.Physics { @@ -76,10 +77,7 @@ public void TestPolyOnPolyManifolds() { var transformB = new Transform(Vector2.One, 0f); var transformA = new Transform(transformB.Position + new Vector2(0.5f, 0.0f), 0f); - var manifold = new Manifold() - { - Points = new ManifoldPoint[2] - }; + var manifold = new Manifold(); var expectedManifold = new Manifold { @@ -87,17 +85,24 @@ public void TestPolyOnPolyManifolds() LocalNormal = new Vector2(-1, 0), LocalPoint = new Vector2(-0.5f, 0), PointCount = 2, - Points = new ManifoldPoint[] - { - new() {LocalPoint = new Vector2(0.5f, -0.5f), Id = new ContactID {Key = 65795}}, - new() {LocalPoint = new Vector2(0.5f, 0.5f), Id = new ContactID {Key = 66051}} - } + Points = new FixedArray2( + new ManifoldPoint + { + LocalPoint = new Vector2(0.5f, -0.5f), + Id = new ContactID {Key = 65795} + }, + new ManifoldPoint + { + LocalPoint = new Vector2(0.5f, 0.5f), + Id = new ContactID {Key = 66051} + } + ) }; _manifoldManager.CollidePolygons(ref manifold, _polyA, transformA, _polyB, transformB); - for (var i = 0; i < manifold.Points.Length; i++) + for (var i = 0; i < manifold.PointCount; i++) { - Assert.That(manifold.Points[i], Is.EqualTo(expectedManifold.Points[i])); + Assert.That(manifold.Points.AsSpan[i], Is.EqualTo(expectedManifold.Points.AsSpan[i])); } Assert.That(manifold, Is.EqualTo(expectedManifold));