space-wizards/RobustToolbox
1
0
Fork 0
mirror of https://github.com/space-wizards/RobustToolbox.git synced 2026-02-15 03:30:53 +01:00
Code Issues Packages Projects Releases Wiki Activity

Add shapecasts + raycasts (#5440)

Browse Source 
* Add shapecasts + raycasts

Actual raycasts. Need this for AI LIDAR experiment.

* cassette

* more cudin

* Mostly ported

* more work

* More ports

* the big house

* rays

* builds

* Janky not working raycasts

* Fix GJK

* Test fixes

* Shapecast + fixes

* free

* tests

* More fixes

* Minor changes

* Not these

* Release notes
This commit is contained in:
metalgearsloth
2024-12-16 16:07:24 +11:00
committed by GitHub
parent 7982aa236c
commit 0b7e8c2560
19 changed files with 2226 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
RELEASE-NOTES.md
View File

@@ -41,6 +41,7 @@ END TEMPLATE-->
* Added stack-like functions to `ValueList<T>` and added an `AddRange(ReadOnlySpan<T>)` overload.
* Added new `AssetPassFilterDrop`.
* Added a new RayCastSystem with the latest Box2D raycast + shapecasts implemented.
### Bugfixes

15
Robust.Shared.Maths/MathHelper.cs
View File

@@ -745,6 +745,21 @@ namespace Robust.Shared.Maths
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
}
}

2
Robust.Shared.Maths/Matrix3Helpers.cs
View File

@@ -73,7 +73,7 @@ public static class Matrix3Helpers
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Angle Rotation(this Matrix3x2 t)
{
return new Vector2(t.M11, t.M12).ToAngle();
return new Angle(Math.Atan2(t.M12, t.M11));
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]

35
Robust.Shared.Maths/Vector2Helpers.cs
View File

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

212
Robust.Shared/Physics/B2DynamicTree.cs
View File

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

1
Robust.Shared/Physics/BroadPhase/DynamicTreeBroadPhase.cs
View File

@@ -24,6 +24,7 @@ public sealed class DynamicTreeBroadPhase : IBroadPhase
}
public int Count => _tree.NodeCount;
public B2DynamicTree<FixtureProxy> Tree => _tree;
public Box2 GetFatAabb(DynamicTree.Proxy proxy)
{

243
Robust.Shared/Physics/Collision/DistanceProxy.cs
View File

@@ -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;
}
/// <summary>
/// Initialize the proxy using the given shape. The shape
/// must remain in scope while the proxy is in use.
@@ -143,6 +149,13 @@ internal ref struct DistanceProxy
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;
}
}
/// <summary>
@@ -306,6 +319,16 @@ internal struct Simplex
}
}
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 struct Simplex
}
}
public static Vector2 ComputeSimplexClosestPoint(Simplex s)
{
switch (s.Count)
{
case 0:
DebugTools.Assert(false);
return Vector2.Zero;
case 1:
return s.V._00.W;
case 2:
return Weight2(s.V._00.A, s.V._00.W, s.V._01.A, s.V._01.W);
case 3:
return Vector2.Zero;
default:
DebugTools.Assert(false);
return Vector2.Zero;
}
}
public static void ComputeSimplexWitnessPoints(ref Vector2 a, ref Vector2 b, Simplex s)
{
switch (s.Count)
{
case 0:
DebugTools.Assert(false);
break;
case 1:
a = s.V._00.WA;
b = s.V._00.WB;
break;
case 2:
a = Weight2(s.V._00.A, s.V._00.WA, s.V._01.A, s.V._01.WA);
b = Weight2(s.V._00.A, s.V._00.WB, s.V._01.A, s.V._01.WB);
break;
case 3:
a = Weight3(s.V._00.A, s.V._00.WA, s.V._01.A, s.V._01.WA, s.V._02.A, s.V._02.WA);
// TODO_ERIN why are these not equal?
//*b = b2Weight3(s->v1.a, s->v1.wB, s->v2.a, s->v2.wB, s->v3.a, s->v3.wB);
b = a;
break;
default:
DebugTools.Assert(false);
break;
}
}
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
public static void SolveSimplex2(ref Simplex s)
{
var w1 = s.V._00.W;
var w2 = s.V._01.W;
var e12 = Vector2.Subtract(w2, w1);
// w1 region
float d12_2 = -Vector2.Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
s.V._00.A = 1.0f;
s.Count = 1;
return;
}
// w2 region
float d12_1 = Vector2.Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
s.V._01.A = 1.0f;
s.Count = 1;
s.V._00 = s.V._01;
return;
}
// Must be in e12 region.
float inv_d12 = 1.0f / ( d12_1 + d12_2 );
s.V._00.A = d12_1 * inv_d12;
s.V._01.A = d12_2 * inv_d12;
s.Count = 2;
}
public static void SolveSimplex3(ref Simplex s)
{
var w1 = s.V._00.W;
var w2 = s.V._01.W;
var w3 = s.V._02.W;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
var e12 = Vector2.Subtract(w2, w1);
float w1e12 = Vector2.Dot(w1, e12);
float w2e12 = Vector2.Dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
var e13 = Vector2.Subtract(w3, w1);
float w1e13 = Vector2.Dot(w1, e13);
float w3e13 = Vector2.Dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
var e23 = Vector2.Subtract(w3, w2);
float w2e23 = Vector2.Dot(w2, e23);
float w3e23 = Vector2.Dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;
// Triangle123
float n123 = Vector2Helpers.Cross(e12, e13);
float d123_1 = n123 * Vector2Helpers.Cross(w2, w3);
float d123_2 = n123 * Vector2Helpers.Cross(w3, w1);
float d123_3 = n123 * Vector2Helpers.Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
s.V._00.A = 1.0f;
s.Count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
float inv_d12 = 1.0f / ( d12_1 + d12_2 );
s.V._00.A = d12_1 * inv_d12;
s.V._01.A = d12_2 * inv_d12;
s.Count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
float inv_d13 = 1.0f / ( d13_1 + d13_2 );
s.V._00.A = d13_1 * inv_d13;
s.V._02.A = d13_2 * inv_d13;
s.Count = 2;
s.V._01 = s.V._02;
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
s.V._01.A = 1.0f;
s.Count = 1;
s.V._00 = s.V._01;
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
s.V._02.A = 1.0f;
s.Count = 1;
s.V._00 = s.V._02;
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
float inv_d23 = 1.0f / ( d23_1 + d23_2 );
s.V._01.A = d23_1 * inv_d23;
s.V._02.A = d23_2 * inv_d23;
s.Count = 2;
s.V._00 = s.V._02;
return;
}
// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
s.V._00.A = d123_1 * inv_d123;
s.V._01.A = d123_2 * inv_d123;
s.V._02.A = d123_3 * inv_d123;
s.Count = 3;
}
internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB)
{
switch (Count)

8
Robust.Shared/Physics/Collision/Shapes/PolygonShape.cs
View File

@@ -27,6 +27,7 @@ using System.Runtime.InteropServices;
using Robust.Shared.Configuration;
using Robust.Shared.IoC;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Shapes;
using Robust.Shared.Physics.Systems;
using Robust.Shared.Serialization;
using Robust.Shared.Serialization.Manager.Attributes;
@@ -172,6 +173,13 @@ namespace Robust.Shared.Physics.Collision.Shapes
{
}
internal PolygonShape(Polygon poly)
{
Vertices = poly.Vertices;
Normals = poly.Normals;
Centroid = poly.Centroid;
}
public PolygonShape(float radius)
{
Radius = radius;

2
Robust.Shared/Physics/IBroadPhase.cs
View File

@@ -10,6 +10,8 @@ public interface IBroadPhase
{
int Count { get; }
public B2DynamicTree<FixtureProxy> Tree { get; }
Box2 GetFatAabb(DynamicTree.Proxy proxy);
DynamicTree.Proxy AddProxy(ref FixtureProxy proxy);

4
Robust.Shared/Physics/Shapes/Polygon.cs
View File

@@ -87,8 +87,8 @@ internal record struct Polygon : IPhysShape
if (hull.Count < 3)
{
Vertices = Array.Empty<Vector2>();
Normals = Array.Empty<Vector2>();
Vertices = [];
Normals = [];
return;
}

736
Robust.Shared/Physics/Systems/RayCastSystem.Geometry.cs Normal file
View File

@@ -0,0 +1,736 @@
using System;
using System.Numerics;
using Robust.Shared.GameObjects;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Collision;
using Robust.Shared.Physics.Collision.Shapes;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Shapes;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Systems;
public sealed partial class RayCastSystem
{
/*
* This is really "geometry and friends" as it has all the private methods.
*/
#region Callbacks
/// <summary>
/// Returns every entity from the callback.
/// </summary>
public static float RayCastAllCallback(FixtureProxy proxy, Vector2 point, Vector2 normal, float fraction, ref RayResult result)
{
result.Results.Add(new RayHit(proxy.Entity, normal, fraction)
{
Point = point,
});
return 1f;
}
/// <summary>
/// Gets the closest entity from the callback.
/// </summary>
public static float RayCastClosestCallback(FixtureProxy proxy, Vector2 point, Vector2 normal, float fraction, ref RayResult result)
{
var add = false;
if (result.Results.Count > 0)
{
if (result.Results[0].Fraction > fraction)
{
add = true;
result.Results.Clear();
}
}
else
{
add = true;
}
if (add)
{
result.Results.Add(new RayHit(proxy.Entity, normal, fraction)
{
Point = point,
});
}
return fraction;
}
#endregion
#region Raycast
private CastOutput RayCastShape(RayCastInput input, IPhysShape shape, Transform transform)
{
var localInput = input;
localInput.Origin = Physics.Transform.InvTransformPoint(transform, input.Origin);
localInput.Translation = Quaternion2D.InvRotateVector(transform.Quaternion2D, input.Translation);
CastOutput output = new();
switch (shape)
{
/*
case b2_capsuleShape:
output = b2RayCastCapsule( &localInput, &shape->capsule );
break;
*/
case PhysShapeCircle circle:
output = RayCastCircle(localInput, circle);
break;
case PolygonShape polyShape:
{
output = RayCastPolygon(localInput, (Polygon) polyShape);
}
break;
case Polygon poly:
{
output = RayCastPolygon(localInput, poly);
}
break;
default:
return output;
}
output.Point = Physics.Transform.Mul(transform, output.Point);
output.Normal = Quaternion2D.RotateVector(transform.Quaternion2D, output.Normal);
return output;
}
/// <summary>
/// This callback is invoked upon every AABB collision.
/// </summary>
private static float RayCastCallback(RayCastInput input, FixtureProxy proxy, ref WorldRayCastContext worldContext)
{
if ((proxy.Fixture.CollisionLayer & worldContext.Filter.MaskBits) == 0 && (proxy.Fixture.CollisionMask & worldContext.Filter.LayerBits) == 0)
{
return input.MaxFraction;
}
if (worldContext.Filter.IsIgnored?.Invoke(proxy.Entity) == true)
{
return input.MaxFraction;
}
var transform = worldContext.Physics.GetLocalPhysicsTransform(proxy.Entity);
var output = worldContext.System.RayCastShape(input, proxy.Fixture.Shape, transform);
if (output.Hit)
{
// Fraction returned determines what B2Dynamictree will do, i.e. shrink the AABB or not.
var fraction = worldContext.fcn(proxy, output.Point, output.Normal, output.Fraction, ref worldContext.Result);
return fraction;
}
return input.MaxFraction;
}
// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019
// http://www.codercorner.com/blog/?p=321
internal CastOutput RayCastCircle(RayCastInput input, PhysShapeCircle shape)
{
DebugTools.Assert(input.IsValidRay());
var p = shape.Position;
var output = new CastOutput();
// Shift ray so circle center is the origin
var s = Vector2.Subtract(input.Origin, p);
float length = 0f;
var d = input.Translation.GetLengthAndNormalize(ref length);
if (length == 0.0f)
{
// zero length ray
return output;
}
// Find closest point on ray to origin
// solve: dot(s + t * d, d) = 0
float t = -Vector2.Dot(s, d);
// c is the closest point on the line to the origin
var c = Vector2.Add(s, t * d);
float cc = Vector2.Dot(c, c);
float r = shape.Radius;
float rr = r * r;
if (cc > rr)
{
// closest point is outside the circle
return output;
}
// Pythagorus
float h = MathF.Sqrt(rr - cc);
float fraction = t - h;
if ( fraction < 0.0f || input.MaxFraction * length < fraction )
{
// outside the range of the ray segment
return output;
}
var hitPoint = Vector2.Add(s, fraction * d);
output.Fraction = fraction / length;
output.Normal = hitPoint.Normalized();
output.Point = Vector2.Add(p, shape.Radius * output.Normal);
output.Hit = true;
return output;
}
private CastOutput RayCastPolygon(RayCastInput input, Polygon shape)
{
if (shape.Radius == 0.0f)
{
// Put the ray into the polygon's frame of reference.
var p1 = input.Origin;
var d = input.Translation;
float lower = 0.0f, upper = input.MaxFraction;
var index = -1;
var output = new CastOutput()
{
Fraction = 0f,
};
for ( var i = 0; i < shape.VertexCount; ++i )
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(shape.Normals[i], Vector2.Subtract( shape.Vertices[i], p1 ) );
float denominator = Vector2.Dot(shape.Normals[i], d );
if ( denominator == 0.0f )
{
if ( numerator < 0.0f )
{
return output;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if ( denominator < 0.0f && numerator < lower * denominator )
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if ( denominator > 0.0f && numerator < upper * denominator )
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the B2_ASSERT on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
// if (upper < lower - b2_epsilon)
if ( upper < lower )
{
return output;
}
}
DebugTools.Assert( 0.0f <= lower && lower <= input.MaxFraction );
if (index >= 0)
{
output.Fraction = lower;
output.Normal = shape.Normals[index];
output.Point = Vector2.Add(p1, lower * d);
output.Hit = true;
}
return output;
}
// TODO_ERIN this is not working for ray vs box (zero radii)
var castInput = new ShapeCastPairInput
{
ProxyA = DistanceProxy.MakeProxy(shape.Vertices, shape.VertexCount, shape.Radius),
ProxyB = DistanceProxy.MakeProxy([input.Origin], 1, 0.0f),
TransformA = Physics.Transform.Empty,
TransformB = Physics.Transform.Empty,
TranslationB = input.Translation,
MaxFraction = input.MaxFraction
};
return ShapeCast(castInput);
}
// Ray vs line segment
private CastOutput RayCastSegment(RayCastInput input, EdgeShape shape, bool oneSided)
{
var output = new CastOutput();
if (oneSided)
{
// Skip left-side collision
float offset = Vector2Helpers.Cross(Vector2.Subtract(input.Origin, shape.Vertex0), Vector2.Subtract( shape.Vertex1, shape.Vertex0));
if ( offset < 0.0f )
{
return output;
}
}
// Put the ray into the edge's frame of reference.
var p1 = input.Origin;
var d = input.Translation;
var v1 = shape.Vertex0;
var v2 = shape.Vertex1;
var e = Vector2.Subtract( v2, v1 );
float length = 0f;
var eUnit = e.GetLengthAndNormalize(ref length);
if (length == 0.0f)
{
return output;
}
// Normal points to the right, looking from v1 towards v2
var normal = eUnit.RightPerp();
// Intersect ray with infinite segment using normal
// Similar to intersecting a ray with an infinite plane
// p = p1 + t * d
// dot(normal, p - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, Vector2.Subtract(v1, p1));
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
// parallel
return output;
}
float t = numerator / denominator;
if ( t < 0.0f || input.MaxFraction < t )
{
// out of ray range
return output;
}
// Intersection point on infinite segment
var p = Vector2.Add(p1, t * d);
// Compute position of p along segment
// p = v1 + s * e
// s = dot(p - v1, e) / dot(e, e)
float s = Vector2.Dot(Vector2.Subtract(p, v1), eUnit);
if ( s < 0.0f || length < s )
{
// out of segment range
return output;
}
if ( numerator > 0.0f )
{
normal = -normal;
}
output.Fraction = t;
output.Point = Vector2.Add(p1, t * d);
output.Normal = normal;
output.Hit = true;
return output;
}
#endregion
#region Shape
private CastOutput ShapeCastShape(ShapeCastInput input, IPhysShape shape, Transform transform)
{
var localInput = input;
for ( int i = 0; i < localInput.Count; ++i )
{
localInput.Points[i] = Physics.Transform.MulT(transform, input.Points[i]);
}
localInput.Translation = Quaternion2D.InvRotateVector(transform.Quaternion2D, input.Translation);
CastOutput output;
switch (shape)
{
case PhysShapeCircle circle:
output = ShapeCastCircle(localInput, circle);
break;
case PolygonShape pShape:
output = ShapeCastPolygon(localInput, (Polygon) pShape);
break;
case Polygon poly:
output = ShapeCastPolygon(localInput, poly);
break;
default:
return new CastOutput();
}
output.Point = Physics.Transform.Mul(transform, output.Point);
output.Normal = Quaternion2D.RotateVector(transform.Quaternion2D, output.Normal);
return output;
}
/// <summary>
/// This callback is invoked upon getting the AABB inside of B2DynamicTree.
/// </summary>
/// <returns>The max fraction to continue checking for. If this is lower then we will start dropping more shapes early</returns>
private float ShapeCastCallback(ShapeCastInput input, FixtureProxy proxy, ref WorldRayCastContext worldContext)
{
var filter = worldContext.Filter;
if ((proxy.Fixture.CollisionLayer & filter.MaskBits) == 0 && (proxy.Fixture.CollisionMask & filter.LayerBits) == 0)
{
return input.MaxFraction;
}
if ((filter.Flags & QueryFlags.Sensors) == 0x0 && !proxy.Fixture.Hard)
{
return input.MaxFraction;
}
if (worldContext.Filter.IsIgnored?.Invoke(proxy.Entity) == true)
{
return input.MaxFraction;
}
var transform = worldContext.Physics.GetLocalPhysicsTransform(proxy.Entity);
var output = ShapeCastShape(input, proxy.Fixture.Shape, transform);
if (output.Hit)
{
var fraction = worldContext.fcn(proxy, output.Point, output.Normal, output.Fraction, ref worldContext.Result);
return fraction;
}
return input.MaxFraction;
}
// GJK-raycast
// Algorithm by Gino van den Bergen.
// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
// todo this is failing when used to raycast a box
// todo this converges slowly with a radius
private CastOutput ShapeCast(ShapeCastPairInput input)
{
var output = new CastOutput()
{
Fraction = input.MaxFraction,
};
var proxyA = input.ProxyA;
var count = input.ProxyB.Vertices.Length;
var xfA = input.TransformA;
var xfB = input.TransformB;
var xf = Physics.Transform.InvMulTransforms(xfA, xfB);
// Put proxyB in proxyA's frame to reduce round-off error
var proxyBVerts = new Vector2[input.ProxyB.Vertices.Length];
for ( int i = 0; i < count; ++i )
{
proxyBVerts[i] = Physics.Transform.Mul(xf, input.ProxyB.Vertices[i]);
}
var proxyB = DistanceProxy.MakeProxy(proxyBVerts, count, input.ProxyB.Radius);
DebugTools.Assert(proxyB.Vertices.Length <= PhysicsConstants.MaxPolygonVertices);
float radius = proxyA.Radius + proxyB.Radius;
var r = Quaternion2D.RotateVector(xf.Quaternion2D, input.TranslationB);
float lambda = 0.0f;
float maxFraction = input.MaxFraction;
// Initial simplex
Simplex simplex;
simplex = new()
{
Count = 0,
V = new FixedArray4<SimplexVertex>()
};
// Get an initial point in A - B
int indexA = FindSupport(proxyA, -r);
var wA = proxyA.Vertices[indexA];
int indexB = FindSupport(proxyB, r);
var wB = proxyB.Vertices[indexB];
var v = Vector2.Subtract(wA, wB);
// Sigma is the target distance between proxies
const float linearSlop = PhysicsConstants.LinearSlop;
var sigma = MathF.Max(linearSlop, radius - linearSlop);
// Main iteration loop.
const int k_maxIters = 20;
int iter = 0;
while ( iter < k_maxIters && v.Length() > sigma + 0.5f * linearSlop )
{
DebugTools.Assert(simplex.Count < 3);
output.Iterations += 1;
// Support in direction -v (A - B)
indexA = FindSupport(proxyA, -v);
wA = proxyA.Vertices[indexA];
indexB = FindSupport(proxyB, v);
wB = proxyB.Vertices[indexB];
var p = Vector2.Subtract(wA, wB);
// -v is a normal at p, normalize to work with sigma
v = v.Normalized();
// Intersect ray with plane
float vp = Vector2.Dot(v, p);
float vr = Vector2.Dot(v, r);
if ( vp - sigma > lambda * vr )
{
if ( vr <= 0.0f )
{
// miss
return output;
}
lambda = ( vp - sigma ) / vr;
if ( lambda > maxFraction )
{
// too far
return output;
}
// reset the simplex
simplex.Count = 0;
}
// Reverse simplex since it works with B - A.
// Shift by lambda * r because we want the closest point to the current clip point.
// Note that the support point p is not shifted because we want the plane equation
// to be formed in unshifted space.
ref var vertex = ref simplex.V.AsSpan[simplex.Count];
vertex.IndexA = indexB;
vertex.WA = new Vector2(wB.X + lambda * r.X, wB.Y + lambda * r.Y);
vertex.IndexB = indexA;
vertex.WB = wA;
vertex.W = Vector2.Subtract(vertex.WB, vertex.WA);
vertex.A = 1.0f;
simplex.Count += 1;
switch (simplex.Count)
{
case 1:
break;
case 2:
Simplex.SolveSimplex2(ref simplex);
break;
case 3:
Simplex.SolveSimplex3(ref simplex);
break;
default:
throw new NotImplementedException();
}
// If we have 3 points, then the origin is in the corresponding triangle.
if ( simplex.Count == 3 )
{
// Overlap
// Yes this means you need to manually query for overlaps.
return output;
}
// Get search direction.
// todo use more accurate segment perpendicular
v = Simplex.ComputeSimplexClosestPoint(simplex);
// Iteration count is equated to the number of support point calls.
++iter;
}
if ( iter == 0 || lambda == 0.0f )
{
// Initial overlap
return output;
}
// Prepare output.
Vector2 pointA = Vector2.Zero, pointB = Vector2.Zero;
Simplex.ComputeSimplexWitnessPoints(ref pointB, ref pointA, simplex);
var n = (-v).Normalized();
var point = new Vector2(pointA.X + proxyA.Radius * n.X, pointA.Y + proxyA.Radius * n.Y);
output.Point = Physics.Transform.Mul(xfA, point);
output.Normal = Quaternion2D.RotateVector(xfA.Quaternion2D, n);
output.Fraction = lambda;
output.Iterations = iter;
output.Hit = true;
return output;
}
private int FindSupport(DistanceProxy proxy, Vector2 direction)
{
int bestIndex = 0;
float bestValue = Vector2.Dot(proxy.Vertices[0], direction);
for ( int i = 1; i < proxy.Vertices.Length; ++i )
{
float value = Vector2.Dot(proxy.Vertices[i], direction);
if ( value > bestValue )
{
bestIndex = i;
bestValue = value;
}
}
return bestIndex;
}
private CastOutput ShapeCastCircle(ShapeCastInput input, PhysShapeCircle shape)
{
var pairInput = new ShapeCastPairInput
{
ProxyA = DistanceProxy.MakeProxy([shape.Position], 1, shape.Radius ),
ProxyB = DistanceProxy.MakeProxy(input.Points, input.Count, input.Radius ),
TransformA = Physics.Transform.Empty,
TransformB = Physics.Transform.Empty,
TranslationB = input.Translation,
MaxFraction = input.MaxFraction
};
var output = ShapeCast(pairInput);
return output;
}
private CastOutput ShapeCastPolygon(ShapeCastInput input, Polygon shape)
{
var pairInput = new ShapeCastPairInput
{
ProxyA = DistanceProxy.MakeProxy(shape.Vertices, shape.VertexCount, shape.Radius),
ProxyB = DistanceProxy.MakeProxy(input.Points, input.Count, input.Radius),
TransformA = Physics.Transform.Empty,
TransformB = Physics.Transform.Empty,
TranslationB = input.Translation,
MaxFraction = input.MaxFraction
};
var output = ShapeCast(pairInput);
return output;
}
private CastOutput ShapeCastSegment(ShapeCastInput input, EdgeShape shape)
{
var pairInput = new ShapeCastPairInput();
pairInput.ProxyA = DistanceProxy.MakeProxy([shape.Vertex0], 2, 0.0f);
pairInput.ProxyB = DistanceProxy.MakeProxy(input.Points, input.Count, input.Radius);
pairInput.TransformA = Physics.Transform.Empty;
pairInput.TransformB = Physics.Transform.Empty;
pairInput.TranslationB = input.Translation;
pairInput.MaxFraction = input.MaxFraction;
var output = ShapeCast(pairInput);
return output;
}
#endregion
}
internal ref struct WorldRayCastContext
{
public RayCastSystem System;
public SharedPhysicsSystem Physics;
public CastResult fcn;
public QueryFilter Filter;
public float Fraction;
public RayResult Result;
}
internal ref struct ShapeCastPairInput
{
public DistanceProxy ProxyA;
public DistanceProxy ProxyB;
public Transform TransformA;
public Transform TransformB;
public Vector2 TranslationB;
/// <summary>
/// The fraction of the translation to consider, typically 1
/// </summary>
public float MaxFraction;
}
internal record struct ShapeCastInput
{
public Transform Origin;
/// A point cloud to cast
public Vector2[] Points;
/// The number of points
public int Count;
/// The radius around the point cloud
public float Radius;
/// The translation of the shape cast
public Vector2 Translation;
/// The maximum fraction of the translation to consider, typically 1
public float MaxFraction;
}
internal record struct RayCastInput
{
public Vector2 Origin;
public Vector2 Translation;
public float MaxFraction;
public bool IsValidRay()
{
bool isValid = Origin.IsValid() && Translation.IsValid() && MaxFraction.IsValid() &&
0.0f <= MaxFraction && MaxFraction < float.MaxValue;
return isValid;
}
}
internal ref struct CastOutput
{
public Vector2 Normal;
public Vector2 Point;
public float Fraction;
public int Iterations;
public bool Hit;
}

465
Robust.Shared/Physics/Systems/RayCastSystem.cs Normal file
View File

@@ -0,0 +1,465 @@
using System;
using System.Collections.Generic;
using System.Numerics;
using JetBrains.Annotations;
using Robust.Shared.Collections;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
using Robust.Shared.Map;
using Robust.Shared.Map.Components;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Collision;
using Robust.Shared.Physics.Collision.Shapes;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Shapes;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics.Systems;
public sealed partial class RayCastSystem : EntitySystem
{
/*
* A few things to keep in mind with the below:
* - Raycasts are done relative to the corresponding broadphases.
* - The raycast results need to be transformed into Map terms.
* - If you wish to add more helper methods make a new partial and dump them there and have them call the below methods.
*/
[Dependency] private readonly SharedBroadphaseSystem _broadphase = default!;
[Dependency] private readonly SharedPhysicsSystem _physics = default!;
private readonly RayComparer _rayComparer = new();
#region RayCast
private sealed class RayComparer : IComparer<RayHit>
{
public int Compare(RayHit x, RayHit y)
{
return x.Fraction.CompareTo(y.Fraction);
}
}
private void AdjustResults(ref RayResult result, int index, Transform xf)
{
for (var i = index; i < result.Results.Count; i++)
{
result.Results[i].Point = Physics.Transform.Mul(xf, result.Results[i].Point);
}
}
/*
* Raycasts that return all entities sorted.
*/
/// <summary>
/// Casts a ray against a broadphase.
/// </summary>
public void CastRay(Entity<BroadphaseComponent?> entity, ref RayResult result, Vector2 origin, Vector2 translation, QueryFilter filter, bool sorted = true)
{
if (!Resolve(entity.Owner, ref entity.Comp))
return;
DebugTools.Assert(origin.IsValid());
DebugTools.Assert(translation.IsValid());
var input = new RayCastInput()
{
Origin = origin,
Translation = translation,
MaxFraction = 1f,
};
var worldContext = new WorldRayCastContext()
{
fcn = RayCastAllCallback,
Filter = filter,
Fraction = 1f,
Physics = _physics,
System = this,
Result = result,
};
entity.Comp.DynamicTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback);
input.MaxFraction = worldContext.Fraction;
entity.Comp.StaticTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback);
result = worldContext.Result;
if (sorted)
{
result.Results.Sort(_rayComparer);
}
}
/// <summary>
/// Returns all entities hit in order.
/// </summary>
[Pure]
public RayResult CastRay(MapId mapId, Vector2 origin, Vector2 translation, QueryFilter filter)
{
DebugTools.Assert(origin.IsValid());
DebugTools.Assert(translation.IsValid());
var input = new RayCastInput
{
Origin = origin,
Translation = translation,
MaxFraction = 1.0f
};
var result = new RayResult();
var start = origin;
var end = origin + translation;
var aabb = new Box2(Vector2.Min(start, end), Vector2.Max(start, end));
var state = (input, filter, result, this, _physics);
_broadphase.GetBroadphases(mapId, aabb, ref state,
static (Entity<BroadphaseComponent> entity, ref (RayCastInput input, QueryFilter filter, RayResult result, RayCastSystem system, SharedPhysicsSystem Physics) tuple) =>
{
var transform = tuple.Physics.GetPhysicsTransform(entity.Owner);
var localOrigin = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin);
var localTranslation = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin + tuple.input.Translation) - localOrigin;
var oldIndex = tuple.result.Results.Count;
tuple.system.CastRay((entity.Owner, entity.Comp), ref tuple.result, localOrigin, localTranslation, filter: tuple.filter, sorted: false);
tuple.system.AdjustResults(ref tuple.result, oldIndex, transform);
});
result = state.result;
result.Results.Sort(_rayComparer);
return result;
}
/*
* Raycasts that only return the closest entity.
*/
/// <summary>
/// Casts a ray against a broadphase.
/// </summary>
public void CastRayClosest(Entity<BroadphaseComponent?> entity, ref RayResult result, Vector2 origin, Vector2 translation, QueryFilter filter)
{
if (!Resolve(entity.Owner, ref entity.Comp))
return;
DebugTools.Assert(origin.IsValid());
DebugTools.Assert(translation.IsValid());
var input = new RayCastInput()
{
Origin = origin,
Translation = translation,
MaxFraction = 1f,
};
var worldContext = new WorldRayCastContext()
{
fcn = RayCastClosestCallback,
Filter = filter,
Fraction = 1f,
Physics = _physics,
System = this,
Result = result,
};
entity.Comp.DynamicTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback);
input.MaxFraction = worldContext.Fraction;
entity.Comp.StaticTree.Tree.RayCastNew(input, filter.MaskBits, ref worldContext, RayCastCallback);
result = worldContext.Result;
DebugTools.Assert(result.Results.Count <= 1);
}
/// <summary>
/// Returns all entities hit in order.
/// </summary>
public RayResult CastRayClosest(MapId mapId, Vector2 origin, Vector2 translation, QueryFilter filter)
{
DebugTools.Assert(origin.IsValid());
DebugTools.Assert(translation.IsValid());
var input = new RayCastInput
{
Origin = origin,
Translation = translation,
MaxFraction = 1.0f
};
var result = new RayResult();
var end = origin + translation;
var aabb = new Box2(Vector2.Min(origin, end), Vector2.Max(origin, end));
var state = (input, filter, result, this, _physics);
_broadphase.GetBroadphases(mapId, aabb, ref state,
static (Entity<BroadphaseComponent> entity, ref (RayCastInput input, QueryFilter filter, RayResult result, RayCastSystem system, SharedPhysicsSystem _physics) tuple) =>
{
var transform = tuple._physics.GetPhysicsTransform(entity.Owner);
var localOrigin = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin);
var localTranslation = Physics.Transform.InvTransformPoint(transform, tuple.input.Origin + tuple.input.Translation) - localOrigin;
var oldIndex = tuple.result.Results.Count;
tuple.system.CastRayClosest((entity.Owner, entity.Comp), ref tuple.result, localOrigin, localTranslation, filter: tuple.filter);
tuple.system.AdjustResults(ref tuple.result, oldIndex, transform);
});
result = state.result;
DebugTools.Assert(result.Results.Count <= 1);
return result;
}
#endregion
#region ShapeCast
/// <summary>
/// Convenience method for shape casts; only supports shapes with area.
/// </summary>
public RayResult CastShape(
MapId mapId,
IPhysShape shape,
Transform originTransform,
Vector2 translation,
QueryFilter filter,
CastResult callback)
{
DebugTools.Assert(originTransform.Position.IsValid());
DebugTools.Assert(originTransform.Quaternion2D.IsValid());
DebugTools.Assert(translation.IsValid());
// Need to get the entire shape AABB to know what broadphases to even query.
var startAabb = shape.ComputeAABB(originTransform, 0);
var endAabb = shape.ComputeAABB(new Transform(originTransform.Position + translation, originTransform.Quaternion2D.Angle), 0);
var aabb = startAabb.Union(endAabb);
var result = new RayResult();
var state = (originTransform, translation, shape: shape, filter, result, this, _physics, callback);
_broadphase.GetBroadphases(mapId, aabb, ref state,
static (
Entity<BroadphaseComponent> entity,
ref (Transform origin, Vector2 translation, IPhysShape shape, QueryFilter filter, RayResult result, RayCastSystem system, SharedPhysicsSystem _physics, CastResult callback
) tuple) =>
{
var transform = tuple._physics.GetPhysicsTransform(entity.Owner);
var localOrigin = Physics.Transform.MulT(transform, tuple.origin);
var localTranslation = Physics.Transform.InvTransformPoint(transform, tuple.origin.Position + tuple.translation) - localOrigin.Position;
var oldIndex = tuple.result.Results.Count;
tuple.system.CastShape((entity.Owner, entity.Comp), ref tuple.result, tuple.shape, localOrigin, localTranslation, filter: tuple.filter, callback: tuple.callback);
tuple.system.AdjustResults(ref tuple.result, oldIndex, transform);
});
result = state.result;
return result;
}
/// <summary>
/// Cast on the broadphase.
/// </summary>
public void CastShape(
Entity<BroadphaseComponent?> entity,
ref RayResult result,
IPhysShape shape,
Transform originTransform,
Vector2 translation,
QueryFilter filter,
CastResult callback)
{
if (!Resolve(entity.Owner, ref entity.Comp))
return;
switch (shape)
{
case PhysShapeCircle circle:
CastCircle(entity, ref result, circle, originTransform, translation, filter, callback);
break;
case Polygon poly:
CastPolygon(entity, ref result, new PolygonShape(poly), originTransform, translation, filter, callback);
break;
case PolygonShape polygon:
CastPolygon(entity, ref result, polygon, originTransform, translation, filter, callback);
break;
default:
Log.Error("Tried to shapecast for shape not implemented.");
DebugTools.Assert(false);
return;
}
}
public void CastCircle(
Entity<BroadphaseComponent?> entity,
ref RayResult result,
PhysShapeCircle circle,
Transform originTransform,
Vector2 translation,
QueryFilter filter,
CastResult callback)
{
if (!Resolve(entity.Owner, ref entity.Comp))
return;
var input = new ShapeCastInput()
{
Points = new Vector2[1],
Count = 1,
Radius = circle.Radius,
Translation = translation,
MaxFraction = 1f,
};
input.Points[0] = Physics.Transform.Mul(originTransform, circle.Position);
var worldContext = new WorldRayCastContext()
{
System = this,
Physics = _physics,
Filter = filter,
Fraction = 1f,
Result = result,
fcn = callback,
};
entity.Comp.StaticTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext);
input.MaxFraction = worldContext.Fraction;
entity.Comp.DynamicTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext);
result = worldContext.Result;
}
public void CastPolygon(
Entity<BroadphaseComponent?> entity,
ref RayResult result,
PolygonShape polygon,
Transform originTransform,
Vector2 translation,
QueryFilter filter,
CastResult callback)
{
if (!Resolve(entity.Owner, ref entity.Comp))
return;
ShapeCastInput input = new()
{
Points = new Vector2[polygon.VertexCount],
};
for ( int i = 0; i < polygon.VertexCount; ++i )
{
input.Points[i] = Physics.Transform.Mul(originTransform, polygon.Vertices[i]);
}
input.Count = polygon.VertexCount;
input.Radius = polygon.Radius;
input.Translation = translation;
input.MaxFraction = 1.0f;
var worldContext = new WorldRayCastContext()
{
System = this,
Physics = _physics,
Filter = filter,
Fraction = 1f,
Result = result,
fcn = callback,
};
if ((filter.Flags & QueryFlags.Static) == QueryFlags.Static)
{
entity.Comp.StaticTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext);
input.MaxFraction = worldContext.Fraction;
}
if ((filter.Flags & QueryFlags.Dynamic) == QueryFlags.Dynamic)
{
entity.Comp.DynamicTree.Tree.ShapeCast(input, filter.MaskBits, ShapeCastCallback, ref worldContext);
}
result = worldContext.Result;
}
#endregion
}
/// Result from b2World_RayCastClosest
/// @ingroup world
public record struct RayResult()
{
public ValueList<RayHit> Results = new();
public bool Hit => Results.Count > 0;
public static readonly RayResult Empty = new();
}
public record struct RayHit(EntityUid Entity, Vector2 LocalNormal, float Fraction)
{
public readonly EntityUid Entity = Entity;
public readonly Vector2 LocalNormal = LocalNormal;
public readonly float Fraction = Fraction;
// When this point gets added it's in broadphase terms, then the caller handles whether it gets turned into map-terms.
public Vector2 Point;
}
/// The query filter is used to filter collisions between queries and shapes. For example,
/// you may want a ray-cast representing a projectile to hit players and the static environment
/// but not debris.
/// @ingroup shape
public record struct QueryFilter()
{
/// <summary>
/// The collision category bits of this query. Normally you would just set one bit.
/// </summary>
public long LayerBits;
/// <summary>
/// The collision mask bits. This states the shape categories that this
/// query would accept for collision.
/// </summary>
public long MaskBits;
/// <summary>
/// Return whether to ignore an entity.
/// </summary>
public Func<EntityUid, bool>? IsIgnored;
public QueryFlags Flags = QueryFlags.Dynamic | QueryFlags.Static;
}
/// <summary>
/// Which trees we wish to query.
/// </summary>
[Flags]
public enum QueryFlags : byte
{
None = 0,
Dynamic = 1 << 0,
Static = 1 << 1,
Sensors = 1 << 2,
// StaticSundries = 1 << 3,
// Sundries = 1 << 4,
}
/// Prototype callback for ray casts.
/// Called for each shape found in the query. You control how the ray cast
/// proceeds by returning a float:
/// return -1: ignore this shape and continue
/// return 0: terminate the ray cast
/// return fraction: clip the ray to this point
/// return 1: don't clip the ray and continue
/// @param shapeId the shape hit by the ray
/// @param point the point of initial intersection
/// @param normal the normal vector at the point of intersection
/// @param fraction the fraction along the ray at the point of intersection
/// @param context the user context
/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue
/// @see b2World_CastRay
/// @ingroup world
public delegate float CastResult(FixtureProxy proxy, Vector2 point, Vector2 normal, float fraction, ref RayResult result);

69
Robust.Shared/Physics/Systems/SharedBroadphaseSystem.cs
View File

@@ -4,6 +4,7 @@ using System.Collections.Generic;
using System.Numerics;
using System.Threading.Tasks;
using Microsoft.Extensions.ObjectPool;
using Robust.Shared.Collections;
using Robust.Shared.Configuration;
using Robust.Shared.GameObjects;
using Robust.Shared.IoC;
@@ -471,38 +472,54 @@ namespace Robust.Shared.Physics.Systems
TouchProxies(xform.MapUid.Value, matrix, fixture);
}
// TODO: The below is slow and should just query the map's broadphase directly. The problem is that
// there's some ordering stuff going on where the broadphase has queued all of its updates but hasn't applied
// them yet so this query will fail on initialization which chains into a whole lot of issues.
internal IEnumerable<(EntityUid uid, BroadphaseComponent comp)> GetBroadphases(MapId mapId, Box2 aabb)
internal void GetBroadphases(MapId mapId, Box2 aabb,BroadphaseCallback callback)
{
// TODO Okay so problem: If we just do Encloses that's a lot faster BUT it also means we don't return the
// map's broadphase which avoids us iterating over it for 99% of bodies.
var internalState = (callback, _broadphaseQuery);
if (mapId == MapId.Nullspace) yield break;
var enumerator = AllEntityQuery<BroadphaseComponent, TransformComponent>();
while (enumerator.MoveNext(out var bUid, out var broadphase, out var xform))
{
if (xform.MapID != mapId) continue;
if (!EntityManager.TryGetComponent(bUid, out MapGridComponent? mapGrid))
_mapManager.FindGridsIntersecting(mapId,
aabb,
ref internalState,
static (
EntityUid uid,
MapGridComponent grid,
ref (BroadphaseCallback callback, EntityQuery<BroadphaseComponent> _broadphaseQuery) tuple) =>
{
yield return (bUid, broadphase);
continue;
}
if (!tuple._broadphaseQuery.TryComp(uid, out var broadphase))
return true;
// Won't worry about accurate bounds checks as it's probably slower in most use cases.
var chunkEnumerator = _map.GetMapChunks(bUid, mapGrid, aabb);
if (chunkEnumerator.MoveNext(out _))
{
yield return (bUid, broadphase);
}
}
tuple.callback((uid, broadphase));
return true;
// Approx because we don't really need accurate checks for these most of the time.
}, approx: true, includeMap: true);
}
internal void GetBroadphases<TState>(MapId mapId, Box2 aabb, ref TState state, BroadphaseCallback<TState> callback)
{
var internalState = (state, callback, _broadphaseQuery);
_mapManager.FindGridsIntersecting(mapId,
aabb,
ref internalState,
static (
EntityUid uid,
MapGridComponent grid,
ref (TState state, BroadphaseCallback<TState> callback, EntityQuery<BroadphaseComponent> _broadphaseQuery) tuple) =>
{
if (!tuple._broadphaseQuery.TryComp(uid, out var broadphase))
return true;
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);
state = internalState.state;
}
internal delegate void BroadphaseCallback(Entity<BroadphaseComponent> entity);
internal delegate void BroadphaseCallback<TState>(Entity<BroadphaseComponent> entity, ref TState state);
private record struct BroadphaseContactJob() : IParallelRobustJob
{
public SharedBroadphaseSystem System = default!;

331
Robust.Shared/Physics/Systems/SharedPhysicsSystem.Queries.cs
View File

@@ -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 @@ namespace Robust.Shared.Physics.Systems
public bool TryCollideRect(Box2 collider, MapId mapId, bool approximate = true)
{
var state = (collider, mapId, found: false);
var broadphases = new ValueList<Entity<BroadphaseComponent>>();
foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, collider))
{
var gridCollider = _transform.GetInvWorldMatrix(uid).TransformBox(collider);
broadphase.StaticTree.QueryAabb(ref state, (ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) =>
_broadphase.GetBroadphases(mapId,
collider,
broadphase =>
{
if (proxy.Fixture.CollisionLayer == 0x0)
return true;
var gridCollider = _transform.GetInvWorldMatrix(broadphase).TransformBox(collider);
if (proxy.AABB.Intersects(gridCollider))
{
state.found = true;
return false;
}
return true;
}, gridCollider, approximate);
broadphase.Comp.StaticTree.QueryAabb(ref state,
(ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) =>
{
if (proxy.Fixture.CollisionLayer == 0x0)
return true;
broadphase.DynamicTree.QueryAabb(ref state, (ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) =>
{
if (proxy.Fixture.CollisionLayer == 0x0)
return true;
if (proxy.AABB.Intersects(gridCollider))
{
state.found = true;
return false;
}
if (proxy.AABB.Intersects(gridCollider))
{
state.found = true;
return false;
}
return true;
}, gridCollider, approximate);
}
return true;
},
gridCollider,
approximate);
broadphase.Comp.DynamicTree.QueryAabb(ref state,
(ref (Box2 collider, MapId map, bool found) state, in FixtureProxy proxy) =>
{
if (proxy.Fixture.CollisionLayer == 0x0)
return true;
if (proxy.AABB.Intersects(gridCollider))
{
state.found = true;
return false;
}
return true;
},
gridCollider,
approximate);
});
return state.found;
}
@@ -130,22 +142,27 @@ namespace Robust.Shared.Physics.Systems
{
if (mapId == MapId.Nullspace) return Array.Empty<PhysicsComponent>();
var aabb = worldAABB;
var bodies = new HashSet<PhysicsComponent>();
var state = (_transform, bodies, aabb);
foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, worldAABB))
{
var gridAABB = _transform.GetInvWorldMatrix(uid).TransformBox(worldAABB);
foreach (var proxy in broadphase.StaticTree.QueryAabb(gridAABB, false))
_broadphase.GetBroadphases(mapId, worldAABB, ref state, static
(
Entity<BroadphaseComponent> entity,
ref (SharedTransformSystem _transform, HashSet<PhysicsComponent> bodies, Box2 aabb) tuple) =>
{
bodies.Add(proxy.Body);
}
var gridAABB = tuple._transform.GetInvWorldMatrix(entity.Owner).TransformBox(tuple.aabb);
foreach (var proxy in broadphase.DynamicTree.QueryAabb(gridAABB, false))
{
bodies.Add(proxy.Body);
}
}
foreach (var proxy in entity.Comp.StaticTree.QueryAabb(gridAABB, false))
{
tuple.bodies.Add(proxy.Body);
}
foreach (var proxy in entity.Comp.DynamicTree.QueryAabb(gridAABB, false))
{
tuple.bodies.Add(proxy.Body);
}
});
return bodies;
}
@@ -160,20 +177,27 @@ namespace Robust.Shared.Physics.Systems
var bodies = new HashSet<Entity<PhysicsComponent>>();
foreach (var (uid, broadphase) in _broadphase.GetBroadphases(mapId, worldBounds.CalcBoundingBox()))
{
var gridAABB = _transform.GetInvWorldMatrix(uid).TransformBox(worldBounds);
var state = (_transform, bodies, worldBounds);
foreach (var proxy in broadphase.StaticTree.QueryAabb(gridAABB, false))
_broadphase.GetBroadphases(mapId, worldBounds.CalcBoundingBox(), ref state,
static (
Entity<BroadphaseComponent> entity,
ref (SharedTransformSystem _transform, HashSet<Entity<PhysicsComponent>> bodies, Box2Rotated
worldBounds
) tuple) =>
{
bodies.Add(new Entity<PhysicsComponent>(proxy.Entity, proxy.Body));
}
var gridAABB = tuple._transform.GetInvWorldMatrix(entity.Owner).TransformBox(tuple.worldBounds);
foreach (var proxy in broadphase.DynamicTree.QueryAabb(gridAABB, false))
{
bodies.Add(new Entity<PhysicsComponent>(proxy.Entity, proxy.Body));
}
}
foreach (var proxy in entity.Comp.StaticTree.QueryAabb(gridAABB, false))
{
tuple.bodies.Add((proxy.Entity, proxy.Body));
}
foreach (var proxy in entity.Comp.DynamicTree.QueryAabb(gridAABB, false))
{
tuple.bodies.Add((proxy.Entity, proxy.Body));
}
});
return bodies;
}
@@ -263,72 +287,91 @@ namespace Robust.Shared.Physics.Systems
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);
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);
broadphase.StaticTree.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
_broadphase.GetBroadphases(mapId,
rayBox,
broadphase =>
{
if (returnOnFirstHit && results.Count > 0)
return true;
var (_, rot, matrix, invMatrix) =
_transform.GetWorldPositionRotationMatrixWithInv(broadphase.Owner);
if (distFromOrigin > maxLength)
return true;
var position = Vector2.Transform(ray.Position, invMatrix);
var gridRot = new Angle(-rot.Theta);
var direction = gridRot.RotateVec(ray.Direction);
if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
return true;
var gridRay = new CollisionRay(position, direction, ray.CollisionMask);
if (!proxy.Fixture.Hard)
return true;
broadphase.Comp.StaticTree.QueryRay(
(in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
{
if (returnOnFirstHit && results.Count > 0)
return true;
if (predicate.Invoke(proxy.Entity, state) == true)
return true;
if (distFromOrigin > maxLength)
return true;
// TODO: Shape raycast here
if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
return true;
// Need to convert it back to world-space.
var result = new RayCastResults(distFromOrigin, Vector2.Transform(point, matrix), proxy.Entity);
results.Add(result);
if (!proxy.Fixture.Hard)
return true;
if (predicate.Invoke(proxy.Entity, state) == true)
return true;
// 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);
#if DEBUG
_sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray, maxLength, result, _netMan.IsServer, mapId));
_sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray,
maxLength,
result,
_netMan.IsServer,
mapId));
#endif
return true;
}, gridRay);
return true;
},
gridRay);
broadphase.DynamicTree.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
{
if (returnOnFirstHit && results.Count > 0)
return true;
broadphase.Comp.DynamicTree.QueryRay(
(in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
{
if (returnOnFirstHit && results.Count > 0)
return true;
if (distFromOrigin > maxLength)
return true;
if (distFromOrigin > maxLength)
return true;
if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
return true;
if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
return true;
if (!proxy.Fixture.Hard)
return true;
if (!proxy.Fixture.Hard)
return true;
if (predicate.Invoke(proxy.Entity, state) == true)
return true;
if (predicate.Invoke(proxy.Entity, state) == true)
return true;
// TODO: Shape raycast here
// TODO: Shape raycast here
// Need to convert it back to world-space.
var result = new RayCastResults(distFromOrigin, Vector2.Transform(point, matrix), proxy.Entity);
results.Add(result);
// Need to convert it back to world-space.
var result = new RayCastResults(distFromOrigin,
Vector2.Transform(point, matrix),
proxy.Entity);
results.Add(result);
#if DEBUG
_sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray, maxLength, result, _netMan.IsServer, mapId));
_sharedDebugRaySystem.ReceiveLocalRayFromAnyThread(new(ray,
maxLength,
result,
_netMan.IsServer,
mapId));
#endif
return true;
}, gridRay);
}
return true;
},
gridRay);
});
#if DEBUG
if (results.Count == 0)
@@ -374,54 +417,68 @@ namespace Robust.Shared.Physics.Systems
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);
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);
broadphase.StaticTree.QueryRay((in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
_broadphase.GetBroadphases(mapId,
rayBox,
broadphase =>
{
if (distFromOrigin > maxLength || proxy.Entity == ignoredEnt)
return true;
var (_, rot, invMatrix) = _transform.GetWorldPositionRotationInvMatrix(broadphase);
if (!proxy.Fixture.Hard)
return true;
var position = Vector2.Transform(ray.Position, invMatrix);
var gridRot = new Angle(-rot.Theta);
var direction = gridRot.RotateVec(ray.Direction);
if ((proxy.Fixture.CollisionLayer & ray.CollisionMask) == 0x0)
return true;
var gridRay = new CollisionRay(position, direction, ray.CollisionMask);
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);
broadphase.Comp.StaticTree.QueryRay(
(in FixtureProxy proxy, in Vector2 point, float distFromOrigin) =>
{
if (distFromOrigin > maxLength || proxy.Entity == ignoredEnt)
return true;
broadphase.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();
}
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);
}
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.CollisionLayer & ray.CollisionMask) == 0x0)
return true;
if (new Ray(point + gridRay.Direction * proxy.AABB.Size.Length() * 2, -gridRay.Direction)
.Intersects(
proxy.AABB,
out _,
out var exitPoint))
{
penetration += (point - exitPoint).Length();
}
return true;
},
gridRay);
});
// This hid rays that didn't penetrate something. Don't hide those because that causes rays to disappear that shouldn't.
#if DEBUG

72
Robust.Shared/Physics/Transform.cs
View File

@@ -38,6 +38,12 @@ namespace Robust.Shared.Physics
public Vector2 Position;
public Quaternion2D Quaternion2D;
public Transform(Vector2 position, Quaternion2D quat)
{
Position = position;
Quaternion2D = quat;
}
public Transform(Vector2 position, float angle)
{
Position = position;
@@ -56,6 +62,16 @@ namespace Robust.Shared.Physics
Quaternion2D = new Quaternion2D(angle);
}
/// Inverse transform a point (e.g. world space to local space)
[Pure]
public static Vector2 InvTransformPoint(Transform t, Vector2 p)
{
float vx = p.X - t.Position.X;
float vy = p.Y - t.Position.Y;
return new Vector2(t.Quaternion2D.C * vx + t.Quaternion2D.S * vy, -t.Quaternion2D.S * vx + t.Quaternion2D.C * vy);
}
[Pure]
public static Vector2 Mul(in Transform transform, in Vector2 vector)
{
float x = (transform.Quaternion2D.C * vector.X - transform.Quaternion2D.S * vector.Y) + transform.Position.X;
@@ -64,12 +80,14 @@ namespace Robust.Shared.Physics
return new Vector2(x, y);
}
[Pure]
public static Vector2 MulT(in Vector2[] A, in Vector2 v)
{
DebugTools.Assert(A.Length == 2);
return new Vector2(v.X * A[0].X + v.Y * A[0].Y, v.X * A[1].X + v.Y * A[1].Y);
}
[Pure]
public static Vector2 MulT(in Transform T, in Vector2 v)
{
float px = v.X - T.Position.X;
@@ -81,6 +99,7 @@ namespace Robust.Shared.Physics
}
/// Transpose multiply two rotations: qT * r
[Pure]
public static Quaternion2D MulT(in Quaternion2D q, in Quaternion2D r)
{
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
@@ -93,8 +112,15 @@ namespace Robust.Shared.Physics
return qr;
}
[Pure]
public static Transform InvMulTransforms(in Transform A, in Transform B)
{
return new Transform(Quaternion2D.InvRotateVector(A.Quaternion2D, Vector2.Subtract(B.Position, A.Position)), Quaternion2D.InvMulRot(A.Quaternion2D, B.Quaternion2D));
}
// v2 = A.q' * (B.q * v1 + B.p - A.p)
// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
[Pure]
public static Transform MulT(in Transform A, in Transform B)
{
Transform C = new Transform
@@ -184,5 +210,51 @@ namespace Robust.Shared.Physics
// TODO_ERIN optimize
return new Quaternion2D(MathF.Cos(angle), MathF.Sin(angle));
}
/// Rotate a vector
[Pure]
public static Vector2 RotateVector(Quaternion2D q, Vector2 v )
{
return new Vector2(q.C * v.X - q.S * v.Y, q.S * v.X + q.C * v.Y);
}
/// Inverse rotate a vector
[Pure]
public static Vector2 InvRotateVector(Quaternion2D q, Vector2 v)
{
return new Vector2(q.C * v.X + q.S * v.Y, -q.S * v.X + q.C * v.Y);
}
public bool IsValid()
{
if (float.IsNaN(S ) || float.IsNaN(C))
{
return false;
}
if (float.IsInfinity(S) || float.IsInfinity(C))
{
return false;
}
return IsNormalized();
}
public bool IsNormalized()
{
// larger tolerance due to failure on mingw 32-bit
float qq = S * S + C * C;
return 1.0f - 0.0006f < qq && qq < 1.0f + 0.0006f;
}
[Pure]
public static Quaternion2D InvMulRot(Quaternion2D q, Quaternion2D r)
{
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
// s(q - r) = qc * rs - qs * rc
// c(q - r) = qc * rc + qs * rs
return new Quaternion2D(q.C * r.C + q.S * r.S, q.C * r.S - q.S * r.C);
}
}
}

8
Robust.UnitTesting/Shared/Maths/Angle_Test.cs
View File

@@ -27,6 +27,14 @@ namespace Robust.UnitTesting.Shared.Maths
(0.92387953251128674f, -0.38268343236508978f, Direction.East, -System.Math.PI / 8.0)
};
[Test]
public void TestAngleDegrees()
{
const double degrees = 75d;
var angle = Angle.FromDegrees(degrees);
Assert.That(angle.Degrees, Is.EqualTo(degrees));
}
[Test]
public void TestAngleZero()
{

21
Robust.UnitTesting/Shared/Maths/Matrix3_Test.cs
View File

@@ -12,18 +12,17 @@ namespace Robust.UnitTesting.Shared.Maths
[TestOf(typeof(Matrix3x2))]
public sealed class Matrix3_Test
{
[Test]
public void GetRotationTest()
private static readonly TestCaseData[] Rotations = new TestCaseData[]
{
Assert.That(Matrix3x2.Identity.Rotation(), Is.EqualTo(Angle.Zero));
new(Matrix3x2.Identity, Angle.Zero),
new(Matrix3x2.CreateRotation(MathF.PI / 2f), new Angle(Math.PI / 2)),
new(Matrix3x2.CreateRotation(MathF.PI), new Angle(Math.PI)),
};
var piOver2 = new Angle(Math.PI / 2);
var piOver2Mat = Matrix3Helpers.CreateRotation(piOver2.Theta);
Assert.That(piOver2Mat.Rotation(), Is.EqualTo(piOver2));
var pi = new Angle(Math.PI);
var piMat = Matrix3Helpers.CreateRotation(pi.Theta);
Assert.That(piMat.Rotation(), Is.EqualTo(pi));
[Test, TestCaseSource(nameof(Rotations))]
public void GetRotationTest(Matrix3x2 matrix, Angle angle)
{
Assert.That(angle, Is.EqualTo(matrix.Rotation()));
}
[Test]
@@ -35,7 +34,7 @@ namespace Robust.UnitTesting.Shared.Maths
var origin = new Vector2(0, 0);
var result = Vector2.Transform(origin, matrix);
Assert.That(control == result, Is.True, result.ToString);
Assert.That(control, Is.EqualTo(result), result.ToString);
}
private static readonly IEnumerable<(Vector2, double)> _rotationTests = new[]

145
Robust.UnitTesting/Shared/Physics/RayCast_Test.cs Normal file
View File

@@ -0,0 +1,145 @@
using System.Linq;
using System.Numerics;
using NUnit.Framework;
using Robust.Shared.GameObjects;
using Robust.Shared.Map;
using Robust.Shared.Maths;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Collision.Shapes;
using Robust.Shared.Physics.Components;
using Robust.Shared.Physics.Dynamics;
using Robust.Shared.Physics.Shapes;
using Robust.Shared.Physics.Systems;
using Robust.UnitTesting.Server;
namespace Robust.UnitTesting.Shared.Physics;
[TestFixture]
public sealed class RayCast_Test
{
private static TestCaseData[] _rayCases =
{
// Ray goes through
new(new Vector2(0f, 0.5f), Vector2.UnitY * 2f, new Vector2(0f, 1f - PhysicsConstants.PolygonRadius)),
// Ray stops inside
new(new Vector2(0f, 0.5f), Vector2.UnitY, new Vector2(0f, 1f - PhysicsConstants.PolygonRadius)),
// Ray starts inside
new(new Vector2(0f, 1.5f), Vector2.UnitY, null),
// No hit
new(new Vector2(0f, 0.5f), -Vector2.UnitY, null),
};
private static TestCaseData[] _shapeCases =
{
// Circle
// - Initial overlap, no shapecast
new(new PhysShapeCircle(0.5f, Vector2.Zero), new Transform(Vector2.UnitY / 2f, Angle.Zero), Vector2.UnitY, null),
// - Cast
new(new PhysShapeCircle(0.5f, Vector2.Zero), new Transform(Vector2.Zero, Angle.Zero), Vector2.UnitY, new Vector2(0f, 1f - PhysicsConstants.PolygonRadius)),
// - Miss
new(new PhysShapeCircle(0.5f, Vector2.Zero), new Transform(Vector2.Zero, Angle.Zero), -Vector2.UnitY, null),
// Polygon
// - Initial overlap, no shapecast
new(new Polygon(Box2.UnitCentered), new Transform(Vector2.UnitY / 2f, Angle.Zero), Vector2.UnitY, null),
// - Cast
new(new Polygon(Box2.UnitCentered), new Transform(Vector2.Zero, Angle.Zero), Vector2.UnitY, new Vector2(0.5f, 1f - PhysicsConstants.PolygonRadius)),
// - Miss
new(new Polygon(Box2.UnitCentered), new Transform(Vector2.Zero, Angle.Zero), -Vector2.UnitY, null),
};
[Test, TestCaseSource(nameof(_rayCases))]
public void RayCast(Vector2 origin, Vector2 direction, Vector2? point)
{
var sim = RobustServerSimulation.NewSimulation().RegisterEntitySystems(f =>
{
f.LoadExtraSystemType<RayCastSystem>();
}).InitializeInstance();
Setup(sim, out var mapId);
var raycast = sim.System<RayCastSystem>();
var hits = raycast.CastRayClosest(mapId,
origin,
direction,
new QueryFilter()
{
LayerBits = 1,
});
if (point == null)
{
Assert.That(!hits.Hit);
}
else
{
Assert.That(hits.Results.First().Point, Is.EqualTo(point.Value));
}
}
[Test, TestCaseSource(nameof(_shapeCases))]
public void ShapeCast(IPhysShape shape, Transform origin, Vector2 direction, Vector2? point)
{
var sim = RobustServerSimulation.NewSimulation().RegisterEntitySystems(f =>
{
f.LoadExtraSystemType<RayCastSystem>();
}).InitializeInstance();
Setup(sim, out var mapId);
var raycast = sim.System<RayCastSystem>();
var hits = raycast.CastShape(mapId,
shape,
origin,
direction,
new QueryFilter()
{
LayerBits = 1,
},
RayCastSystem.RayCastAllCallback);
if (point == null)
{
Assert.That(!hits.Hit);
}
else
{
Assert.That(hits.Results.First().Point, Is.EqualTo(point.Value));
}
}
private void Setup(ISimulation sim, out MapId mapId)
{
var entManager = sim.Resolve<IEntityManager>();
var mapSystem = entManager.System<SharedMapSystem>();
sim.System<SharedMapSystem>().CreateMap(out mapId);
var grid = sim.Resolve<IMapManager>().CreateGridEntity(mapId);
for (var i = 0; i < 3; i++)
{
mapSystem.SetTile(grid, new Vector2i(i, 0), new Tile(1));
}
// Spawn a wall in the middle tile.
var wall = entManager.SpawnEntity(null, new EntityCoordinates(grid.Owner, new Vector2(1.5f, 0.5f)));
var physics = entManager.AddComponent<PhysicsComponent>(wall);
var poly = new PolygonShape();
poly.SetAsBox(Box2.UnitCentered);
entManager.System<FixtureSystem>().CreateFixture(wall, "fix1", new Fixture(poly, 1, 1, true));
entManager.System<SharedPhysicsSystem>().SetCanCollide(wall, true, body: physics);
Assert.That(physics.CanCollide);
// Rotate it to be vertical
entManager.System<SharedTransformSystem>().SetLocalRotation(grid.Owner, Angle.FromDegrees(90));
entManager.System<SharedTransformSystem>().SetLocalPosition(grid.Owner, Vector2.UnitX / 2f);
}
}

43
Robust.UnitTesting/Shared/Serialization/TypeSerializers/AngleSerializerTest.cs
View File

@@ -1,4 +1,5 @@
using System.Globalization;
using System;
using System.Globalization;
using NUnit.Framework;
using Robust.Shared.Maths;
using Robust.Shared.Serialization.Manager;
@@ -11,24 +12,46 @@ namespace Robust.UnitTesting.Shared.Serialization.TypeSerializers
[TestOf(typeof(AngleSerializer))]
public sealed class AngleSerializerTest : SerializationTest
{
[Test]
public void SerializationTest()
private static readonly TestCaseData[] _source = new[]
{
var degrees = 75d;
var angle = Angle.FromDegrees(degrees);
new TestCaseData(Math.PI),
new TestCaseData(Math.PI / 2),
new TestCaseData(Math.PI / 4),
new TestCaseData(0.515),
new TestCaseData(75),
};
[Test, TestCaseSource(nameof(_source))]
public void SerializationRadsTest(double radians)
{
var angle = new Angle(radians);
var node = Serialization.WriteValueAs<ValueDataNode>(angle);
var serializedValue = $"{MathHelper.DegreesToRadians(degrees).ToString(CultureInfo.InvariantCulture)} rad";
var serializedValue = $"{radians.ToString(CultureInfo.InvariantCulture)} rad";
Assert.That(node.Value, Is.EqualTo(serializedValue));
}
[Test]
public void DeserializationTest()
[Test, TestCaseSource(nameof(_source))]
public void DeserializationRadsTest(double radians)
{
var degrees = 75;
var node = new ValueDataNode(degrees.ToString());
var angle = new Angle(radians);
var node = new ValueDataNode($"{radians.ToString(CultureInfo.InvariantCulture)} rad");
var deserializedAngle = Serialization.Read<Angle>(node);
Assert.That(deserializedAngle, Is.EqualTo(angle));
}
/*
* Serialization of degrees test won't work because it's comparing degrees to radians.
*/
[Test, TestCaseSource(nameof(_source))]
public void DeserializationDegreesTest(double radians)
{
var degrees = MathHelper.RadiansToDegrees(radians);
var angle = Angle.FromDegrees(degrees);
var node = new ValueDataNode($"{degrees.ToString(CultureInfo.InvariantCulture)}");
var deserializedAngle = Serialization.Read<Angle>(node);
Assert.That(deserializedAngle, Is.EqualTo(angle));
}