SoA contact force reads the final sub-step impulse; contact fact tests

totalNormalImpulse accumulates the accumulated impulse once per solve and
once per relax pass, so a resting body read twice its weight. ContactForce
is now normalImpulse * sub_steps / dt, which reads the weight at rest.
Tests cover drop begin/hit, lift end, resting force, materials, one-step
lifetime, overflow counting, per-step determinism and zero allocation.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
This commit is contained in:
Meta Vens 2026-11-20 01:09:44 +01:00
commit 5a4cde33ae
3 changed files with 234 additions and 8 deletions

View file

@ -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);

View file

@ -304,7 +304,7 @@ public readonly unsafe struct Box3DSoaView
public Span<float> TorqueZ => Write(_v.TorqueZ, Count);
// Contact lanes, read-only, refreshed by every Step.
/// <summary>Newtons: the sum over the body's touching contact manifolds of totalNormalImpulse / dt. 0 for a body touching nothing.</summary>
/// <summary>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.</summary>
public ReadOnlySpan<float> ContactForce => Lane(_v.ContactForce, Count);
/// <summary>Number of touching contacts.</summary>
public ReadOnlySpan<int> ContactCount => Lane(_v.ContactCount, Count);

View file

@ -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
/// <summary>A ground slab with its top face at y = 0 (lane 0) and a ball (lane 1) dropped from 1.4 m above it.</summary>
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;
}
/// <summary>Steps until the first step that reports a begin; returns false if none within the budget.</summary>
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<Box3DSoaTouch>();
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<int, ulong> { [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<InvalidOperationException>(() => 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);
}
}