more work

space-wizards · Sep 18, 2024 · 4646858 · 4646858
1 parent 6bd6d6c
commit 4646858
Show file tree

Hide file tree

Showing 5 changed files with 87 additions and 28 deletions.
diff --git a/Robust.Shared.Maths/MathHelper.cs b/Robust.Shared.Maths/MathHelper.cs
@@ -745,6 +745,21 @@ public static T CeilMultipleOfPowerOfTwo<T>(T value, T powerOfTwo) where T : IBi
             return remainder == T.Zero ? value : (value | mask) + T.One;
         }
 
+        public static bool IsValid(this float value)
+        {
+            if (float.IsNaN(value))
+            {
+                return false;
+            }
+
+            if (float.IsInfinity(value))
+            {
+                return false;
+            }
+
+            return true;
+        }
+
         #endregion Public Members
     }
 }
diff --git a/Robust.Shared.Maths/Vector2Helpers.cs b/Robust.Shared.Maths/Vector2Helpers.cs
@@ -15,6 +15,21 @@ public static class Vector2Helpers
     /// </summary>
     public static readonly Vector2 Half = new(0.5f, 0.5f);
 
+    public static bool IsValid(this Vector2 v)
+    {
+        if (float.IsNaN(v.X) || float.IsNaN(v.Y))
+        {
+            return false;
+        }
+
+        if (float.IsInfinity(v.X) || float.IsInfinity(v.Y))
+        {
+            return false;
+        }
+
+        return true;
+    }
+
     public static Vector2 GetLengthAndNormalize(this Vector2 v, ref float length)
     {
         length = v.Length();

diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs
@@ -50,7 +50,7 @@ public void RayCast(MapCoordinates coordinates, Vector2 translation, uint collis
                 var localOrigin = Physics.Transform.InvTransformPoint(transform, state.Origin);
                 var localTranslation = Physics.Transform.InvTransformPoint(transform, state.Origin + state.Translation);
 
-                state.System.RayCast((entity.Owner, entity.Comp), localOrigin, localTranslation);
+                state.System.RayCast((entity.Owner, entity.Comp), localOrigin, localTranslation, collisionMask: state.CollisionMask);
             });
     }
 

diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs
@@ -472,6 +472,27 @@ public void Refilter(EntityUid uid, Fixture fixture, TransformComponent? xform =
             TouchProxies(xform.MapUid.Value, matrix, fixture);
         }
 
+        internal void GetBroadphases(MapId mapId, Box2 aabb,BroadphaseCallback callback)
+        {
+            var internalState = (callback, _broadphaseQuery);
+
+            _mapManager.FindGridsIntersecting(mapId,
+                aabb,
+                ref internalState,
+                static (
+                    EntityUid uid,
+                    MapGridComponent grid,
+                    ref (BroadphaseCallback callback, EntityQuery<BroadphaseComponent> _broadphaseQuery) tuple) =>
+                {
+                    if (!tuple._broadphaseQuery.TryComp(uid, out var broadphase))
+                        return true;
+
+                    tuple.callback((uid, broadphase));
+                    return true;
+                    // Approx because we don't really need accurate checks for these most of the time.
+                }, approx: true, includeMap: true);
+        }
+
         internal void GetBroadphases<TState>(MapId mapId, Box2 aabb, ref TState state, BroadphaseCallback<TState> callback)
         {
             var internalState = (state, callback, _broadphaseQuery);
@@ -493,6 +514,8 @@ internal void GetBroadphases<TState>(MapId mapId, Box2 aabb, ref TState state, B
                 }, approx: true, includeMap: true);
         }
 
+        internal delegate void BroadphaseCallback(Entity<BroadphaseComponent> entity);
+
         internal delegate void BroadphaseCallback<TState>(Entity<BroadphaseComponent> entity, ref TState state);
 
         private record struct BroadphaseContactJob() : IParallelRobustJob

diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs
@@ -144,21 +144,25 @@ public IEnumerable<PhysicsComponent> GetCollidingEntities(MapId mapId, in Box2 w
 
             var aabb = worldAABB;
             var bodies = new HashSet<PhysicsComponent>();
+            var state = (_transform, bodies, aabb);
 
-            _broadphase.GetBroadphases(mapId, worldAABB, broadphase =>
-            {
-                var gridAABB = _transform.GetInvWorldMatrix(broadphase.Owner).TransformBox(aabb);
-
-                foreach (var proxy in broadphase.Comp.StaticTree.QueryAabb(gridAABB, false))
+            _broadphase.GetBroadphases(mapId, worldAABB, ref state, static
+                (
+                    Entity<BroadphaseComponent> entity,
+                    ref (SharedTransformSystem _transform, HashSet<PhysicsComponent> bodies, Box2 aabb) tuple) =>
                 {
-                    bodies.Add(proxy.Body);
-                }
+                    var gridAABB = tuple._transform.GetInvWorldMatrix(entity.Owner).TransformBox(tuple.aabb);
 
-                foreach (var proxy in broadphase.Comp.DynamicTree.QueryAabb(gridAABB, false))
-                {
-                    bodies.Add(proxy.Body);
-                }
-            });
+                    foreach (var proxy in entity.Comp.StaticTree.QueryAabb(gridAABB, false))
+                    {
+                        tuple.bodies.Add(proxy.Body);
+                    }
+
+                    foreach (var proxy in entity.Comp.DynamicTree.QueryAabb(gridAABB, false))
+                    {
+                        tuple.bodies.Add(proxy.Body);
+                    }
+                });
 
             return bodies;
         }
@@ -171,25 +175,27 @@ public IEnumerable<Entity<PhysicsComponent>> GetCollidingEntities(MapId mapId, i
             if (mapId == MapId.Nullspace)
                 return Array.Empty<Entity<PhysicsComponent>>();
 
-            var bounds = worldBounds;
             var bodies = new HashSet<Entity<PhysicsComponent>>();
 
-            _broadphase.GetBroadphases(mapId,
-                worldBounds.CalcBoundingBox(),
-                broadphase =>
-                {
-                    var gridAABB = _transform.GetInvWorldMatrix(broadphase).TransformBox(bounds);
+            var state = (_transform, bodies, worldBounds);
 
-                    foreach (var proxy in broadphase.Comp.StaticTree.QueryAabb(gridAABB, false))
-                    {
-                        bodies.Add(new Entity<PhysicsComponent>(proxy.Entity, proxy.Body));
-                    }
+            _broadphase.GetBroadphases(mapId, worldBounds.CalcBoundingBox(), ref state, static
+            (
+                Entity<BroadphaseComponent> entity,
+                ref (SharedTransformSystem _transform, HashSet<PhysicsComponent> bodies, Box2Rotated bounds) tuple) =>
+            {
+                var gridAABB = tuple._transform.GetInvWorldMatrix(entity.Owner).TransformBox(tuple.bounds);
 
-                    foreach (var proxy in broadphase.Comp.DynamicTree.QueryAabb(gridAABB, false))
-                    {
-                        bodies.Add(new Entity<PhysicsComponent>(proxy.Entity, proxy.Body));
-                    }
-                });
+                foreach (var proxy in entity.Comp.StaticTree.QueryAabb(gridAABB, false))
+                {
+                    tuple.bodies.Add(proxy.Body);
+                }
+
+                foreach (var proxy in entity.Comp.DynamicTree.QueryAabb(gridAABB, false))
+                {
+                    tuple.bodies.Add(proxy.Body);
+                }
+            });
 
             return bodies;
         }