2025 04 01 velocity unit tests

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;
 

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;

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;
         }
 }

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)
 			    {

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;

Robust.Shared/Physics/Collision/CollisionManager.Polygons.cs

@@ -238,14 +238,16 @@ 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;
             float separation = normalX * value.X + normalY * value.Y - frontOffset;
 
             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;
 

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;

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<Manifold>, IApproxEquatable<Manifold>
     /// <summary>
     ///     Points of contact, can only be 0 -> 2.
     /// </summary>
-    internal ManifoldPoint[] Points;
+    internal FixedArray2<ManifoldPoint> 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<ManifoldPoint>, IApproxEquatable<ManifoldPoint>

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)

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
         /// </summary>
         public int IndexB { get; set; }
 
-        public Vector2[] LocalPoints;
+        internal FixedArray2<Vector2> LocalPoints;
 
         public Vector2 LocalNormal;
 

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<VelocityConstraintPoint> 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;

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;
         }

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