SoA world: exact per-step contact impulse per body and per hit; ABI 7

contact_impulse (N s) replaces last_substep_force in the same lane slot: the sum of
Box3D's stepNormalImpulse over the body's touching manifold points, the normal impulse
contacts delivered over the whole step. The hit fact's impulse replaces impulse_estimate in
the same slot: the same sum over the manifold of the hit's shape pair. The approach-speed
formula (ymir_soa_hit_impulse) and the last-sub-step scaling are gone, with the tests that
pinned their estimates; the new tests check the lane and the hit against momentum change
per step on a jointed forearm (5 kg ball, 0.3 g BB, arm onto a slab), drops, rest and two
free bodies. Build id names the fork commit.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
This commit is contained in:
Meta Vens 2026-11-20 02:57:48 +01:00
commit 99f1d0bfaf
7 changed files with 273 additions and 183 deletions

View file

@ -1244,7 +1244,7 @@ YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_get_abi_version(void)
YMIR_EXPORT const char* YMIR_CALL ymir_box3d_get_build_id(void)
{
return "box3d-8441b4a06d6d09dcfb0b0f704df4d847d1437b92-abi5-f32";
return "box3d-8441b4a-gc-6cfeeff-abi5-f32";
}
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_get_abi_layout(

View file

@ -57,7 +57,7 @@ enum
YMIR_SOA_JOINT_REMOVED = 4 // a destroyed joint's lane: never accepted by add_joint
};
#define YMIR_SOA_ABI_VERSION 6u
#define YMIR_SOA_ABI_VERSION 7u
// A body to create. `size` by kind: sphere {radius}, capsule {radius, half_length} (segment along
// local Y), box and static box {half_x, half_y, half_z}. `parent` is the lane index of an earlier body
@ -168,13 +168,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. 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.02x to 1.9x the true force on an impact step. For impact severity see the hit fact's impulse_estimate and
// the limits stated there.
// Contact lanes, read-only, refreshed every step. contact_impulse is N s: the sum over the body's touching
// contact manifold points of stepNormalImpulse, the normal impulse the contacts delivered to the body over
// the whole step (0 for a body touching nothing). Exact, normal channel only: Box3D exposes no tangent
// impulse. A resting load is contact_impulse / dt.
// contact_count is the number of touching contacts. material_id is the body's desc material_id.
const float* last_substep_force;
const float* contact_impulse;
const int32_t* contact_count;
const uint64_t* material_id;
// Per joint slot: counts the destroys of that slot. A grab's handle is (lane, generation); destroy_joint refuses
@ -201,14 +200,9 @@ typedef struct ymir_box3d_soa_hit_fact
float point[3];
float normal[3];
float approach_speed;
// 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 centre-to-centre hit between free bodies (a sphere on a plane, two spheres). It can over-count
// when one collision yields several hit facts (box corners, tilted capsules), which sum, and it undercounts
// heavily for jointed bodies (it takes the limb's own mass, not the joint-coupled effective mass). It is not
// a bound either way. A mass-aware per-body impulse that holds for jointed bodies is being designed.
float impulse_estimate;
// The normal impulse (N s) the pair's contact delivered over the whole step: the sum of stepNormalImpulse
// over the manifold of the two shapes, read after the step. Exact, normal channel only.
float impulse;
uint64_t material_a;
uint64_t material_b;
} ymir_box3d_soa_hit_fact;
@ -246,8 +240,8 @@ typedef struct ymir_box3d_soa_ray_hit
#define YMIR_SOA_STATE_LANES 13
#define YMIR_SOA_WRITE_LANES 6
#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_CONTACT_IMPULSE_LANE (YMIR_SOA_STATE_LANES + YMIR_SOA_WRITE_LANES)
#define YMIR_SOA_BODY_FLOAT_LANES (YMIR_SOA_CONTACT_IMPULSE_LANE + 1)
#define YMIR_SOA_JOINT_FLOAT_LANES 12
typedef struct ymir_box3d_soa_world
@ -902,24 +896,36 @@ static void ymir_soa_touch_fact(
(uint64_t)(uintptr_t)b3Shape_GetUserData(b)};
}
static float ymir_soa_inverse_mass(b3BodyId body)
// The normal impulse (N s) a contact's manifold points delivered over the step.
static float ymir_soa_manifold_impulse(const b3ContactData* data)
{
return b3Body_GetType(body) == b3_dynamicBody ? 1.0f / b3Body_GetMass(body) : 0.0f;
float impulse = 0.0f;
for (int m = 0; m < data->manifoldCount; ++m)
{
const b3Manifold* manifold = data->manifolds + m;
for (int p = 0; p < manifold->pointCount; ++p)
{
impulse += manifold->points[p].stepNormalImpulse;
}
}
return impulse;
}
static float ymir_soa_hit_impulse(const ymir_box3d_soa_world* world, const b3ContactHitEvent* e)
// A hit's impulse: the contact between its two shapes. A body has one shape, so a shape has at most
// capacity - 1 contacts and the scratch holds them all.
static float ymir_soa_pair_impulse(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)
int n = b3Shape_GetContactData(e->shapeIdA, world->contact_scratch, world->capacity);
for (int c = 0; c < n; ++c)
{
return 0.0f;
const b3ContactData* data = world->contact_scratch + c;
b3ShapeId other = B3_ID_EQUALS(data->shapeIdA, e->shapeIdA) ? data->shapeIdB : data->shapeIdA;
if (B3_ID_EQUALS(other, e->shapeIdB))
{
return ymir_soa_manifold_impulse(data);
}
}
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;
return 0.0f;
}
// Rewrites the fact arrays from Box3D's contact events for the step just taken, in Box3D's event order.
@ -956,39 +962,27 @@ 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),
.impulse = ymir_soa_pair_impulse(world, e),
.material_a = e->userMaterialIdA,
.material_b = e->userMaterialIdB,
};
}
}
// 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)
// Per body: the touching contacts' count and the normal impulse they delivered over the whole step.
static void ymir_soa_collect_contact_lanes(ymir_box3d_soa_world* world)
{
float* force = ymir_soa_lane(world, YMIR_SOA_LAST_SUBSTEP_FORCE_LANE);
float* lane = ymir_soa_lane(world, YMIR_SOA_CONTACT_IMPULSE_LANE);
for (int i = 0; i < world->count; ++i)
{
int n = b3Body_GetContactData(world->bodies[i], world->contact_scratch, world->capacity);
float impulse = 0.0f;
for (int c = 0; c < n; ++c)
{
const b3ContactData* data = world->contact_scratch + c;
for (int m = 0; m < data->manifoldCount; ++m)
{
const b3Manifold* manifold = data->manifolds + m;
for (int p = 0; p < manifold->pointCount; ++p)
{
impulse += manifold->points[p].normalImpulse;
}
}
impulse += ymir_soa_manifold_impulse(world->contact_scratch + c);
}
world->contact_count[i] = n;
force[i] = impulse * (float)sub_step_count / time_step;
lane[i] = impulse;
}
}
@ -1015,7 +1009,7 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f
}
}
ymir_soa_collect_facts(world);
ymir_soa_collect_contact_lanes(world, time_step, sub_step_count);
ymir_soa_collect_contact_lanes(world);
for (int i = 0; i < world->count; ++i)
{
ymir_soa_refresh(world, i);
@ -1169,7 +1163,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,
.last_substep_force = base + YMIR_SOA_LAST_SUBSTEP_FORCE_LANE * n,
.contact_impulse = base + YMIR_SOA_CONTACT_IMPULSE_LANE * n,
.contact_count = world->contact_count,
.material_id = world->material_id,
.joint_generation = world->joint_generation,

View file

@ -3,7 +3,7 @@ namespace Ymir.Box3D;
public static class Box3DRuntime
{
public const string NativeBuildId =
"box3d-8441b4a06d6d09dcfb0b0f704df4d847d1437b92-abi5-f32";
"box3d-8441b4a-gc-6cfeeff-abi5-f32";
public static string ValidatedBuildId
{

View file

@ -132,7 +132,7 @@ internal unsafe struct Box3DSoaViewNative
public float* JointAngularVelocityX;
public float* JointAngularVelocityY;
public float* JointAngularVelocityZ;
public float* LastSubstepForce;
public float* ContactImpulse;
public int* ContactCount;
public ulong* MaterialId;
public uint* JointGeneration;
@ -177,8 +177,8 @@ public readonly struct Box3DSoaHit
public readonly float NormalX, NormalY, NormalZ;
/// <summary>Metres per second, always positive.</summary>
public readonly float ApproachSpeed;
/// <summary>Normal impulse (N s) the collision must deliver: v (1 + e) / (1/mA + 1/mB). Exact for a centre-to-centre hit between free bodies; it can over-count when one collision yields several hit facts (box corners, tilted capsules), which sum, and it undercounts heavily for jointed bodies (it takes the limb's own mass). It is not a bound. A mass-aware per-body impulse that holds for jointed bodies is being designed.</summary>
public readonly float ImpulseEstimate;
/// <summary>Normal impulse (N s) the pair's contact delivered over the whole step: the exact sum of Box3D's stepNormalImpulse over the manifold of the two shapes. Normal channel only.</summary>
public readonly float Impulse;
public readonly ulong MaterialA;
public readonly ulong MaterialB;
}
@ -371,8 +371,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 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>N s: the normal impulse the body's touching contact points delivered over the whole step, exact (the sum of Box3D's stepNormalImpulse). Normal channel only. A resting load is this over dt. 0 for a body touching nothing.</summary>
public ReadOnlySpan<float> ContactImpulse => Lane(_v.ContactImpulse, 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>
@ -417,7 +417,7 @@ public readonly unsafe struct Box3DSoaView
/// </summary>
public sealed class Box3DSoaWorld : IDisposable
{
private const uint ExpectedAbi = 6;
private const uint ExpectedAbi = 7;
private nint _handle;
/// <summary>

View file

@ -90,21 +90,6 @@ public sealed class Box3DSoaContactTests
Assert.Equal(0, world.View.ContactCount[1]);
}
[Fact]
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.LastSubstepForce[1], weight * 0.98f, weight * 1.02f);
Assert.Equal(1, view.ContactCount[1]);
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]);
}
[Fact]
public void Materials_ride_hits_in_pair_order_and_read_back_as_a_lane()
{
@ -159,110 +144,6 @@ public sealed class Box3DSoaContactTests
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);
}
[Theory]
[InlineData(1f, 1f, 0f)]
[InlineData(1f, 10f, 0f)]
[InlineData(10f, 1f, 0f)]
[InlineData(1f, 1f, 0.5f)]
[InlineData(1f, 10f, 0.5f)]
[InlineData(10f, 1f, 0.5f)]
public void The_hit_impulse_estimate_of_two_free_bodies_uses_both_masses(float densityA, float densityB, float restitution)
{
// No gravity and nothing else touching: the impulse the pair exchanges is exactly m_A (v_A after - before).
const int hz = 120;
using var world = new Box3DSoaWorld(capacity: 4, gravityY: 0f);
var a = world.AddSphere(10, (-1f, 0f, 0f), Radius, densityA, velocity: (3f, 0f, 0f), restitution: restitution);
world.AddSphere(11, (1f, 0f, 0f), Radius, densityB, velocity: (-3f, 0f, 0f), restitution: restitution);
var massA = BallMass * densityA;
var vBefore = world.View.VelocityX[a];
var hits = new List<Box3DSoaHit>();
for (var i = 0; i < hz / 2; i++)
{
world.Step(1f / hz);
hits.AddRange(world.Facts.Hit.ToArray());
}
var hit = Assert.Single(hits);
Assert.InRange(hit.ApproachSpeed, 5.8f, 6.2f);
var truth = MathF.Abs(massA * (world.View.VelocityX[a] - vBefore));
Assert.InRange(hit.ImpulseEstimate, truth * 0.95f, truth * 1.05f);
}
[Fact]
public void Restitution_is_ignored_below_the_restitution_threshold_even_for_a_hit()
{
// Hit threshold 0.1 m/s sits below Box3D's restitution threshold (1 m/s): a 0.5 m/s touch is a hit that
// does not bounce, so its impulse is m v, not m v (1 + e).
using var bouncy = new Box3DSoaWorld(capacity: 4, gravityY: -G, hitThreshold: 0.1f);
bouncy.AddStaticBox(GroundId, (0f, -0.5f, 0f), (50f, 0.5f, 50f));
bouncy.AddSphere(BallId, (0f, Radius + 0.0125f, 0f), Radius, density: 1f, restitution: 0.5f);
Box3DSoaHit hit = default;
var hits = 0;
for (var i = 0; i < 60; i++)
{
bouncy.Step(Dt);
hits += bouncy.Facts.Hit.Length;
if (bouncy.Facts.Hit.Length > 0) hit = bouncy.Facts.Hit[0];
}
Assert.Equal(1, hits);
Assert.InRange(hit.ApproachSpeed, 0.15f, 0.9f); // above the hit threshold, below the restitution threshold
Assert.InRange(hit.ImpulseEstimate, BallMass * hit.ApproachSpeed * 0.99f, BallMass * hit.ApproachSpeed * 1.01f);
Assert.True(MathF.Abs(bouncy.View.VelocityY[1]) < 0.1f, "the ball did not bounce");
}
private static Box3DSoaWorld Pile()
{
var world = new Box3DSoaWorld(capacity: 32, gravityY: -G);
@ -286,7 +167,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.LastSubstepForce).ToArray())
.Concat(MemoryMarshal.AsBytes(view.ContactImpulse).ToArray())
.Concat(MemoryMarshal.AsBytes(view.ContactCount).ToArray())
.Concat(BitConverter.GetBytes(facts.Dropped.Begin + facts.Dropped.End + facts.Dropped.Hit))
.ToArray();
@ -307,7 +188,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.LastSubstepForce.ToArray(), f => f > 0f);
Assert.Contains(a.View.ContactImpulse.ToArray(), f => f > 0f);
}
[Fact]
@ -319,7 +200,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.LastSubstepForce[1] + view.ContactCount[1];
return facts.Begin.Length + facts.End.Length + facts.Hit.Length + (long)view.ContactImpulse[1] + view.ContactCount[1];
}
Read();
var before = GC.GetAllocatedBytesForCurrentThread();

View file

@ -165,10 +165,10 @@ public sealed class Box3DSoaGrabTests
[Fact]
public void The_managed_side_refuses_a_native_library_of_another_abi_in_either_direction()
{
Assert.Equal(6u, Box3DSoaNative.ymir_box3d_soa_get_abi_version());
Box3DSoaWorld.RequireNativeAbi(6);
Assert.Throws<InvalidOperationException>(() => Box3DSoaWorld.RequireNativeAbi(5)); // an older native under this managed build
Assert.Throws<InvalidOperationException>(() => Box3DSoaWorld.RequireNativeAbi(7)); // a newer one
Assert.Equal(7u, Box3DSoaNative.ymir_box3d_soa_get_abi_version());
Box3DSoaWorld.RequireNativeAbi(7);
Assert.Throws<InvalidOperationException>(() => Box3DSoaWorld.RequireNativeAbi(6)); // an older native under this managed build
Assert.Throws<InvalidOperationException>(() => Box3DSoaWorld.RequireNativeAbi(8)); // a newer one
}
[Fact]

View file

@ -0,0 +1,215 @@
using Ymir.Box3D;
namespace Ymir.Box3D.Soa.Tests;
/// <summary>
/// ContactImpulse and the hit fact's Impulse are the normal impulse contacts delivered over the whole step. The
/// oracle is momentum: only contacts and gravity act, so over one step the contact impulse along the contact normal
/// is the change of the system's momentum along it plus the weight's impulse, whatever the joints and sub-steps did.
/// </summary>
public sealed class Box3DSoaImpulseTests
{
private const float G = 10f;
private static readonly (float, float, float, float) QuarterTurnZ = (0f, 0f, MathF.Sin(MathF.PI / 4), MathF.Cos(MathF.PI / 4));
private static float SphereMass(float radius, float density) => 4f / 3f * MathF.PI * radius * radius * radius * density;
private static float CapsuleMass(float radius, float halfLength, float density) =>
MathF.PI * radius * radius * (2f * halfLength + 4f / 3f * radius) * density;
/// <summary>Momentum along Y of the given lanes with their masses.</summary>
private static double MomentumY(Box3DSoaWorld world, (int Lane, float Mass)[] bodies)
{
double p = 0;
foreach (var (lane, mass) in bodies) p += (double)mass * world.View.VelocityY[lane];
return p;
}
/// <summary>
/// Torso box 23.8 kg, upper-arm capsule on a spherical shoulder and forearm capsule on a revolute elbow, as in the
/// probe that measured Box3D. Lanes 0, 1, 2; the arm lies along +X with the forearm's centre at x = 0.78.
/// </summary>
private static (Box3DSoaWorld World, (int Lane, float Mass)[] Arm) Arm(float gravityY)
{
var world = new Box3DSoaWorld(capacity: 16, gravityY: gravityY, jointCapacity: 2);
var torso = world.AddBox(1, (0f, 0f, 0f), (0.175f, 0.25f, 0.1f), 680f);
var upper = world.AddCapsule(2, (0.40f, 0.2f, 0f), 0.045f, 0.15f, 1000f, rotation: QuarterTurnZ);
var fore = world.AddCapsule(3, (0.78f, 0.2f, 0f), 0.04f, 0.14f, 1000f, rotation: QuarterTurnZ);
world.AddJoint(Box3DSoaJointKind.Spherical, torso, upper, (0.2f, 0.2f, 0f), (0f, 0f, 1f));
world.AddJoint(Box3DSoaJointKind.Revolute, upper, fore, (0.6f, 0.2f, 0f), (0f, 0f, 1f));
var arm = new[] { (torso, 0.35f * 0.5f * 0.2f * 680f), (upper, CapsuleMass(0.045f, 0.15f, 1000f)), (fore, CapsuleMass(0.04f, 0.14f, 1000f)) };
return (world, arm);
}
/// <summary>Fires a ball at the forearm from above; returns the sum over the run of the forearm's ContactImpulse and the ball's -m dv.</summary>
private static (double Delivered, double Truth, double Peak, int Hits) Strike(float radius, float mass, float speed, float restitution, float gap, int hz, int subSteps, int steps)
{
var (world, _) = Arm(0f);
using var _w = world;
const int fore = 2;
// Box3D opens a contact only once shapes overlap and a fast ball would be through the forearm by then (its
// continuous collision is for static shapes and bullets, which the SoA world has none of). So the ball starts at
// rest where the contact exists (a negative gap overlaps by that much) and an impulse launches it into the forearm.
var ball = world.AddSphere(10, (0.78f, 0.2f + 0.04f + radius + gap, 0f), radius, mass / SphereMass(radius, 1f), restitution: restitution);
var dt = 1f / hz;
world.Step(dt, subSteps);
world.Step(dt, subSteps);
world.ApplyImpulse(ball, (0f, -mass * speed, 0f), (0.78f, 0.2f + 0.04f + radius + gap, 0f));
var launched = -(double)mass * speed;
var truths = new double[steps];
var lanes = new float[steps];
var hitImpulse = new float[steps];
var hits = 0;
var before = launched;
for (var i = 0; i < steps; i++)
{
world.Step(dt, subSteps);
var after = (double)mass * world.View.VelocityY[ball];
truths[i] = after - before;
before = after;
lanes[i] = world.View.ContactImpulse[fore];
foreach (var hit in world.Facts.Hit) { hitImpulse[i] += hit.Impulse; hits++; }
}
var peak = truths.Max(Math.Abs);
for (var i = 0; i < steps; i++)
{
Assert.True(Math.Abs(lanes[i] - truths[i]) <= 1e-4 * peak, $"step {i}: forearm ContactImpulse {lanes[i]} vs the ball's momentum change {truths[i]}");
if (hitImpulse[i] != 0f) Assert.Equal(lanes[i], hitImpulse[i], 1e-4 * peak);
}
var delivered = (double)lanes.Sum();
var truth = truths.Sum();
return (delivered, truth, peak, hits);
}
[Theory]
[InlineData(0f)]
[InlineData(0.5f)]
public void A_jointed_forearm_hit_is_exact_per_step_and_the_hit_carries_the_same_impulse(float restitution)
{
var (delivered, truth, peak, hits) = Strike(0.1f, 5f, 10f, restitution, gap: 0.05f, hz: 60, subSteps: 4, steps: 9);
Assert.InRange(peak, 14f, 25f); // a 5 kg ball at 10 m/s onto a forearm: tens of N s, not the tenth the ball's own approach speed implies
Assert.Equal(truth, delivered, 1e-4 * peak);
Assert.Equal(1, hits); // the hit fact carried the impulse checked above
}
[Fact]
public void A_BB_is_not_a_cannonball()
{
var (bb, bbTruth, _, _) = Strike(0.003f, 0.0003f, 100f, 0f, gap: -0.001f, hz: 60, subSteps: 4, steps: 9);
Assert.True(bb >= 0.030f * 0.99f && bb <= 0.030f * 1.01f, $"BB delivered {bb} N s");
Assert.Equal(bbTruth, bb, 1e-6);
var (ball, _, _, _) = Strike(0.1f, 5f, 10f, 0f, gap: 0.05f, hz: 60, subSteps: 4, steps: 9);
Assert.True(ball > 400f * bb, $"the ball's {ball} N s dwarfs the BB's {bb} N s");
}
[Fact]
public void A_forearm_falling_onto_a_wall_delivers_the_whole_arms_momentum_change()
{
// The forearm lands on a slab while the shoulder and elbow keep the rest of the arm moving: the wall's impulse is
// the change of the three bodies' momentum plus their weight, not the forearm's own.
var (world, arm) = Arm(-G);
using var _w = world;
world.AddStaticBox(9, (1.0f, 0.2f - 0.04f - 0.8f - 0.1f, 0f), (0.3f, 0.1f, 0.5f)); // under the forearm only: the upper arm never reaches it
var totalMass = arm.Sum(b => b.Mass);
const int fore = 2;
var dt = 1f / 60f;
double peak = 0;
var truths = new List<(double Truth, float Lane)>();
for (var i = 0; i < 90; i++)
{
var before = MomentumY(world, arm);
world.Step(dt, 4);
var truth = MomentumY(world, arm) - before + totalMass * G * dt;
truths.Add((truth, world.View.ContactImpulse[fore]));
peak = Math.Max(peak, Math.Abs(truth));
}
Assert.True(peak > 10.0, $"the arm hit the slab (peak {peak} N s)");
for (var i = 0; i < truths.Count; i++)
{
Assert.True(Math.Abs(truths[i].Lane - truths[i].Truth) <= 1e-4 * peak, $"step {i}: {truths[i].Lane} vs {truths[i].Truth}");
}
}
[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 Drop_impulse_is_exact_every_step(int hz, int subSteps, float restitution)
{
using var world = new Box3DSoaWorld(capacity: 4, gravityY: -G);
world.AddStaticBox(1, (0f, -0.5f, 0f), (50f, 0.5f, 50f));
const float radius = 0.1f;
const float density = 125f; // 0.52 kg
var ball = world.AddSphere(2, (0f, 1.5f, 0f), radius, density, restitution: restitution);
var mass = SphereMass(radius, density);
var dt = 1f / hz;
var truths = new double[hz];
var lanes = new float[hz];
var hits = 0;
for (var i = 0; i < hz; i++)
{
var before = (double)mass * world.View.VelocityY[ball];
world.Step(dt, subSteps);
truths[i] = mass * (double)world.View.VelocityY[ball] - before + mass * G * dt;
lanes[i] = world.View.ContactImpulse[ball];
foreach (var hit in world.Facts.Hit)
{
hits++;
Assert.Equal(lanes[i], hit.Impulse, 1e-4 * truths.Max(Math.Abs));
}
}
var peak = truths.Max(Math.Abs);
Assert.InRange(peak, 2.5, 6.0);
Assert.True(hits >= 1);
for (var i = 0; i < hz; i++)
{
Assert.True(Math.Abs(lanes[i] - truths[i]) <= 1e-4 * peak, $"step {i}: {lanes[i]} vs {truths[i]}");
}
}
[Fact]
public void Resting_impulse_is_the_weight_and_a_body_touching_nothing_reads_zero()
{
using var world = new Box3DSoaWorld(capacity: 8, gravityY: -G);
world.AddStaticBox(1, (0f, -0.5f, 0f), (50f, 0.5f, 50f));
world.AddSphere(2, (0f, 0.6f, 0f), 0.5f, 1f);
world.AddSphere(3, (10f, 1000f, 10f), 0.5f, 1f); // still falling after the whole run
var dt = 1f / 60f;
for (var i = 0; i < 300; i++) world.Step(dt);
var weight = SphereMass(0.5f, 1f) * G;
var view = world.View;
Assert.InRange(view.ContactImpulse[1] / dt, weight * 0.999f, weight * 1.001f);
Assert.InRange(view.ContactImpulse[0] / dt, weight * 0.999f, weight * 1.001f); // the slab carries the same load
Assert.Equal(0f, view.ContactImpulse[2]);
}
[Theory]
[InlineData(0f)]
[InlineData(0.5f)]
public void Two_free_bodies_exchange_exactly_the_momentum_B_gains(float restitution)
{
using var world = new Box3DSoaWorld(capacity: 4, gravityY: 0f);
world.AddSphere(10, (-0.5f, 0f, 0f), 0.1f, 1f / SphereMass(0.1f, 1f), velocity: (5f, 0f, 0f), restitution: restitution);
var b = world.AddSphere(11, (0f, 0f, 0f), 0.1f, 3f / SphereMass(0.1f, 1f), restitution: restitution);
const float massB = 3f;
const float dt = 1f / 60f;
double delivered = 0, truth = 0, peak = 0;
for (var i = 0; i < 12; i++)
{
var before = (double)massB * world.View.VelocityX[b];
world.Step(dt);
var step = massB * (double)world.View.VelocityX[b] - before;
peak = Math.Max(peak, Math.Abs(step));
Assert.True(Math.Abs(world.View.ContactImpulse[b] - step) <= 1e-4 * Math.Max(peak, 1e-6), $"step {i}");
foreach (var hit in world.Facts.Hit) Assert.Equal(world.View.ContactImpulse[b], hit.Impulse, 1e-4 * peak);
delivered += world.View.ContactImpulse[b];
truth += step;
}
Assert.InRange(peak, 3.5, 6.0);
Assert.Equal(truth, delivered, 1e-4 * peak);
}
}