Skip to content

Commit

Permalink
cassette
Browse files Browse the repository at this point in the history
  • Loading branch information
@metalgearsloth
metalgearsloth committed Sep 15, 2024
1 parent caf6e90 commit 412940b
Show file tree
Hide file tree
Showing 6 changed files with 846 additions and 80 deletions.
20 changes: 20 additions & 0 deletions Robust.Shared.Maths/Vector2Helpers.cs
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
using System;
using System.Numerics;
using System.Runtime.CompilerServices;
using JetBrains.Annotations;

namespace Robust.Shared.Maths;

Expand All @@ -14,6 +15,19 @@ public static class Vector2Helpers
/// </summary>
public static readonly Vector2 Half = new(0.5f, 0.5f);

public static Vector2 GetLengthAndNormalize(this Vector2 v, ref float length)
{
length = v.Length();
if (length < float.Epsilon)
{
return Vector2.Zero;
}

float invLength = 1.0f / length;
var n = new Vector2(invLength * v.X, invLength * v.Y);
return n;
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static Vector2 InterpolateCubic(Vector2 preA, Vector2 a, Vector2 b, Vector2 postB, float t)
{
Expand Down Expand Up @@ -255,6 +269,12 @@ public static Vector2 Cross(float s, in Vector2 a)
return new(-s * a.Y, s * a.X);
}

[Pure]
public static Vector2 RightPerp(this Vector2 v)
{
return new Vector2(v.Y, -v.X);
}

/// <summary>
/// Perform the cross product on a scalar and a vector. In 2D this produces
/// a vector.
Expand Down
243 changes: 243 additions & 0 deletions Robust.Shared/Physics/Collision/DistanceProxy.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -143,6 +149,13 @@ public Vector2 GetSupportVertex(Vector2 direction)

return Vertices[bestIndex];
}

internal static DistanceProxy MakeProxy(Vector2[] vertices, int count, float radius )
{
count = Math.Min(count, PhysicsConstants.MaxPolygonVertices);
var proxy = new DistanceProxy(vertices[..count], radius);
return proxy;
}
}

/// <summary>
Expand Down Expand Up @@ -306,6 +319,16 @@ internal Vector2 GetSearchDirection()
}
}

public static Vector2 Weight2( float a1, Vector2 w1, float a2, Vector2 w2 )
{
return new Vector2(a1 * w1.X + a2 * w2.X, a1 * w1.Y + a2 * w2.Y);
}

public static Vector2 Weight3(float a1, Vector2 w1, float a2, Vector2 w2, float a3, Vector2 w3 )
{
return new Vector2(a1 * w1.X + a2 * w2.X + a3 * w3.X, a1 * w1.Y + a2 * w2.Y + a3 * w3.Y);
}

internal Vector2 GetClosestPoint()
{
switch (Count)
Expand All @@ -329,6 +352,226 @@ internal Vector2 GetClosestPoint()
}
}

public static Vector2 ComputeSimplexClosestPoint(Simplex s)
{
switch (s.Count)
{
case 0:
DebugTools.Assert(false);
return Vector2.Zero;

case 1:
return s.V._00.W;

case 2:
return Weight2(s.V._00.A, s.V._00.W, s.V._01.A, s.V._01.W);

case 3:
return Vector2.Zero;

default:
DebugTools.Assert(false);
return Vector2.Zero;
}
}

public static void ComputeSimplexWitnessPoints(ref Vector2 a, ref Vector2 b, Simplex s)
{
switch (s.Count)
{
case 0:
DebugTools.Assert(false);
break;

case 1:
a = s.V._00.WA;
b = s.V._00.WB;
break;

case 2:
a = Weight2(s.V._00.A, s.V._00.WA, s.V._01.A, s.V._01.WA);
b = Weight2(s.V._00.A, s.V._00.WB, s.V._01.A, s.V._01.WB);
break;

case 3:
a = Weight3(s.V._00.A, s.V._00.WA, s.V._01.A, s.V._01.WA, s.V._02.A, s.V._02.WA);
// TODO_ERIN why are these not equal?
//*b = b2Weight3(s->v1.a, s->v1.wB, s->v2.a, s->v2.wB, s->v3.a, s->v3.wB);
b = a;
break;

default:
DebugTools.Assert(false);
break;
}
}

// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
public static void SolveSimplex2(Simplex s)
{
var w1 = s.V._00.W;
var w2 = s.V._01.W;
var e12 = Vector2.Subtract(w2, w1);

// w1 region
float d12_2 = -Vector2.Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
s.V._00.A = 1.0f;
s.Count = 1;
return;
}

// w2 region
float d12_1 = Vector2.Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
s.V._01.A = 1.0f;
s.Count = 1;
s.V._00 = s.V._01;
return;
}

// Must be in e12 region.
float inv_d12 = 1.0f / ( d12_1 + d12_2 );
s.V._00.A = d12_1 * inv_d12;
s.V._01.A = d12_2 * inv_d12;
s.Count = 2;
}

public static void SolveSimplex3(Simplex s)
{
var w1 = s.V._00.W;
var w2 = s.V._01.W;
var w3 = s.V._02.W;

// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
var e12 = Vector2.Subtract(w2, w1);
float w1e12 = Vector2.Dot(w1, e12);
float w2e12 = Vector2.Dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;

// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
var e13 = Vector2.Subtract(w3, w1);
float w1e13 = Vector2.Dot(w1, e13);
float w3e13 = Vector2.Dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;

// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
var e23 = Vector2.Subtract(w3, w2);
float w2e23 = Vector2.Dot(w2, e23);
float w3e23 = Vector2.Dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;

// Triangle123
float n123 = Vector2Helpers.Cross(e12, e13);

float d123_1 = n123 * Vector2Helpers.Cross(w2, w3);
float d123_2 = n123 * Vector2Helpers.Cross(w3, w1);
float d123_3 = n123 * Vector2Helpers.Cross(w1, w2);

// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
s.V._00.A = 1.0f;
s.Count = 1;
return;
}

// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
float inv_d12 = 1.0f / ( d12_1 + d12_2 );
s.V._00.A = d12_1 * inv_d12;
s.V._01.A = d12_2 * inv_d12;
s.Count = 2;
return;
}

// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
float inv_d13 = 1.0f / ( d13_1 + d13_2 );
s.V._00.A = d13_1 * inv_d13;
s.V._02.A = d13_2 * inv_d13;
s.Count = 2;
s.V._01 = s.V._02;
return;
}

// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
s.V._01.A = 1.0f;
s.Count = 1;
s.V._00 = s.V._01;
return;
}

// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
s.V._02.A = 1.0f;
s.Count = 1;
s.V._00 = s.V._02;
return;
}

// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
float inv_d23 = 1.0f / ( d23_1 + d23_2 );
s.V._01.A = d23_1 * inv_d23;
s.V._02.A = d23_2 * inv_d23;
s.Count = 2;
s.V._00 = s.V._02;
return;
}

// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
s.V._00.A = d123_1 * inv_d123;
s.V._01.A = d123_2 * inv_d123;
s.V._02.A = d123_3 * inv_d123;
s.Count = 3;
}

internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB)
{
switch (Count)
Expand Down
4 changes: 2 additions & 2 deletions Robust.Shared/Physics/Shapes/Polygon.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,8 @@ public Polygon(Vector2[] vertices)

if (hull.Count < 3)
{
Vertices = Array.Empty<Vector2>();
Normals = Array.Empty<Vector2>();
Vertices = [];
Normals = [];
return;
}

Expand Down
Loading

0 comments on commit 412940b

Please sign in to comment.