Merge pull request #172 from notgiven688/prog

src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs

@@ -161,6 +161,11 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
 #if DEBUG_EDGEFILTER
                 Console.WriteLine($"case #1: adjusting; normal {normal} -> {nnormal}");
 #endif
+                if (f5 < ProjectionThreshold)
+                {
+                    return false;
+                }
+
                 penetration *= f5;
                 normal = nnormal;
             }
@@ -169,6 +174,11 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB,
 #if DEBUG_EDGEFILTER
                 Console.WriteLine($"case #1: adjusting; normal {normal} -> {tnormal}");
 #endif
+                if (f6 < ProjectionThreshold)
+                {
+                    return false;
+                }
+
                 penetration *= f6;
                 normal = tnormal;
             }

src/Jitter2/Collision/DynamicTree.cs

@@ -489,7 +489,7 @@ private void OverlapCheck(int index, int node, bool add)
         {
             if (node == index) return;
 
-            if (!Filter(Nodes[node].Proxy, Nodes[index].Proxy)) return;
+            if (add && !Filter(Nodes[node].Proxy, Nodes[index].Proxy)) return;
 
             lock (PotentialPairs)
             {

src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs

@@ -885,8 +885,10 @@ public static bool MPREPA(in ISupportMappable supportA, in ISupportMappable supp
     /// 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="pointA">Collision point on shapeA in world space at t = 0, where collision will occur.
+    /// Zero if no hit is detected.</param>
+    /// <param name="pointB">Collision point on shapeB in world space at t = 0, where collision will occur.
+    /// 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>
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -924,8 +926,12 @@ public static bool SweepTest(in ISupportMappable supportA, in ISupportMappable s
         JVector.Transform(normal, orientationA, out normal);
 
         // transform back from the relative velocities
-        pointA += fraction * sweepA;
-        pointB += fraction * sweepA; // sweepA is not a typo
+
+        // This is where the collision will occur in world space:
+        //      pointA += fraction * sweepA;
+        //      pointB += fraction * sweepA; // sweepA is not a typo
+
+        pointB += fraction * (sweepA - sweepB);
 
         return true;
     }

src/Jitter2/Collision/Shapes/FatTriangleShape.cs

@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) Thorben Linneweber and others
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+using System;
+using Jitter2.LinearMath;
+
+namespace Jitter2.Collision.Shapes;
+
+/// <summary>
+/// Represents a single triangle within a mesh. The triangle is not flat but extends
+/// along its negative normal direction. This extension gives the triangle thickness,
+/// which can be controlled by the <see cref="Thickness"/> parameter.
+/// </summary>
+public class FatTriangleShape : TriangleShape
+{
+    private float thickness;
+
+    /// <summary>
+    /// Set or get the thickness of the triangle.
+    /// </summary>
+    /// <exception cref="ArgumentException">Thickness must be larger than 0.01 length units.</exception>
+    public float Thickness
+    {
+        get => thickness;
+        set
+        {
+            const float minimumThickness = 0.01f;
+
+            if (value < minimumThickness)
+            {
+                throw new ArgumentException($"{nameof(Thickness)} must not be smaller than {minimumThickness}");
+            }
+
+            thickness = value;
+        }
+    }
+
+    /// <summary>
+    /// Initializes a new instance of the TriangleShape class.
+    /// </summary>
+    /// <param name="mesh">The triangle mesh to which this triangle belongs.</param>
+    /// <param name="index">The index representing the position of the triangle within the mesh.</param>
+    public FatTriangleShape(TriangleMesh mesh, int index, float thickness = 0.2f) : base(mesh, index)
+    {
+        this.thickness = thickness;
+        UpdateWorldBoundingBox();
+    }
+
+    public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box)
+    {
+        ref var triangle = ref Mesh.Indices[Index];
+        var a = Mesh.Vertices[triangle.IndexA];
+        var b = Mesh.Vertices[triangle.IndexB];
+        var c = Mesh.Vertices[triangle.IndexC];
+
+        JVector.Transform(a, orientation, out a);
+        JVector.Transform(b, orientation, out b);
+        JVector.Transform(c, orientation, out c);
+
+        JVector delta = JVector.Normalize((a - b) % (a - c)) * Thickness;
+
+        box = JBBox.SmallBox;
+
+        box.AddPoint(a);
+        box.AddPoint(b);
+        box.AddPoint(c);
+
+        box.AddPoint(a + delta);
+        box.AddPoint(b + delta);
+        box.AddPoint(c + delta);
+    }
+
+    public override void SupportMap(in JVector direction, out JVector result)
+    {
+        ref var triangle = ref Mesh.Indices[Index];
+
+        JVector a = Mesh.Vertices[triangle.IndexA];
+        JVector b = Mesh.Vertices[triangle.IndexB];
+        JVector c = Mesh.Vertices[triangle.IndexC];
+
+        float min = JVector.Dot(a, direction);
+        float dot = JVector.Dot(b, direction);
+
+        result = a;
+
+        if (dot > min)
+        {
+            min = dot;
+            result = b;
+        }
+
+        dot = JVector.Dot(c, direction);
+
+        if (dot > min)
+        {
+            result = c;
+        }
+
+        JVector nnorm = (a - b) % (a - c);
+
+        if (JVector.Dot(nnorm, direction) > 0.0f)
+            result += JVector.Normalize(nnorm) * Thickness;
+    }
+}

src/Jitter2/World.Detect.cs

@@ -308,7 +308,7 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB)
 
         bool speculative = sA.RigidBody.EnableSpeculativeContacts || sB.RigidBody.EnableSpeculativeContacts;
 
-        if (UseFullEPASolver || speculative)
+        if (UseFullEPASolver)
         {
             bool success = NarrowPhase.GJKEPA(sA, sB, b1.Orientation, b2.Orientation, b1.Position, b2.Position,
                 out pA, out pB, out normal, out penetration);
@@ -323,13 +323,20 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB)
                 out pA, out pB, out normal, out penetration);
         }
 
-        Debug.Assert(!float.IsNaN(normal.X));
-
         if (!colliding)
         {
             if (!speculative) return;
 
             JVector dv = sB.RigidBody.Velocity - sA.RigidBody.Velocity;
+
+            if (dv.LengthSquared() < SpeculativeVelocityThreshold * SpeculativeVelocityThreshold) return;
+
+            bool success = NarrowPhase.SweepTest(sA, sB, b1.Orientation, b2.Orientation,
+                b1.Position, b2.Position,b1.Velocity, b2.Velocity,
+                out pA, out pB, out normal, out float toi);
+
+            if (!success || toi > step_dt || toi == 0.0f) return;
+
             penetration = normal * (pA - pB) * SpeculativeRelaxationFactor;
 
             if (NarrowPhaseFilter != null)
@@ -340,20 +347,15 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB)
                 }
             }
 
-            float dvn = -normal * dv;
+            GetArbiter(sA.ShapeId, sB.ShapeId, sA.RigidBody, sB.RigidBody, out Arbiter arbiter2);
 
-            if (dvn > SpeculativeVelocityThreshold)
+            lock (arbiter2)
             {
-                GetArbiter(sA.ShapeId, sB.ShapeId, sA.RigidBody, sB.RigidBody, out Arbiter arbiter2);
-
-                lock (arbiter2)
-                {
-                    // (see. 1)
-                    arbiter2.Handle.Data.IsSpeculative = true;
-                    memContacts.ResizeLock.EnterReadLock();
-                    arbiter2.Handle.Data.AddContact(pA, pB, normal, penetration);
-                    memContacts.ResizeLock.ExitReadLock();
-                }
+                // (see. 1)
+                arbiter2.Handle.Data.IsSpeculative = true;
+                memContacts.ResizeLock.EnterReadLock();
+                arbiter2.Handle.Data.AddContact(pA, pB, normal, penetration);
+                memContacts.ResizeLock.ExitReadLock();
             }
 
             return;

src/JitterDemo/Demos/Demo05.cs

@@ -33,6 +33,8 @@ public class Demo05 : IDemo
 
     private RigidBody level = null!;
 
+    private bool debugDraw = false;
+
     public List<RigidBodyShape> CreateShapes()
     {
         var indices = tm.mesh.Indices;
@@ -54,7 +56,7 @@ public List<RigidBodyShape> CreateShapes()
 
         for (int i = 0; i < jtm.Indices.Length; i++)
         {
-            TriangleShape ts = new TriangleShape(jtm, i);
+            FatTriangleShape ts = new FatTriangleShape(jtm, i);
             shapesToAdd.Add(ts);
         }
 
@@ -84,8 +86,26 @@ public void Draw()
     {
         tm.PushMatrix(Conversion.FromJitter(level), new Vector3(0.35f, 0.35f, 0.35f));
 
+        if (debugDraw)
+        {
+            Playground pg = (Playground)RenderWindow.Instance;
+
+            foreach (var triangle in tm.mesh.Indices)
+            {
+                var a = tm.mesh.Vertices[triangle.T1].Position;
+                var b = tm.mesh.Vertices[triangle.T2].Position;
+                var c = tm.mesh.Vertices[triangle.T3].Position;
+
+                pg.DebugRenderer.PushLine(DebugRenderer.Color.Green, a, b);
+                pg.DebugRenderer.PushLine(DebugRenderer.Color.Green, b, c);
+                pg.DebugRenderer.PushLine(DebugRenderer.Color.Green, c, a);
+            }
+        }
+
         Keyboard kb = Keyboard.Instance;
 
+        if (kb.KeyPressBegin(Keyboard.Key.O)) debugDraw = !debugDraw;
+
         if (kb.IsKeyDown(Keyboard.Key.Left)) player.SetAngularInput(-1.0f);
         else if (kb.IsKeyDown(Keyboard.Key.Right)) player.SetAngularInput(1.0f);
         else player.SetAngularInput(0.0f);

src/JitterTests/CollisionTests.cs

@@ -116,10 +116,12 @@ public void SweepTest()
         float expectedFraction = (MathF.Sqrt(200.0f) - 1.0f) * (1.0f / 3.0f);
         JVector expectedNormal = JVector.Normalize(new JVector(1, 1, 0));
         JVector expectedPoint = new JVector(1, 1, 3) + expectedNormal * (0.5f + expectedFraction);
+        JVector expectedPointA = expectedPoint - sweep * fraction;
+        JVector expectedPointB = expectedPoint + 2.0f * sweep * fraction;
 
         Assert.That((normal - expectedNormal).LengthSquared(), Is.LessThan(1e-4f));
-        Assert.That((pA - expectedPoint).LengthSquared(), Is.LessThan(1e-4f));
-        Assert.That((pB - expectedPoint).LengthSquared(), Is.LessThan(1e-4f));
+        Assert.That((pA - expectedPointA).LengthSquared(), Is.LessThan(1e-4f));
+        Assert.That((pB - expectedPointB).LengthSquared(), Is.LessThan(1e-4f));
         Assert.That(MathF.Abs(fraction - expectedFraction), Is.LessThan(1e-4f));
     }