diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index a085789..23e27a1 100644 --- a/native/Ymir.Box3D/ymir_box3d_soa.c +++ b/native/Ymir.Box3D/ymir_box3d_soa.c @@ -152,7 +152,7 @@ typedef struct ymir_box3d_soa_view const float* joint_angular_velocity_y; const float* joint_angular_velocity_z; // Contact lanes, read-only, refreshed every step. contact_force is N: the sum over the body's touching - // contact manifolds of totalNormalImpulse / dt (0 for a body touching nothing); contact_count is the + // contact manifold points of normalImpulse * sub_steps / dt (0 for a body touching nothing); contact_count is the // number of touching contacts. material_id is the body's desc material_id. const float* contact_force; const int32_t* contact_count; @@ -786,9 +786,11 @@ static void ymir_soa_collect_facts(ymir_box3d_soa_world* world) } } -// Per body: the touching contacts' count and the force they carry, totalNormalImpulse over every -// sub-step of the step, summed over manifold points, divided by the step. -static void ymir_soa_collect_contact_lanes(ymir_box3d_soa_world* world, float time_step) +// Per body: the touching contacts' count and the force they carry. normalImpulse is the final sub-step's +// impulse, so over a sub-step of time_step / sub_step_count it is a force of normalImpulse * sub_steps / +// time_step. (totalNormalImpulse is not that: Box3D adds the accumulated impulse once per solve pass and +// once per relax pass, so at rest it reads twice the step's impulse.) +static void ymir_soa_collect_contact_lanes(ymir_box3d_soa_world* world, float time_step, int32_t sub_step_count) { float* force = ymir_soa_lane(world, YMIR_SOA_CONTACT_FORCE_LANE); for (int i = 0; i < world->count; ++i) @@ -803,12 +805,12 @@ static void ymir_soa_collect_contact_lanes(ymir_box3d_soa_world* world, float ti const b3Manifold* manifold = data->manifolds + m; for (int p = 0; p < manifold->pointCount; ++p) { - impulse += manifold->points[p].totalNormalImpulse; + impulse += manifold->points[p].normalImpulse; } } } world->contact_count[i] = n; - force[i] = impulse / time_step; + force[i] = impulse * (float)sub_step_count / time_step; } } @@ -825,7 +827,7 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f } b3World_Step(world->world_id, time_step, sub_step_count); ymir_soa_collect_facts(world); - ymir_soa_collect_contact_lanes(world, time_step); + ymir_soa_collect_contact_lanes(world, time_step, sub_step_count); for (int i = 0; i < world->count; ++i) { ymir_soa_refresh(world, i); diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index 07415bb..7342658 100644 --- a/src/Ymir.Box3D/Box3DSoaWorld.cs +++ b/src/Ymir.Box3D/Box3DSoaWorld.cs @@ -304,7 +304,7 @@ public readonly unsafe struct Box3DSoaView public Span TorqueZ => Write(_v.TorqueZ, Count); // Contact lanes, read-only, refreshed by every Step. - /// Newtons: the sum over the body's touching contact manifolds of totalNormalImpulse / dt. 0 for a body touching nothing. + /// Newtons: the sum over the body's touching contact manifold points of normalImpulse * sub-steps / dt (the final sub-step's impulse as a force). 0 for a body touching nothing. public ReadOnlySpan ContactForce => Lane(_v.ContactForce, Count); /// Number of touching contacts. public ReadOnlySpan ContactCount => Lane(_v.ContactCount, Count); diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs new file mode 100644 index 0000000..22253a5 --- /dev/null +++ b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs @@ -0,0 +1,224 @@ +using System.Runtime.InteropServices; +using Ymir.Box3D; + +namespace Ymir.Box3D.Soa.Tests; + +public sealed class Box3DSoaContactTests +{ + private const float Dt = 1f / 60f; + private const float G = 10f; + private const float Radius = 0.5f; + private const ulong GroundId = 1; + private const ulong BallId = 2; + + private static float BallMass => 4f / 3f * MathF.PI * Radius * Radius * Radius; // density 1 + + /// A ground slab with its top face at y = 0 (lane 0) and a ball (lane 1) dropped from 1.4 m above it. + private static Box3DSoaWorld Drop(float height = 1.4f, int? factCapacity = null, float hitThreshold = 1f) + { + var world = new Box3DSoaWorld(capacity: 8, gravityY: -G, factCapacity: factCapacity, hitThreshold: hitThreshold); + world.AddStaticBox(GroundId, (0f, -0.5f, 0f), (50f, 0.5f, 50f), materialId: 7); + world.AddSphere(BallId, (0f, Radius + height, 0f), Radius, density: 1f, materialId: 42); + return world; + } + + /// Steps until the first step that reports a begin; returns false if none within the budget. + private static bool StepToFirstBegin(Box3DSoaWorld world, int budget = 120) + { + for (var i = 0; i < budget; i++) + { + world.Step(Dt); + if (world.Facts.Begin.Length > 0) return true; + } + return false; + } + + [Fact] + public void A_drop_gives_one_begin_and_one_hit() + { + using var world = Drop(); + int begins = 0, hits = 0, ends = 0; + Box3DSoaHit hit = default; + Box3DSoaTouch begin = default; + for (var i = 0; i < 240; i++) + { + world.Step(Dt); + var facts = world.Facts; + if (facts.Begin.Length > 0) begin = facts.Begin[0]; + if (facts.Hit.Length > 0) hit = facts.Hit[0]; + begins += facts.Begin.Length; + hits += facts.Hit.Length; + ends += facts.End.Length; + Assert.Equal(0u, facts.Dropped.Begin + facts.Dropped.End + facts.Dropped.Hit); + } + Assert.Equal((1, 1, 0), (begins, hits, ends)); + + Assert.Equal(new[] { 0, 1 }, new[] { begin.LaneA, begin.LaneB }.Order()); + Assert.Equal(new[] { GroundId, BallId }, new[] { begin.StableIdA, begin.StableIdB }.Order()); + Assert.Equal((begin.LaneA, begin.LaneB, begin.StableIdA, begin.StableIdB), (hit.LaneA, hit.LaneB, hit.StableIdA, hit.StableIdB)); + Assert.Equal(begin.LaneA == 0 ? GroundId : BallId, begin.StableIdA); // the stable id is the lane's own + + var expected = MathF.Sqrt(2f * G * 1.4f); + Assert.InRange(hit.ApproachSpeed, expected * 0.95f, expected * 1.05f); + Assert.InRange(hit.PointY, -0.02f, 0.02f); + Assert.InRange(MathF.Abs(hit.PointX), 0f, 0.05f); + Assert.InRange(MathF.Abs(hit.NormalY), 0.99f, 1.001f); + // A to B: the normal points from the ground up to the ball when the ground is A. + Assert.True(hit.LaneA == 0 ? hit.NormalY > 0f : hit.NormalY < 0f); + } + + [Fact] + public void Lifting_gives_one_end() + { + using var world = Drop(); + Run(world, 240); + Assert.Equal(1, world.View.ContactCount[1]); + + world.View.ForceY[1] = 100f; // one step's push: about 3 m/s upward, well clear of the slab within a few steps + var begins = 0; + var ends = new List(); + for (var i = 0; i < 20; i++) + { + world.Step(Dt); + begins += world.Facts.Begin.Length; + ends.AddRange(world.Facts.End.ToArray()); + } + Assert.Equal(0, begins); + var end = Assert.Single(ends); + Assert.Equal(new[] { 0, 1 }, new[] { end.LaneA, end.LaneB }.Order()); + Assert.Equal(new[] { GroundId, BallId }, new[] { end.StableIdA, end.StableIdB }.Order()); + Assert.Equal(0, world.View.ContactCount[1]); + } + + [Fact] + public void Resting_contact_force_is_the_weight_and_a_body_touching_nothing_reads_zero() + { + using var world = Drop(); + world.AddSphere(3, (10f, 1000f, 10f), Radius, density: 1f); // lane 2: still falling after the whole run (5 s drops 125 m) + Run(world, 300); + var view = world.View; + var weight = BallMass * G; + Assert.InRange(view.ContactForce[1], weight * 0.98f, weight * 1.02f); + Assert.Equal(1, view.ContactCount[1]); + Assert.InRange(view.ContactForce[0], weight * 0.98f, weight * 1.02f); // the slab carries the same load + Assert.Equal(0f, view.ContactForce[2]); + Assert.Equal(0, view.ContactCount[2]); + } + + [Fact] + public void Materials_ride_hits_in_pair_order_and_read_back_as_a_lane() + { + using var world = Drop(); + Assert.Equal(new ulong[] { 7, 42 }, world.View.MaterialId.ToArray()); + Assert.True(StepToFirstBegin(world)); + var hit = Assert.Single(world.Facts.Hit.ToArray()); + var material = new Dictionary { [0] = 7, [1] = 42 }; + Assert.Equal((material[hit.LaneA], material[hit.LaneB]), (hit.MaterialA, hit.MaterialB)); + Assert.Equal(new ulong[] { 7, 42 }, world.View.MaterialId.ToArray()); // unchanged by stepping + } + + [Fact] + public void Facts_last_one_step() + { + using var world = Drop(); + Assert.True(StepToFirstBegin(world)); + Assert.Equal(1, world.Facts.Hit.Length); + world.Step(Dt); + var facts = world.Facts; + Assert.Equal((0, 0, 0), (facts.Begin.Length, facts.End.Length, facts.Hit.Length)); + } + + [Fact] + public void Overflow_is_counted_and_a_zero_capacity_is_refused() + { + using var world = new Box3DSoaWorld(capacity: 8, gravityY: -G, factCapacity: 1); + world.AddStaticBox(GroundId, (0f, -0.5f, 0f), (50f, 0.5f, 50f)); + world.AddSphere(2, (-3f, 1.9f, 0f), Radius, density: 1f); + world.AddSphere(3, (3f, 1.9f, 0f), Radius, density: 1f); + Assert.True(StepToFirstBegin(world)); + var facts = world.Facts; + Assert.Equal(1, facts.Begin.Length); + Assert.Equal(1, facts.Hit.Length); + Assert.Equal(1u, facts.Dropped.Begin); + Assert.Equal(1u, facts.Dropped.Hit); + Assert.Equal(1, facts.Capacity); + + Assert.Throws(() => new Box3DSoaWorld(capacity: 8, factCapacity: 0)); + } + + private static Box3DSoaWorld Pile() + { + var world = new Box3DSoaWorld(capacity: 32, gravityY: -G); + world.AddStaticBox(GroundId, (0f, -0.5f, 0f), (50f, 0.5f, 50f), materialId: 3); + ulong id = 100; + for (var x = 0; x < 4; x++) + { + for (var z = 0; z < 3; z++) + { + world.AddSphere(id++, (x * 0.9f - 1.4f, 2f + 0.6f * (x + z), z * 0.9f - 0.9f), Radius, density: 1f, + velocity: (0.2f * z, 0f, -0.1f * x), materialId: (ulong)(x + 10)); + } + } + return world; + } + + private static byte[] Snapshot(Box3DSoaWorld world) + { + var facts = world.Facts; + var view = world.View; + return MemoryMarshal.AsBytes(facts.Begin).ToArray() + .Concat(MemoryMarshal.AsBytes(facts.End).ToArray()) + .Concat(MemoryMarshal.AsBytes(facts.Hit).ToArray()) + .Concat(MemoryMarshal.AsBytes(view.ContactForce).ToArray()) + .Concat(MemoryMarshal.AsBytes(view.ContactCount).ToArray()) + .Concat(BitConverter.GetBytes(facts.Dropped.Begin + facts.Dropped.End + facts.Dropped.Hit)) + .ToArray(); + } + + [Fact] + public void Determinism_covers_facts_and_contact_lanes_at_every_step() + { + using var a = Pile(); + using var b = Pile(); + var factsSeen = 0; + for (var step = 0; step < 300; step++) + { + a.Step(Dt); + b.Step(Dt); + var snapshot = Snapshot(a); + Assert.True(snapshot.AsSpan().SequenceEqual(Snapshot(b)), $"facts or contact lanes diverged at step {step}"); + factsSeen += a.Facts.Begin.Length + a.Facts.Hit.Length + a.Facts.End.Length; + } + Assert.True(factsSeen > 12, "the pile produced facts, so equality is not trivially empty"); + Assert.Contains(a.View.ContactForce.ToArray(), f => f > 0f); + } + + [Fact] + public void A_steady_state_step_with_fact_reads_allocates_nothing() + { + using var world = Pile(); + Run(world, 120); // warm up: JIT, and the pile has settled into steady contact + long Read() + { + var facts = world.Facts; + var view = world.View; + return facts.Begin.Length + facts.End.Length + facts.Hit.Length + (long)view.ContactForce[1] + view.ContactCount[1]; + } + Read(); + var before = GC.GetAllocatedBytesForCurrentThread(); + long sink = 0; + for (var i = 0; i < 100; i++) + { + world.Step(Dt); + sink += Read(); + } + var allocated = GC.GetAllocatedBytesForCurrentThread() - before; + Assert.Equal(0L, allocated); + Assert.True(sink != long.MinValue); + } + + private static void Run(Box3DSoaWorld world, int steps) + { + for (var i = 0; i < steps; i++) world.Step(Dt); + } +}