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 <noreply@anthropic.com>
This commit is contained in:
Meta Vens 2026-11-20 01:37:21 +01:00
commit 5c35a40ccf
4 changed files with 131 additions and 29 deletions

View file

@ -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.

View file

@ -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,
};

View file

@ -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;
/// <summary>Metres per second, always positive.</summary>
public readonly float ApproachSpeed;
private readonly uint _reserved;
/// <summary>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.</summary>
public readonly float ImpulseEstimate;
public readonly ulong MaterialA;
public readonly ulong MaterialB;
}
@ -304,8 +305,8 @@ 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 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>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 <see cref="Box3DSoaHit.ImpulseEstimate"/> for impacts. 0 for a body touching nothing.</summary>
public ReadOnlySpan<float> LastSubstepForce => Lane(_v.LastSubstepForce, Count);
/// <summary>Number of touching contacts.</summary>
public ReadOnlySpan<int> ContactCount => Lane(_v.ContactCount, Count);
/// <summary>The material id the body was added with (0 by default).</summary>

View file

@ -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<InvalidOperationException>(() => 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();