From b1e3f35b1d6d6389e67ed7851f51487adbca82a9 Mon Sep 17 00:00:00 2001 From: notgiven688 <37874600+notgiven688@users.noreply.github.com> Date: Fri, 2 Feb 2024 17:27:07 +0100 Subject: [PATCH] Improve ConvexPolytope (#92) --- .../Collision/NarrowPhase/ConvexPolytope.cs | 89 +++++++++++-------- .../NarrowPhase/MinkowskiDifference.cs | 4 +- .../Collision/NarrowPhase/NarrowPhase.cs | 19 ++-- 3 files changed, 66 insertions(+), 46 deletions(-) diff --git a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs index 72328e45..2f0cc5d9 100644 --- a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs +++ b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs @@ -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; @@ -102,10 +102,18 @@ public static bool Equals(in Edge a, in Edge b) private JVector center; + public Span HullTriangles => new Span(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]; + } + /// /// Indicates whether the origin is enclosed within the polyhedron. - /// Important: For accurate results, call this method only after invoking . - /// The return value may be invalidated by subsequent calls to or . + /// Important: This property returns correct values after invoking . /// public readonly bool OriginEnclosed => originEnclosed; @@ -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. @@ -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(); @@ -246,7 +254,7 @@ 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) { @@ -254,13 +262,13 @@ private bool CreateTriangle(short a, short b, short c) 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 @@ -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]; } /// - /// Initializes the structure with a tetrahedron formed using the first four vertices in the array. + /// Initializes the structure with a tetrahedron formed using the first four vertices in the array. /// 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); @@ -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); @@ -338,16 +346,16 @@ public void InitTetrahedron(in JVector point) } /// - /// Initializes the memory for and . + /// Initializes the memory for and . /// Must be invoked prior to calling any other method in this struct. /// Note: Can be called multiple times; however, initialization occurs only once. /// public void InitHeap() { - if (Vertices == (void*)0) + if (vertices == (void*)0) { - Vertices = MemoryHelper.AllocateHeap(MaxVertices); - Triangles = MemoryHelper.AllocateHeap(MaxTriangles); + vertices = MemoryHelper.AllocateHeap(MaxVertices); + triangles = MemoryHelper.AllocateHeap(MaxTriangles); } } @@ -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;) @@ -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;) { @@ -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; } } \ No newline at end of file diff --git a/src/Jitter2/Collision/NarrowPhase/MinkowskiDifference.cs b/src/Jitter2/Collision/NarrowPhase/MinkowskiDifference.cs index 681d3fa6..39eb6083 100644 --- a/src/Jitter2/Collision/NarrowPhase/MinkowskiDifference.cs +++ b/src/Jitter2/Collision/NarrowPhase/MinkowskiDifference.cs @@ -27,7 +27,9 @@ namespace Jitter2.Collision; /// -/// 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. /// public struct MinkowskiDifference { diff --git a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs index 2cba778f..fd99ccef 100644 --- a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs +++ b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs @@ -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; @@ -190,6 +190,7 @@ public bool SweepTest(ref MinkowskiDifference mkd, in JVector sweep, if (VdotR >= -1e-12f) { + fraction = float.PositiveInfinity; return false; } @@ -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); @@ -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. /// + /// Collision point on shapeA in world space. Zero if no hit is detected. + /// Collision point on shapeB in world space. Zero if no hit is detected. + /// Time of impact. Infinity if no hit is detected. /// True if the shapes hit, false otherwise. public static bool SweepTest(ISupportMap supportA, ISupportMap supportB, in JMatrix orientationA, in JMatrix orientationB, @@ -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. /// + /// Collision point on shapeA in world space. Zero if no hit is detected. + /// Collision point on shapeB in world space. Zero if no hit is detected. + /// Time of impact. Infinity if no hit is detected. /// True if the shapes hit, false otherwise. public static bool SweepTest(ISupportMap supportA, ISupportMap supportB, in JMatrix orientationB, in JVector positionB, in JVector sweepB,