space-wizards/RobustToolbox
1
0
Fork 0
mirror of https://github.com/space-wizards/RobustToolbox.git synced 2026-02-14 19:29:36 +01:00
Code Issues Packages Projects Releases Wiki Activity

Box2D updates (#3957)

Browse Source
This commit is contained in:
metalgearsloth
2023-04-22 16:41:05 +10:00
committed by GitHub
parent 62aa9bfb49
commit 733c0e7a45
25 changed files with 1432 additions and 1597 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Robust.Client/Debugging/DebugPhysicsSystem.cs
View File

@@ -467,7 +467,7 @@ namespace Robust.Client.Debugging
break;
case PolygonShape poly:
Span<Vector2> verts = stackalloc Vector2[poly.Vertices.Length];
Span<Vector2> verts = stackalloc Vector2[poly.VertexCount];
for (var i = 0; i < verts.Length; i++)
{

4
Robust.Client/GameObjects/EntitySystems/GridChunkBoundsDebugSystem.cs
View File

@@ -84,9 +84,9 @@ namespace Robust.Client.GameObjects
{
var poly = (PolygonShape) fixture.Shape;
var verts = new Vector2[poly.Vertices.Length];
var verts = new Vector2[poly.VertexCount];
for (var i = 0; i < poly.Vertices.Length; i++)
for (var i = 0; i < poly.VertexCount; i++)
{
verts[i] = Transform.Mul(transform, poly.Vertices[i]);
}

2
Robust.Server/Console/Commands/ScaleCommand.cs
View File

@@ -75,7 +75,7 @@ public sealed class ScaleCommand : LocalizedCommands
case PolygonShape poly:
var verts = poly.Vertices;
for (var i = 0; i < verts.Length; i++)
for (var i = 0; i < poly.VertexCount; i++)
{
verts[i] *= scale;
}

18
Robust.Shared.Maths/Vector2.cs
View File

@@ -79,6 +79,24 @@ namespace Robust.Shared.Maths
get => X * X + Y * Y;
}
/// <summary>
/// Normalizes this vector if its length > 0, otherwise sets it to 0.
/// </summary>
public float Normalize()
{
var length = Length;
if (length < float.Epsilon)
{
return 0f;
}
var invLength = 1f / length;
X *= invLength;
Y *= invLength;
return length;
}
/// <summary>
/// Returns a new, normalized, vector.
/// </summary>

2
Robust.Shared/GameObjects/Systems/SharedGridFixtureSystem.cs
View File

@@ -132,7 +132,7 @@ namespace Robust.Shared.GameObjects
vertices[2] = bounds.TopRight;
vertices[3] = bounds.TopLeft;
poly.SetVertices(vertices, PhysicsConstants.ConvexHulls);
poly.Set(vertices, 4);
var newFixture = new Fixture(
poly,

115
Robust.Shared/Physics/Collision/AlignedRectangle.cs
View File

@@ -1,115 +0,0 @@
using System;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// A rectangle that is always axis-aligned.
/// </summary>
[Serializable]
internal readonly struct AlignedRectangle : IEquatable<AlignedRectangle>
{
/// <summary>
/// Center point of the rectangle in world space.
/// </summary>
public readonly Vector2 Center;
/// <summary>
/// Half of the total width and height of the rectangle.
/// </summary>
public readonly Vector2 HalfExtents;
/// <summary>
/// A 1x1 unit rectangle with the origin centered on the world origin.
/// </summary>
public static readonly AlignedRectangle UnitCentered = new(new Vector2(0.5f, 0.5f));
/// <summary>
/// The lower X coordinate of the left edge of the box.
/// </summary>
public float Left => Center.X - HalfExtents.X;
/// <summary>
/// The higher X coordinate of the right edge of the box.
/// </summary>
public float Right => Center.X + HalfExtents.X;
/// <summary>
/// The lower Y coordinate of the top edge of the box.
/// </summary>
public float Bottom => Center.Y - HalfExtents.Y;
/// <summary>
/// The higher Y coordinate of the bottom of the box.
/// </summary>
public float Top => Center.Y + HalfExtents.Y;
public AlignedRectangle(Box2 box)
{
var halfWidth = box.Width / 2;
var halfHeight = box.Height / 2;
HalfExtents = new Vector2(halfWidth, halfHeight);
Center = new Vector2(box.Left + halfWidth, box.Height + halfHeight);
}
public AlignedRectangle(Vector2 halfExtents)
{
Center = default;
HalfExtents = halfExtents;
}
public AlignedRectangle(Vector2 center, Vector2 halfExtents)
{
Center = center;
HalfExtents = halfExtents;
}
/// <summary>
/// Given a point, returns the closest point to it inside the box.
/// </summary>
public Vector2 ClosestPoint(in Vector2 position)
{
// clamp the point to the border of the box
var cx = MathHelper.Clamp(position.X, Left, Right);
var cy = MathHelper.Clamp(position.Y, Bottom, Top);
return new Vector2(cx, cy);
}
#region Equality members
public bool Equals(AlignedRectangle other)
{
return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents);
}
public override bool Equals(object? obj)
{
return obj is AlignedRectangle other && Equals(other);
}
public override int GetHashCode()
{
return HashCode.Combine(Center, HalfExtents);
}
public static bool operator ==(AlignedRectangle left, AlignedRectangle right) {
return left.Equals(right);
}
public static bool operator !=(AlignedRectangle left, AlignedRectangle right) {
return !left.Equals(right);
}
#endregion
/// <summary>
/// Returns the string representation of this object.
/// </summary>
public override string ToString()
{
return $"({Left}, {Bottom}, {Right}, {Top})";
}
}
}

17
Robust.Shared/Physics/Collision/ClipVertex.cs
View File

@@ -22,14 +22,13 @@
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
namespace Robust.Shared.Physics.Collision;
/// <summary>
/// Used for computing contact manifolds.
/// </summary>
internal record struct ClipVertex
{
/// <summary>
/// Used for computing contact manifolds.
/// </summary>
internal struct ClipVertex
{
public ContactID ID;
public Vector2 V;
}
public ContactID ID;
public Vector2 V;
}

10
Robust.Shared/Physics/Collision/CollisionManager.Polygons.cs
View File

@@ -44,8 +44,8 @@ internal sealed partial class CollisionManager
var n1s = poly1.Normals;
var v1s = poly1.Vertices;
var v2s = poly2.Vertices;
var count1 = v1s.Length;
var count2 = v2s.Length;
var count1 = poly1.VertexCount;
var count2 = poly2.VertexCount;
var xf = Transform.MulT(xf2, xf1);
var bestIndex = 0;
@@ -83,11 +83,11 @@ internal sealed partial class CollisionManager
{
var normals1 = poly1.Normals;
var count2 = poly2.Vertices.Length;
var count2 = poly2.VertexCount;
var vertices2 = poly2.Vertices;
var normals2 = poly2.Normals;
DebugTools.Assert(0 <= edge1 && edge1 < poly1.Vertices.Length);
DebugTools.Assert(0 <= edge1 && edge1 < poly1.VertexCount);
// Get the normal of the reference edge in poly2's frame.
var normal1 = Transform.MulT(xf2.Quaternion2D, Transform.Mul(xf1.Quaternion2D, normals1[edge1]));
@@ -185,7 +185,7 @@ internal sealed partial class CollisionManager
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.Vertices.Length;
int count1 = poly1.VertexCount;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

229
Robust.Shared/Physics/Collision/CollisionManager.cs
View File

@@ -23,148 +23,147 @@
using System;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
namespace Robust.Shared.Physics.Collision;
/// <summary>
/// Handles several collision features: Generating contact manifolds, testing shape overlap,
/// </summary>
internal sealed partial class CollisionManager : IManifoldManager
{
/*
* Farseer had this as a static class with a ThreadStatic DistanceInput
*/
/// <summary>
/// Handles several collision features: Generating contact manifolds, testing shape overlap,
/// Used for debugging contact points.
/// </summary>
internal sealed partial class CollisionManager : IManifoldManager
/// <param name="state1"></param>
/// <param name="state2"></param>
/// <param name="manifold1"></param>
/// <param name="manifold2"></param>
public static void GetPointStates(ref PointState[] state1, ref PointState[] state2, in Manifold manifold1,
in Manifold manifold2)
{
/*
* Farseer had this as a static class with a ThreadStatic DistanceInput
*/
/// <summary>
/// Used for debugging contact points.
/// </summary>
/// <param name="state1"></param>
/// <param name="state2"></param>
/// <param name="manifold1"></param>
/// <param name="manifold2"></param>
public static void GetPointStates(ref PointState[] state1, ref PointState[] state2, in Manifold manifold1,
in Manifold manifold2)
// Detect persists and removes.
for (int i = 0; i < manifold1.PointCount; ++i)
{
// Detect persists and removes.
for (int i = 0; i < manifold1.PointCount; ++i)
ContactID id = manifold1.Points[i].Id;
state1[i] = PointState.Remove;
for (int j = 0; j < manifold2.PointCount; ++j)
{
ContactID id = manifold1.Points[i].Id;
state1[i] = PointState.Remove;
for (int j = 0; j < manifold2.PointCount; ++j)
if (manifold2.Points[j].Id.Key == id.Key)
{
if (manifold2.Points[j].Id.Key == id.Key)
{
state1[i] = PointState.Persist;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2.PointCount; ++i)
{
ContactID id = manifold2.Points[i].Id;
state2[i] = PointState.Add;
for (int j = 0; j < manifold1.PointCount; ++j)
{
if (manifold1.Points[j].Id.Key == id.Key)
{
state2[i] = PointState.Persist;
break;
}
state1[i] = PointState.Persist;
break;
}
}
}
/// <summary>
/// Clipping for contact manifolds.
/// </summary>
/// <param name="vOut">The v out.</param>
/// <param name="vIn">The v in.</param>
/// <param name="normal">The normal.</param>
/// <param name="offset">The offset.</param>
/// <param name="vertexIndexA">The vertex index A.</param>
/// <returns></returns>
private static int ClipSegmentToLine(Span<ClipVertex> vOut, Span<ClipVertex> vIn, Vector2 normal,
float offset, int vertexIndexA)
// Detect persists and adds.
for (int i = 0; i < manifold2.PointCount; ++i)
{
ClipVertex v0 = vIn[0];
ClipVertex v1 = vIn[1];
ContactID id = manifold2.Points[i].Id;
// Start with no output points
int numOut = 0;
state2[i] = PointState.Add;
// Calculate the distance of end points to the line
float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset;
float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
vOut[numOut++] = v0;
if (distance1 <= 0.0f)
vOut[numOut++] = v1;
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
for (int j = 0; j < manifold1.PointCount; ++j)
{
// Find intersection point of edge and plane
var interp = distance0 / (distance0 - distance1);
ref var cv = ref vOut[numOut];
cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X);
cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y);
// VertexA is hitting edgeB.
cv.ID.Features.IndexA = (byte) vertexIndexA;
cv.ID.Features.IndexB = v0.ID.Features.IndexB;
cv.ID.Features.TypeA = (byte) ContactFeatureType.Vertex;
cv.ID.Features.TypeB = (byte) ContactFeatureType.Face;
++numOut;
if (manifold1.Points[j].Id.Key == id.Key)
{
state2[i] = PointState.Persist;
break;
}
}
return numOut;
}
}
/// <summary>
/// This structure is used to keep track of the best separating axis.
/// Clipping for contact manifolds.
/// </summary>
public struct EPAxis
/// <param name="vOut">The v out.</param>
/// <param name="vIn">The v in.</param>
/// <param name="normal">The normal.</param>
/// <param name="offset">The offset.</param>
/// <param name="vertexIndexA">The vertex index A.</param>
/// <returns></returns>
private static int ClipSegmentToLine(Span<ClipVertex> vOut, Span<ClipVertex> vIn, Vector2 normal,
float offset, int vertexIndexA)
{
public int Index;
public float Separation;
public EPAxisType Type;
public Vector2 Normal;
}
ClipVertex v0 = vIn[0];
ClipVertex v1 = vIn[1];
/// <summary>
/// Reference face used for clipping
/// </summary>
public struct ReferenceFace
{
public int i1, i2;
// Start with no output points
int numOut = 0;
public Vector2 v1, v2;
// Calculate the distance of end points to the line
float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset;
float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset;
public Vector2 normal;
// If the points are behind the plane
if (distance0 <= 0.0f)
vOut[numOut++] = v0;
public Vector2 sideNormal1;
public float sideOffset1;
if (distance1 <= 0.0f)
vOut[numOut++] = v1;
public Vector2 sideNormal2;
public float sideOffset2;
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
var interp = distance0 / (distance0 - distance1);
public enum EPAxisType : byte
{
Unknown,
EdgeA,
EdgeB,
ref var cv = ref vOut[numOut];
cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X);
cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y);
// VertexA is hitting edgeB.
cv.ID.Features.IndexA = (byte) vertexIndexA;
cv.ID.Features.IndexB = v0.ID.Features.IndexB;
cv.ID.Features.TypeA = (byte) ContactFeatureType.Vertex;
cv.ID.Features.TypeB = (byte) ContactFeatureType.Face;
++numOut;
}
return numOut;
}
}
/// <summary>
/// This structure is used to keep track of the best separating axis.
/// </summary>
public struct EPAxis
{
public int Index;
public float Separation;
public EPAxisType Type;
public Vector2 Normal;
}
/// <summary>
/// Reference face used for clipping
/// </summary>
public struct ReferenceFace
{
public int i1, i2;
public Vector2 v1, v2;
public Vector2 normal;
public Vector2 sideNormal1;
public float sideOffset1;
public Vector2 sideNormal2;
public float sideOffset2;
}
public enum EPAxisType : byte
{
Unknown,
EdgeA,
EdgeB,
}

27
Robust.Shared/Physics/Collision/DistanceInput.cs
View File

@@ -20,19 +20,18 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
namespace Robust.Shared.Physics.Collision
namespace Robust.Shared.Physics.Collision;
/// <summary>
/// Input for Distance.ComputeDistance().
/// DANGEROUS TO USE DUE TO C# LIMITATIONS, DO NOT USE DIRECTLY EVER I WILL SHED YOU.
/// You have to option to use the shape radii in the computation.
/// </summary>
internal ref struct DistanceInput
{
/// <summary>
/// Input for Distance.ComputeDistance().
/// DANGEROUS TO USE DUE TO C# LIMITATIONS, DO NOT USE DIRECTLY EVER I WILL SHED YOU.
/// You have to option to use the shape radii in the computation.
/// </summary>
internal ref struct DistanceInput
{
public DistanceProxy ProxyA;
public DistanceProxy ProxyB;
public Transform TransformA;
public Transform TransformB;
public bool UseRadii;
}
public DistanceProxy ProxyA;
public DistanceProxy ProxyB;
public Transform TransformA;
public Transform TransformB;
public bool UseRadii;
}

286
Robust.Shared/Physics/Collision/DistanceManager.cs
View File

@@ -23,168 +23,166 @@
using System;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
namespace Robust.Shared.Physics.Collision;
internal static class DistanceManager
{
private const byte MaxGJKIterations = 20;
internal static class DistanceManager
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, in DistanceInput input)
{
private const byte MaxGJKIterations = 20;
cache = new SimplexCache();
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, in DistanceInput input)
/*
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKCalls;
*/
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, in input.TransformA, input.ProxyB, in input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
Span<int> saveA = stackalloc int[3];
Span<int> saveB = stackalloc int[3];
saveA.Clear();
saveB.Clear();
//float distanceSqr1 = Settings.MaxFloat;
var vSpan = simplex.V.AsSpan;
// Main iteration loop.
int iter = 0;
while (iter < MaxGJKIterations)
{
cache = new SimplexCache();
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (var i = 0; i < saveCount; ++i)
{
saveA[i] = vSpan[i].IndexA;
saveB[i] = vSpan[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
throw new ArgumentOutOfRangeException();
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//float distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared < float.Epsilon * float.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vSpan[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(Transform.MulT(input.TransformA.Quaternion2D, -d));
vertex.WA = Transform.Mul(input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexB = input.ProxyB.GetSupport(Transform.MulT(input.TransformB.Quaternion2D, d));
vertex.WB = Transform.Mul(input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
vertex.W = vertex.WB - vertex.WA;
vSpan[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
/*
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKCalls;
++GJKIters;
*/
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, in input.TransformA, input.ProxyB, in input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
Span<int> saveA = stackalloc int[3];
Span<int> saveB = stackalloc int[3];
saveA.Clear();
saveB.Clear();
//float distanceSqr1 = Settings.MaxFloat;
var vSpan = simplex.V.AsSpan;
// Main iteration loop.
int iter = 0;
while (iter < MaxGJKIterations)
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (var i = 0; i < saveCount; ++i)
{
saveA[i] = vSpan[i].IndexA;
saveB[i] = vSpan[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
throw new ArgumentOutOfRangeException();
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//float distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared < float.Epsilon * float.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vSpan[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(Transform.MulT(input.TransformA.Quaternion2D, -d));
vertex.WA = Transform.Mul(input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexB = input.ProxyB.GetSupport(Transform.MulT(input.TransformB.Quaternion2D, d));
vertex.WB = Transform.Mul(input.TransformB, input.ProxyB.Vertices[vertex.IndexB]);
vertex.W = vertex.WB - vertex.WA;
vSpan[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
/*
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKIters;
*/
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length;
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
float rA = input.ProxyA.Radius;
float rB = input.ProxyB.Radius;
break;
}
if (output.Distance > rA + rB && output.Distance > float.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 normal = output.PointB - output.PointA;
normal = normal.Normalized;
output.PointA += normal * rA;
output.PointB -= normal * rB;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = (output.PointA + output.PointB) * 0.5f;
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
// New vertex is ok and needed.
++simplex.Count;
}
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length;
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float rB = input.ProxyB.Radius;
if (output.Distance > rA + rB && output.Distance > float.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 normal = output.PointB - output.PointA;
normal = normal.Normalized;
output.PointA += normal * rA;
output.PointB -= normal * rB;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = (output.PointA + output.PointB) * 0.5f;
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}

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

File diff suppressed because it is too large Load Diff

443
Robust.Shared/Physics/Collision/Manifold.cs
View File

@@ -25,245 +25,244 @@ using System.Runtime.InteropServices;
using Robust.Shared.Localization;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
namespace Robust.Shared.Physics.Collision;
public enum ManifoldType : byte
{
public enum ManifoldType : byte
{
Invalid = 0,
Circles,
FaceA,
FaceB,
}
Invalid = 0,
Circles,
FaceA,
FaceB,
}
internal enum ContactFeatureType : byte
{
Vertex = 0,
Face = 1,
}
internal enum ContactFeatureType : byte
{
Vertex = 0,
Face = 1,
}
/// <summary>
/// The features that intersect to form the contact point
/// This must be 4 bytes or less.
/// </summary>
public struct ContactFeature
{
/// <summary>
/// Feature index on ShapeA
/// </summary>
public byte IndexA;
/// <summary>
/// Feature index on ShapeB
/// </summary>
public byte IndexB;
/// <summary>
/// The feature type on ShapeA
/// </summary>
public byte TypeA;
/// <summary>
/// The feature type on ShapeB
/// </summary>
public byte TypeB;
}
/// <summary>
/// Contact ids to facilitate warm starting.
/// </summary>
[StructLayout(LayoutKind.Explicit)]
public struct ContactID
{
/// <summary>
/// The features that intersect to form the contact point
/// This must be 4 bytes or less.
/// </summary>
public struct ContactFeature
{
/// <summary>
/// Feature index on ShapeA
/// </summary>
public byte IndexA;
/// <summary>
/// Feature index on ShapeB
/// </summary>
public byte IndexB;
/// <summary>
/// The feature type on ShapeA
/// </summary>
public byte TypeA;
/// <summary>
/// The feature type on ShapeB
/// </summary>
public byte TypeB;
}
[FieldOffset(0)]
public ContactFeature Features;
/// <summary>
/// Contact ids to facilitate warm starting.
/// Used to quickly compare contact ids.
/// </summary>
[StructLayout(LayoutKind.Explicit)]
public struct ContactID
[FieldOffset(0)]
public uint Key;
public static bool operator ==(ContactID id, ContactID other)
{
/// <summary>
/// The features that intersect to form the contact point
/// </summary>
[FieldOffset(0)]
public ContactFeature Features;
/// <summary>
/// Used to quickly compare contact ids.
/// </summary>
[FieldOffset(0)]
public uint Key;
public static bool operator ==(ContactID id, ContactID other)
{
return id.Key == other.Key;
}
public static bool operator !=(ContactID id, ContactID other)
{
return !(id == other);
}
public override bool Equals(object? obj)
{
if (obj is not ContactID otherID) return false;
return Key == otherID.Key;
}
public bool Equals(ContactID other)
{
return Key == other.Key;
}
public override int GetHashCode()
{
return Key.GetHashCode();
}
return id.Key == other.Key;
}
/// <summary>
/// A manifold for two touching convex Shapes.
/// Box2D supports multiple types of contact:
/// - Clip point versus plane with radius
/// - Point versus point with radius (circles)
/// The local point usage depends on the manifold type:
/// - ShapeType.Circles: the local center of circleA
/// - SeparationFunction.FaceA: the center of faceA
/// - SeparationFunction.FaceB: the center of faceB
/// Similarly the local normal usage:
/// - ShapeType.Circles: not used
/// - SeparationFunction.FaceA: the normal on polygonA
/// - SeparationFunction.FaceB: the normal on polygonB
/// We store contacts in this way so that position correction can
/// account for movement, which is critical for continuous physics.
/// All contact scenarios must be expressed in one of these types.
/// This structure is stored across time steps, so we keep it small.
/// </summary>
public struct Manifold : IEquatable<Manifold>, IApproxEquatable<Manifold>
public static bool operator !=(ContactID id, ContactID other)
{
public Vector2 LocalNormal;
/// <summary>
/// Usage depends on manifold type.
/// </summary>
public Vector2 LocalPoint;
public int PointCount;
/// <summary>
/// Points of contact, can only be 0 -> 2.
/// </summary>
internal ManifoldPoint[] Points;
public ManifoldType Type;
public bool Equals(Manifold other)
{
if (!(PointCount == other.PointCount &&
Type == other.Type &&
LocalNormal.Equals(other.LocalNormal) &&
LocalPoint.Equals(other.LocalPoint))) return false;
for (var i = 0; i < PointCount; i++)
{
if (!Points[i].Equals(other.Points[i])) return false;
}
return true;
}
public bool EqualsApprox(Manifold other)
{
if (!(PointCount == other.PointCount &&
Type == other.Type &&
LocalNormal.EqualsApprox(other.LocalNormal) &&
LocalPoint.EqualsApprox(other.LocalPoint))) return false;
for (var i = 0; i < PointCount; i++)
{
if (!Points[i].EqualsApprox(other.Points[i])) return false;
}
return true;
}
public bool EqualsApprox(Manifold other, double tolerance)
{
if (!(PointCount == other.PointCount &&
Type == other.Type &&
LocalNormal.EqualsApprox(other.LocalNormal, tolerance) &&
LocalPoint.EqualsApprox(other.LocalPoint, tolerance))) return false;
for (var i = 0; i < PointCount; i++)
{
if (!Points[i].EqualsApprox(other.Points[i], tolerance)) return false;
}
return true;
}
return !(id == other);
}
public struct ManifoldPoint : IEquatable<ManifoldPoint>, IApproxEquatable<ManifoldPoint>
public override bool Equals(object? obj)
{
/// <summary>
/// Unique identifier for the contact point between 2 shapes.
/// </summary>
public ContactID Id;
if (obj is not ContactID otherID) return false;
return Key == otherID.Key;
}
/// <summary>
/// Usage depends on manifold type.
/// </summary>
public Vector2 LocalPoint;
public bool Equals(ContactID other)
{
return Key == other.Key;
}
/// <summary>
/// The non-penetration impulse.
/// </summary>
public float NormalImpulse;
/// <summary>
/// Friction impulse.
/// </summary>
public float TangentImpulse;
public static bool operator ==(ManifoldPoint point, ManifoldPoint other)
{
return point.Id == other.Id &&
point.LocalPoint.Equals(other.LocalPoint) &&
point.NormalImpulse.Equals(other.NormalImpulse) &&
point.TangentImpulse.Equals(other.TangentImpulse);
}
public static bool operator !=(ManifoldPoint point, ManifoldPoint other)
{
return !(point == other);
}
public override bool Equals(object? obj)
{
if (obj is not ManifoldPoint otherManifold) return false;
return this == otherManifold;
}
public bool Equals(ManifoldPoint other)
{
return this == other;
}
public override int GetHashCode()
{
var hashcode = Id.GetHashCode();
hashcode = (hashcode * 397) ^ LocalPoint.GetHashCode();
hashcode = (hashcode * 397) ^ NormalImpulse.GetHashCode();
hashcode = (hashcode * 397) ^ TangentImpulse.GetHashCode();
return hashcode;
}
public bool EqualsApprox(ManifoldPoint other)
{
return Id == other.Id &&
LocalPoint.EqualsApprox(other.LocalPoint) &&
MathHelper.CloseToPercent(NormalImpulse, other.NormalImpulse) &&
MathHelper.CloseToPercent(TangentImpulse, other.TangentImpulse);
}
public bool EqualsApprox(ManifoldPoint other, double tolerance)
{
return Id == other.Id &&
LocalPoint.EqualsApprox(other.LocalPoint, tolerance) &&
MathHelper.CloseToPercent(NormalImpulse, other.NormalImpulse, tolerance) &&
MathHelper.CloseToPercent(TangentImpulse, other.TangentImpulse, tolerance);
}
public override int GetHashCode()
{
return Key.GetHashCode();
}
}
/// <summary>
/// A manifold for two touching convex Shapes.
/// Box2D supports multiple types of contact:
/// - Clip point versus plane with radius
/// - Point versus point with radius (circles)
/// The local point usage depends on the manifold type:
/// - ShapeType.Circles: the local center of circleA
/// - SeparationFunction.FaceA: the center of faceA
/// - SeparationFunction.FaceB: the center of faceB
/// Similarly the local normal usage:
/// - ShapeType.Circles: not used
/// - SeparationFunction.FaceA: the normal on polygonA
/// - SeparationFunction.FaceB: the normal on polygonB
/// We store contacts in this way so that position correction can
/// account for movement, which is critical for continuous physics.
/// All contact scenarios must be expressed in one of these types.
/// This structure is stored across time steps, so we keep it small.
/// </summary>
public struct Manifold : IEquatable<Manifold>, IApproxEquatable<Manifold>
{
public Vector2 LocalNormal;
/// <summary>
/// Usage depends on manifold type.
/// </summary>
public Vector2 LocalPoint;
public int PointCount;
/// <summary>
/// Points of contact, can only be 0 -> 2.
/// </summary>
internal ManifoldPoint[] Points;
public ManifoldType Type;
public bool Equals(Manifold other)
{
if (!(PointCount == other.PointCount &&
Type == other.Type &&
LocalNormal.Equals(other.LocalNormal) &&
LocalPoint.Equals(other.LocalPoint))) return false;
for (var i = 0; i < PointCount; i++)
{
if (!Points[i].Equals(other.Points[i])) return false;
}
return true;
}
public bool EqualsApprox(Manifold other)
{
if (!(PointCount == other.PointCount &&
Type == other.Type &&
LocalNormal.EqualsApprox(other.LocalNormal) &&
LocalPoint.EqualsApprox(other.LocalPoint))) return false;
for (var i = 0; i < PointCount; i++)
{
if (!Points[i].EqualsApprox(other.Points[i])) return false;
}
return true;
}
public bool EqualsApprox(Manifold other, double tolerance)
{
if (!(PointCount == other.PointCount &&
Type == other.Type &&
LocalNormal.EqualsApprox(other.LocalNormal, tolerance) &&
LocalPoint.EqualsApprox(other.LocalPoint, tolerance))) return false;
for (var i = 0; i < PointCount; i++)
{
if (!Points[i].EqualsApprox(other.Points[i], tolerance)) return false;
}
return true;
}
}
public struct ManifoldPoint : IEquatable<ManifoldPoint>, IApproxEquatable<ManifoldPoint>
{
/// <summary>
/// Unique identifier for the contact point between 2 shapes.
/// </summary>
public ContactID Id;
/// <summary>
/// Usage depends on manifold type.
/// </summary>
public Vector2 LocalPoint;
/// <summary>
/// The non-penetration impulse.
/// </summary>
public float NormalImpulse;
/// <summary>
/// Friction impulse.
/// </summary>
public float TangentImpulse;
public static bool operator ==(ManifoldPoint point, ManifoldPoint other)
{
return point.Id == other.Id &&
point.LocalPoint.Equals(other.LocalPoint) &&
point.NormalImpulse.Equals(other.NormalImpulse) &&
point.TangentImpulse.Equals(other.TangentImpulse);
}
public static bool operator !=(ManifoldPoint point, ManifoldPoint other)
{
return !(point == other);
}
public override bool Equals(object? obj)
{
if (obj is not ManifoldPoint otherManifold) return false;
return this == otherManifold;
}
public bool Equals(ManifoldPoint other)
{
return this == other;
}
public override int GetHashCode()
{
var hashcode = Id.GetHashCode();
hashcode = (hashcode * 397) ^ LocalPoint.GetHashCode();
hashcode = (hashcode * 397) ^ NormalImpulse.GetHashCode();
hashcode = (hashcode * 397) ^ TangentImpulse.GetHashCode();
return hashcode;
}
public bool EqualsApprox(ManifoldPoint other)
{
return Id == other.Id &&
LocalPoint.EqualsApprox(other.LocalPoint) &&
MathHelper.CloseToPercent(NormalImpulse, other.NormalImpulse) &&
MathHelper.CloseToPercent(TangentImpulse, other.TangentImpulse);
}
public bool EqualsApprox(ManifoldPoint other, double tolerance)
{
return Id == other.Id &&
LocalPoint.EqualsApprox(other.LocalPoint, tolerance) &&
MathHelper.CloseToPercent(NormalImpulse, other.NormalImpulse, tolerance) &&
MathHelper.CloseToPercent(TangentImpulse, other.TangentImpulse, tolerance);
}
}

213
Robust.Shared/Physics/Collision/OrientedRectangle.cs
View File

@@ -1,213 +0,0 @@
using System;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics.Collision
{
/// <summary>
/// A rectangle that can be rotated.
/// </summary>
[Serializable]
internal readonly struct OrientedRectangle : IEquatable<OrientedRectangle>
{
/// <summary>
/// Center point of the rectangle in world space.
/// </summary>
public readonly Vector2 Center;
/// <summary>
/// Half of the total width and height of the rectangle.
/// </summary>
public readonly Vector2 HalfExtents;
/// <summary>
/// World rotation of the rectangle in radians.
/// </summary>
public readonly float Rotation;
/// <summary>
/// A 1x1 unit box with the origin centered and identity rotation.
/// </summary>
public static readonly OrientedRectangle UnitCentered = new(Vector2.Zero, Vector2.One, 0);
public OrientedRectangle(Box2 worldBox)
{
Center = worldBox.Center;
var hWidth = MathF.Abs(worldBox.Right - worldBox.Left) * 0.5f;
var hHeight = MathF.Abs(worldBox.Bottom - worldBox.Top) * 0.5f;
HalfExtents = new Vector2(hWidth, hHeight);
Rotation = 0;
}
public OrientedRectangle(Vector2 halfExtents)
{
Center = default;
HalfExtents = halfExtents;
Rotation = default;
}
public OrientedRectangle(Vector2 center, Vector2 halfExtents)
{
Center = center;
HalfExtents = halfExtents;
Rotation = default;
}
public OrientedRectangle(Vector2 center, Vector2 halfExtents, float rotation)
{
Center = center;
HalfExtents = halfExtents;
Rotation = rotation;
}
public OrientedRectangle(in Vector2 center, in Box2 localBox, float rotation)
{
Center = center;
HalfExtents = new Vector2(localBox.Width / 2, localBox.Height / 2);
Rotation = rotation;
}
/// <summary>
/// calculates the smallest AABB that will encompass this rectangle. The AABB is in local space.
/// </summary>
public Box2 CalcBoundingBox()
{
var Fi = Rotation;
var CX = Center.X;
var CY = Center.Y;
var WX = HalfExtents.X;
var WY = HalfExtents.Y;
var SF = MathF.Sin(Fi);
var CF = MathF.Cos(Fi);
var NH = MathF.Abs(WX * SF) + MathF.Abs(WY * CF); //boundrect half-height
var NW = MathF.Abs(WX * CF) + MathF.Abs(WY * SF); //boundrect half-width
return new Box2(CX - NW, CY - NH, CX + NW, CY + NH); //draw bound rectangle
}
/// <summary>
/// Tests if a point is contained inside this rectangle.
/// </summary>
/// <param name="point">Point to test.</param>
/// <returns>True if the point is contained inside this rectangle.</returns>
public bool Contains(Vector2 point)
{
// rotate around rectangle center by -rectAngle
var s = MathF.Sin(-Rotation);
var c = MathF.Cos(-Rotation);
// set origin to rect center
var newPoint = point - Center;
// rotate
newPoint = new Vector2(newPoint.X * c - newPoint.Y * s, newPoint.X * s + newPoint.Y * c);
// put origin back
newPoint += Center;
// check if our transformed point is in the rectangle, which is no longer
// rotated relative to the point
var xMin = -HalfExtents.X;
var xMax = HalfExtents.X;
var yMin = -HalfExtents.Y;
var yMax = HalfExtents.Y;
return newPoint.X >= xMin && newPoint.X <= xMax && newPoint.Y >= yMin && newPoint.Y <= yMax;
}
/// <summary>
/// Returns the closest point inside the rectangle to the given point in world space.
/// </summary>
public Vector2 ClosestPointWorld(Vector2 worldPoint)
{
// inverse-transform the sphere's center into the box's local space.
var localPoint = InverseTransformPoint(worldPoint);
var xMin = -HalfExtents.X;
var xMax = HalfExtents.X;
var yMin = -HalfExtents.Y;
var yMax = HalfExtents.Y;
// clamp the point to the border of the box
var cx = MathHelper.Clamp(localPoint.X, xMin, xMax);
var cy = MathHelper.Clamp(localPoint.Y, yMin, yMax);
return TransformPoint(new Vector2(cx, cy));
}
/// <summary>
/// Transforms a point from the rectangle's local space to world space.
/// </summary>
public Vector2 TransformPoint(Vector2 localPoint)
{
var theta = Rotation;
var (x, y) = localPoint;
var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
return new Vector2(dx, dy) + Center;
}
/// <summary>
/// Transforms a point from world space to the rectangle's local space.
/// </summary>
public Vector2 InverseTransformPoint(Vector2 worldPoint)
{
var theta = -Rotation;
var (x, y) = worldPoint + -Center;
var dx = MathF.Cos(theta) * x - MathF.Sin(theta) * y;
var dy = MathF.Sin(theta) * x + MathF.Cos(theta) * y;
return new Vector2(dx, dy);
}
#region Equality Members
/// <inheritdoc />
public bool Equals(OrientedRectangle other)
{
return Center.Equals(other.Center) && HalfExtents.Equals(other.HalfExtents) && Rotation.Equals(other.Rotation);
}
/// <inheritdoc />
public override bool Equals(object? obj)
{
return obj is OrientedRectangle other && Equals(other);
}
/// <inheritdoc />
public override int GetHashCode()
{
return HashCode.Combine(Center, HalfExtents, Rotation);
}
/// <summary>
/// Check for equality by value between two <see cref="OrientedRectangle"/>.
/// </summary>
public static bool operator ==(OrientedRectangle left, OrientedRectangle right) {
return left.Equals(right);
}
/// <summary>
/// Check for inequality by value between two <see cref="OrientedRectangle"/>.
/// </summary>
public static bool operator !=(OrientedRectangle left, OrientedRectangle right) {
return !left.Equals(right);
}
#endregion
/// <summary>
/// Returns the string representation of this object.
/// </summary>
public override string ToString()
{
var box = new Box2(-HalfExtents.X, -HalfExtents.Y, HalfExtents.X, HalfExtents.Y).Translated(Center);
return $"{box}, {Rotation}";
}
}
}

335
Robust.Shared/Physics/Collision/PhysicsHull.cs Normal file
View File

@@ -0,0 +1,335 @@
using System;
using Robust.Shared.Maths;
using Robust.Shared.Utility;
namespace Robust.Shared.Physics;
/// <summary>
/// Convex hull used for poly collision.
/// </summary>
public record struct PhysicsHull()
{
public readonly Vector2[] Points = new Vector2[PhysicsConstants.MaxPolygonVertices];
public int Count;
private static PhysicsHull RecurseHull(Vector2 p1, Vector2 p2, Span<Vector2> ps, int count)
{
PhysicsHull hull = new();
hull.Count = 0;
if (count == 0)
{
return hull;
}
// create an edge vector pointing from p1 to p2
var e = p2 - p1;
e.Normalize();
// discard points left of e and find point furthest to the right of e
var rightPoints = new Vector2[PhysicsConstants.MaxPolygonVertices];
var rightCount = 0;
var bestIndex = 0;
float bestDistance = Vector2.Cross(ps[bestIndex] - p1, e);
if (bestDistance > 0.0f)
{
rightPoints[rightCount++] = ps[bestIndex];
}
for (var i = 1; i < count; ++i)
{
float distance = Vector2.Cross(ps[i] - p1, e);
if (distance > bestDistance)
{
bestIndex = i;
bestDistance = distance;
}
if (distance > 0.0f)
{
rightPoints[rightCount++] = ps[i];
}
}
if (bestDistance < 2.0f * PhysicsConstants.LinearSlop)
{
return hull;
}
var bestPoint = ps[bestIndex];
// compute hull to the right of p1-bestPoint
PhysicsHull hull1 = RecurseHull(p1, bestPoint, rightPoints, rightCount);
// compute hull to the right of bestPoint-p2
PhysicsHull hull2 = RecurseHull(bestPoint, p2, rightPoints, rightCount);
// stich together hulls
for (var i = 0; i < hull1.Count; ++i)
{
hull.Points[hull.Count++] = hull1.Points[i];
}
hull.Points[hull.Count++] = bestPoint;
for (var i = 0; i < hull2.Count; ++i)
{
hull.Points[hull.Count++] = hull2.Points[i];
}
DebugTools.Assert(hull.Count < PhysicsConstants.MaxPolygonVertices);
return hull;
}
// quickhull algorithm
// - merges vertices based on b2_linearSlop
// - removes collinear points using b2_linearSlop
// - returns an empty hull if it fails
public static PhysicsHull ComputeHull(ReadOnlySpan<Vector2> points, int count)
{
PhysicsHull hull = new();
if (count is < 3 or > PhysicsConstants.MaxPolygonVertices)
{
DebugTools.Assert(false);
// check your data
return hull;
}
count = Math.Min(count, PhysicsConstants.MaxPolygonVertices);
Box2 aabb = new Box2(float.MaxValue, float.MaxValue, float.MinValue, float.MinValue);
// Perform aggressive point welding. First point always remains.
// Also compute the bounding box for later.
Span<Vector2> ps = stackalloc Vector2[PhysicsConstants.MaxPolygonVertices];
var n = 0;
const float tolSqr = 16.0f * PhysicsConstants.LinearSlop * PhysicsConstants.LinearSlop;
for (var i = 0; i < count; ++i)
{
aabb.BottomLeft = Vector2.ComponentMin(aabb.BottomLeft, points[i]);
aabb.TopRight = Vector2.ComponentMax(aabb.TopRight, points[i]);
var vi = points[i];
bool unique = true;
for (var j = 0; j < i; ++j)
{
var vj = points[j];
float distSqr = (vj - vi).LengthSquared;
if (distSqr < tolSqr)
{
unique = false;
break;
}
}
if (unique)
{
ps[n++] = vi;
}
}
if (n < 3)
{
// all points very close together, check your data and check your scale
return hull;
}
// Find an extreme point as the first point on the hull
var c = aabb.Center;
var i1 = 0;
float dsq1 = (ps[i1] - c).LengthSquared;
for (var i = 1; i < n; ++i)
{
float dsq = (ps[i] - c).LengthSquared;
if (dsq > dsq1)
{
i1 = i;
dsq1 = dsq;
}
}
// remove p1 from working set
var p1 = ps[i1];
ps[i1] = ps[n - 1];
n = n - 1;
var i2 = 0;
float dsq2 = (ps[i2] - p1).LengthSquared;
for (var i = 1; i < n; ++i)
{
float dsq = (ps[i] - p1).LengthSquared;
if (dsq > dsq2)
{
i2 = i;
dsq2 = dsq;
}
}
// remove p2 from working set
var p2 = ps[i2];
ps[i2] = ps[n - 1];
n = n - 1;
// split the points into points that are left and right of the line p1-p2.
Span<Vector2> rightPoints = stackalloc Vector2[PhysicsConstants.MaxPolygonVertices - 2];
var rightCount = 0;
Span<Vector2> leftPoints = stackalloc Vector2[PhysicsConstants.MaxPolygonVertices - 2];
var leftCount = 0;
var e = p2 - p1;
e.Normalize();
for (var i = 0; i < n; ++i)
{
float d = Vector2.Cross(ps[i] - p1, e);
// slop used here to skip points that are very close to the line p1-p2
if (d >= 2.0f * PhysicsConstants.LinearSlop)
{
rightPoints[rightCount++] = ps[i];
}
else if (d <= -2.0f * PhysicsConstants.LinearSlop)
{
leftPoints[leftCount++] = ps[i];
}
}
// compute hulls on right and left
var hull1 = RecurseHull(p1, p2, rightPoints, rightCount);
var hull2 = RecurseHull(p2, p1, leftPoints, leftCount);
if (hull1.Count == 0 && hull2.Count == 0)
{
// all points collinear
return hull;
}
// stitch hulls together, preserving CCW winding order
hull.Points[hull.Count++] = p1;
for (var i = 0; i < hull1.Count; ++i)
{
hull.Points[hull.Count++] = hull1.Points[i];
}
hull.Points[hull.Count++] = p2;
for (var i = 0; i < hull2.Count; ++i)
{
hull.Points[hull.Count++] = hull2.Points[i];
}
DebugTools.Assert(hull.Count <= PhysicsConstants.MaxPolygonVertices);
// merge collinear
bool searching = true;
while (searching && hull.Count > 2)
{
searching = false;
for (var i = 0; i < hull.Count; ++i)
{
i1 = i;
i2 = (i + 1) % hull.Count;
var i3 = (i + 2) % hull.Count;
p1 = hull.Points[i1];
p2 = hull.Points[i2];
var p3 = hull.Points[i3];
e = p3 - p1;
e.Normalize();
var v = p2 - p1;
float distance = Vector2.Cross(p2 - p1, e);
if (distance <= 2.0f * PhysicsConstants.LinearSlop)
{
// remove midpoint from hull
for (var j = i2; j < hull.Count - 1; ++j)
{
hull.Points[j] = hull.Points[j + 1];
}
hull.Count -= 1;
// continue searching for collinear points
searching = true;
break;
}
}
}
if (hull.Count < 3)
{
// all points collinear, shouldn't be reached since this was validated above
hull.Count = 0;
}
return hull;
}
public static bool ValidateHull(PhysicsHull hull)
{
if (hull.Count < 3 || PhysicsConstants.MaxPolygonVertices < hull.Count)
{
return false;
}
// test that every point is behind every edge
for (var i = 0; i < hull.Count; ++i)
{
// create an edge vector
var i1 = i;
var i2 = i < hull.Count - 1 ? i1 + 1 : 0;
var p = hull.Points[i1];
var e = hull.Points[i2] - p;
e.Normalize();
for (var j = 0; j < hull.Count; ++j)
{
// skip points that subtend the current edge
if (j == i1 || j == i2)
{
continue;
}
float distance = Vector2.Cross(hull.Points[j] - p, e);
if (distance >= 0.0f)
{
return false;
}
}
}
// test for collinear points
for (var i = 0; i < hull.Count; ++i)
{
var i1 = i;
var i2 = (i + 1) % hull.Count;
var i3 = (i + 2) % hull.Count;
var p1 = hull.Points[i1];
var p2 = hull.Points[i2];
var p3 = hull.Points[i3];
var e = p3 - p1;
e.Normalize();
var v = p2 - p1;
float distance = Vector2.Cross(p2 - p1, e);
if (distance <= PhysicsConstants.LinearSlop)
{
// p1-p2-p3 are collinear
return false;
}
}
return true;
}
}

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

@@ -37,14 +37,16 @@ namespace Robust.Shared.Physics.Collision.Shapes
[DataDefinition]
public sealed class PolygonShape : IPhysShape, ISerializationHooks, IEquatable<PolygonShape>, IApproxEquatable<PolygonShape>
{
[ViewVariables]
public int VertexCount => Vertices.Length;
[DataField("vertexCount")]
public int VertexCount;
[DataField("vertices"), Access(typeof(SharedPhysicsSystem), Friend = AccessPermissions.ReadWriteExecute, Other = AccessPermissions.Read)]
public Vector2[] Vertices = Array.Empty<Vector2>();
[DataField("vertices"),
Access(typeof(SharedPhysicsSystem), Friend = AccessPermissions.ReadWriteExecute,
Other = AccessPermissions.Read)]
public Vector2[] Vertices = new Vector2[PhysicsConstants.MaxPolygonVertices];
[ViewVariables, Access(typeof(SharedPhysicsSystem), Friend = AccessPermissions.ReadWriteExecute, Other = AccessPermissions.Read)]
public Vector2[] Normals = Array.Empty<Vector2>();
public Vector2[] Normals = new Vector2[PhysicsConstants.MaxPolygonVertices];
[ViewVariables, Access(typeof(SharedPhysicsSystem), Friend = AccessPermissions.ReadWriteExecute, Other = AccessPermissions.Read)]
internal Vector2 Centroid { get; set; } = Vector2.Zero;
@@ -57,7 +59,7 @@ namespace Robust.Shared.Physics.Collision.Shapes
[DataField("radius"), Access(typeof(SharedPhysicsSystem), Friend = AccessPermissions.ReadWriteExecute, Other = AccessPermissions.Read)]
public float Radius { get; set; } = PhysicsConstants.PolygonRadius;
public void SetVertices(List<Vector2> vertices)
public bool Set(List<Vector2> vertices)
{
Span<Vector2> verts = stackalloc Vector2[vertices.Count];
@@ -66,42 +68,33 @@ namespace Robust.Shared.Physics.Collision.Shapes
verts[i] = vertices[i];
}
SetVertices(verts);
return Set(verts, vertices.Count);
}
public void SetVertices(Span<Vector2> vertices)
public bool Set(ReadOnlySpan<Vector2> vertices, int count)
{
DebugTools.Assert(vertices.Length >= 3 && vertices.Length <= PhysicsConstants.MaxPolygonVertices);
SetVertices(vertices, PhysicsConstants.ConvexHulls);
DebugTools.Assert(count is >= 3 and <= PhysicsConstants.MaxPolygonVertices);
var hull = PhysicsHull.ComputeHull(vertices, count);
if (hull.Count < 3)
{
return false;
}
Set(hull);
return true;
}
public void SetVertices(Span<Vector2> vertices, bool convexHulls)
public void Set(PhysicsHull hull)
{
var vertexCount = vertices.Length;
DebugTools.Assert(hull.Count >= 3);
var vertexCount = VertexCount = hull.Count;
if (convexHulls)
for (var i = 0; i < vertexCount; i++)
{
//FPE note: This check is required as the GiftWrap algorithm early exits on triangles
//So instead of giftwrapping a triangle, we just force it to be clock wise.
if (vertexCount <= 3)
Vertices = Physics.Vertices.ForceCounterClockwise(vertices);
else
Vertices = GiftWrap.SetConvexHull(vertices);
Vertices[i] = hull.Points[i];
}
else
{
Array.Resize(ref Vertices, vertexCount);
for (var i = 0; i < vertices.Length; i++)
{
Vertices[i] = vertices[i];
}
}
// Convex hull may prune some vertices hence the count may change by this point.
vertexCount = Vertices.Length;
Array.Resize(ref Normals, vertexCount);
// Compute normals. Ensure the edges have non-zero length.
for (var i = 0; i < vertexCount; i++)
@@ -110,15 +103,29 @@ namespace Robust.Shared.Physics.Collision.Shapes
var edge = Vertices[next] - Vertices[i];
DebugTools.Assert(edge.LengthSquared > float.Epsilon * float.Epsilon);
//FPE optimization: Normals.Add(MathHelper.Cross(edge, 1.0f));
var temp = new Vector2(edge.Y, -edge.X);
var temp = Vector2.Cross(edge, 1f);
Normals[i] = temp.Normalized;
}
// TODO: Updates (network etc)
Centroid = ComputeCentroid(Vertices, VertexCount);
}
public bool Validate()
{
var count = VertexCount;
if (count is < 3 or > PhysicsConstants.MaxPolygonVertices)
return false;
var hull = new PhysicsHull();
for (var i = 0; i < count; i++)
{
hull.Points[i] = Vertices[i];
}
hull.Count = count;
return PhysicsHull.ValidateHull(hull);
}
private static Vector2 ComputeCentroid(Vector2[] vs, int count)
{
DebugTools.Assert(count >= 3);
@@ -170,15 +177,14 @@ namespace Robust.Shared.Physics.Collision.Shapes
void ISerializationHooks.AfterDeserialization()
{
SetVertices(Vertices);
DebugTools.Assert(Physics.Vertices.IsCounterClockwise(Vertices.AsSpan()));
// TODO: Someday don't need this.
var span = Vertices.AsSpan();
Set(span, span.Length);
}
public void SetAsBox(float halfWidth, float halfHeight)
{
Array.Resize(ref Vertices, 4);
Array.Resize(ref Normals, 4);
VertexCount = 4;
Vertices[0] = new Vector2(-halfWidth, -halfHeight);
Vertices[1] = new Vector2(halfWidth, -halfHeight);
@@ -195,14 +201,13 @@ namespace Robust.Shared.Physics.Collision.Shapes
public void SetAsBox(float halfWidth, float halfHeight, Vector2 center, float angle)
{
Vertices = new Vector2[4];
Normals = new Vector2[4];
// Damn normies
VertexCount = 4;
Vertices[0] = new Vector2(-halfWidth, -halfHeight);
Vertices[1] = new Vector2(halfWidth, -halfHeight);
Vertices[2] = new Vector2(halfWidth, halfHeight);
Vertices[3] = new Vector2(-halfWidth, halfHeight);
Normals[0] = new Vector2(0f, -1f);
Normals[1] = new Vector2(1f, 0f);
Normals[2] = new Vector2(0f, 1f);
@@ -225,8 +230,8 @@ namespace Robust.Shared.Physics.Collision.Shapes
public bool Equals(IPhysShape? other)
{
if (other is not PolygonShape poly) return false;
if (Vertices.Length != poly.Vertices.Length) return false;
for (var i = 0; i < Vertices.Length; i++)
if (VertexCount != poly.VertexCount) return false;
for (var i = 0; i < VertexCount; i++)
{
var vert = Vertices[i];
if (!vert.Equals(poly.Vertices[i])) return false;
@@ -242,9 +247,9 @@ namespace Robust.Shared.Physics.Collision.Shapes
public bool EqualsApprox(PolygonShape other, double tolerance)
{
if (Vertices.Length != other.Vertices.Length || !MathHelper.CloseTo(Radius, other.Radius, tolerance)) return false;
if (VertexCount != other.VertexCount || !MathHelper.CloseTo(Radius, other.Radius, tolerance)) return false;
for (var i = 0; i < Vertices.Length; i++)
for (var i = 0; i < VertexCount; i++)
{
if (!Vertices[i].EqualsApprox(other.Vertices[i], tolerance)) return false;
}
@@ -258,7 +263,7 @@ namespace Robust.Shared.Physics.Collision.Shapes
var lower = Transform.Mul(transform, Vertices[0]);
var upper = lower;
for (var i = 1; i < Vertices.Length; ++i)
for (var i = 1; i < VertexCount; ++i)
{
var v = Transform.Mul(transform, Vertices[i]);
lower = Vector2.ComponentMin(lower, v);
@@ -323,7 +328,7 @@ namespace Robust.Shared.Physics.Collision.Shapes
public override int GetHashCode()
{
return HashCode.Combine(Vertices, Radius);
return HashCode.Combine(VertexCount, Vertices.AsSpan(0, VertexCount).ToArray(), Radius);
}
}
}

3
Robust.Shared/Physics/Dynamics/Fixture.cs
View File

@@ -145,9 +145,8 @@ namespace Robust.Shared.Physics.Dynamics
verts[1] = bounds.BottomRight;
verts[2] = bounds.TopRight;
verts[3] = bounds.TopLeft;
poly.SetVertices(verts);
poly.Set(verts, 4);
Shape = poly;
DebugTools.Assert(Vertices.IsCounterClockwise(poly.Vertices));
}
}

115
Robust.Shared/Physics/GiftWrap.cs
View File

@@ -1,115 +0,0 @@
/*
* Farseer Physics Engine:
* Copyright (c) 2012 Ian Qvist
*
* Original source Box2D:
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using Robust.Shared.Maths;
namespace Robust.Shared.Physics
{
/// <summary>
/// Giftwrap convex hull algorithm.
/// O(nh) time complexity, where n is the number of points and h is the number of points on the convex hull.
///
/// See http://en.wikipedia.org/wiki/Gift_wrapping_algorithm for more details.
/// </summary>
public static class GiftWrap
{
//Extracted from Box2D
/// <summary>
/// Sets the convex hull from the given vertices.
/// </summary>
/// <param name="vertices">The vertices.</param>
public static Vector2[] SetConvexHull(Span<Vector2> vertices)
{
if (vertices.Length <= 3)
{
return vertices.ToArray();
}
// Find the right most point on the hull
int i0 = 0;
float x0 = vertices[0].X;
for (int i = 1; i < vertices.Length; ++i)
{
float x = vertices[i].X;
if (x > x0 || (MathHelper.CloseToPercent(x, x0) && vertices[i].Y < vertices[i0].Y))
{
i0 = i;
x0 = x;
}
}
Span<int> hull = stackalloc int[vertices.Length];
int m = 0;
int ih = i0;
for (; ; )
{
hull[m] = ih;
int ie = 0;
for (int j = 1; j < vertices.Length; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
Vector2 r = vertices[ie] - vertices[hull[m]];
Vector2 v = vertices[j] - vertices[hull[m]];
float c = Vector2.Cross(r, v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared > r.LengthSquared)
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
var result = new Vector2[m];
// Copy vertices.
for (var i = 0; i < m; ++i)
{
result[i] = vertices[hull[i]];
}
return result;
}
}
}

10
Robust.Shared/Physics/PhysicsConstants.cs
View File

@@ -23,15 +23,7 @@ namespace Robust.Shared.Physics
/// Minimum buffer distance for angles.
/// </summary>
public const float AngularSlop = 2.0f / 180.0f * MathF.PI;
/// <summary>
/// If true, it will run a GiftWrap convex hull on all polygon inputs.
/// This makes for a more stable engine when given random input,
/// but if speed of the creation of polygons are more important,
/// you might want to set this to false.
/// </summary>
public const bool ConvexHulls = true;
public const byte MaxPolygonVertices = 8;
public const float DefaultContactFriction = 0.4f;

7
Robust.Shared/Physics/Systems/FixtureSystem.Shapes.cs
View File

@@ -28,7 +28,7 @@ namespace Robust.Shared.Physics.Systems
case PolygonShape poly:
var pLocal = Physics.Transform.MulT(xform.Quaternion2D, worldPoint - xform.Position);
for (var i = 0; i < poly.Vertices.Length; i++)
for (var i = 0; i < poly.VertexCount; i++)
{
var dot = Vector2.Dot(poly.Normals[i], pLocal - poly.Vertices[i]);
if (dot > 0f) return false;
@@ -89,7 +89,7 @@ namespace Robust.Shared.Physics.Systems
//
// The rest of the derivation is handled by computer algebra.
var count = poly.Vertices.Length;
var count = poly.VertexCount;
DebugTools.Assert(count >= 3);
Vector2 center = new(0.0f, 0.0f);
@@ -166,6 +166,7 @@ namespace Robust.Shared.Physics.Systems
break;
case PhysShapeAabb aabb:
var polygon = (PolygonShape) aabb;
polygon.VertexCount = 4;
GetMassData(polygon, ref data, density);
break;
case PolygonShape poly:
@@ -193,7 +194,7 @@ namespace Robust.Shared.Physics.Systems
//
// The rest of the derivation is handled by computer algebra.
var count = poly.Vertices.Length;
var count = poly.VertexCount;
DebugTools.Assert(count >= 3);
Vector2 center = new(0.0f, 0.0f);

14
Robust.Shared/Physics/Systems/FixtureSystem.cs
View File

@@ -187,8 +187,13 @@ namespace Robust.Shared.Physics.Systems
_lookup.DestroyProxies(fixture, xform, broadphase, physicsMap);
}
if (updates)
FixtureUpdate(uid, manager: manager, body: body);
{
var resetMass = fixture.Density > 0f;
FixtureUpdate(uid, resetMass: resetMass, manager: manager, body: body);
}
}
#endregion
@@ -219,7 +224,7 @@ namespace Robust.Shared.Physics.Systems
}
// Make sure all the right stuff is set on the body
FixtureUpdate(uid, false, component, body);
FixtureUpdate(uid, dirty: false, manager: component, body: body);
}
}
@@ -347,7 +352,7 @@ namespace Robust.Shared.Physics.Systems
/// <summary>
/// Updates all of the cached physics information on the body derived from fixtures.
/// </summary>
public void FixtureUpdate(EntityUid uid, bool dirty = true, FixturesComponent? manager = null, PhysicsComponent? body = null)
public void FixtureUpdate(EntityUid uid, bool dirty = true, bool resetMass = true, FixturesComponent? manager = null, PhysicsComponent? body = null)
{
if (!Resolve(uid, ref body, ref manager))
return;
@@ -363,7 +368,8 @@ namespace Robust.Shared.Physics.Systems
hard |= fixture.Hard;
}
_physics.ResetMassData(uid, manager, body);
if (resetMass)
_physics.ResetMassData(uid, manager, body);
// Normally this method is called when fixtures need to be dirtied anyway so no point in returning early I think
body.CollisionMask = mask;

97
Robust.Shared/Physics/Systems/SharedPhysicsSystem.Shapes.cs
View File

@@ -10,8 +10,6 @@ namespace Robust.Shared.Physics.Systems;
public abstract partial class SharedPhysicsSystem
{
private bool _convexHulls;
public void SetRadius(
EntityUid uid,
Fixture fixture,
@@ -34,7 +32,7 @@ public abstract partial class SharedPhysicsSystem
_lookup.CreateProxies(xform, fixture);
}
Dirty(manager);
_fixtures.FixtureUpdate(uid, manager: manager, body: body);
}
#region Circle
@@ -124,7 +122,7 @@ public abstract partial class SharedPhysicsSystem
_lookup.CreateProxies(xform, fixture);
}
Dirty(manager);
_fixtures.FixtureUpdate(uid, manager: manager, body: body);
}
#endregion
@@ -140,89 +138,20 @@ public abstract partial class SharedPhysicsSystem
PhysicsComponent? body = null,
TransformComponent? xform = null)
{
if (vertices.Length > PhysicsConstants.MaxPolygonVertices)
if (!Resolve(uid, ref manager, ref body, ref xform))
return;
poly.Set(vertices, vertices.Length);
if (body.CanCollide &&
TryComp<BroadphaseComponent>(xform.Broadphase?.Uid, out var broadphase) &&
TryComp<PhysicsMapComponent>(xform.MapUid, out var physicsMap))
{
throw new InvalidOperationException(
$"Tried to set too many vertices of {vertices.Length} for {ToPrettyString(uid)}!");
_lookup.DestroyProxies(fixture, xform, broadphase, physicsMap);
_lookup.CreateProxies(xform, fixture);
}
var vertexCount = vertices.Length;
if (_convexHulls)
{
//FPE note: This check is required as the GiftWrap algorithm early exits on triangles
//So instead of giftwrapping a triangle, we just force it to be clock wise.
if (vertexCount <= 3)
poly.Vertices = Vertices.ForceCounterClockwise(vertices.AsSpan());
else
poly.Vertices = GiftWrap.SetConvexHull(vertices.AsSpan());
}
else
{
Array.Resize(ref poly.Vertices, vertexCount);
for (var i = 0; i < vertices.Length; i++)
{
poly.Vertices[i] = vertices[i];
}
}
// Convex hull may prune some vertices hence the count may change by this point.
vertexCount = poly.Vertices.Length;
Array.Resize(ref poly.Normals, vertexCount);
// Compute normals. Ensure the edges have non-zero length.
for (var i = 0; i < vertexCount; i++)
{
var next = i + 1 < vertexCount ? i + 1 : 0;
var edge = poly.Vertices[next] - poly.Vertices[i];
DebugTools.Assert(edge.LengthSquared > float.Epsilon * float.Epsilon);
//FPE optimization: Normals.Add(MathHelper.Cross(edge, 1.0f));
var temp = new Vector2(edge.Y, -edge.X);
poly.Normals[i] = temp.Normalized;
}
poly.Centroid = ComputeCentroid(poly.Vertices, vertexCount);
}
private Vector2 ComputeCentroid(Vector2[] vs, int count)
{
DebugTools.Assert(count >= 3);
var c = new Vector2(0.0f, 0.0f);
float area = 0.0f;
// Get a reference point for forming triangles.
// Use the first vertex to reduce round-off errors.
var s = vs[0];
const float inv3 = 1.0f / 3.0f;
for (var i = 0; i < count; ++i)
{
// Triangle vertices.
var p1 = vs[0] - s;
var p2 = vs[i] - s;
var p3 = i + 1 < count ? vs[i+1] - s : vs[0] - s;
var e1 = p2 - p1;
var e2 = p3 - p1;
float D = Vector2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += (p1 + p2 + p3) * triangleArea * inv3;
}
// Centroid
DebugTools.Assert(area > float.Epsilon);
c = c * (1.0f / area) + s;
return c;
_fixtures.FixtureUpdate(uid, manager: manager, body: body);
}
#endregion

2
Robust.UnitTesting/Shared/Physics/Collision_Test.cs
View File

@@ -65,7 +65,7 @@ public sealed class Collision_Test
vertices[3] = new Vector2(center.X + hx, center.Y + hy);
PolygonShape polygon2 = new();
polygon2.SetVertices(vertices, true);
polygon2.Set(vertices, 4);
Assert.That(Math.Abs(polygon2.Centroid.X - center.X), Is.LessThan(absTol + relTol * Math.Abs(center.X)));
Assert.That(Math.Abs(polygon2.Centroid.Y - center.Y), Is.LessThan(absTol + relTol * Math.Abs(center.Y)));

6
Robust.UnitTesting/Shared/Physics/Shape_Test.cs
View File

@@ -1,4 +1,5 @@
using System;
using System.Collections.Generic;
using NUnit.Framework;
using Robust.Shared.Maths;
using Robust.Shared.Physics.Collision.Shapes;
@@ -21,9 +22,8 @@ namespace Robust.UnitTesting.Shared.Physics
verts[2] = new Vector2(-1f, 1f);
verts[3] = new Vector2(-1f, -1f);
poly.SetVertices(verts);
Assert.That(poly.Normals.Length, Is.EqualTo(4));
poly.Set(verts, 4);
Assert.That(poly.VertexCount == 4);
Assert.That(poly.Normals[0], Is.EqualTo(new Vector2(1, 0)), $"Vert is {poly.Vertices[0]}");
Assert.That(poly.Normals[1], Is.EqualTo(new Vector2(0, 1)), $"Vert is {poly.Vertices[1]}");

2
Robust.UnitTesting/Shared/Physics/Stack_Test.cs
View File

@@ -100,7 +100,7 @@ public sealed class PhysicsTestBedTest : RobustIntegrationTest
physSystem.SetBodyType(boxUid, BodyType.Dynamic, manager: manager, body: box);
var poly = new PolygonShape(0.001f);
poly.SetVertices(new List<Vector2>()
poly.Set(new List<Vector2>()
{
new(0.5f, -0.5f),
new(0.5f, 0.5f),