diff --git a/Robust.Shared.Maths/MathHelper.cs b/Robust.Shared.Maths/MathHelper.cs index 43d04a6f9de..346060d0d79 100644 --- a/Robust.Shared.Maths/MathHelper.cs +++ b/Robust.Shared.Maths/MathHelper.cs @@ -745,6 +745,21 @@ public static T CeilMultipleOfPowerOfTwo(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/Matrix3Helpers.cs b/Robust.Shared.Maths/Matrix3Helpers.cs index 85a5a853a10..912b0698af4 100644 --- a/Robust.Shared.Maths/Matrix3Helpers.cs +++ b/Robust.Shared.Maths/Matrix3Helpers.cs @@ -73,7 +73,7 @@ public static Box2 TransformBox(this Matrix3x2 refFromBox, in Box2 box) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static Angle Rotation(this Matrix3x2 t) { - return new Vector2(t.M11, t.M12).ToAngle(); + return new Angle(Math.Atan2(t.M12, t.M11)); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/Robust.Shared.Maths/Vector2Helpers.cs b/Robust.Shared.Maths/Vector2Helpers.cs index 3744c178b1d..3faaa27501a 100644 --- a/Robust.Shared.Maths/Vector2Helpers.cs +++ b/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 /// 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); + } + /// /// Perform the cross product on a scalar and a vector. In 2D this produces /// a vector. diff --git a/Robust.Shared/Physics/B2DynamicTree.cs b/Robust.Shared/Physics/B2DynamicTree.cs index a0d86d5b7d4..c2c906c0ded 100644 --- a/Robust.Shared/Physics/B2DynamicTree.cs +++ b/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,217 @@ public void FastQuery(ref Box2 aabb, FastQueryCallback callback) private static readonly RayQueryCallback EasyRayQueryCallback = (ref RayQueryCallback callback, Proxy proxy, in Vector2 hitPos, float distance) => callback(proxy, hitPos, distance); + internal delegate float RayCallback(RayCastInput input, T context, ref WorldRayCastContext state); + + internal void RayCastNew(RayCastInput input, long mask, ref WorldRayCastContext state, RayCallback 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(stackalloc Proxy[256]); + ref var baseRef = ref _nodes[0]; + stack.Push(_root); + + var subInput = input; + + while (stack.GetCount() > 0) + { + 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, 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 + { + var stackCount = stack.GetCount(); + 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(ShapeCastInput input, T userData, ref WorldRayCastContext state); + + internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCastCallback callback, ref WorldRayCastContext 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(stackalloc Proxy[256]); + stack.Push(_root); + + while (stack.GetCount() > 0) + { + var nodeId = stack.Pop(); + + 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(subInput, 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 + { + var stackCount = stack.GetCount(); + 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); diff --git a/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs b/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs index e869ebacb9b..b83a1adc025 100644 --- a/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs +++ b/Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs @@ -24,6 +24,7 @@ private static Box2 ExtractAabbFunc(in FixtureProxy proxy) } public int Count => _tree.NodeCount; + public B2DynamicTree Tree => _tree; public Box2 GetFatAabb(DynamicTree.Proxy proxy) { diff --git a/Robust.Shared/Physics/Collision/DistanceProxy.cs b/Robust.Shared/Physics/Collision/DistanceProxy.cs index 7312d732a66..762ae3f3225 100644 --- a/Robust.Shared/Physics/Collision/DistanceProxy.cs +++ b/Robust.Shared/Physics/Collision/DistanceProxy.cs @@ -45,6 +45,12 @@ internal ref struct DistanceProxy // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. + internal DistanceProxy(Vector2[] vertices, float radius) + { + Vertices = vertices; + Radius = radius; + } + /// /// Initialize the proxy using the given shape. The shape /// must remain in scope while the proxy is in use. @@ -143,6 +149,13 @@ public Vector2 GetSupportVertex(Vector2 direction) return Vertices[bestIndex]; } + + internal static DistanceProxy MakeProxy(Vector2[] vertices, int count, float radius ) + { + count = Math.Min(count, PhysicsConstants.MaxPolygonVertices); + var proxy = new DistanceProxy(vertices[..count], radius); + return proxy; + } } /// @@ -306,6 +319,16 @@ internal Vector2 GetSearchDirection() } } + public static Vector2 Weight2( float a1, Vector2 w1, float a2, Vector2 w2 ) + { + return new Vector2(a1 * w1.X + a2 * w2.X, a1 * w1.Y + a2 * w2.Y); + } + + public static Vector2 Weight3(float a1, Vector2 w1, float a2, Vector2 w2, float a3, Vector2 w3 ) + { + return new Vector2(a1 * w1.X + a2 * w2.X + a3 * w3.X, a1 * w1.Y + a2 * w2.Y + a3 * w3.Y); + } + internal Vector2 GetClosestPoint() { switch (Count) @@ -329,6 +352,226 @@ internal Vector2 GetClosestPoint() } } + public static Vector2 ComputeSimplexClosestPoint(Simplex s) + { + switch (s.Count) + { + case 0: + DebugTools.Assert(false); + return Vector2.Zero; + + case 1: + return s.V._00.W; + + case 2: + return Weight2(s.V._00.A, s.V._00.W, s.V._01.A, s.V._01.W); + + case 3: + return Vector2.Zero; + + default: + DebugTools.Assert(false); + return Vector2.Zero; + } + } + + public static void ComputeSimplexWitnessPoints(ref Vector2 a, ref Vector2 b, Simplex s) + { + switch (s.Count) + { + case 0: + DebugTools.Assert(false); + break; + + case 1: + a = s.V._00.WA; + b = s.V._00.WB; + break; + + case 2: + a = Weight2(s.V._00.A, s.V._00.WA, s.V._01.A, s.V._01.WA); + b = Weight2(s.V._00.A, s.V._00.WB, s.V._01.A, s.V._01.WB); + break; + + case 3: + a = Weight3(s.V._00.A, s.V._00.WA, s.V._01.A, s.V._01.WA, s.V._02.A, s.V._02.WA); + // TODO_ERIN why are these not equal? + //*b = b2Weight3(s->v1.a, s->v1.wB, s->v2.a, s->v2.wB, s->v3.a, s->v3.wB); + b = a; + break; + + default: + DebugTools.Assert(false); + break; + } + } + + // 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 + public static void SolveSimplex2(ref Simplex s) + { + var w1 = s.V._00.W; + var w2 = s.V._01.W; + var e12 = Vector2.Subtract(w2, w1); + + // w1 region + float d12_2 = -Vector2.Dot(w1, e12); + if (d12_2 <= 0.0f) + { + // a2 <= 0, so we clamp it to 0 + s.V._00.A = 1.0f; + s.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 + s.V._01.A = 1.0f; + s.Count = 1; + s.V._00 = s.V._01; + return; + } + + // Must be in e12 region. + float inv_d12 = 1.0f / ( d12_1 + d12_2 ); + s.V._00.A = d12_1 * inv_d12; + s.V._01.A = d12_2 * inv_d12; + s.Count = 2; + } + + public static void SolveSimplex3(ref Simplex s) + { + var w1 = s.V._00.W; + var w2 = s.V._01.W; + var w3 = s.V._02.W; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + var e12 = Vector2.Subtract(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 + var e13 = Vector2.Subtract(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 + var e23 = Vector2.Subtract(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 = Vector2Helpers.Cross(e12, e13); + + float d123_1 = n123 * Vector2Helpers.Cross(w2, w3); + float d123_2 = n123 * Vector2Helpers.Cross(w3, w1); + float d123_3 = n123 * Vector2Helpers.Cross(w1, w2); + + // w1 region + if (d12_2 <= 0.0f && d13_2 <= 0.0f) + { + s.V._00.A = 1.0f; + s.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 ); + s.V._00.A = d12_1 * inv_d12; + s.V._01.A = d12_2 * inv_d12; + s.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 ); + s.V._00.A = d13_1 * inv_d13; + s.V._02.A = d13_2 * inv_d13; + s.Count = 2; + s.V._01 = s.V._02; + return; + } + + // w2 region + if (d12_1 <= 0.0f && d23_2 <= 0.0f) + { + s.V._01.A = 1.0f; + s.Count = 1; + s.V._00 = s.V._01; + return; + } + + // w3 region + if (d13_1 <= 0.0f && d23_1 <= 0.0f) + { + s.V._02.A = 1.0f; + s.Count = 1; + s.V._00 = s.V._02; + return; + } + + // e23 + if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) + { + float inv_d23 = 1.0f / ( d23_1 + d23_2 ); + s.V._01.A = d23_1 * inv_d23; + s.V._02.A = d23_2 * inv_d23; + s.Count = 2; + s.V._00 = s.V._02; + return; + } + + // Must be in triangle123 + float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); + s.V._00.A = d123_1 * inv_d123; + s.V._01.A = d123_2 * inv_d123; + s.V._02.A = d123_3 * inv_d123; + s.Count = 3; + } + internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB) { switch (Count) diff --git a/Robust.Shared/Physics/Collision/Shapes/PolygonShape.cs b/Robust.Shared/Physics/Collision/Shapes/PolygonShape.cs index a6b00d1f599..240be70ed20 100644 --- a/Robust.Shared/Physics/Collision/Shapes/PolygonShape.cs +++ b/Robust.Shared/Physics/Collision/Shapes/PolygonShape.cs @@ -27,6 +27,7 @@ using Robust.Shared.Configuration; using Robust.Shared.IoC; using Robust.Shared.Maths; +using Robust.Shared.Physics.Shapes; using Robust.Shared.Physics.Systems; using Robust.Shared.Serialization; using Robust.Shared.Serialization.Manager.Attributes; @@ -172,6 +173,13 @@ public PolygonShape() { } + internal PolygonShape(Polygon poly) + { + Vertices = poly.Vertices; + Normals = poly.Normals; + Centroid = poly.Centroid; + } + public PolygonShape(float radius) { Radius = radius; diff --git a/Robust.Shared/Physics/IBroadPhase.cs b/Robust.Shared/Physics/IBroadPhase.cs index 2caf7862106..d66c63de1c0 100644 --- a/Robust.Shared/Physics/IBroadPhase.cs +++ b/Robust.Shared/Physics/IBroadPhase.cs @@ -10,6 +10,8 @@ public interface IBroadPhase { int Count { get; } + public B2DynamicTree Tree { get; } + Box2 GetFatAabb(DynamicTree.Proxy proxy); DynamicTree.Proxy AddProxy(ref FixtureProxy proxy); diff --git a/Robust.Shared/Physics/Shapes/Polygon.cs b/Robust.Shared/Physics/Shapes/Polygon.cs index 64f0f3bd77c..1539064e3a4 100644 --- a/Robust.Shared/Physics/Shapes/Polygon.cs +++ b/Robust.Shared/Physics/Shapes/Polygon.cs @@ -87,8 +87,8 @@ public Polygon(Vector2[] vertices) if (hull.Count < 3) { - Vertices = Array.Empty(); - Normals = Array.Empty(); + Vertices = []; + Normals = []; return; } diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs b/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs new file mode 100644 index 00000000000..1185b8f6c4a --- /dev/null +++ b/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs @@ -0,0 +1,728 @@ +using System; +using System.Numerics; +using Robust.Shared.Maths; +using Robust.Shared.Physics.Collision; +using Robust.Shared.Physics.Collision.Shapes; +using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Shapes; +using Robust.Shared.Utility; + +namespace Robust.Shared.Physics.Systems; + +public sealed partial class RayCastSystem +{ + /* + * This is really "geometry and friends" as it has all the private methods. + */ + + #region Callbacks + + /// + /// Tells the callback we want every entity. + /// + private static float RayCastAllCallback(FixtureProxy proxy, Vector2 point, Vector2 normal, float fraction, ref RayResult result) + { + result.Results.Add(new RayHit(proxy.Entity, normal, fraction) + { + Point = point, + }); + return 1f; + } + + /// + /// This just lets the callback continue. + /// + private static float RayCastClosestCallback(FixtureProxy proxy, Vector2 point, Vector2 normal, float fraction, ref RayResult result) + { + var add = false; + + if (result.Results.Count > 0) + { + if (result.Results[0].Fraction > fraction) + { + add = true; + result.Results.Clear(); + } + } + else + { + add = true; + } + + if (add) + { + result.Results.Add(new RayHit(proxy.Entity, normal, fraction) + { + Point = point, + }); + } + + return fraction; + } + + #endregion + + #region Raycast + + private CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform transform) + { + var localInput = input; + localInput.Origin = Physics.Transform.InvTransformPoint(transform, input.Origin); + localInput.Translation = Quaternion2D.InvRotateVector(transform.Quaternion2D, input.Translation); + + CastOutput output = new(); + + switch (shape) + { + /* + case b2_capsuleShape: + output = b2RayCastCapsule( &localInput, &shape->capsule ); + break; + */ + case PhysShapeCircle circle: + output = RayCastCircle(localInput, circle); + break; + case PolygonShape polyShape: + { + output = RayCastPolygon(localInput, (Polygon) polyShape); + } + break; + case Polygon poly: + { + output = RayCastPolygon(localInput, poly); + } + break; + default: + return output; + } + + output.Point = Physics.Transform.Mul(transform, output.Point); + output.Normal = Quaternion2D.RotateVector(transform.Quaternion2D, output.Normal); + return output; + } + + /// + /// This callback is invoked upon every AABB collision. + /// + private static float RayCastCallback(RayCastInput input, FixtureProxy proxy, ref WorldRayCastContext worldContext) + { + if ((proxy.Fixture.CollisionLayer & worldContext.Filter.MaskBits) == 0 && (proxy.Fixture.CollisionMask & worldContext.Filter.LayerBits) == 0) + { + return input.MaxFraction; + } + + if (worldContext.Filter.IsIgnored?.Invoke(proxy.Entity) == true) + { + return input.MaxFraction; + } + + var transform = worldContext.Physics.GetLocalPhysicsTransform(proxy.Entity); + var output = worldContext.System.RayCastShape(input, proxy.Fixture.Shape, transform); + + if (output.Hit) + { + // Fraction returned determines what B2Dynamictree will do, i.e. shrink the AABB or not. + var fraction = worldContext.fcn(proxy, output.Point, output.Normal, output.Fraction, ref worldContext.Result); + return fraction; + } + + return input.MaxFraction; + } + + // Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 + // http://www.codercorner.com/blog/?p=321 + internal CastOutput RayCastCircle(RayCastInput input, PhysShapeCircle shape) + { + DebugTools.Assert(input.IsValidRay()); + + var p = shape.Position; + + var output = new CastOutput(); + + // Shift ray so circle center is the origin + var s = Vector2.Subtract(input.Origin, p); + float length = 0f; + var d = input.Translation.GetLengthAndNormalize(ref length); + if (length == 0.0f) + { + // zero length ray + return output; + } + + // Find closest point on ray to origin + + // solve: dot(s + t * d, d) = 0 + float t = -Vector2.Dot(s, d); + + // c is the closest point on the line to the origin + var c = Vector2.Add(s, t * d); + + float cc = Vector2.Dot(c, c); + float r = shape.Radius; + float rr = r * r; + + if (cc > rr) + { + // closest point is outside the circle + return output; + } + + // Pythagorus + float h = MathF.Sqrt(rr - cc); + + float fraction = t - h; + + if ( fraction < 0.0f || input.MaxFraction * length < fraction ) + { + // outside the range of the ray segment + return output; + } + + var hitPoint = Vector2.Add(s, fraction * d); + + output.Fraction = fraction / length; + output.Normal = hitPoint.Normalized(); + output.Point = Vector2.Add(p, shape.Radius * output.Normal); + output.Hit = true; + + return output; + } + + private CastOutput RayCastPolygon(RayCastInput input, Polygon shape) + { + if (shape.Radius == 0.0f) + { + // Put the ray into the polygon's frame of reference. + var p1 = input.Origin; + var d = input.Translation; + + float lower = 0.0f, upper = input.MaxFraction; + + var index = -1; + + var output = new CastOutput() + { + Fraction = 0f, + }; + + for ( var i = 0; i < shape.VertexCount; ++i ) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = Vector2.Dot(shape.Normals[i], Vector2.Subtract( shape.Vertices[i], p1 ) ); + float denominator = Vector2.Dot(shape.Normals[i], d ); + + if ( denominator == 0.0f ) + { + if ( numerator < 0.0f ) + { + return output; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if ( denominator < 0.0f && numerator < lower * denominator ) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if ( denominator > 0.0f && numerator < upper * denominator ) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the B2_ASSERT on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + // if (upper < lower - b2_epsilon) + if ( upper < lower ) + { + return output; + } + } + + DebugTools.Assert( 0.0f <= lower && lower <= input.MaxFraction ); + + if (index >= 0) + { + output.Fraction = lower; + output.Normal = shape.Normals[index]; + output.Point = Vector2.Add(p1, lower * d); + output.Hit = true; + } + + return output; + } + + // TODO_ERIN this is not working for ray vs box (zero radii) + var castInput = new ShapeCastPairInput + { + ProxyA = DistanceProxy.MakeProxy(shape.Vertices, shape.VertexCount, shape.Radius), + ProxyB = DistanceProxy.MakeProxy([input.Origin], 1, 0.0f), + TransformA = Physics.Transform.Empty, + TransformB = Physics.Transform.Empty, + TranslationB = input.Translation, + MaxFraction = input.MaxFraction + }; + return ShapeCast(castInput); + } + + // Ray vs line segment + private CastOutput RayCastSegment(RayCastInput input, EdgeShape shape, bool oneSided) + { + var output = new CastOutput(); + + if (oneSided) + { + // Skip left-side collision + float offset = Vector2Helpers.Cross(Vector2.Subtract(input.Origin, shape.Vertex0), Vector2.Subtract( shape.Vertex1, shape.Vertex0)); + if ( offset < 0.0f ) + { + return output; + } + } + + // Put the ray into the edge's frame of reference. + var p1 = input.Origin; + var d = input.Translation; + + var v1 = shape.Vertex0; + var v2 = shape.Vertex1; + var e = Vector2.Subtract( v2, v1 ); + + float length = 0f; + var eUnit = e.GetLengthAndNormalize(ref length); + if (length == 0.0f) + { + return output; + } + + // Normal points to the right, looking from v1 towards v2 + var normal = eUnit.RightPerp(); + + // Intersect ray with infinite segment using normal + // Similar to intersecting a ray with an infinite plane + // p = p1 + t * d + // dot(normal, p - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float numerator = Vector2.Dot(normal, Vector2.Subtract(v1, p1)); + float denominator = Vector2.Dot(normal, d); + + if (denominator == 0.0f) + { + // parallel + return output; + } + + float t = numerator / denominator; + if ( t < 0.0f || input.MaxFraction < t ) + { + // out of ray range + return output; + } + + // Intersection point on infinite segment + var p = Vector2.Add(p1, t * d); + + // Compute position of p along segment + // p = v1 + s * e + // s = dot(p - v1, e) / dot(e, e) + + float s = Vector2.Dot(Vector2.Subtract(p, v1), eUnit); + if ( s < 0.0f || length < s ) + { + // out of segment range + return output; + } + + if ( numerator > 0.0f ) + { + normal = -normal; + } + + output.Fraction = t; + output.Point = Vector2.Add(p1, t * d); + output.Normal = normal; + output.Hit = true; + + return output; + } + + #endregion + + #region Shape + + private CastOutput ShapeCastShape(ShapeCastInput input, IPhysShape shape, Transform transform) + { + var localInput = input; + + for ( int i = 0; i < localInput.Count; ++i ) + { + localInput.Points[i] = Physics.Transform.MulT(transform, input.Points[i]); + } + + localInput.Translation = Quaternion2D.InvRotateVector(transform.Quaternion2D, input.Translation); + + CastOutput output; + + switch (shape) + { + case PhysShapeCircle circle: + output = ShapeCastCircle(localInput, circle); + break; + case PolygonShape pShape: + output = ShapeCastPolygon(localInput, (Polygon) pShape); + break; + case Polygon poly: + output = ShapeCastPolygon(localInput, poly); + break; + default: + return new CastOutput(); + } + + output.Point = Physics.Transform.Mul(transform, output.Point); + output.Normal = Quaternion2D.RotateVector(transform.Quaternion2D, output.Normal); + return output; + } + + /// + /// This callback is invoked upon getting the AABB inside of B2DynamicTree. + /// + private float ShapeCastCallback(ShapeCastInput input, FixtureProxy proxy, ref WorldRayCastContext worldContext) + { + var filter = worldContext.Filter; + + if ((proxy.Fixture.CollisionLayer & filter.MaskBits) == 0 && (proxy.Fixture.CollisionMask & filter.LayerBits) == 0) + { + return input.MaxFraction; + } + + if (worldContext.Filter.IsIgnored?.Invoke(proxy.Entity) == true) + { + return input.MaxFraction; + } + + var transform = worldContext.Physics.GetLocalPhysicsTransform(proxy.Entity); + var output = ShapeCastShape(input, proxy.Fixture.Shape, transform); + + if (output.Hit) + { + var fraction = worldContext.fcn(proxy, output.Point, output.Normal, output.Fraction, ref worldContext.Result); + return fraction; + } + + return input.MaxFraction; + } + + // GJK-raycast + // Algorithm by Gino van den Bergen. + // "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 + // todo this is failing when used to raycast a box + // todo this converges slowly with a radius + private CastOutput ShapeCast(ShapeCastPairInput input) + { + var output = new CastOutput() + { + Fraction = input.MaxFraction, + }; + + var proxyA = input.ProxyA; + var count = input.ProxyB.Vertices.Length; + + var xfA = input.TransformA; + var xfB = input.TransformB; + var xf = Physics.Transform.InvMulTransforms(xfA, xfB); + + // Put proxyB in proxyA's frame to reduce round-off error + var proxyBVerts = new Vector2[input.ProxyB.Vertices.Length]; + + for ( int i = 0; i < count; ++i ) + { + proxyBVerts[i] = Physics.Transform.Mul(xf, input.ProxyB.Vertices[i]); + } + + var proxyB = DistanceProxy.MakeProxy(proxyBVerts, count, input.ProxyB.Radius); + + DebugTools.Assert(proxyB.Vertices.Length <= PhysicsConstants.MaxPolygonVertices); + float radius = proxyA.Radius + proxyB.Radius; + + var r = Quaternion2D.RotateVector(xf.Quaternion2D, input.TranslationB); + float lambda = 0.0f; + float maxFraction = input.MaxFraction; + + // Initial simplex + Simplex simplex; + simplex = new() + { + Count = 0, + V = new FixedArray4() + }; + + // Get an initial point in A - B + int indexA = FindSupport(proxyA, -r); + var wA = proxyA.Vertices[indexA]; + int indexB = FindSupport(proxyB, r); + var wB = proxyB.Vertices[indexB]; + var v = Vector2.Subtract(wA, wB); + + // Sigma is the target distance between proxies + const float linearSlop = PhysicsConstants.LinearSlop; + var sigma = MathF.Max(linearSlop, radius - linearSlop); + + // Main iteration loop. + const int k_maxIters = 20; + int iter = 0; + while ( iter < k_maxIters && v.Length() > sigma + 0.5f * linearSlop ) + { + DebugTools.Assert(simplex.Count < 3); + + output.Iterations += 1; + + // Support in direction -v (A - B) + indexA = FindSupport(proxyA, -v); + wA = proxyA.Vertices[indexA]; + indexB = FindSupport(proxyB, v); + wB = proxyB.Vertices[indexB]; + var p = Vector2.Subtract(wA, wB); + + // -v is a normal at p, normalize to work with sigma + v = v.Normalized(); + + // Intersect ray with plane + float vp = Vector2.Dot(v, p); + float vr = Vector2.Dot(v, r); + if ( vp - sigma > lambda * vr ) + { + if ( vr <= 0.0f ) + { + // miss + return output; + } + + lambda = ( vp - sigma ) / vr; + if ( lambda > maxFraction ) + { + // too far + return output; + } + + // reset the simplex + simplex.Count = 0; + } + + // Reverse simplex since it works with B - A. + // Shift by lambda * r because we want the closest point to the current clip point. + // Note that the support point p is not shifted because we want the plane equation + // to be formed in unshifted space. + ref var vertex = ref simplex.V.AsSpan[simplex.Count]; + vertex.IndexA = indexB; + vertex.WA = new Vector2(wB.X + lambda * r.X, wB.Y + lambda * r.Y); + vertex.IndexB = indexA; + vertex.WB = wA; + vertex.W = Vector2.Subtract(vertex.WB, vertex.WA); + vertex.A = 1.0f; + simplex.Count += 1; + + switch (simplex.Count) + { + case 1: + break; + + case 2: + Simplex.SolveSimplex2(ref simplex); + break; + + case 3: + Simplex.SolveSimplex3(ref simplex); + break; + + default: + throw new NotImplementedException(); + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if ( simplex.Count == 3 ) + { + // Overlap + return output; + } + + // Get search direction. + // todo use more accurate segment perpendicular + v = Simplex.ComputeSimplexClosestPoint(simplex); + + // Iteration count is equated to the number of support point calls. + ++iter; + } + + if ( iter == 0 || lambda == 0.0f ) + { + // Initial overlap + return output; + } + + // Prepare output. + Vector2 pointA = Vector2.Zero, pointB = Vector2.Zero; + Simplex.ComputeSimplexWitnessPoints(ref pointB, ref pointA, simplex); + + var n = (-v).Normalized(); + var point = new Vector2(pointA.X + proxyA.Radius * n.X, pointA.Y + proxyA.Radius * n.Y); + + output.Point = Physics.Transform.Mul(xfA, point); + output.Normal = Quaternion2D.RotateVector(xfA.Quaternion2D, n); + output.Fraction = lambda; + output.Iterations = iter; + output.Hit = true; + return output; + } + + private int FindSupport(DistanceProxy proxy, Vector2 direction) + { + int bestIndex = 0; + float bestValue = Vector2.Dot(proxy.Vertices[0], direction); + for ( int i = 1; i < proxy.Vertices.Length; ++i ) + { + float value = Vector2.Dot(proxy.Vertices[i], direction); + if ( value > bestValue ) + { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; + } + + private CastOutput ShapeCastCircle(ShapeCastInput input, PhysShapeCircle shape) + { + var pairInput = new ShapeCastPairInput + { + ProxyA = DistanceProxy.MakeProxy([shape.Position], 1, shape.Radius ), + ProxyB = DistanceProxy.MakeProxy(input.Points, input.Count, input.Radius ), + TransformA = Physics.Transform.Empty, + TransformB = Physics.Transform.Empty, + TranslationB = input.Translation, + MaxFraction = input.MaxFraction + }; + + var output = ShapeCast(pairInput); + return output; + } + + private CastOutput ShapeCastPolygon(ShapeCastInput input, Polygon shape) + { + var pairInput = new ShapeCastPairInput + { + ProxyA = DistanceProxy.MakeProxy(shape.Vertices, shape.VertexCount, shape.Radius), + ProxyB = DistanceProxy.MakeProxy(input.Points, input.Count, input.Radius), + TransformA = Physics.Transform.Empty, + TransformB = Physics.Transform.Empty, + TranslationB = input.Translation, + MaxFraction = input.MaxFraction + }; + + var output = ShapeCast(pairInput); + return output; + } + + private CastOutput ShapeCastSegment(ShapeCastInput input, EdgeShape shape) + { + var pairInput = new ShapeCastPairInput(); + pairInput.ProxyA = DistanceProxy.MakeProxy([shape.Vertex0], 2, 0.0f); + pairInput.ProxyB = DistanceProxy.MakeProxy(input.Points, input.Count, input.Radius); + pairInput.TransformA = Physics.Transform.Empty; + pairInput.TransformB = Physics.Transform.Empty; + pairInput.TranslationB = input.Translation; + pairInput.MaxFraction = input.MaxFraction; + + var output = ShapeCast(pairInput); + return output; + } + + #endregion +} + +internal ref struct WorldRayCastContext +{ + public RayCastSystem System; + public SharedPhysicsSystem Physics; + + public CastResult fcn; + public QueryFilter Filter; + public float Fraction; + + public RayResult Result; +} + +internal ref struct ShapeCastPairInput +{ + public DistanceProxy ProxyA; + public DistanceProxy ProxyB; + public Transform TransformA; + public Transform TransformB; + public Vector2 TranslationB; + + /// + /// The fraction of the translation to consider, typically 1 + /// + public float MaxFraction; +} + +internal record struct ShapeCastInput +{ + public Transform Origin; + + /// A point cloud to cast + public Vector2[] Points; + + /// The number of points + public int Count; + + /// The radius around the point cloud + public float Radius; + + /// The translation of the shape cast + public Vector2 Translation; + + /// The maximum fraction of the translation to consider, typically 1 + public float MaxFraction; +} + +internal record struct RayCastInput +{ + public Vector2 Origin; + + public Vector2 Translation; + + public float MaxFraction; + + public bool IsValidRay() + { + bool isValid = Origin.IsValid() && Translation.IsValid() && MaxFraction.IsValid() && + 0.0f <= MaxFraction && MaxFraction < float.MaxValue; + return isValid; + } +} + +internal ref struct CastOutput +{ + public Vector2 Normal; + + public Vector2 Point; + + public float Fraction; + + public int Iterations; + + public bool Hit; +} diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs new file mode 100644 index 00000000000..37a81a9d6cc --- /dev/null +++ b/Robust.Shared/Physics/Systems/RayCastSystem.cs @@ -0,0 +1,426 @@ +using System; +using System.Collections.Generic; +using System.Numerics; +using JetBrains.Annotations; +using Robust.Shared.Collections; +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.Collision.Shapes; +using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Shapes; +using Robust.Shared.Utility; + +namespace Robust.Shared.Physics.Systems; + +public sealed partial class RayCastSystem : EntitySystem +{ + /* + * A few things to keep in mind with the below: + * - Raycasts are done relative to the corresponding broadphases. + * - The raycast results need to be transformed into Map terms. + * - If you wish to add more helper methods make a new partial and dump them there and have them call the below methods. + */ + + [Dependency] private readonly SharedBroadphaseSystem _broadphase = default!; + [Dependency] private readonly SharedPhysicsSystem _physics = default!; + + private readonly RayComparer _rayComparer = new(); + + #region RayCast + + private sealed class RayComparer : IComparer + { + public int Compare(RayHit x, RayHit y) + { + return x.Fraction.CompareTo(y.Fraction); + } + } + + private void AdjustResults(ref RayResult result, int index, Transform xf) + { + for (var i = index; i < result.Results.Count; i++) + { + result.Results[i].Point = Physics.Transform.Mul(xf, result.Results[i].Point); + } + } + + /* + * Raycasts that return all entities sorted. + */ + + /// + /// Casts a ray against a broadphase. + /// + public void CastRay(Entity entity, ref RayResult result, Vector2 origin, Vector2 translation, QueryFilter filter, bool sorted = true) + { + if (!Resolve(entity.Owner, ref entity.Comp)) + return; + + DebugTools.Assert(origin.IsValid()); + DebugTools.Assert(translation.IsValid()); + + var input = new RayCastInput() + { + Origin = origin, + Translation = translation, + MaxFraction = 1f, + }; + + var worldContext = new WorldRayCastContext() + { + fcn = RayCastAllCallback, + Filter = filter, + Fraction = 1f, + Physics = _physics, + System = this, + Result = result, + }; + + entity.Comp.DynamicTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback); + input.MaxFraction = worldContext.Fraction; + entity.Comp.StaticTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback); + result = worldContext.Result; + + if (sorted) + { + result.Results.Sort(_rayComparer); + } + } + + /// + /// Returns all entities hit in order. + /// + [Pure] + public RayResult CastRay(MapId mapId, Vector2 origin, Vector2 translation, QueryFilter filter) + { + DebugTools.Assert(origin.IsValid()); + DebugTools.Assert(translation.IsValid()); + + var input = new RayCastInput + { + Origin = origin, + Translation = translation, + MaxFraction = 1.0f + }; + + var result = new RayResult(); + + var start = origin; + var end = origin + translation; + + var aabb = new Box2(Vector2.Min(start, end), Vector2.Max(start, end)); + var state = (input, filter, result, this, _physics); + + _broadphase.GetBroadphases(mapId, aabb, ref state, + static (Entity entity, ref (RayCastInput input, QueryFilter filter, RayResult result, RayCastSystem system, SharedPhysicsSystem Physics) tuple) => + { + var transform = tuple.Physics.GetPhysicsTransform(entity.Owner); + var localOrigin = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin); + var localTranslation = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin + tuple.input.Translation) - localOrigin; + var oldIndex = tuple.result.Results.Count; + tuple.system.CastRay((entity.Owner, entity.Comp), ref tuple.result, localOrigin, localTranslation, filter: tuple.filter, sorted: false); + tuple.system.AdjustResults(ref tuple.result, oldIndex, transform); + }); + + result = state.result; + result.Results.Sort(_rayComparer); + return result; + } + + /* + * Raycasts that only return the closest entity. + */ + + /// + /// Casts a ray against a broadphase. + /// + public void CastRayClosest(Entity entity, ref RayResult result, Vector2 origin, Vector2 translation, QueryFilter filter) + { + if (!Resolve(entity.Owner, ref entity.Comp)) + return; + + DebugTools.Assert(origin.IsValid()); + DebugTools.Assert(translation.IsValid()); + + var input = new RayCastInput() + { + Origin = origin, + Translation = translation, + MaxFraction = 1f, + }; + + var worldContext = new WorldRayCastContext() + { + fcn = RayCastClosestCallback, + Filter = filter, + Fraction = 1f, + Physics = _physics, + System = this, + Result = result, + }; + + entity.Comp.DynamicTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback); + input.MaxFraction = worldContext.Fraction; + entity.Comp.StaticTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback); + result = worldContext.Result; + DebugTools.Assert(result.Results.Count <= 1); + } + + /// + /// Returns all entities hit in order. + /// + public RayResult CastRayClosest(MapId mapId, Vector2 origin, Vector2 translation, QueryFilter filter) + { + DebugTools.Assert(origin.IsValid()); + DebugTools.Assert(translation.IsValid()); + + var input = new RayCastInput + { + Origin = origin, + Translation = translation, + MaxFraction = 1.0f + }; + + var result = new RayResult(); + + var end = origin + translation; + + var aabb = new Box2(Vector2.Min(origin, end), Vector2.Max(origin, end)); + var state = (input, filter, result, this, _physics); + + _broadphase.GetBroadphases(mapId, aabb, ref state, + static (Entity entity, ref (RayCastInput input, QueryFilter filter, RayResult result, RayCastSystem system, SharedPhysicsSystem _physics) tuple) => + { + var transform = tuple._physics.GetPhysicsTransform(entity.Owner); + var localOrigin = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin); + var localTranslation = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin + tuple.input.Translation) - localOrigin; + + var oldIndex = tuple.result.Results.Count; + tuple.system.CastRayClosest((entity.Owner, entity.Comp), ref tuple.result, localOrigin, localTranslation, filter: tuple.filter); + tuple.system.AdjustResults(ref tuple.result, oldIndex, transform); + }); + + result = state.result; + DebugTools.Assert(result.Results.Count <= 1); + return result; + } + + #endregion + + #region ShapeCast + + /// + /// Convenience method for shape casts; only supports shapes with area. + /// + public RayResult CastShape( + MapId mapId, + IPhysShape shape, + Transform originTransform, + Vector2 translation, + QueryFilter filter) + { + DebugTools.Assert(originTransform.Position.IsValid()); + DebugTools.Assert(originTransform.Quaternion2D.IsValid()); + DebugTools.Assert(translation.IsValid()); + + // Need to get the entire shape AABB to know what broadphases to even query. + var startAabb = shape.ComputeAABB(originTransform, 0); + var endAabb = shape.ComputeAABB(new Transform(originTransform.Position + translation, originTransform.Quaternion2D.Angle), 0); + var aabb = startAabb.Union(endAabb); + + var result = new RayResult(); + var state = (originTransform, translation, shape: shape, filter, result, this, _physics); + + _broadphase.GetBroadphases(mapId, aabb, ref state, + static ( + Entity entity, + ref (Transform origin, Vector2 translation, IPhysShape shape, QueryFilter filter, RayResult result, RayCastSystem system, SharedPhysicsSystem _physics + ) tuple) => + { + var transform = tuple._physics.GetPhysicsTransform(entity.Owner); + var localOrigin = Physics.Transform.MulT(transform, tuple.origin); + var localTranslation = Physics.Transform.InvTransformPoint(transform, tuple.origin.Position + tuple.translation) - localOrigin.Position; + + var oldIndex = tuple.result.Results.Count; + tuple.system.CastShape((entity.Owner, entity.Comp), ref tuple.result, tuple.shape, localOrigin, localTranslation, filter: tuple.filter); + tuple.system.AdjustResults(ref tuple.result, oldIndex, transform); + }); + + result = state.result; + return result; + } + + public void CastShape( + Entity entity, + ref RayResult result, + IPhysShape shape, + Transform originTransform, + Vector2 translation, + QueryFilter filter) + { + if (!Resolve(entity.Owner, ref entity.Comp)) + return; + + switch (shape) + { + case PhysShapeCircle circle: + CastCircle(entity, ref result, circle, originTransform, translation, filter); + break; + case Polygon poly: + CastPolygon(entity, ref result, new PolygonShape(poly), originTransform, translation, filter); + break; + case PolygonShape polygon: + CastPolygon(entity, ref result, polygon, originTransform, translation, filter); + break; + default: + Log.Error("Tried to shapecast for shape not implemented."); + DebugTools.Assert(false); + return; + } + } + + public void CastCircle( + Entity entity, + ref RayResult result, + PhysShapeCircle circle, + Transform originTransform, + Vector2 translation, + QueryFilter filter) + { + if (!Resolve(entity.Owner, ref entity.Comp)) + return; + + var input = new ShapeCastInput() + { + Points = new Vector2[1], + Count = 1, + Radius = circle.Radius, + Translation = translation, + MaxFraction = 1f, + }; + + input.Points[0] = Physics.Transform.Mul(originTransform, circle.Position); + + var worldContext = new WorldRayCastContext() + { + System = this, + Physics = _physics, + Filter = filter, + Fraction = 1f, + Result = result, + fcn = RayCastAllCallback, + }; + + entity.Comp.StaticTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext); + input.MaxFraction = worldContext.Fraction; + entity.Comp.DynamicTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext); + result = worldContext.Result; + } + + public void CastPolygon( + Entity entity, + ref RayResult result, + PolygonShape polygon, + Transform originTransform, + Vector2 translation, + QueryFilter filter) + { + if (!Resolve(entity.Owner, ref entity.Comp)) + return; + + ShapeCastInput input = new() + { + Points = new Vector2[polygon.VertexCount], + }; + + for ( int i = 0; i < polygon.VertexCount; ++i ) + { + input.Points[i] = Physics.Transform.Mul(originTransform, polygon.Vertices[i]); + } + + input.Count = polygon.VertexCount; + input.Radius = polygon.Radius; + input.Translation = translation; + input.MaxFraction = 1.0f; + + var worldContext = new WorldRayCastContext() + { + System = this, + Physics = _physics, + Filter = filter, + Fraction = 1f, + Result = result, + fcn = RayCastAllCallback, + }; + + entity.Comp.StaticTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext); + input.MaxFraction = worldContext.Fraction; + entity.Comp.DynamicTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext); + result = worldContext.Result; + } + + #endregion +} + +/// Result from b2World_RayCastClosest +/// @ingroup world +public record struct RayResult() +{ + public ValueList Results = new(); + + public bool Hit => Results.Count > 0; +} + +public record struct RayHit(EntityUid Entity, Vector2 LocalNormal, float Fraction) +{ + public readonly EntityUid Entity = Entity; + public readonly Vector2 LocalNormal = LocalNormal; + public readonly float Fraction = Fraction; + + // When this point gets added it's in broadphase terms, then the caller handles whether it gets turned into map-terms. + + public Vector2 Point; +} + +/// The query filter is used to filter collisions between queries and shapes. For example, +/// you may want a ray-cast representing a projectile to hit players and the static environment +/// but not debris. +/// @ingroup shape +public record struct QueryFilter +{ + /// + /// The collision category bits of this query. Normally you would just set one bit. + /// + public long LayerBits; + + /// + /// The collision mask bits. This states the shape categories that this + /// query would accept for collision. + /// + public long MaskBits; + + /// + /// Return whether to ignore an entity. + /// + public Func? IsIgnored; +} + +/// Prototype callback for ray casts. +/// Called for each shape found in the query. You control how the ray cast +/// proceeds by returning a float: +/// return -1: ignore this shape and continue +/// return 0: terminate the ray cast +/// return fraction: clip the ray to this point +/// return 1: don't clip the ray and continue +/// @param shapeId the shape hit by the ray +/// @param point the point of initial intersection +/// @param normal the normal vector at the point of intersection +/// @param fraction the fraction along the ray at the point of intersection +/// @param context the user context +/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue +/// @see b2World_CastRay +/// @ingroup world +public delegate float CastResult(FixtureProxy proxy, Vector2 point, Vector2 normal, float fraction, ref RayResult result); diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs index 18aee0396f3..bec02a11eec 100644 --- a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs +++ b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs @@ -4,6 +4,7 @@ using System.Numerics; using System.Threading.Tasks; using Microsoft.Extensions.ObjectPool; +using Robust.Shared.Collections; using Robust.Shared.Configuration; using Robust.Shared.GameObjects; using Robust.Shared.IoC; @@ -471,38 +472,54 @@ public void Refilter(EntityUid uid, Fixture fixture, TransformComponent? xform = TouchProxies(xform.MapUid.Value, matrix, fixture); } - // TODO: The below is slow and should just query the map's broadphase directly. The problem is that - // there's some ordering stuff going on where the broadphase has queued all of its updates but hasn't applied - // them yet so this query will fail on initialization which chains into a whole lot of issues. - internal IEnumerable<(EntityUid uid, BroadphaseComponent comp)> GetBroadphases(MapId mapId, Box2 aabb) + internal void GetBroadphases(MapId mapId, Box2 aabb,BroadphaseCallback callback) { - // TODO Okay so problem: If we just do Encloses that's a lot faster BUT it also means we don't return the - // map's broadphase which avoids us iterating over it for 99% of bodies. - - if (mapId == MapId.Nullspace) yield break; - - var enumerator = AllEntityQuery(); + var internalState = (callback, _broadphaseQuery); + + _mapManager.FindGridsIntersecting(mapId, + aabb, + ref internalState, + static ( + EntityUid uid, + MapGridComponent grid, + ref (BroadphaseCallback callback, EntityQuery _broadphaseQuery) tuple) => + { + if (!tuple._broadphaseQuery.TryComp(uid, out var broadphase)) + return true; - while (enumerator.MoveNext(out var bUid, out var broadphase, out var xform)) - { - if (xform.MapID != mapId) continue; + 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); + } - if (!EntityManager.TryGetComponent(bUid, out MapGridComponent? mapGrid)) + internal void GetBroadphases(MapId mapId, Box2 aabb, ref TState state, BroadphaseCallback callback) + { + var internalState = (state, callback, _broadphaseQuery); + + _mapManager.FindGridsIntersecting(mapId, + aabb, + ref internalState, + static ( + EntityUid uid, + MapGridComponent grid, + ref (TState state, BroadphaseCallback callback, EntityQuery _broadphaseQuery) tuple) => { - yield return (bUid, broadphase); - continue; - } + if (!tuple._broadphaseQuery.TryComp(uid, out var broadphase)) + return true; - // Won't worry about accurate bounds checks as it's probably slower in most use cases. - var chunkEnumerator = _map.GetMapChunks(bUid, mapGrid, aabb); + tuple.callback((uid, broadphase), ref tuple.state); + return true; + // Approx because we don't really need accurate checks for these most of the time. + }, approx: true, includeMap: true); - if (chunkEnumerator.MoveNext(out _)) - { - yield return (bUid, broadphase); - } - } + state = internalState.state; } + internal delegate void BroadphaseCallback(Entity entity); + + internal delegate void BroadphaseCallback(Entity entity, ref TState state); + private record struct BroadphaseContactJob() : IParallelRobustJob { public SharedBroadphaseSystem System = default!; diff --git a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs index 69893eaf9d4..7beb786c484 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Numerics; +using Robust.Shared.Collections; using Robust.Shared.Debugging; using Robust.Shared.GameObjects; using Robust.Shared.IoC; @@ -34,37 +35,48 @@ public partial class SharedPhysicsSystem public bool TryCollideRect(Box2 collider, MapId mapId, bool approximate = true) { var state = (collider, mapId, found: false); + var broadphases = new ValueList>(); - foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, collider)) - { - var gridCollider = _transform.GetInvWorldMatrix(uid).TransformBox(collider); - - broadphase.StaticTree.QueryAabb(ref state, (ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) => + _broadphase.GetBroadphases(mapId, + collider, + broadphase => { - if (proxy.Fixture.CollisionLayer == 0x0) - return true; + var gridCollider = _transform.GetInvWorldMatrix(broadphase).TransformBox(collider); - if (proxy.AABB.Intersects(gridCollider)) - { - state.found = true; - return false; - } - return true; - }, gridCollider, approximate); + broadphase.Comp.StaticTree.QueryAabb(ref state, + (ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) => + { + if (proxy.Fixture.CollisionLayer == 0x0) + return true; - broadphase.DynamicTree.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(gridCollider)) + { + state.found = true; + return false; + } - if (proxy.AABB.Intersects(gridCollider)) - { - state.found = true; - return false; - } - return true; - }, gridCollider, approximate); - } + return true; + }, + gridCollider, + approximate); + + broadphase.Comp.DynamicTree.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(gridCollider)) + { + state.found = true; + return false; + } + + return true; + }, + gridCollider, + approximate); + }); return state.found; } @@ -130,22 +142,27 @@ public IEnumerable GetCollidingEntities(MapId mapId, in Box2 w { if (mapId == MapId.Nullspace) return Array.Empty(); + var aabb = worldAABB; var bodies = new HashSet(); + var state = (_transform, bodies, aabb); - foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, worldAABB)) - { - var gridAABB = _transform.GetInvWorldMatrix(uid).TransformBox(worldAABB); - - foreach (var proxy in broadphase.StaticTree.QueryAabb(gridAABB, false)) + _broadphase.GetBroadphases(mapId, worldAABB, ref state, static + ( + Entity entity, + ref (SharedTransformSystem _transform, HashSet bodies, Box2 aabb) tuple) => { - bodies.Add(proxy.Body); - } + var gridAABB = tuple._transform.GetInvWorldMatrix(entity.Owner).TransformBox(tuple.aabb); - foreach (var proxy in broadphase.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; } @@ -160,20 +177,27 @@ public IEnumerable> GetCollidingEntities(MapId mapId, i var bodies = new HashSet>(); - foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, worldBounds.CalcBoundingBox())) - { - var gridAABB = _transform.GetInvWorldMatrix(uid).TransformBox(worldBounds); + var state = (_transform, bodies, worldBounds); - foreach (var proxy in broadphase.StaticTree.QueryAabb(gridAABB, false)) + _broadphase.GetBroadphases(mapId, worldBounds.CalcBoundingBox(), ref state, + static ( + Entity entity, + ref (SharedTransformSystem _transform, HashSet> bodies, Box2Rotated + worldBounds + ) tuple) => { - bodies.Add(new Entity(proxy.Entity, proxy.Body)); - } + var gridAABB = tuple._transform.GetInvWorldMatrix(entity.Owner).TransformBox(tuple.worldBounds); - foreach (var proxy in broadphase.DynamicTree.QueryAabb(gridAABB, false)) - { - bodies.Add(new Entity(proxy.Entity, proxy.Body)); - } - } + foreach (var proxy in entity.Comp.StaticTree.QueryAabb(gridAABB, false)) + { + tuple.bodies.Add((proxy.Entity, proxy.Body)); + } + + foreach (var proxy in entity.Comp.DynamicTree.QueryAabb(gridAABB, false)) + { + tuple.bodies.Add((proxy.Entity, proxy.Body)); + } + }); return bodies; } @@ -263,72 +287,91 @@ public IEnumerable IntersectRayWithPredicate(MapId mapId var rayBox = new Box2(Vector2.Min(ray.Position, endPoint), Vector2.Max(ray.Position, endPoint)); - foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, rayBox)) - { - var (_, rot, matrix, invMatrix) = _transform.GetWorldPositionRotationMatrixWithInv(uid); + _broadphase.GetBroadphases(mapId, + rayBox, + broadphase => + { + var (_, rot, matrix, invMatrix) = + _transform.GetWorldPositionRotationMatrixWithInv(broadphase.Owner); - var position = Vector2.Transform(ray.Position, invMatrix); - var gridRot = new Angle(-rot.Theta); - var direction = gridRot.RotateVec(ray.Direction); + var position = Vector2.Transform(ray.Position, invMatrix); + var gridRot = new Angle(-rot.Theta); + var direction = gridRot.RotateVec(ray.Direction); - var gridRay = new CollisionRay(position, direction, ray.CollisionMask); + var gridRay = new CollisionRay(position, direction, ray.CollisionMask); - broadphase.StaticTree.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => - { - if (returnOnFirstHit && results.Count > 0) - return true; + broadphase.Comp.StaticTree.QueryRay( + (in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => + { + if (returnOnFirstHit && results.Count > 0) + return true; - if (distFromOrigin > maxLength) - return true; + if (distFromOrigin > maxLength) + return true; - if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) - return true; + if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) + return true; - if (!proxy.Fixture.Hard) - return true; + if (!proxy.Fixture.Hard) + return true; - if (predicate.Invoke(proxy.Entity, state) == true) - return true; + if (predicate.Invoke(proxy.Entity, state) == true) + return true; - // TODO: Shape raycast here + // TODO: Shape raycast here - // Need to convert it back to world-space. - var result = new RayCastResults(distFromOrigin, Vector2.Transform(point, matrix), proxy.Entity); - results.Add(result); + // Need to convert it back to world-space. + var result = new RayCastResults(distFromOrigin, + Vector2.Transform(point, matrix), + proxy.Entity); + results.Add(result); #if DEBUG - _sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray, maxLength, result, _netMan.IsServer, mapId)); + _sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray, + maxLength, + result, + _netMan.IsServer, + mapId)); #endif - return true; - }, gridRay); + return true; + }, + gridRay); - broadphase.DynamicTree.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => - { - if (returnOnFirstHit && results.Count > 0) - return true; + broadphase.Comp.DynamicTree.QueryRay( + (in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => + { + if (returnOnFirstHit && results.Count > 0) + return true; - if (distFromOrigin > maxLength) - return true; + if (distFromOrigin > maxLength) + return true; - if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) - return true; + if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) + return true; - if (!proxy.Fixture.Hard) - return true; + if (!proxy.Fixture.Hard) + return true; - if (predicate.Invoke(proxy.Entity, state) == true) - return true; + if (predicate.Invoke(proxy.Entity, state) == true) + return true; - // TODO: Shape raycast here + // TODO: Shape raycast here - // Need to convert it back to world-space. - var result = new RayCastResults(distFromOrigin, Vector2.Transform(point, matrix), proxy.Entity); - results.Add(result); + // Need to convert it back to world-space. + var result = new RayCastResults(distFromOrigin, + Vector2.Transform(point, matrix), + proxy.Entity); + results.Add(result); #if DEBUG - _sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray, maxLength, result, _netMan.IsServer, mapId)); + _sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray, + maxLength, + result, + _netMan.IsServer, + mapId)); #endif - return true; - }, gridRay); - } + return true; + }, + gridRay); + }); #if DEBUG if (results.Count == 0) @@ -374,54 +417,68 @@ public float IntersectRayPenetration(MapId mapId, CollisionRay ray, float maxLen var rayBox = new Box2(Vector2.Min(ray.Position, endPoint), Vector2.Max(ray.Position, endPoint)); - foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, rayBox)) - { - var (_, rot, invMatrix) = _transform.GetWorldPositionRotationInvMatrix(uid); + _broadphase.GetBroadphases(mapId, + rayBox, + broadphase => + { + var (_, rot, invMatrix) = _transform.GetWorldPositionRotationInvMatrix(broadphase); - var position = Vector2.Transform(ray.Position, invMatrix); - var gridRot = new Angle(-rot.Theta); - var direction = gridRot.RotateVec(ray.Direction); + var position = Vector2.Transform(ray.Position, invMatrix); + var gridRot = new Angle(-rot.Theta); + var direction = gridRot.RotateVec(ray.Direction); - var gridRay = new CollisionRay(position, direction, ray.CollisionMask); + var gridRay = new CollisionRay(position, direction, ray.CollisionMask); - broadphase.StaticTree.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => - { - if (distFromOrigin > maxLength || proxy.Entity == ignoredEnt) - return true; + broadphase.Comp.StaticTree.QueryRay( + (in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => + { + if (distFromOrigin > maxLength || proxy.Entity == ignoredEnt) + return true; - if (!proxy.Fixture.Hard) - return true; + if (!proxy.Fixture.Hard) + return true; - if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) - return true; + if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) + return true; - if (new Ray(point + gridRay.Direction * proxy.AABB.Size.Length() * 2, -gridRay.Direction).Intersects( - proxy.AABB, out _, out var exitPoint)) - { - penetration += (point - exitPoint).Length(); - } - return true; - }, gridRay); + if (new Ray(point + gridRay.Direction * proxy.AABB.Size.Length() * 2, -gridRay.Direction) + .Intersects( + proxy.AABB, + out _, + out var exitPoint)) + { + penetration += (point - exitPoint).Length(); + } - broadphase.DynamicTree.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => - { - if (distFromOrigin > maxLength || proxy.Entity == ignoredEnt) - return true; + return true; + }, + gridRay); - if (!proxy.Fixture.Hard) - return true; + broadphase.Comp.DynamicTree.QueryRay( + (in FixtureProxy proxy, in Vector2 point, float distFromOrigin) => + { + if (distFromOrigin > maxLength || proxy.Entity == ignoredEnt) + return true; - if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) - return true; + if (!proxy.Fixture.Hard) + return true; - if (new Ray(point + gridRay.Direction * proxy.AABB.Size.Length() * 2, -gridRay.Direction).Intersects( - proxy.AABB, out _, out var exitPoint)) - { - penetration += (point - exitPoint).Length(); - } - return true; - }, gridRay); - } + if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0) + return true; + + if (new Ray(point + gridRay.Direction * proxy.AABB.Size.Length() * 2, -gridRay.Direction) + .Intersects( + proxy.AABB, + out _, + out var exitPoint)) + { + penetration += (point - exitPoint).Length(); + } + + return true; + }, + gridRay); + }); // This hid rays that didn't penetrate something. Don't hide those because that causes rays to disappear that shouldn't. #if DEBUG diff --git a/Robust.Shared/Physics/Transform.cs b/Robust.Shared/Physics/Transform.cs index 149c11ce022..0efa327d5aa 100644 --- a/Robust.Shared/Physics/Transform.cs +++ b/Robust.Shared/Physics/Transform.cs @@ -38,6 +38,12 @@ public struct Transform public Vector2 Position; public Quaternion2D Quaternion2D; + public Transform(Vector2 position, Quaternion2D quat) + { + Position = position; + Quaternion2D = quat; + } + public Transform(Vector2 position, float angle) { Position = position; @@ -56,6 +62,16 @@ public Transform(Vector2 position, Angle angle) Quaternion2D = new Quaternion2D(angle); } + /// Inverse transform a point (e.g. world space to local space) + [Pure] + public static Vector2 InvTransformPoint(Transform t, Vector2 p) + { + float vx = p.X - t.Position.X; + float vy = p.Y - t.Position.Y; + return new Vector2(t.Quaternion2D.C * vx + t.Quaternion2D.S * vy, -t.Quaternion2D.S * vx + t.Quaternion2D.C * vy); + } + + [Pure] 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; @@ -64,12 +80,14 @@ public static Vector2 Mul(in Transform transform, in Vector2 vector) return new Vector2(x, y); } + [Pure] 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); } + [Pure] public static Vector2 MulT(in Transform T, in Vector2 v) { float px = v.X - T.Position.X; @@ -81,6 +99,7 @@ public static Vector2 MulT(in Transform T, in Vector2 v) } /// Transpose multiply two rotations: qT * r + [Pure] public static Quaternion2D MulT(in Quaternion2D q, in Quaternion2D r) { // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] @@ -93,8 +112,15 @@ public static Quaternion2D MulT(in Quaternion2D q, in Quaternion2D r) return qr; } + [Pure] + public static Transform InvMulTransforms(in Transform A, in Transform B) + { + return new Transform(Quaternion2D.InvRotateVector(A.Quaternion2D, Vector2.Subtract(B.Position, A.Position)), Quaternion2D.InvMulRot(A.Quaternion2D, B.Quaternion2D)); + } + // v2 = A.q' * (B.q * v1 + B.p - A.p) // = A.q' * B.q * v1 + A.q' * (B.p - A.p) + [Pure] public static Transform MulT(in Transform A, in Transform B) { Transform C = new Transform @@ -184,5 +210,51 @@ public Quaternion2D Set(float angle) // TODO_ERIN optimize return new Quaternion2D(MathF.Cos(angle), MathF.Sin(angle)); } + + /// Rotate a vector + [Pure] + public static Vector2 RotateVector(Quaternion2D q, Vector2 v ) + { + return new Vector2(q.C * v.X - q.S * v.Y, q.S * v.X + q.C * v.Y); + } + + /// Inverse rotate a vector + [Pure] + public static Vector2 InvRotateVector(Quaternion2D q, Vector2 v) + { + return new Vector2(q.C * v.X + q.S * v.Y, -q.S * v.X + q.C * v.Y); + } + + public bool IsValid() + { + if (float.IsNaN(S ) || float.IsNaN(C)) + { + return false; + } + + if (float.IsInfinity(S) || float.IsInfinity(C)) + { + return false; + } + + return IsNormalized(); + } + + public bool IsNormalized() + { + // larger tolerance due to failure on mingw 32-bit + float qq = S * S + C * C; + return 1.0f - 0.0006f < qq && qq < 1.0f + 0.0006f; + } + + [Pure] + public static Quaternion2D InvMulRot(Quaternion2D q, 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(q - r) = qc * rs - qs * rc + // c(q - r) = qc * rc + qs * rs + return new Quaternion2D(q.C * r.C + q.S * r.S, q.C * r.S - q.S * r.C); + } } } diff --git a/Robust.UnitTesting/Shared/Maths/Angle_Test.cs b/Robust.UnitTesting/Shared/Maths/Angle_Test.cs index 089426093a1..9e7494f7f38 100644 --- a/Robust.UnitTesting/Shared/Maths/Angle_Test.cs +++ b/Robust.UnitTesting/Shared/Maths/Angle_Test.cs @@ -27,6 +27,14 @@ public sealed class Angle_Test (0.92387953251128674f, -0.38268343236508978f, Direction.East, -System.Math.PI / 8.0) }; + [Test] + public void TestAngleDegrees() + { + const double degrees = 75d; + var angle = Angle.FromDegrees(degrees); + Assert.That(angle.Degrees, Is.EqualTo(degrees)); + } + [Test] public void TestAngleZero() { diff --git a/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs b/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs index bb67e63fec5..ff668753b72 100644 --- a/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs +++ b/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs @@ -12,18 +12,17 @@ namespace Robust.UnitTesting.Shared.Maths [TestOf(typeof(Matrix3x2))] public sealed class Matrix3_Test { - [Test] - public void GetRotationTest() + private static readonly TestCaseData[] Rotations = new TestCaseData[] { - Assert.That(Matrix3x2.Identity.Rotation(), Is.EqualTo(Angle.Zero)); - - var piOver2 = new Angle(Math.PI / 2); - var piOver2Mat = Matrix3Helpers.CreateRotation(piOver2.Theta); - Assert.That(piOver2Mat.Rotation(), Is.EqualTo(piOver2)); + new(Matrix3x2.Identity, Angle.Zero), + new(Matrix3x2.CreateRotation(MathF.PI / 2f), new Angle(Math.PI / 2)), + new(Matrix3x2.CreateRotation(MathF.PI), new Angle(Math.PI)), + }; - var pi = new Angle(Math.PI); - var piMat = Matrix3Helpers.CreateRotation(pi.Theta); - Assert.That(piMat.Rotation(), Is.EqualTo(pi)); + [Test, TestCaseSource(nameof(Rotations))] + public void GetRotationTest(Matrix3x2 matrix, Angle angle) + { + Assert.That(angle, Is.EqualTo(matrix.Rotation())); } [Test] @@ -35,7 +34,7 @@ public void TranslationTest() var origin = new Vector2(0, 0); var result = Vector2.Transform(origin, matrix); - Assert.That(control == result, Is.True, result.ToString); + Assert.That(control, Is.EqualTo(result), result.ToString); } private static readonly IEnumerable<(Vector2, double)> _rotationTests = new[] diff --git a/Robust.UnitTesting/Shared/Physics/RayCast_Test.cs b/Robust.UnitTesting/Shared/Physics/RayCast_Test.cs new file mode 100644 index 00000000000..a10cbc1631b --- /dev/null +++ b/Robust.UnitTesting/Shared/Physics/RayCast_Test.cs @@ -0,0 +1,144 @@ +using System.Linq; +using System.Numerics; +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.Shapes; +using Robust.Shared.Physics.Systems; +using Robust.UnitTesting.Server; + +namespace Robust.UnitTesting.Shared.Physics; + +[TestFixture] +public sealed class RayCast_Test +{ + private static TestCaseData[] _rayCases = + { + // Ray goes through + new(new Vector2(0f, 0.5f), Vector2.UnitY * 2f, new Vector2(0f, 1f - PhysicsConstants.PolygonRadius)), + + // Ray stops inside + new(new Vector2(0f, 0.5f), Vector2.UnitY, new Vector2(0f, 1f - PhysicsConstants.PolygonRadius)), + + // Ray starts inside + new(new Vector2(0f, 1.5f), Vector2.UnitY, null), + + // No hit + new(new Vector2(0f, 0.5f), -Vector2.UnitY, null), + }; + + private static TestCaseData[] _shapeCases = + { + // Circle + // - Initial overlap, no shapecast + new(new PhysShapeCircle(0.5f, Vector2.Zero), new Transform(Vector2.UnitY / 2f, Angle.Zero), Vector2.UnitY, null), + + // - Cast + new(new PhysShapeCircle(0.5f, Vector2.Zero), new Transform(Vector2.Zero, Angle.Zero), Vector2.UnitY, new Vector2(0f, 1f - PhysicsConstants.PolygonRadius)), + + // - Miss + new(new PhysShapeCircle(0.5f, Vector2.Zero), new Transform(Vector2.Zero, Angle.Zero), -Vector2.UnitY, null), + + // Polygon + // - Initial overlap, no shapecast + new(new Polygon(Box2.UnitCentered), new Transform(Vector2.UnitY / 2f, Angle.Zero), Vector2.UnitY, null), + + // - Cast + new(new Polygon(Box2.UnitCentered), new Transform(Vector2.Zero, Angle.Zero), Vector2.UnitY, new Vector2(0.5f, 1f - PhysicsConstants.PolygonRadius)), + + // - Miss + new(new Polygon(Box2.UnitCentered), new Transform(Vector2.Zero, Angle.Zero), -Vector2.UnitY, null), + }; + + [Test, TestCaseSource(nameof(_rayCases))] + public void RayCast(Vector2 origin, Vector2 direction, Vector2? point) + { + var sim = RobustServerSimulation.NewSimulation().RegisterEntitySystems(f => + { + f.LoadExtraSystemType(); + }).InitializeInstance(); + Setup(sim, out var mapId); + var raycast = sim.System(); + + var hits = raycast.CastRayClosest(mapId, + origin, + direction, + new QueryFilter() + { + LayerBits = 1, + }); + + if (point == null) + { + Assert.That(!hits.Hit); + } + else + { + Assert.That(hits.Results.First().Point, Is.EqualTo(point.Value)); + } + } + + [Test, TestCaseSource(nameof(_shapeCases))] + public void ShapeCast(IPhysShape shape, Transform origin, Vector2 direction, Vector2? point) + { + var sim = RobustServerSimulation.NewSimulation().RegisterEntitySystems(f => + { + f.LoadExtraSystemType(); + }).InitializeInstance(); + Setup(sim, out var mapId); + var raycast = sim.System(); + + var hits = raycast.CastShape(mapId, + shape, + origin, + direction, + new QueryFilter() + { + LayerBits = 1, + }); + + if (point == null) + { + Assert.That(!hits.Hit); + } + else + { + Assert.That(hits.Results.First().Point, Is.EqualTo(point.Value)); + } + } + + private void Setup(ISimulation sim, out MapId mapId) + { + var entManager = sim.Resolve(); + var mapSystem = entManager.System(); + + sim.System().CreateMap(out mapId); + + var grid = sim.Resolve().CreateGridEntity(mapId); + + for (var i = 0; i < 3; i++) + { + mapSystem.SetTile(grid, new Vector2i(i, 0), new Tile(1)); + } + + // Spawn a wall in the middle tile. + var wall = entManager.SpawnEntity(null, new EntityCoordinates(grid.Owner, new Vector2(1.5f, 0.5f))); + + var physics = entManager.AddComponent(wall); + var poly = new PolygonShape(); + poly.SetAsBox(Box2.UnitCentered); + entManager.System().CreateFixture(wall, "fix1", new Fixture(poly, 1, 1, true)); + + entManager.System().SetCanCollide(wall, true, body: physics); + Assert.That(physics.CanCollide); + + // Rotate it to be vertical + entManager.System().SetLocalRotation(grid.Owner, Angle.FromDegrees(90)); + entManager.System().SetLocalPosition(grid.Owner, Vector2.UnitX / 2f); + } +} diff --git a/Robust.UnitTesting/Shared/Serialization/TypeSerializers/AngleSerializerTest.cs b/Robust.UnitTesting/Shared/Serialization/TypeSerializers/AngleSerializerTest.cs index ae1c6b61c5c..8d91188f008 100644 --- a/Robust.UnitTesting/Shared/Serialization/TypeSerializers/AngleSerializerTest.cs +++ b/Robust.UnitTesting/Shared/Serialization/TypeSerializers/AngleSerializerTest.cs @@ -1,4 +1,5 @@ -using System.Globalization; +using System; +using System.Globalization; using NUnit.Framework; using Robust.Shared.Maths; using Robust.Shared.Serialization.Manager; @@ -11,24 +12,46 @@ namespace Robust.UnitTesting.Shared.Serialization.TypeSerializers [TestOf(typeof(AngleSerializer))] public sealed class AngleSerializerTest : SerializationTest { - [Test] - public void SerializationTest() + private static readonly TestCaseData[] _source = new[] { - var degrees = 75d; - var angle = Angle.FromDegrees(degrees); + new TestCaseData(Math.PI), + new TestCaseData(Math.PI / 2), + new TestCaseData(Math.PI / 4), + new TestCaseData(0.515), + new TestCaseData(75), + }; + + [Test, TestCaseSource(nameof(_source))] + public void SerializationRadsTest(double radians) + { + var angle = new Angle(radians); var node = Serialization.WriteValueAs(angle); - var serializedValue = $"{MathHelper.DegreesToRadians(degrees).ToString(CultureInfo.InvariantCulture)} rad"; + var serializedValue = $"{radians.ToString(CultureInfo.InvariantCulture)} rad"; Assert.That(node.Value, Is.EqualTo(serializedValue)); } - [Test] - public void DeserializationTest() + [Test, TestCaseSource(nameof(_source))] + public void DeserializationRadsTest(double radians) { - var degrees = 75; - var node = new ValueDataNode(degrees.ToString()); + var angle = new Angle(radians); + var node = new ValueDataNode($"{radians.ToString(CultureInfo.InvariantCulture)} rad"); var deserializedAngle = Serialization.Read(node); + + Assert.That(deserializedAngle, Is.EqualTo(angle)); + } + + /* + * Serialization of degrees test won't work because it's comparing degrees to radians. + */ + + [Test, TestCaseSource(nameof(_source))] + public void DeserializationDegreesTest(double radians) + { + var degrees = MathHelper.RadiansToDegrees(radians); var angle = Angle.FromDegrees(degrees); + var node = new ValueDataNode($"{degrees.ToString(CultureInfo.InvariantCulture)}"); + var deserializedAngle = Serialization.Read(node); Assert.That(deserializedAngle, Is.EqualTo(angle)); }