From caf6e907d0ef18e1d15ecf2586c1f907311ac4c7 Mon Sep 17 00:00:00 2001 From: metalgearsloth Date: Sun, 15 Sep 2024 20:09:30 +1000 Subject: [PATCH 1/7] Add shapecasts + raycasts Actual raycasts. Need this for AI LIDAR experiment. --- Robust.Shared.Maths/Matrix3Helpers.cs | 9 + Robust.Shared/Physics/B2DynamicTree.cs | 95 ++++++++++ .../Physics/Systems/RayCastSystem.cs | 170 ++++++++++++++++++ Robust.Shared/Physics/Transform.cs | 1 + 4 files changed, 275 insertions(+) create mode 100644 Robust.Shared/Physics/Systems/RayCastSystem.cs diff --git a/Robust.Shared.Maths/Matrix3Helpers.cs b/Robust.Shared.Maths/Matrix3Helpers.cs index 85a5a853a10..4bf02dc265a 100644 --- a/Robust.Shared.Maths/Matrix3Helpers.cs +++ b/Robust.Shared.Maths/Matrix3Helpers.cs @@ -67,6 +67,15 @@ public static Box2 TransformBox(this Matrix3x2 refFromBox, in Box2 box) return Unsafe.As, Box2>(ref lbrt); } + /// + /// Gets the position of the Matrix. Will have some precision loss. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Vector2 Position(this Matrix3x2 t) + { + return new Vector2(t.M31, t.M32); + } + /// /// Gets the rotation of the Matrix. Will have some precision loss. /// diff --git a/Robust.Shared/Physics/B2DynamicTree.cs b/Robust.Shared/Physics/B2DynamicTree.cs index a0d86d5b7d4..fcb2d64a3c6 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,100 @@ 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); + public delegate float NewRayCallback(RayCastInput input, Proxy proxy, T context); + + public void RayCastNew(RayCastInput input, uint mask, NewRayCallback 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]; + 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); + + 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); + } + } + } + } + public void RayCast(RayQueryCallback callback, in Ray input) { RayCast(ref callback, EasyRayQueryCallback, input); diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs new file mode 100644 index 00000000000..7ea1a1daa79 --- /dev/null +++ b/Robust.Shared/Physics/Systems/RayCastSystem.cs @@ -0,0 +1,170 @@ +using System; +using System.Numerics; +using Robust.Shared.GameObjects; +using Robust.Shared.IoC; +using Robust.Shared.Map; +using Robust.Shared.Physics.Collision.Shapes; +using Robust.Shared.Physics.Dynamics; +using Robust.Shared.Physics.Shapes; + +namespace Robust.Shared.Physics.Systems; + +public sealed class RayCastSystem : EntitySystem +{ + [Dependency] private readonly SharedPhysicsSystem _physics = default!; + + internal CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform transform) + { + var localInput = input; + localInput.Origin = b2InvTransformPoint( transform, input.Origin ); + localInput.Translation = b2InvRotateVector( transform.Quaternion2D, input.Translation ); + + CastOutput output = new(); + + switch (shape) + { + /* + case b2_capsuleShape: + output = b2RayCastCapsule( &localInput, &shape->capsule ); + break; + */ + case PhysShapeCircle: + output = b2RayCastCircle( &localInput, &shape->circle ); + break; + case PolygonShape polyShape: + break; + case Polygon: + output = b2RayCastPolygon( &localInput, &shape->polygon ); + break; + case b2_segmentShape: + output = b2RayCastSegment( &localInput, &shape->segment, false ); + break; + case b2_smoothSegmentShape: + output = b2RayCastSegment( &localInput, &shape->smoothSegment.segment, true ); + break; + default: + return output; + } + + output.Point = b2TransformPoint( transform, output.Point ); + output.Normal = b2RotateVector( transform.Quaternion2D, output.Normal ); + return output; + } + + internal CastOutput RayCast(IPhysShape shape, Vector2 origin, Vector2 translation ) + { + var transform = b2GetOwnerTransform( world, shape ); + + // input in local coordinates + var input = new RayCastInput(); + input.MaxFraction = 1.0f; + input.Origin = b2InvTransformPoint( transform, origin ); + input.Translation = b2InvRotateVector( transform.q, translation ); + + var output = new CastOutput(); + switch (shape) + { + /* + case b2_capsuleShape: + output = b2RayCastCapsule( &input, &shape->capsule ); + break; + */ + + case PhysShapeCircle circle: + output = b2RayCastCircle(input, circle); + break; + + case b2_segmentShape: + output = b2RayCastSegment( &input, &shape->segment, false ); + break; + + case Polygon poly: + output = b2RayCastPolygon( &input, &shape->polygon ); + break; + + case b2_smoothSegmentShape: + output = b2RayCastSegment( &input, &shape->smoothSegment.segment, true ); + break; + + default: + throw new NotImplementedException(); + } + + if ( output.hit ) + { + // convert to world coordinates + output.normal = b2RotateVector( transform.q, output.normal ); + output.point = b2TransformPoint( transform, output.point ); + } + return output; + } + + public void RayCast(MapCoordinates coordinates, Vector2 translation) + { + // TODO: Get trees in range. + } + + public void RayCast(Entity grid, Vector2 origin, Vector2 translation, uint collisionMask = uint.MaxValue - 1) + { + if (!Resolve(grid.Owner, ref grid.Comp)) + return; + + var input = new RayCastInput() + { + Origin = origin, + Translation = translation, + MaxFraction = 1f, + }; + + var broadphaseTransform = _physics.GetPhysicsTransform(grid); + + ((B2DynamicTree) grid.Comp.DynamicTree).RayCastNew(input, collisionMask, + (castInput, proxy, context) => + { + + // TODO: Collision check. + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return input->maxFraction; + } + + var body = context.Body; + var transform = _physics.GetPhysicsTransform(context.Entity); + var relative = Physics.Transform.MulT(transform, broadphaseTransform); + + var output = RayCastShape(input, context.Fixture.Shape, transform); + + if (output.Hit) + { + b2ShapeId id = { shapeId + 1, world->worldId, shape->revision }; + float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, worldContext->userContext ); + worldContext->fraction = fraction; + return fraction; + } + + return input.MaxFraction; + }); + } +} + +internal ref struct RayCastInput +{ + public Vector2 Origin; + + public Vector2 Translation; + + public float MaxFraction; +} + +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/Transform.cs b/Robust.Shared/Physics/Transform.cs index 8297079753b..307463c4866 100644 --- a/Robust.Shared/Physics/Transform.cs +++ b/Robust.Shared/Physics/Transform.cs @@ -94,6 +94,7 @@ public static Quaternion2D MulT(in Quaternion2D q, in Quaternion2D r) // 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 From 412940b0c14bf8beff709af818002b47db2a1117 Mon Sep 17 00:00:00 2001 From: metalgearsloth Date: Mon, 16 Sep 2024 00:54:26 +1000 Subject: [PATCH 2/7] cassette --- Robust.Shared.Maths/Vector2Helpers.cs | 20 + .../Physics/Collision/DistanceProxy.cs | 243 +++++++++ Robust.Shared/Physics/Shapes/Polygon.cs | 4 +- .../Physics/Systems/RayCastSystem.Geometry.cs | 463 ++++++++++++++++++ .../Physics/Systems/RayCastSystem.cs | 179 ++++--- Robust.Shared/Physics/Transform.cs | 17 + 6 files changed, 846 insertions(+), 80 deletions(-) create mode 100644 Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs diff --git a/Robust.Shared.Maths/Vector2Helpers.cs b/Robust.Shared.Maths/Vector2Helpers.cs index 3744c178b1d..6819cc0c8cd 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,19 @@ public static class Vector2Helpers /// public static readonly Vector2 Half = new(0.5f, 0.5f); + 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 +269,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/Collision/DistanceProxy.cs b/Robust.Shared/Physics/Collision/DistanceProxy.cs index 7312d732a66..e50d6b6b5fb 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(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(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/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..c217c8a23f9 --- /dev/null +++ b/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs @@ -0,0 +1,463 @@ +using System; +using System.Numerics; +using Robust.Shared.Maths; +using Robust.Shared.Physics.Collision; +using Robust.Shared.Physics.Collision.Shapes; +using Robust.Shared.Physics.Shapes; +using Robust.Shared.Utility; + +namespace Robust.Shared.Physics.Systems; + +public sealed partial class RayCastSystem +{ + #region Raycast + + // 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; + } + + // 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; + } + + 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); + } + + #endregion + + #region Shape + + // 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 = 0f, + }; + output.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.MulT(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.TransformPoint(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 simplex vertices as an array. + var vertices = new SimplexVertex[] { simplex.V._00, simplex.V._01, simplex.V._02 }; + + // 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. + var vertex = vertices[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(simplex); + break; + + case 3: + Simplex.SolveSimplex3(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.TransformPoint(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; + } + + #endregion +} diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs index 7ea1a1daa79..de8a3283fa0 100644 --- a/Robust.Shared/Physics/Systems/RayCastSystem.cs +++ b/Robust.Shared/Physics/Systems/RayCastSystem.cs @@ -3,17 +3,65 @@ using Robust.Shared.GameObjects; using Robust.Shared.IoC; using Robust.Shared.Map; +using Robust.Shared.Physics.Collision; using Robust.Shared.Physics.Collision.Shapes; using Robust.Shared.Physics.Dynamics; using Robust.Shared.Physics.Shapes; namespace Robust.Shared.Physics.Systems; -public sealed class RayCastSystem : EntitySystem +public sealed partial class RayCastSystem : EntitySystem { [Dependency] private readonly SharedPhysicsSystem _physics = default!; - internal CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform transform) + public void RayCast(MapCoordinates coordinates, Vector2 translation) + { + // TODO: Get trees in range. + } + + public void RayCast(Entity grid, Vector2 origin, Vector2 translation, uint collisionMask = uint.MaxValue - 1) + { + if (!Resolve(grid.Owner, ref grid.Comp)) + return; + + var input = new RayCastInput() + { + Origin = origin, + Translation = translation, + MaxFraction = 1f, + }; + + var broadphaseTransform = _physics.GetPhysicsTransform(grid); + + ((B2DynamicTree) grid.Comp.DynamicTree).RayCastNew(input, collisionMask, + (castInput, proxy, context) => + { + + // TODO: Collision check. + if ((shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return castInput.MaxFraction; + } + + var body = context.Body; + var transform = _physics.GetPhysicsTransform(context.Entity); + var relative = Physics.Transform.MulT(transform, broadphaseTransform); + + var output = RayCastShape(castInput, context.Fixture.Shape, transform); + + if (output.Hit) + { + b2ShapeId id = { shapeId + 1, world->worldId, shape->revision }; + float fraction = worldContext->fcn( id, output.Point, output.Normal, output.Fraction, worldContext->userContext ); + worldContext->fraction = fraction; + return fraction; + } + + return castInput.MaxFraction; + }); + } + + private CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform transform) { var localInput = input; localInput.Origin = b2InvTransformPoint( transform, input.Origin ); @@ -28,26 +76,21 @@ internal CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform output = b2RayCastCapsule( &localInput, &shape->capsule ); break; */ - case PhysShapeCircle: - output = b2RayCastCircle( &localInput, &shape->circle ); + case PhysShapeCircle circle: + output = RayCastCircle(localInput, circle); break; case PolygonShape polyShape: + output = RayCastPolygon(localInput, (Polygon) polyShape); break; - case Polygon: - output = b2RayCastPolygon( &localInput, &shape->polygon ); - break; - case b2_segmentShape: - output = b2RayCastSegment( &localInput, &shape->segment, false ); - break; - case b2_smoothSegmentShape: - output = b2RayCastSegment( &localInput, &shape->smoothSegment.segment, true ); + case Polygon poly: + output = RayCastPolygon(localInput, poly); break; default: return output; } - output.Point = b2TransformPoint( transform, output.Point ); - output.Normal = b2RotateVector( transform.Quaternion2D, output.Normal ); + output.Point = Physics.Transform.TransformPoint(transform, output.Point); + output.Normal = Quaternion2D.RotateVector(transform.Quaternion2D, output.Normal); return output; } @@ -56,95 +99,68 @@ internal CastOutput RayCast(IPhysShape shape, Vector2 origin, Vector2 translatio var transform = b2GetOwnerTransform( world, shape ); // input in local coordinates - var input = new RayCastInput(); - input.MaxFraction = 1.0f; - input.Origin = b2InvTransformPoint( transform, origin ); - input.Translation = b2InvRotateVector( transform.q, translation ); + var input = new RayCastInput + { + MaxFraction = 1.0f, + Origin = b2InvTransformPoint(transform, origin), + Translation = b2InvRotateVector(transform.q, translation) + }; var output = new CastOutput(); switch (shape) { - /* - case b2_capsuleShape: - output = b2RayCastCapsule( &input, &shape->capsule ); - break; - */ - case PhysShapeCircle circle: - output = b2RayCastCircle(input, circle); + output = RayCastCircle(input, circle); break; - - case b2_segmentShape: - output = b2RayCastSegment( &input, &shape->segment, false ); + case EdgeShape edge: + output = RayCastSegment(input, edge, false); break; - case Polygon poly: - output = b2RayCastPolygon( &input, &shape->polygon ); + output = RayCastPolygon(input, poly); break; - - case b2_smoothSegmentShape: - output = b2RayCastSegment( &input, &shape->smoothSegment.segment, true ); + case PolygonShape pShape: + output = RayCastPolygon(input, (Polygon) pShape); break; - default: throw new NotImplementedException(); } - if ( output.hit ) + if (output.Hit) { // convert to world coordinates - output.normal = b2RotateVector( transform.q, output.normal ); - output.point = b2TransformPoint( transform, output.point ); + output.Normal = Quaternion2D.RotateVector(transform.q, output.Normal); + output.Point = Physics.Transform.TransformPoint(transform, output.Point); } return output; } +} - public void RayCast(MapCoordinates coordinates, Vector2 translation) - { - // TODO: Get trees in range. - } - - public void RayCast(Entity grid, Vector2 origin, Vector2 translation, uint collisionMask = uint.MaxValue - 1) - { - if (!Resolve(grid.Owner, ref grid.Comp)) - return; - - var input = new RayCastInput() - { - Origin = origin, - Translation = translation, - MaxFraction = 1f, - }; - - var broadphaseTransform = _physics.GetPhysicsTransform(grid); - - ((B2DynamicTree) grid.Comp.DynamicTree).RayCastNew(input, collisionMask, - (castInput, proxy, context) => - { +internal ref struct ShapeCastPairInput +{ + public DistanceProxy ProxyA; ///< The proxy for shape A + public DistanceProxy ProxyB; ///< The proxy for shape B + public Transform TransformA; ///< The world transform for shape A + public Transform TransformB; ///< The world transform for shape B + public Vector2 TranslationB; ///< The translation of shape B + public float MaxFraction; ///< The fraction of the translation to consider, typically 1 +} - // TODO: Collision check. - if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) - { - return input->maxFraction; - } +internal ref struct ShapeCastInput +{ + /// A point cloud to cast + public Vector2[] Points; - var body = context.Body; - var transform = _physics.GetPhysicsTransform(context.Entity); - var relative = Physics.Transform.MulT(transform, broadphaseTransform); + /// The number of points + public int Count; - var output = RayCastShape(input, context.Fixture.Shape, transform); + /// The radius around the point cloud + public float Radius; - if (output.Hit) - { - b2ShapeId id = { shapeId + 1, world->worldId, shape->revision }; - float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, worldContext->userContext ); - worldContext->fraction = fraction; - return fraction; - } + /// The translation of the shape cast + public Vector2 Translation; - return input.MaxFraction; - }); - } + /// The maximum fraction of the translation to consider, typically 1 + public float MaxFraction; } internal ref struct RayCastInput @@ -154,6 +170,13 @@ internal ref struct RayCastInput 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 diff --git a/Robust.Shared/Physics/Transform.cs b/Robust.Shared/Physics/Transform.cs index 307463c4866..6e2ce0521b4 100644 --- a/Robust.Shared/Physics/Transform.cs +++ b/Robust.Shared/Physics/Transform.cs @@ -55,6 +55,15 @@ public Transform(Vector2 position, Angle angle) Quaternion2D = new Quaternion2D(angle); } + /// Transform a point (e.g. local space to world space) + public static Vector2 TransformPoint(Transform xf, Vector2 p) + { + float x = xf.Quaternion2D.C * p.X - xf.Quaternion2D.S * p.Y + p.X; + float y = xf.Quaternion2D.S * p.X + xf.Quaternion2D.C * p.Y + p.Y; + + return new Vector2(x, y); + } + 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; @@ -80,6 +89,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] @@ -184,5 +194,12 @@ 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); + } } } From a85cf318877c1bf51f59289e2561cfa868113cb4 Mon Sep 17 00:00:00 2001 From: metalgearsloth Date: Mon, 16 Sep 2024 01:02:26 +1000 Subject: [PATCH 3/7] more cudin --- .../Physics/Systems/RayCastSystem.Geometry.cs | 176 ++++++++++-------- .../Physics/Systems/RayCastSystem.cs | 10 +- Robust.Shared/Physics/Transform.cs | 15 ++ 3 files changed, 115 insertions(+), 86 deletions(-) diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs b/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs index c217c8a23f9..60b22454ea4 100644 --- a/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs +++ b/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs @@ -71,87 +71,6 @@ internal CastOutput RayCastCircle(RayCastInput input, PhysShapeCircle shape) return output; } - // 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; - } - private CastOutput RayCastPolygon(RayCastInput input, Polygon shape) { if (shape.Radius == 0.0f) @@ -241,6 +160,87 @@ private CastOutput RayCastPolygon(RayCastInput input, Polygon shape) 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 @@ -459,5 +459,19 @@ private CastOutput ShapeCastPolygon(ShapeCastInput input, Polygon shape) 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 } diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs index de8a3283fa0..3e500749533 100644 --- a/Robust.Shared/Physics/Systems/RayCastSystem.cs +++ b/Robust.Shared/Physics/Systems/RayCastSystem.cs @@ -64,8 +64,8 @@ public void RayCast(Entity grid, Vector2 origin, Vector2 t private CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform transform) { var localInput = input; - localInput.Origin = b2InvTransformPoint( transform, input.Origin ); - localInput.Translation = b2InvRotateVector( transform.Quaternion2D, input.Translation ); + localInput.Origin = Physics.Transform.InvTransformPoint(transform, input.Origin); + localInput.Translation = Quaternion2D.InvRotateVector(transform.Quaternion2D, input.Translation); CastOutput output = new(); @@ -102,11 +102,11 @@ internal CastOutput RayCast(IPhysShape shape, Vector2 origin, Vector2 translatio var input = new RayCastInput { MaxFraction = 1.0f, - Origin = b2InvTransformPoint(transform, origin), - Translation = b2InvRotateVector(transform.q, translation) + Origin = Physics.Transform.InvTransformPoint(transform, origin), + Translation = Quaternion2D.InvRotateVector(transform.q, translation) }; - var output = new CastOutput(); + CastOutput output; switch (shape) { case PhysShapeCircle circle: diff --git a/Robust.Shared/Physics/Transform.cs b/Robust.Shared/Physics/Transform.cs index 6e2ce0521b4..4ac8f3e0283 100644 --- a/Robust.Shared/Physics/Transform.cs +++ b/Robust.Shared/Physics/Transform.cs @@ -55,6 +55,14 @@ public Transform(Vector2 position, Angle angle) Quaternion2D = new Quaternion2D(angle); } + /// Inverse transform a point (e.g. world space to local space) + 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); + } + /// Transform a point (e.g. local space to world space) public static Vector2 TransformPoint(Transform xf, Vector2 p) { @@ -201,5 +209,12 @@ 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); + } } } From 13a8a51810f9f2686cf10ff7b25d952b38218ee3 Mon Sep 17 00:00:00 2001 From: metalgearsloth Date: Mon, 16 Sep 2024 15:26:25 +1000 Subject: [PATCH 4/7] Mostly ported --- Robust.Shared/Physics/B2DynamicTree.cs | 4 +- .../BroadPhase/DynamicTreeBroadPhase.cs | 1 + Robust.Shared/Physics/IBroadPhase.cs | 2 + .../Physics/Systems/RayCastSystem.Geometry.cs | 2 +- .../Physics/Systems/RayCastSystem.cs | 66 +++- .../Physics/Systems/SharedBroadphaseSystem.cs | 46 ++- .../Systems/SharedPhysicsSystem.Queries.cs | 303 ++++++++++-------- Robust.Shared/Physics/Transform.cs | 5 + .../Shared/Maths/Matrix3_Test.cs | 30 +- 9 files changed, 285 insertions(+), 174 deletions(-) diff --git a/Robust.Shared/Physics/B2DynamicTree.cs b/Robust.Shared/Physics/B2DynamicTree.cs index fcb2d64a3c6..0e2dc3b4d67 100644 --- a/Robust.Shared/Physics/B2DynamicTree.cs +++ b/Robust.Shared/Physics/B2DynamicTree.cs @@ -944,9 +944,9 @@ 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); - public delegate float NewRayCallback(RayCastInput input, Proxy proxy, T context); + internal delegate float NewRayCallback(RayCastInput input, Proxy proxy, T context); - public void RayCastNew(RayCastInput input, uint mask, NewRayCallback callback) + internal void RayCastNew(RayCastInput input, uint mask, NewRayCallback callback) { var p1 = input.Origin; var d = input.Translation; 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/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/Systems/RayCastSystem.Geometry.cs b/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs index 60b22454ea4..93db32d9859 100644 --- a/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs +++ b/Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs @@ -136,7 +136,7 @@ private CastOutput RayCastPolygon(RayCastInput input, Polygon shape) DebugTools.Assert( 0.0f <= lower && lower <= input.MaxFraction ); - if ( index >= 0 ) + if (index >= 0) { output.Fraction = lower; output.Normal = shape.Normals[index]; diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs index 3e500749533..ec9d55fb2f7 100644 --- a/Robust.Shared/Physics/Systems/RayCastSystem.cs +++ b/Robust.Shared/Physics/Systems/RayCastSystem.cs @@ -1,8 +1,10 @@ using System; using System.Numerics; +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; @@ -12,14 +14,64 @@ namespace Robust.Shared.Physics.Systems; public sealed partial class RayCastSystem : EntitySystem { + [Dependency] private readonly SharedBroadphaseSystem _broadphase = default!; [Dependency] private readonly SharedPhysicsSystem _physics = default!; - public void RayCast(MapCoordinates coordinates, Vector2 translation) + private record struct RayCastQueryState { - // TODO: Get trees in range. + public RayCastSystem System; + public SharedPhysicsSystem Physics; + + public uint CollisionMask; + public Vector2 Origin; + public Vector2 Translation; } - public void RayCast(Entity grid, Vector2 origin, Vector2 translation, uint collisionMask = uint.MaxValue - 1) + public void RayCast(MapCoordinates coordinates, Vector2 translation, uint collisionMask = uint.MaxValue - 1) + { + var end = coordinates.Position + translation; + var aabb = new Box2(Vector2.Min(coordinates.Position, end), Vector2.Max(coordinates.Position, end)); + + var state = new RayCastQueryState() + { + System = this, + Physics = _physics, + + CollisionMask = collisionMask, + Origin = coordinates.Position, + Translation = translation, + }; + + _broadphase.GetBroadphases(coordinates.MapId, + aabb, ref state, + static (Entity entity, ref RayCastQueryState state) => + { + var transform = state.Physics.GetPhysicsTransform(entity.Owner); + 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); + }); + } + + public void RayCast( + Entity grid, + Vector2 origin, + Vector2 translation, + uint collisionMask = uint.MaxValue - 1) + { + if (!Resolve(grid.Owner, ref grid.Comp)) + return; + + var state = new RayCastQueryState() + { + System = this, + }; + + RayCast(grid, state, origin, translation, collisionMask); + } + + private void RayCast(Entity grid, RayCastQueryState state, Vector2 origin, Vector2 translation, uint collisionMask = uint.MaxValue - 1) { if (!Resolve(grid.Owner, ref grid.Comp)) return; @@ -33,10 +85,9 @@ public void RayCast(Entity grid, Vector2 origin, Vector2 t var broadphaseTransform = _physics.GetPhysicsTransform(grid); - ((B2DynamicTree) grid.Comp.DynamicTree).RayCastNew(input, collisionMask, - (castInput, proxy, context) => + grid.Comp.DynamicTree.Tree.RayCastNew(input, collisionMask, + static (castInput, proxy, context) => { - // TODO: Collision check. if ((shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) { @@ -51,8 +102,7 @@ public void RayCast(Entity grid, Vector2 origin, Vector2 t if (output.Hit) { - b2ShapeId id = { shapeId + 1, world->worldId, shape->revision }; - float fraction = worldContext->fcn( id, output.Point, output.Normal, output.Fraction, worldContext->userContext ); + float fraction = worldContext->fcn( id, output.Point, output.Normal, output.Fraction, worldContext->userContext); worldContext->fraction = fraction; return fraction; } diff --git a/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs b/Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs index 18aee0396f3..2da62734258 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,29 @@ 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, ref TState state, 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(); - - while (enumerator.MoveNext(out var bUid, out var broadphase, out var xform)) - { - if (xform.MapID != mapId) continue; - - if (!EntityManager.TryGetComponent(bUid, out MapGridComponent? mapGrid)) + 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; - } - - // Won't worry about accurate bounds checks as it's probably slower in most use cases. - var chunkEnumerator = _map.GetMapChunks(bUid, mapGrid, aabb); + if (!tuple._broadphaseQuery.TryComp(uid, out var broadphase)) + return true; - if (chunkEnumerator.MoveNext(out _)) - { - yield return (bUid, broadphase); - } - } + 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); } + 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..2879b18f2f7 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,23 @@ public IEnumerable GetCollidingEntities(MapId mapId, in Box2 w { if (mapId == MapId.Nullspace) return Array.Empty(); + var aabb = worldAABB; var bodies = new HashSet(); - foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, worldAABB)) + _broadphase.GetBroadphases(mapId, worldAABB, broadphase => { - var gridAABB = _transform.GetInvWorldMatrix(uid).TransformBox(worldAABB); + var gridAABB = _transform.GetInvWorldMatrix(broadphase.Owner).TransformBox(aabb); - foreach (var proxy in broadphase.StaticTree.QueryAabb(gridAABB, false)) + foreach (var proxy in broadphase.Comp.StaticTree.QueryAabb(gridAABB, false)) { bodies.Add(proxy.Body); } - foreach (var proxy in broadphase.DynamicTree.QueryAabb(gridAABB, false)) + foreach (var proxy in broadphase.Comp.DynamicTree.QueryAabb(gridAABB, false)) { bodies.Add(proxy.Body); } - } + }); return bodies; } @@ -158,22 +171,25 @@ public IEnumerable> GetCollidingEntities(MapId mapId, i if (mapId == MapId.Nullspace) return Array.Empty>(); + var bounds = worldBounds; var bodies = new HashSet>(); - foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, worldBounds.CalcBoundingBox())) - { - var gridAABB = _transform.GetInvWorldMatrix(uid).TransformBox(worldBounds); - - foreach (var proxy in broadphase.StaticTree.QueryAabb(gridAABB, false)) + _broadphase.GetBroadphases(mapId, + worldBounds.CalcBoundingBox(), + broadphase => { - bodies.Add(new Entity(proxy.Entity, proxy.Body)); - } + var gridAABB = _transform.GetInvWorldMatrix(broadphase).TransformBox(bounds); - foreach (var proxy in broadphase.DynamicTree.QueryAabb(gridAABB, false)) - { - bodies.Add(new Entity(proxy.Entity, proxy.Body)); - } - } + foreach (var proxy in broadphase.Comp.StaticTree.QueryAabb(gridAABB, false)) + { + bodies.Add(new Entity(proxy.Entity, proxy.Body)); + } + + foreach (var proxy in broadphase.Comp.DynamicTree.QueryAabb(gridAABB, false)) + { + bodies.Add(new Entity(proxy.Entity, proxy.Body)); + } + }); return bodies; } @@ -263,72 +279,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 +409,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); + + broadphase.Comp.DynamicTree.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(); + } + + 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 4ac8f3e0283..6b607ebed14 100644 --- a/Robust.Shared/Physics/Transform.cs +++ b/Robust.Shared/Physics/Transform.cs @@ -56,6 +56,7 @@ public Transform(Vector2 position, Angle 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; @@ -64,6 +65,7 @@ public static Vector2 InvTransformPoint(Transform t, Vector2 p) } /// Transform a point (e.g. local space to world space) + [Pure] public static Vector2 TransformPoint(Transform xf, Vector2 p) { float x = xf.Quaternion2D.C * p.X - xf.Quaternion2D.S * p.Y + p.X; @@ -72,6 +74,7 @@ public static Vector2 TransformPoint(Transform xf, Vector2 p) return new Vector2(x, y); } + [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; @@ -80,12 +83,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; diff --git a/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs b/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs index bb67e63fec5..43f295a3a85 100644 --- a/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs +++ b/Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs @@ -12,18 +12,30 @@ namespace Robust.UnitTesting.Shared.Maths [TestOf(typeof(Matrix3x2))] public sealed class Matrix3_Test { - [Test] - public void GetRotationTest() + private static readonly TestCaseData[] Positions = new TestCaseData[] + { + new(Matrix3x2.Identity, Vector2.Zero), + new(Matrix3x2.CreateTranslation(Vector2.One), Vector2.One), + new(Matrix3x2.CreateTranslation(new Vector2(1f, 0f)), new Vector2(1f, 0f)), + }; + + private static readonly TestCaseData[] Rotations = new TestCaseData[] { - Assert.That(Matrix3x2.Identity.Rotation(), Is.EqualTo(Angle.Zero)); + new(Matrix3x2.Identity, Angle.Zero), + new(Matrix3x2.CreateRotation(MathF.PI / 2), new Angle(MathF.PI / 2)), + new(Matrix3x2.CreateRotation(MathF.PI), Math.PI), + }; - var piOver2 = new Angle(Math.PI / 2); - var piOver2Mat = Matrix3Helpers.CreateRotation(piOver2.Theta); - Assert.That(piOver2Mat.Rotation(), Is.EqualTo(piOver2)); + [Test, TestCaseSource(nameof(Positions))] + public void GetPositionTest(Matrix3x2 matrix, Vector2 vec) + { + Assert.That(matrix.Position(), Is.EqualTo(vec)); + } - 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(matrix.Rotation(), Is.EqualTo(angle)); } [Test] From 46468582e73c742f97bba572b73c5e05682510de Mon Sep 17 00:00:00 2001 From: metalgearsloth Date: Wed, 18 Sep 2024 19:12:22 +1000 Subject: [PATCH 5/7] more work --- Robust.Shared.Maths/MathHelper.cs | 15 +++++ Robust.Shared.Maths/Vector2Helpers.cs | 15 +++++ .../Physics/Systems/RayCastSystem.cs | 2 +- .../Physics/Systems/SharedBroadphaseSystem.cs | 23 +++++++ .../Systems/SharedPhysicsSystem.Queries.cs | 60 ++++++++++--------- 5 files changed, 87 insertions(+), 28 deletions(-) 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/Vector2Helpers.cs b/Robust.Shared.Maths/Vector2Helpers.cs index 6819cc0c8cd..3faaa27501a 100644 --- a/Robust.Shared.Maths/Vector2Helpers.cs +++ b/Robust.Shared.Maths/Vector2Helpers.cs @@ -15,6 +15,21 @@ 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(); diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs index ec9d55fb2f7..189ceaaf9e5 100644 --- 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 index 2da62734258..65128310321 100644 --- 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 _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(MapId mapId, Box2 aabb, ref TState state, BroadphaseCallback callback) { var internalState = (state, callback, _broadphaseQuery); @@ -493,6 +514,8 @@ internal void GetBroadphases(MapId mapId, Box2 aabb, ref TState state, B }, approx: true, includeMap: true); } + internal delegate void BroadphaseCallback(Entity entity); + internal delegate void BroadphaseCallback(Entity 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 index 2879b18f2f7..c6a7b19b472 100644 --- a/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs +++ b/Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs @@ -144,21 +144,25 @@ public IEnumerable GetCollidingEntities(MapId mapId, in Box2 w var aabb = worldAABB; var bodies = new HashSet(); + 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 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.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> GetCollidingEntities(MapId mapId, i if (mapId == MapId.Nullspace) return Array.Empty>(); - var bounds = worldBounds; var bodies = new HashSet>(); - _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(proxy.Entity, proxy.Body)); - } + _broadphase.GetBroadphases(mapId, worldBounds.CalcBoundingBox(), ref state, static + ( + Entity entity, + ref (SharedTransformSystem _transform, HashSet 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(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; } From 1e89e4651b7ca77ee94be3ba86dc8c44bac5bb5b Mon Sep 17 00:00:00 2001 From: metalgearsloth Date: Thu, 19 Sep 2024 01:10:07 +1000 Subject: [PATCH 6/7] More ports --- Robust.Shared/Physics/B2DynamicTree.cs | 125 +++++++++- .../Physics/Systems/RayCastSystem.cs | 231 +++++++++++++++--- 2 files changed, 323 insertions(+), 33 deletions(-) diff --git a/Robust.Shared/Physics/B2DynamicTree.cs b/Robust.Shared/Physics/B2DynamicTree.cs index 0e2dc3b4d67..c0d2d498675 100644 --- a/Robust.Shared/Physics/B2DynamicTree.cs +++ b/Robust.Shared/Physics/B2DynamicTree.cs @@ -944,9 +944,9 @@ 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 NewRayCallback(RayCastInput input, Proxy proxy, T context); + internal delegate float RayCallback(RayCastInput input, Proxy proxy, T context, ref TState State); - internal void RayCastNew(RayCastInput input, uint mask, NewRayCallback callback) + internal void RayCastNew(RayCastInput input, uint mask, ref TState state, RayCallback callback) { var p1 = input.Origin; var d = input.Translation; @@ -1007,15 +1007,15 @@ internal void RayCastNew(RayCastInput input, uint mask, NewRayCallback callback) { subInput.MaxFraction = maxFraction; - float value = callback(subInput, nodeId, node.UserData); + float value = callback(subInput, nodeId, node.UserData, ref state); - if ( value == 0.0f ) + if (value == 0.0f) { // The client has terminated the ray cast. return; } - if ( 0.0f < value && value < maxFraction ) + if (0.0f < value && value < maxFraction) { // Update segment bounding box. maxFraction = value; @@ -1038,6 +1038,121 @@ internal void RayCastNew(RayCastInput input, uint mask, NewRayCallback callback) } } + /// 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(ref ShapeCastInput input, Proxy proxyId, T userData, ref TState state); + + internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCastCallback 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 = b2CrossSV( 1.0f, r ); + var abs_v = v.Abs(); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input.MaxFraction; + + // Build total box for the shape cast + b2Vec2 t = b2MulSV(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]); + var stackCount = 0; + 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(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 = b2MulSV( maxFraction, input.Translation); + totalAABB.BottomLeft = Vector2.Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) ); + totalAABB.TopRight = Vector2.Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) ); + } + } + else + { + B2_ASSERT( stackCount < b2_treeStackSize - 1 ); + if ( stackCount < b2_treeStackSize - 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[stackCount++] = node->child1; + stack[stackCount++] = node->child2; + } + } + } + } + public void RayCast(RayQueryCallback callback, in Ray input) { RayCast(ref callback, EasyRayQueryCallback, input); diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs index 189ceaaf9e5..cd50c84544d 100644 --- a/Robust.Shared/Physics/Systems/RayCastSystem.cs +++ b/Robust.Shared/Physics/Systems/RayCastSystem.cs @@ -17,16 +17,27 @@ public sealed partial class RayCastSystem : EntitySystem [Dependency] private readonly SharedBroadphaseSystem _broadphase = default!; [Dependency] private readonly SharedPhysicsSystem _physics = default!; + #region RayCast + private record struct RayCastQueryState { public RayCastSystem System; public SharedPhysicsSystem Physics; + public Transform BroadphaseTransform; public uint CollisionMask; public Vector2 Origin; public Vector2 Translation; } + /// + /// Returns the closest entity hit. + /// + public void RayCastClosest(Vector2 origin, Vector2 translation, QueryFilter filter) + { + + } + public void RayCast(MapCoordinates coordinates, Vector2 translation, uint collisionMask = uint.MaxValue - 1) { var end = coordinates.Position + translation; @@ -58,6 +69,7 @@ public void RayCast( Entity grid, Vector2 origin, Vector2 translation, + uint collisionMask = uint.MaxValue - 1) { if (!Resolve(grid.Owner, ref grid.Comp)) @@ -66,12 +78,16 @@ public void RayCast( var state = new RayCastQueryState() { System = this, + Origin = origin, + Translation = translation, + Physics = _physics, + CollisionMask = collisionMask, }; - RayCast(grid, state, origin, translation, collisionMask); + RayCast(grid, ref state, origin, translation, collisionMask); } - private void RayCast(Entity grid, RayCastQueryState state, Vector2 origin, Vector2 translation, uint collisionMask = uint.MaxValue - 1) + private void RayCast(Entity grid, ref RayCastQueryState state, Vector2 origin, Vector2 translation, uint collisionMask = uint.MaxValue - 1) { if (!Resolve(grid.Owner, ref grid.Comp)) return; @@ -83,32 +99,150 @@ private void RayCast(Entity grid, RayCastQueryState state, MaxFraction = 1f, }; - var broadphaseTransform = _physics.GetPhysicsTransform(grid); + state.BroadphaseTransform = _physics.GetPhysicsTransform(grid); - grid.Comp.DynamicTree.Tree.RayCastNew(input, collisionMask, - static (castInput, proxy, context) => + grid.Comp.DynamicTree.Tree.RayCastNew(input, collisionMask, ref state, static ( + RayCastInput castInput, + DynamicTree.Proxy proxy, + FixtureProxy context, + ref RayCastQueryState queryState) => + { + // TODO: Collision check. + if ((shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) { - // TODO: Collision check. - if ((shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) - { - return castInput.MaxFraction; - } + return castInput.MaxFraction; + } + + var body = context.Body; + var localTransform = queryState.Physics.GetLocalPhysicsTransform(context.Entity); - var body = context.Body; - var transform = _physics.GetPhysicsTransform(context.Entity); - var relative = Physics.Transform.MulT(transform, broadphaseTransform); + var output = queryState.System.RayCastShape(castInput, context.Fixture.Shape, localTransform); - var output = RayCastShape(castInput, context.Fixture.Shape, transform); + if (output.Hit) + { + float fraction = worldContext->fcn( id, output.Point, output.Normal, output.Fraction, worldContext->userContext); + worldContext->fraction = fraction; + return fraction; + } - if (output.Hit) - { - float fraction = worldContext->fcn( id, output.Point, output.Normal, output.Fraction, worldContext->userContext); - worldContext->fraction = fraction; - return fraction; - } + return castInput.MaxFraction; + }); + } - return castInput.MaxFraction; - }); + #endregion + + #region ShapeCast + + /// + /// Convenience method for shape casts; only supports shapes with area. + /// + public void CastShape( + IPhysShape shape, + Transform originTransform, + Vector2 translation, + QueryFilter filter, + CastResult fcn) + { + switch (shape) + { + case PhysShapeCircle circle: + CastCircle(circle, originTransform, translation, filter, fcn); + break; + case PolygonShape poly: + CastPolygon(poly, originTransform, translation, filter, fcn); + break; + default: + throw new ArgumentOutOfRangeException(); + } + } + + /// + /// Cast a circle through the world. Similar to a cast ray except that a circle is cast instead of a point. + /// + public void CastCircle( + PhysShapeCircle circle, + Transform originTransform, + Vector2 translation, + QueryFilter filter, + CastResult fcn) + { + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); + B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); + B2_ASSERT( b2Vec2_IsValid( translation ) ); + + b2ShapeCastInput input; + input.points[0] = b2TransformPoint( originTransform, circle->center ); + input.count = 1; + input.radius = circle->radius; + input.translation = translation; + input.maxFraction = 1.0f; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return; + } + + input.maxFraction = worldContext.fraction; + } + } + + /// + /// Cast a polygon through the world. Similar to a cast ray except that a polygon is cast instead of a point. + /// + public void CastPolygon( + PolygonShape polygon, + Transform originTransform, + Vector2 translation, + QueryFilter filter, + CastResult fcn) + { + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); + B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); + B2_ASSERT( b2Vec2_IsValid( translation ) ); + + b2ShapeCastInput input; + for ( int i = 0; i < polygon->count; ++i ) + { + input.points[i] = b2TransformPoint( originTransform, polygon->vertices[i] ); + } + input.count = polygon->count; + input.radius = polygon->radius; + input.translation = translation; + input.maxFraction = 1.0f; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return; + } + + input.maxFraction = worldContext.fraction; + } } private CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform transform) @@ -183,16 +317,22 @@ internal CastOutput RayCast(IPhysShape shape, Vector2 origin, Vector2 translatio } return output; } + + #endregion } internal ref struct ShapeCastPairInput { - public DistanceProxy ProxyA; ///< The proxy for shape A - public DistanceProxy ProxyB; ///< The proxy for shape B - public Transform TransformA; ///< The world transform for shape A - public Transform TransformB; ///< The world transform for shape B - public Vector2 TranslationB; ///< The translation of shape B - public float MaxFraction; ///< The fraction of the translation to consider, typically 1 + 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 ref struct ShapeCastInput @@ -241,3 +381,38 @@ internal ref struct CastOutput public bool Hit; } + +/// 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; +} + +/// 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); From 7eeea109e74164170140e0417fcb0569dfe0c634 Mon Sep 17 00:00:00 2001 From: metalgearsloth Date: Thu, 19 Sep 2024 17:37:37 +1000 Subject: [PATCH 7/7] the big house --- Robust.Shared/Physics/B2DynamicTree.cs | 34 +++++++++++-------- .../Physics/Systems/RayCastSystem.cs | 28 +++++++-------- 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/Robust.Shared/Physics/B2DynamicTree.cs b/Robust.Shared/Physics/B2DynamicTree.cs index c0d2d498675..72877e4c722 100644 --- a/Robust.Shared/Physics/B2DynamicTree.cs +++ b/Robust.Shared/Physics/B2DynamicTree.cs @@ -1070,8 +1070,8 @@ internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCa // v is perpendicular to the segment. var r = input.Translation; - var v = b2CrossSV( 1.0f, r ); - var abs_v = v.Abs(); + 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) @@ -1079,8 +1079,9 @@ internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCa float maxFraction = input.MaxFraction; // Build total box for the shape cast - b2Vec2 t = b2MulSV(maxFraction, input.Translation); - var totalAABB = new Box2( + 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)) ); @@ -1089,12 +1090,14 @@ internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCa ref var baseRef = ref _nodes[0]; var stack = new GrowableStack(stackalloc Proxy[256]); - var stackCount = 0; + var stackCount = 1; stack.Push(_root); - while (stack.GetCount() > 0) - { + while (stackCount > 0) + { var nodeId = stack.Pop(); + stackCount = stack.GetCount(); + if (nodeId == Proxy.Free) { continue; @@ -1130,24 +1133,25 @@ internal void ShapeCast(ShapeCastInput input, long maskBits, TreeShapeCa return; } - if ( 0.0f < value && value < maxFraction ) + if (0.0f < value && value < maxFraction) { // Update segment bounding box. maxFraction = value; - t = b2MulSV( maxFraction, input.Translation); - totalAABB.BottomLeft = Vector2.Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) ); - totalAABB.TopRight = Vector2.Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) ); + 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 { - B2_ASSERT( stackCount < b2_treeStackSize - 1 ); - if ( stackCount < b2_treeStackSize - 1 ) + 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[stackCount++] = node->child1; - stack[stackCount++] = node->child2; + stack.Push(node.Child1); + stack.Push(node.Child2); } } } diff --git a/Robust.Shared/Physics/Systems/RayCastSystem.cs b/Robust.Shared/Physics/Systems/RayCastSystem.cs index cd50c84544d..708211c292f 100644 --- a/Robust.Shared/Physics/Systems/RayCastSystem.cs +++ b/Robust.Shared/Physics/Systems/RayCastSystem.cs @@ -209,26 +209,24 @@ public void CastPolygon( QueryFilter filter, CastResult fcn) { - b2World* world = b2GetWorldFromId( worldId ); - B2_ASSERT( world->locked == false ); - if ( world->locked ) - { - return; - } - B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); B2_ASSERT( b2Vec2_IsValid( translation ) ); - b2ShapeCastInput input; - for ( int i = 0; i < polygon->count; ++i ) + + ShapeCastInput input = new() + { + Points = new Vector2[polygon.VertexCount], + }; + for ( int i = 0; i < polygon.VertexCount; ++i ) { - input.points[i] = b2TransformPoint( originTransform, polygon->vertices[i] ); + input.Points[i] = Physics.Transform.TransformPoint(originTransform, polygon.Vertices[i]); } - input.count = polygon->count; - input.radius = polygon->radius; - input.translation = translation; - input.maxFraction = 1.0f; + + input.Count = polygon->count; + input.Radius = polygon->radius; + input.Translation = translation; + input.MaxFraction = 1.0f; WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; @@ -241,7 +239,7 @@ public void CastPolygon( return; } - input.maxFraction = worldContext.fraction; + input.MaxFraction = worldContext.fraction; } }