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:
parent
a614477d68
commit
5c35a40ccf
4 changed files with 131 additions and 29 deletions
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Reference in a new issue