Skip to content

Commit

Permalink
Improve ConvexPolytope (#92)
Browse files Browse the repository at this point in the history
  • Loading branch information
@notgiven688
notgiven688 authored Feb 2, 2024
1 parent dcdeceb commit b1e3f35
Show file tree
Hide file tree
Showing 3 changed files with 66 additions and 46 deletions.
89 changes: 50 additions & 39 deletions src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,8 +92,8 @@ public static bool Equals(in Edge a, in Edge b)
private const int MaxVertices = 100;
private const int MaxTriangles = 3 * MaxVertices;

public Triangle* Triangles;
public Vertex* Vertices;
private Triangle* triangles;
private Vertex* vertices;

private short tCount;
private short vPointer;
Expand All @@ -102,10 +102,18 @@ public static bool Equals(in Edge a, in Edge b)

private JVector center;

public Span<Triangle> HullTriangles => new Span<Triangle>(triangles, tCount);

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public ref ConvexPolytope.Vertex GetVertex(int index)
{
System.Diagnostics.Debug.Assert(index < MaxTriangles, "Out of bounds.");
return ref vertices[index];
}

/// <summary>
/// Indicates whether the origin is enclosed within the polyhedron.
/// Important: For accurate results, call this method only after invoking <see cref="GetClosestTriangle"/>.
/// The return value may be invalidated by subsequent calls to <see cref="AddVertex"/> or <see cref="AddPoint"/>.
/// Important: This property returns correct values after invoking <see cref="GetClosestTriangle"/>.
/// </summary>
public readonly bool OriginEnclosed => originEnclosed;

Expand All @@ -116,17 +124,17 @@ public static bool Equals(in Edge a, in Edge b)
public void CalculatePoints(in Triangle ctri, out JVector pA, out JVector pB)
{
CalcBarycentric(ctri, out JVector bc, !originEnclosed);
pA = bc.X * Vertices[ctri.A].A + bc.Y * Vertices[ctri.B].A + bc.Z * Vertices[ctri.C].A;
pB = bc.X * Vertices[ctri.A].B + bc.Y * Vertices[ctri.B].B + bc.Z * Vertices[ctri.C].B;
pA = bc.X * vertices[ctri.A].A + bc.Y * vertices[ctri.B].A + bc.Z * vertices[ctri.C].A;
pB = bc.X * vertices[ctri.A].B + bc.Y * vertices[ctri.B].B + bc.Z * vertices[ctri.C].B;
}

private bool CalcBarycentric(in Triangle tri, out JVector result, bool clamp = false)
{
bool clamped = false;

JVector a = Vertices[tri.A].V;
JVector b = Vertices[tri.B].V;
JVector c = Vertices[tri.C].V;
JVector a = vertices[tri.A].V;
JVector b = vertices[tri.B].V;
JVector c = vertices[tri.C].V;

// Calculate the barycentric coordinates of the origin (0,0,0) projected
// onto the plane of the triangle.
Expand Down Expand Up @@ -222,20 +230,20 @@ private readonly bool IsLit(int candidate, int w)
{
// Checks if the triangle would be lit, if there would
// be a light at the origin.
ref Triangle tr = ref Triangles[candidate];
JVector deltaA = Vertices[w].V - Vertices[tr.A].V;
ref Triangle tr = ref triangles[candidate];
JVector deltaA = vertices[w].V - vertices[tr.A].V;
return JVector.Dot(deltaA, tr.Normal) > 0;
}

private bool CreateTriangle(short a, short b, short c)
{
ref Triangle triangle = ref Triangles[tCount];
ref Triangle triangle = ref triangles[tCount];
triangle.A = a;
triangle.B = b;
triangle.C = c;

JVector.Subtract(Vertices[a].V, Vertices[b].V, out JVector u);
JVector.Subtract(Vertices[a].V, Vertices[c].V, out JVector v);
JVector.Subtract(vertices[a].V, vertices[b].V, out JVector u);
JVector.Subtract(vertices[a].V, vertices[c].V, out JVector v);
JVector.Cross(u, v, out triangle.Normal);
triangle.NormalSq = triangle.Normal.LengthSquared();

Expand All @@ -246,21 +254,21 @@ private bool CreateTriangle(short a, short b, short c)
}

// do we need to flip the triangle? (the origin of the md has to be enclosed)
float delta = JVector.Dot(triangle.Normal, Vertices[a].V - center);
float delta = JVector.Dot(triangle.Normal, vertices[a].V - center);

if (delta < 0)
{
(triangle.A, triangle.B) = (triangle.B, triangle.A);
triangle.Normal.Negate();
}

delta = JVector.Dot(triangle.Normal, Vertices[a].V);
delta = JVector.Dot(triangle.Normal, vertices[a].V);
triangle.FacingOrigin = delta >= 0.0f;

if (!originEnclosed && CalcBarycentric(triangle, out JVector bc, true))
{
triangle.ClosestToOrigin = bc.X * Vertices[triangle.A].V + bc.Y * Vertices[triangle.B].V +
bc.Z * Vertices[triangle.C].V;
triangle.ClosestToOrigin = bc.X * vertices[triangle.A].V + bc.Y * vertices[triangle.B].V +
bc.Z * vertices[triangle.C].V;
triangle.ClosestToOriginSq = triangle.ClosestToOrigin.LengthSquared();
}
else
Expand All @@ -286,28 +294,28 @@ public ref Triangle GetClosestTriangle()

for (int i = 0; i < tCount; i++)
{
if (Triangles[i].ClosestToOriginSq < currentMin)
if (triangles[i].ClosestToOriginSq < currentMin)
{
currentMin = Triangles[i].ClosestToOriginSq;
currentMin = triangles[i].ClosestToOriginSq;
closestIndex = i;
}

if (!Triangles[i].FacingOrigin) originEnclosed = false;
if (!triangles[i].FacingOrigin) originEnclosed = false;
}

return ref Triangles[closestIndex];
return ref triangles[closestIndex];
}

/// <summary>
/// Initializes the structure with a tetrahedron formed using the first four vertices in the <see cref="Vertices"/> array.
/// Initializes the structure with a tetrahedron formed using the first four vertices in the <see cref="vertices"/> array.
/// </summary>
public void InitTetrahedron()
{
originEnclosed = false;
vPointer = 3;
vPointer = 4;
tCount = 0;

center = 0.25f * (Vertices[0].V + Vertices[1].V + Vertices[2].V + Vertices[3].V);
center = 0.25f * (vertices[0].V + vertices[1].V + vertices[2].V + vertices[3].V);

CreateTriangle(0, 2, 1);
CreateTriangle(0, 1, 3);
Expand All @@ -321,15 +329,15 @@ public void InitTetrahedron()
public void InitTetrahedron(in JVector point)
{
originEnclosed = false;
vPointer = 3;
vPointer = 4;
tCount = 0;
center = point;

const float scale = 1e-2f; // minkowski sums not allowed to be thinner
Vertices[0] = new Vertex(center + scale * new JVector(MathF.Sqrt(8.0f / 9.0f), 0.0f, -1.0f / 3.0f));
Vertices[1] = new Vertex(center + scale * new JVector(-MathF.Sqrt(2.0f / 9.0f), MathF.Sqrt(2.0f / 3.0f), -1.0f / 3.0f));
Vertices[2] = new Vertex(center + scale * new JVector(-MathF.Sqrt(2.0f / 9.0f), -MathF.Sqrt(2.0f / 3.0f), -1.0f / 3.0f));
Vertices[3] = new Vertex(center + scale * new JVector(0.0f, 0.0f, 1.0f));
vertices[0] = new Vertex(center + scale * new JVector(MathF.Sqrt(8.0f / 9.0f), 0.0f, -1.0f / 3.0f));
vertices[1] = new Vertex(center + scale * new JVector(-MathF.Sqrt(2.0f / 9.0f), MathF.Sqrt(2.0f / 3.0f), -1.0f / 3.0f));
vertices[2] = new Vertex(center + scale * new JVector(-MathF.Sqrt(2.0f / 9.0f), -MathF.Sqrt(2.0f / 3.0f), -1.0f / 3.0f));
vertices[3] = new Vertex(center + scale * new JVector(0.0f, 0.0f, 1.0f));

CreateTriangle(2, 0, 1);
CreateTriangle(1, 0, 3);
Expand All @@ -338,16 +346,16 @@ public void InitTetrahedron(in JVector point)
}

/// <summary>
/// Initializes the memory for <see cref="Vertices"/> and <see cref="Triangles"/>.
/// Initializes the memory for <see cref="vertices"/> and <see cref="triangles"/>.
/// Must be invoked prior to calling any other method in this struct.
/// Note: Can be called multiple times; however, initialization occurs only once.
/// </summary>
public void InitHeap()
{
if (Vertices == (void*)0)
if (vertices == (void*)0)
{
Vertices = MemoryHelper.AllocateHeap<Vertex>(MaxVertices);
Triangles = MemoryHelper.AllocateHeap<Triangle>(MaxTriangles);
vertices = MemoryHelper.AllocateHeap<Vertex>(MaxVertices);
triangles = MemoryHelper.AllocateHeap<Triangle>(MaxTriangles);
}
}

Expand All @@ -373,8 +381,7 @@ public bool AddVertex(in Vertex vertex)
{
Edge* edges = stackalloc Edge[256];

vPointer++;
Vertices[vPointer] = vertex;
vertices[vPointer] = vertex;

int ePointer = 0;
for (int index = tCount; index-- > 0;)
Expand All @@ -385,7 +392,7 @@ public bool AddVertex(in Vertex vertex)

for (int k = 0; k < 3; k++)
{
edge = new Edge(Triangles[index][(k + 0) % 3], Triangles[index][(k + 1) % 3]);
edge = new Edge(triangles[index][(k + 0) % 3], triangles[index][(k + 1) % 3]);
added = true;
for (int e = ePointer; e-- > 0;)
{
Expand All @@ -399,15 +406,19 @@ public bool AddVertex(in Vertex vertex)
if (added) edges[ePointer++] = edge;
}

Triangles[index] = Triangles[--tCount];
triangles[index] = triangles[--tCount];
}

if (ePointer == 0) return false;

for (int i = 0; i < ePointer; i++)
{
if (!CreateTriangle(edges[i].A, edges[i].B, vPointer))
return false;
}

return ePointer > 0;
vPointer++;

return true;
}
}
4 changes: 3 additions & 1 deletion src/Jitter2/Collision/NarrowPhase/MinkowskiDifference.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,9 @@
namespace Jitter2.Collision;

/// <summary>
/// Stores data representing the Minkowski Difference, also known as the Configuration Space Object (CSO), between two support functions: SupportA and SupportB. SupportB is transformed according to the specified OrientationB and PositionB.
/// Stores data representing the Minkowski Difference, also known as the Configuration Space Object (CSO),
/// between two support functions: SupportA and SupportB. SupportB is transformed according to the specified
/// OrientationB and PositionB.
/// </summary>
public struct MinkowskiDifference
{
Expand Down
19 changes: 13 additions & 6 deletions src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ public bool RayCast(ISupportMap supportA, in JVector origin, in JVector directio
const int MaxIter = 34;

normal = JVector.Zero;
fraction = float.MaxValue;
fraction = float.PositiveInfinity;

float lambda = 0.0f;

Expand Down Expand Up @@ -190,6 +190,7 @@ public bool SweepTest(ref MinkowskiDifference mkd, in JVector sweep,

if (VdotR >= -1e-12f)
{
fraction = float.PositiveInfinity;
return false;
}

Expand Down Expand Up @@ -312,11 +313,11 @@ 3. This notice may not be removed or altered from any source distribution.

convexPolytope.InitHeap();

ref ConvexPolytope.Vertex v0 = ref convexPolytope.Vertices[0];
ref ConvexPolytope.Vertex v1 = ref convexPolytope.Vertices[1];
ref ConvexPolytope.Vertex v2 = ref convexPolytope.Vertices[2];
ref ConvexPolytope.Vertex v3 = ref convexPolytope.Vertices[3];
ref ConvexPolytope.Vertex v4 = ref convexPolytope.Vertices[4];
ref ConvexPolytope.Vertex v0 = ref convexPolytope.GetVertex(0);
ref ConvexPolytope.Vertex v1 = ref convexPolytope.GetVertex(1);
ref ConvexPolytope.Vertex v2 = ref convexPolytope.GetVertex(2);
ref ConvexPolytope.Vertex v3 = ref convexPolytope.GetVertex(3);
ref ConvexPolytope.Vertex v4 = ref convexPolytope.GetVertex(4);

Unsafe.SkipInit(out JVector temp1);
Unsafe.SkipInit(out JVector temp2);
Expand Down Expand Up @@ -816,6 +817,9 @@ public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
/// Calculates the time of impact and the collision points in world space for two shapes with velocities
/// sweepA and sweepB.
/// </summary>
/// <param name="pointA">Collision point on shapeA in world space. Zero if no hit is detected.</param>
/// <param name="pointB">Collision point on shapeB in world space. Zero if no hit is detected.</param>
/// <param name="fraction">Time of impact. Infinity if no hit is detected.</param>
/// <returns>True if the shapes hit, false otherwise.</returns>
public static bool SweepTest(ISupportMap supportA, ISupportMap supportB,
in JMatrix orientationA, in JMatrix orientationB,
Expand Down Expand Up @@ -861,6 +865,9 @@ public static bool SweepTest(ISupportMap supportA, ISupportMap supportB,
/// Perform a sweep test where support shape A is at position zero, not rotated and has no sweep
/// direction.
/// </summary>
/// <param name="pointA">Collision point on shapeA in world space. Zero if no hit is detected.</param>
/// <param name="pointB">Collision point on shapeB in world space. Zero if no hit is detected.</param>
/// <param name="fraction">Time of impact. Infinity if no hit is detected.</param>
/// <returns>True if the shapes hit, false otherwise.</returns>
public static bool SweepTest(ISupportMap supportA, ISupportMap supportB,
in JMatrix orientationB, in JVector positionB, in JVector sweepB,
Expand Down

0 comments on commit b1e3f35

Please sign in to comment.