diff --git a/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs b/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs index 5917656c..931cb356 100644 --- a/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs +++ b/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; } diff --git a/src/Jitter2/Collision/DynamicTree.cs b/src/Jitter2/Collision/DynamicTree.cs index dc5e0147..40bc653b 100644 --- a/src/Jitter2/Collision/DynamicTree.cs +++ b/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) { diff --git a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs index ce56d308..9f393386 100644 --- a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs +++ b/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. /// - /// 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. + /// Collision point on shapeA in world space at t = 0, where collision will occur. + /// Zero if no hit is detected. + /// Collision point on shapeB in world space at t = 0, where collision will occur. + /// Zero if no hit is detected. /// Time of impact. Infinity if no hit is detected. /// True if the shapes hit, false otherwise. [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; } diff --git a/src/Jitter2/Collision/Shapes/FatTriangleShape.cs b/src/Jitter2/Collision/Shapes/FatTriangleShape.cs new file mode 100644 index 00000000..3aa52942 --- /dev/null +++ b/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; + +/// +/// 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 parameter. +/// +public class FatTriangleShape : TriangleShape +{ + private float thickness; + + /// + /// Set or get the thickness of the triangle. + /// + /// Thickness must be larger than 0.01 length units. + 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; + } + } + + /// + /// Initializes a new instance of the TriangleShape class. + /// + /// The triangle mesh to which this triangle belongs. + /// The index representing the position of the triangle within the mesh. + 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; + } +} \ No newline at end of file diff --git a/src/Jitter2/World.Detect.cs b/src/Jitter2/World.Detect.cs index da0865ba..320da0fc 100644 --- a/src/Jitter2/World.Detect.cs +++ b/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; diff --git a/src/JitterDemo/Demos/Demo05.cs b/src/JitterDemo/Demos/Demo05.cs index 0aeb1af2..3ef1baf3 100644 --- a/src/JitterDemo/Demos/Demo05.cs +++ b/src/JitterDemo/Demos/Demo05.cs @@ -33,6 +33,8 @@ public class Demo05 : IDemo private RigidBody level = null!; + private bool debugDraw = false; + public List CreateShapes() { var indices = tm.mesh.Indices; @@ -54,7 +56,7 @@ public List 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); diff --git a/src/JitterTests/CollisionTests.cs b/src/JitterTests/CollisionTests.cs index 8e1cdc17..d70a6af5 100644 --- a/src/JitterTests/CollisionTests.cs +++ b/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)); }