Add shapecasts + raycasts

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

Robust.Shared.Maths/Matrix3Helpers.cs

@@ -67,6 +67,15 @@ public static Box2 TransformBox(this Matrix3x2 refFromBox, in Box2 box)
         return Unsafe.As<Vector128<float>, Box2>(ref lbrt);
     }
 
+    /// <summary>
+    /// Gets the position of the Matrix. Will have some precision loss.
+    /// </summary>
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static Vector2 Position(this Matrix3x2 t)
+    {
+        return new Vector2(t.M31, t.M32);
+    }
+
     /// <summary>
     /// Gets the rotation of the Matrix. Will have some precision loss.
     /// </summary>

Robust.Shared.Maths/Vector2Helpers.cs

@@ -1,6 +1,7 @@
 using System;
 using System.Numerics;
 using System.Runtime.CompilerServices;
+using JetBrains.Annotations;
 
 namespace Robust.Shared.Maths;
 
@@ -14,6 +15,34 @@ 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();
+        if (length < float.Epsilon)
+        {
+            return Vector2.Zero;
+        }
+
+        float invLength = 1.0f / length;
+        var n = new Vector2(invLength * v.X, invLength * v.Y);
+        return n;
+    }
+
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
     public static Vector2 InterpolateCubic(Vector2 preA, Vector2 a, Vector2 b, Vector2 postB, float t)
     {
@@ -255,6 +284,12 @@ public static Vector2 Cross(float s, in Vector2 a)
         return new(-s * a.Y, s * a.X);
     }
 
+    [Pure]
+    public static Vector2 RightPerp(this Vector2 v)
+    {
+        return new Vector2(v.Y, -v.X);
+    }
+
     /// <summary>
     /// Perform the cross product on a scalar and a vector. In 2D this produces
     /// a vector.

Robust.Shared/Physics/B2DynamicTree.cs

@@ -27,6 +27,7 @@
 using System.Numerics;
 using System.Runtime.CompilerServices;
 using Robust.Shared.Maths;
+using Robust.Shared.Physics.Systems;
 using Robust.Shared.Utility;
 
 namespace Robust.Shared.Physics
@@ -943,6 +944,219 @@ public void FastQuery(ref Box2 aabb, FastQueryCallback callback)
         private static readonly RayQueryCallback<RayQueryCallback> EasyRayQueryCallback =
             (ref RayQueryCallback callback, Proxy proxy, in Vector2 hitPos, float distance) => callback(proxy, hitPos, distance);
 
+        internal delegate float RayCallback<TState>(RayCastInput input, Proxy proxy, T context, ref TState State);
+
+        internal void RayCastNew<TState>(RayCastInput input, uint mask, ref TState state, RayCallback<TState> callback)
+        {
+            var p1 = input.Origin;
+            var d = input.Translation;
+
+            var r = d.Normalized();
+
+	        // v is perpendicular to the segment.
+	        var v = Vector2Helpers.Cross(1.0f, r);
+            var abs_v = Vector2.Abs(v);
+
+	        // Separating axis for segment (Gino, p80).
+	        // |dot(v, p1 - c)| > dot(|v|, h)
+
+	        float maxFraction = input.MaxFraction;
+
+	        var p2 = Vector2.Add(p1, maxFraction * d);
+
+	        // Build a bounding box for the segment.
+	        var segmentAABB = new Box2(Vector2.Min( p1, p2 ), Vector2.Max( p1, p2 ));
+
+	        var stack = new GrowableStack<Proxy>(stackalloc Proxy[256]);
+            ref var baseRef = ref _nodes[0];
+            var stackCount = 1;
+	        stack.Push(_root);
+
+	        var subInput = input;
+
+	        while (stackCount > 0)
+            {
+                stackCount = stack.GetCount();
+                var nodeId = stack.Pop();
+
+		        if ( nodeId == Proxy.Free)
+		        {
+			        continue;
+		        }
+
+		        var node = Unsafe.Add(ref baseRef, nodeId);
+
+		        if (!node.Aabb.Intersects(segmentAABB))// || ( node->categoryBits & maskBits ) == 0 )
+		        {
+			        continue;
+		        }
+
+		        // Separating axis for segment (Gino, p80).
+		        // |dot(v, p1 - c)| > dot(|v|, h)
+		        // radius extension is added to the node in this case
+		        var c = node.Aabb.Center;
+		        var h = node.Aabb.Extents;
+		        float term1 = MathF.Abs(Vector2.Dot(v, Vector2.Subtract(p1, c)));
+		        float term2 = Vector2.Dot(abs_v, h);
+		        if ( term2 < term1 )
+		        {
+			        continue;
+		        }
+
+		        if (node.IsLeaf)
+		        {
+			        subInput.MaxFraction = maxFraction;
+
+			        float value = callback(subInput, nodeId, node.UserData, ref state);
+
+			        if (value == 0.0f)
+			        {
+				        // The client has terminated the ray cast.
+				        return;
+			        }
+
+			        if (0.0f < value && value < maxFraction)
+			        {
+				        // Update segment bounding box.
+				        maxFraction = value;
+				        p2 = Vector2.Add(p1, maxFraction * d);
+				        segmentAABB.BottomLeft = Vector2.Min( p1, p2 );
+				        segmentAABB.TopRight = Vector2.Max( p1, p2 );
+			        }
+		        }
+		        else
+		        {
+			        Assert( stackCount < 256 - 1 );
+			        if (stackCount < 256 - 1 )
+			        {
+				        // TODO_ERIN just put one node on the stack, continue on a child node
+				        // TODO_ERIN test ordering children by nearest to ray origin
+				        stack.Push(node.Child1);
+				        stack.Push(node.Child2);
+			        }
+		        }
+	        }
+        }
+
+        /// This function receives clipped ray-cast input for a proxy. The function
+        /// returns the new ray fraction.
+        /// - return a value of 0 to terminate the ray-cast
+        /// - return a value less than input->maxFraction to clip the ray
+        /// - return a value of input->maxFraction to continue the ray cast without clipping
+        internal delegate float TreeShapeCastCallback<TState>(ref ShapeCastInput input, Proxy proxyId, T userData, ref TState state);
+
+        internal void ShapeCast<TState>(ShapeCastInput input, long maskBits, TreeShapeCastCallback<TState> callback, ref TState state)
+        {
+	        if (input.Count == 0)
+	        {
+		        return;
+	        }
+
+            var originAABB = new Box2(input.Points[0], input.Points[0]);
+
+	        for (var i = 1; i < input.Count; ++i)
+	        {
+		        originAABB.BottomLeft = Vector2.Min(originAABB.BottomLeft, input.Points[i]);
+		        originAABB.TopRight = Vector2.Max(originAABB.TopRight, input.Points[i]);
+	        }
+
+	        var radius = new Vector2(input.Radius, input.Radius);
+
+	        originAABB.BottomLeft = Vector2.Subtract(originAABB.BottomLeft, radius);
+	        originAABB.TopRight = Vector2.Add(originAABB.TopRight, radius );
+
+	        var p1 = originAABB.Center;
+	        var extension = originAABB.Extents;
+
+	        // v is perpendicular to the segment.
+	        var r = input.Translation;
+	        var v = Vector2Helpers.Cross(1.0f, r);
+	        var abs_v = Vector2.Abs(v);
+
+	        // Separating axis for segment (Gino, p80).
+	        // |dot(v, p1 - c)| > dot(|v|, h)
+
+	        float maxFraction = input.MaxFraction;
+
+	        // Build total box for the shape cast
+	        var t = Vector2.Multiply(maxFraction, input.Translation);
+
+            var totalAABB = new Box2(
+		        Vector2.Min(originAABB.BottomLeft, Vector2.Add(originAABB.BottomLeft, t)),
+		        Vector2.Max(originAABB.TopRight, Vector2.Add( originAABB.TopRight, t))
+	        );
+
+	        var subInput = input;
+
+            ref var baseRef = ref _nodes[0];
+            var stack = new GrowableStack<Proxy>(stackalloc Proxy[256]);
+	        var stackCount = 1;
+	        stack.Push(_root);
+
+	        while (stackCount > 0)
+            {
+		        var nodeId = stack.Pop();
+                stackCount = stack.GetCount();
+
+		        if (nodeId == Proxy.Free)
+		        {
+			        continue;
+		        }
+
+                var node = Unsafe.Add(ref baseRef, nodeId);
+		        if (!node.Aabb.Intersects(totalAABB))// || ( node->categoryBits & maskBits ) == 0 )
+		        {
+			        continue;
+		        }
+
+		        // Separating axis for segment (Gino, p80).
+		        // |dot(v, p1 - c)| > dot(|v|, h)
+		        // radius extension is added to the node in this case
+		        var c = node.Aabb.Center;
+		        var h = Vector2.Add(node.Aabb.Extents, extension);
+		        float term1 = MathF.Abs(Vector2.Dot(v, Vector2.Subtract(p1, c)));
+		        float term2 = Vector2.Dot(abs_v, h);
+		        if (term2 < term1)
+		        {
+			        continue;
+		        }
+
+		        if (node.IsLeaf)
+		        {
+			        subInput.MaxFraction = maxFraction;
+
+			        float value = callback(ref subInput, nodeId, node.UserData, ref state);
+
+			        if ( value == 0.0f )
+			        {
+				        // The client has terminated the ray cast.
+				        return;
+			        }
+
+			        if (0.0f < value && value < maxFraction)
+			        {
+				        // Update segment bounding box.
+				        maxFraction = value;
+				        t = Vector2.Multiply(maxFraction, input.Translation);
+				        totalAABB.BottomLeft = Vector2.Min( originAABB.BottomLeft, Vector2.Add(originAABB.BottomLeft, t));
+				        totalAABB.TopRight = Vector2.Max( originAABB.TopRight, Vector2.Add( originAABB.TopRight, t));
+			        }
+		        }
+		        else
+		        {
+			        Assert(stackCount < 256 - 1);
+
+			        if (stackCount < 255)
+			        {
+				        // TODO_ERIN just put one node on the stack, continue on a child node
+				        // TODO_ERIN test ordering children by nearest to ray origin
+				        stack.Push(node.Child1);
+				        stack.Push(node.Child2);
+			        }
+		        }
+	        }
+        }
+
         public void RayCast(RayQueryCallback callback, in Ray input)
         {
             RayCast(ref callback, EasyRayQueryCallback, input);

Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs

@@ -24,6 +24,7 @@ private static Box2 ExtractAabbFunc(in FixtureProxy proxy)
     }
 
     public int Count => _tree.NodeCount;
+    public B2DynamicTree<FixtureProxy> Tree => _tree;
 
     public Box2 GetFatAabb(DynamicTree.Proxy proxy)
     {