From 5c35a40ccf8b95ed87637aa45d64d1ec523c7766 Mon Sep 17 00:00:00 2001 From: Meta Vens Date: Fri, 20 Nov 2026 01:37:21 +0100 Subject: [PATCH] SoA: contact force lane renamed LastSubstepForce; hit facts carry an impulse estimate Box3D exposes no whole-step normal impulse: normalImpulse is the last sub-step and totalNormalImpulse is a solve plus relax mixture, so no lane built from them is true on an impact step. The per-body lane is renamed LastSubstepForce and documented as a resting load only. Impact severity is the hit fact's ImpulseEstimate, v (1 + e) / (1/mA + 1/mB), checked against the ball's momentum change at 60 and 240 Hz, 1 and 4 sub-steps, restitution 0 and 0.5. Also pins that dropped counters reset each step. Co-Authored-By: Claude Sonnet 5.5 --- docs/soa-world.md | 16 ++-- native/Ymir.Box3D/ymir_box3d_soa.c | 54 ++++++++++--- src/Ymir.Box3D/Box3DSoaWorld.cs | 9 ++- .../Box3DSoaContactTests.cs | 81 +++++++++++++++++-- 4 files changed, 131 insertions(+), 29 deletions(-) diff --git a/docs/soa-world.md b/docs/soa-world.md index 52a280a..65d5e96 100644 --- a/docs/soa-world.md +++ b/docs/soa-world.md @@ -32,11 +32,16 @@ separate native object (`ymir_box3d_soa.c`, ABI 4) so the Aetheria session's con (world orientation of frame A, quaternion), `joint_rotation` (frame B relative to frame A, quaternion, w >= 0) and `joint_angular_velocity` (B minus A, in frame A). A consumer can compute a joint torque from these and write it as equal and opposite torques on the two body lanes. -- Contact lanes (read-only, rewritten by every `step`): `contact_force` (N: the final sub-step's +- Contact lanes (read-only, rewritten by every `step`): `last_substep_force` (N: the final sub-step's `normalImpulse` summed over the body's touching manifold points, times sub-steps over dt; 0 for a body - touching nothing; the weight at rest), `contact_count` (touching contacts) and `material_id` (the body's - `material_id`, also set as the shape's Box3D user material). `totalNormalImpulse` is not used: Box3D adds - the accumulated impulse once per solve and once per relax pass, so at rest it reads twice the weight. + touching nothing; the weight at rest; NOT an impact measure, it sees one sub-step and misses an impact + step by a large factor either way), `contact_count` (touching contacts) and `material_id` (the body's + `material_id`, also set as the shape's Box3D user material). Box3D exposes no whole-step impulse: + `totalNormalImpulse` adds the accumulated impulse once per solve and once per relax pass (so it reads twice + the weight at rest, and on impacts is no fixed multiple of the step's impulse). Impact severity is the hit + fact's `impulse_estimate` (N s): `approach_speed * (1 + e) / (1/mA + 1/mB)`, e the larger shape restitution + and only above Box3D's restitution threshold, a static body's 1/m taken as 0. Exact for a hit through both + centres of mass, an upper bound otherwise (rotation is ignored). - Contact facts: every shape is created with contact and hit events on (Box3D's default is off). After each `step`, `ymir_box3d_soa_get_facts` returns pointers and counts into buffers fixed at create (`fact_capacity` per kind, no allocation in the step): Begin and End `{lane_a, lane_b, stable_id_a, stable_id_b}` and Hit @@ -65,7 +70,8 @@ lanes agreeing, an off-axis hinge, a limit and a weld, a spherical joint leaving torque lanes (exact acceleration, cleared after the step, static bodies ignoring them, NaN refusing the step), capsule and box rest heights with parent and limb lanes, refusals, and bit-identical repeat runs of a torque-driven jointed body. `Box3DSoaContactTests`: a drop gives one begin and one hit (speed within 5% -of sqrt(2gh), point and normal), lifting gives one end, resting `contact_force` is the weight, materials on +of sqrt(2gh), point and normal), lifting gives one end, resting `last_substep_force` is the weight, the hit `impulse_estimate` matches the ball's momentum change +(plus gravity's share) at 60 and 240 Hz, 1 and 4 sub-steps, restitution 0 and 0.5, dropped counters resetting each step, materials on hits, facts lasting one step, overflow counted, per-step byte determinism of facts and contact lanes, and no allocation across 100 steps with fact reads. diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index 23e27a1..3184ca4 100644 --- a/native/Ymir.Box3D/ymir_box3d_soa.c +++ b/native/Ymir.Box3D/ymir_box3d_soa.c @@ -151,10 +151,12 @@ typedef struct ymir_box3d_soa_view const float* joint_angular_velocity_x; 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 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; + // Contact lanes, read-only, refreshed every step. last_substep_force is N: the sum over the body's touching + // contact manifold points of the final sub-step's normalImpulse * sub_steps / dt (0 for a body touching + // nothing). It is the load a body carries at rest and NOT an impact measure: it sees one sub-step, so it + // reads 0.3x to 7x the true force on an impact step. Impact severity is the hit fact's impulse_estimate. + // contact_count is the number of touching contacts. material_id is the body's desc material_id. + const float* last_substep_force; const int32_t* contact_count; const uint64_t* material_id; } ymir_box3d_soa_view; @@ -178,7 +180,11 @@ typedef struct ymir_box3d_soa_hit_fact float point[3]; float normal[3]; float approach_speed; - uint32_t reserved; + // The normal impulse (N s) this collision must deliver, from the approach speed, the two bodies' masses + // and the pair's restitution: v (1 + e) / (1/mA + 1/mB), with e applying only above Box3D's restitution + // threshold and a static body's 1/m taken as 0. Exact for a hit through both centres of mass (a sphere on + // a plane); an upper bound otherwise, since rotation only lowers the effective mass's inverse. + float impulse_estimate; uint64_t material_a; uint64_t material_b; } ymir_box3d_soa_hit_fact; @@ -203,8 +209,8 @@ typedef struct ymir_box3d_soa_facts #define YMIR_SOA_STATE_LANES 13 #define YMIR_SOA_WRITE_LANES 6 -#define YMIR_SOA_CONTACT_FORCE_LANE (YMIR_SOA_STATE_LANES + YMIR_SOA_WRITE_LANES) -#define YMIR_SOA_BODY_FLOAT_LANES (YMIR_SOA_CONTACT_FORCE_LANE + 1) +#define YMIR_SOA_LAST_SUBSTEP_FORCE_LANE (YMIR_SOA_STATE_LANES + YMIR_SOA_WRITE_LANES) +#define YMIR_SOA_BODY_FLOAT_LANES (YMIR_SOA_LAST_SUBSTEP_FORCE_LANE + 1) #define YMIR_SOA_JOINT_FLOAT_LANES 12 typedef struct ymir_box3d_soa_world @@ -746,6 +752,26 @@ static void ymir_soa_touch_fact( (uint64_t)(uintptr_t)b3Shape_GetUserData(b)}; } +static float ymir_soa_inverse_mass(b3BodyId body) +{ + return b3Body_GetType(body) == b3_dynamicBody ? 1.0f / b3Body_GetMass(body) : 0.0f; +} + +static float ymir_soa_hit_impulse(const ymir_box3d_soa_world* world, const b3ContactHitEvent* e) +{ + float inverse = ymir_soa_inverse_mass(b3Shape_GetBody(e->shapeIdA)) + ymir_soa_inverse_mass(b3Shape_GetBody(e->shapeIdB)); + if (inverse <= 0.0f) + { + return 0.0f; + } + float restitution = fmaxf(b3Shape_GetRestitution(e->shapeIdA), b3Shape_GetRestitution(e->shapeIdB)); + if (e->approachSpeed <= b3World_GetRestitutionThreshold(world->world_id)) + { + restitution = 0.0f; + } + return e->approachSpeed * (1.0f + restitution) / inverse; +} + // Rewrites the fact arrays from Box3D's contact events for the step just taken, in Box3D's event order. // Writes only into the buffers allocated at create: nothing is allocated here. static void ymir_soa_collect_facts(ymir_box3d_soa_world* world) @@ -780,19 +806,21 @@ static void ymir_soa_collect_facts(ymir_box3d_soa_world* world) .point = {(float)e->point.x, (float)e->point.y, (float)e->point.z}, .normal = {e->normal.x, e->normal.y, e->normal.z}, .approach_speed = e->approachSpeed, + .impulse_estimate = ymir_soa_hit_impulse(world, e), .material_a = e->userMaterialIdA, .material_b = e->userMaterialIdB, }; } } -// 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.) +// Per body: the touching contacts' count and the force of the final sub-step. normalImpulse is that +// sub-step's impulse, so it is a force of normalImpulse * sub_steps / time_step. Box3D exposes no +// whole-step impulse: totalNormalImpulse adds the accumulated impulse once per solve pass and once per +// relax pass (twice per sub-step, bias and no-bias, with restitution's delta on top), so it is neither the +// step's impulse nor a fixed multiple of it. The lane is honest at rest only. 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); + float* force = ymir_soa_lane(world, YMIR_SOA_LAST_SUBSTEP_FORCE_LANE); for (int i = 0; i < world->count; ++i) { int n = b3Body_GetContactData(world->bodies[i], world->contact_scratch, world->capacity); @@ -898,7 +926,7 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_get_view(const ymir_box3d_soa_world .joint_angular_velocity_x = jb + 9 * m, .joint_angular_velocity_y = jb + 10 * m, .joint_angular_velocity_z = jb + 11 * m, - .contact_force = base + YMIR_SOA_CONTACT_FORCE_LANE * n, + .last_substep_force = base + YMIR_SOA_LAST_SUBSTEP_FORCE_LANE * n, .contact_count = world->contact_count, .material_id = world->material_id, }; diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index 7342658..c16d833 100644 --- a/src/Ymir.Box3D/Box3DSoaWorld.cs +++ b/src/Ymir.Box3D/Box3DSoaWorld.cs @@ -99,7 +99,7 @@ internal unsafe struct Box3DSoaViewNative public float* JointAngularVelocityX; public float* JointAngularVelocityY; public float* JointAngularVelocityZ; - public float* ContactForce; + public float* LastSubstepForce; public int* ContactCount; public ulong* MaterialId; } @@ -143,7 +143,8 @@ public readonly struct Box3DSoaHit public readonly float NormalX, NormalY, NormalZ; /// Metres per second, always positive. public readonly float ApproachSpeed; - private readonly uint _reserved; + /// Normal impulse (N s) the collision must deliver: v (1 + e) / (1/mA + 1/mB); exact for a hit through both centres of mass, an upper bound otherwise. + public readonly float ImpulseEstimate; public readonly ulong MaterialA; public readonly ulong MaterialB; } @@ -304,8 +305,8 @@ 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 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); + /// Newtons: the final sub-step's normal impulse over the body's touching contact points, as a force (impulse * sub-steps / dt). The load carried at rest; NOT an impact measure (it sees one sub-step). Use for impacts. 0 for a body touching nothing. + public ReadOnlySpan LastSubstepForce => Lane(_v.LastSubstepForce, Count); /// Number of touching contacts. public ReadOnlySpan ContactCount => Lane(_v.ContactCount, Count); /// The material id the body was added with (0 by default). diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs index 22253a5..531c95d 100644 --- a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs +++ b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs @@ -91,17 +91,17 @@ public sealed class Box3DSoaContactTests } [Fact] - public void Resting_contact_force_is_the_weight_and_a_body_touching_nothing_reads_zero() + public void Resting_last_substep_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.InRange(view.LastSubstepForce[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.InRange(view.LastSubstepForce[0], weight * 0.98f, weight * 1.02f); // the slab carries the same load + Assert.Equal(0f, view.LastSubstepForce[2]); Assert.Equal(0, view.ContactCount[2]); } @@ -146,6 +146,73 @@ public sealed class Box3DSoaContactTests Assert.Throws(() => new Box3DSoaWorld(capacity: 8, factCapacity: 0)); } + [Fact] + public void Dropped_counters_reset_every_step() + { + 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)); + Assert.Equal(1u, world.Facts.Dropped.Begin); + world.Step(Dt); + Assert.Equal(new Box3DSoaDropped(0, 0, 0), world.Facts.Dropped); + } + + [Theory] + [InlineData(60, 1, 0f)] + [InlineData(60, 4, 0f)] + [InlineData(60, 1, 0.5f)] + [InlineData(60, 4, 0.5f)] + [InlineData(240, 1, 0f)] + [InlineData(240, 4, 0f)] + [InlineData(240, 1, 0.5f)] + [InlineData(240, 4, 0.5f)] + public void The_hit_impulse_estimate_matches_the_balls_momentum_change_over_the_impact(int hz, int subSteps, float restitution) + { + // Oracle: the only forces on the ball are gravity and the slab's contact, so over a window that starts + // at the hit step the contact impulse is m (v_after - v_before) + m g T. The impact is that impulse less + // the weight the slab then carries while the ball lies at rest, so steps spent at rest (touching, nearly + // still) are left out of T. + var dt = 1f / hz; + using var world = new Box3DSoaWorld(capacity: 8, gravityY: -G); + world.AddStaticBox(GroundId, (0f, -0.5f, 0f), (50f, 0.5f, 50f)); + world.AddSphere(BallId, (0f, Radius + 1.4f, 0f), Radius, density: 1f, restitution: restitution); + var mass = BallMass; + var before = 0f; + var window = (int)MathF.Ceiling(0.05f * hz); + var inWindow = 0; + var hitSteps = 0; + var activeSteps = 0; + Box3DSoaHit hit = default; + float vBefore = 0f, vAfter = 0f; + for (var i = 0; i < 4 * hz && inWindow < window; i++) + { + world.Step(dt, subSteps); + var vy = world.View.VelocityY[1]; + var atRest = world.View.ContactCount[1] > 0 && MathF.Abs(vy) < 0.05f; + if (world.Facts.Hit.Length > 0 && inWindow == 0) + { + hit = world.Facts.Hit[0]; + vBefore = before; + hitSteps++; + inWindow = 1; + activeSteps = atRest ? 0 : 1; + } + else if (inWindow > 0) + { + hitSteps += world.Facts.Hit.Length; + inWindow++; + activeSteps += atRest ? 0 : 1; + } + vAfter = vy; + before = vy; + } + Assert.Equal(1, hitSteps); // one hit in the window: no bounce-and-rehit mixed into the oracle + var truth = mass * (vAfter - vBefore) + mass * G * (activeSteps * dt); + Assert.InRange(hit.ImpulseEstimate, truth * 0.92f, truth * 1.08f); + } + private static Box3DSoaWorld Pile() { var world = new Box3DSoaWorld(capacity: 32, gravityY: -G); @@ -169,7 +236,7 @@ public sealed class Box3DSoaContactTests 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.LastSubstepForce).ToArray()) .Concat(MemoryMarshal.AsBytes(view.ContactCount).ToArray()) .Concat(BitConverter.GetBytes(facts.Dropped.Begin + facts.Dropped.End + facts.Dropped.Hit)) .ToArray(); @@ -190,7 +257,7 @@ public sealed class Box3DSoaContactTests 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); + Assert.Contains(a.View.LastSubstepForce.ToArray(), f => f > 0f); } [Fact] @@ -202,7 +269,7 @@ public sealed class Box3DSoaContactTests { 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]; + return facts.Begin.Length + facts.End.Length + facts.Hit.Length + (long)view.LastSubstepForce[1] + view.ContactCount[1]; } Read(); var before = GC.GetAllocatedBytesForCurrentThread();