diff --git a/native/Ymir.Box3D/ymir_box3d.c b/native/Ymir.Box3D/ymir_box3d.c index 860bbbd..47c0854 100644 --- a/native/Ymir.Box3D/ymir_box3d.c +++ b/native/Ymir.Box3D/ymir_box3d.c @@ -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( diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index b54abf7..39f2080 100644 --- a/native/Ymir.Box3D/ymir_box3d_soa.c +++ b/native/Ymir.Box3D/ymir_box3d_soa.c @@ -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, diff --git a/src/Ymir.Box3D/Box3DRuntime.cs b/src/Ymir.Box3D/Box3DRuntime.cs index f1df20e..f44bfda 100644 --- a/src/Ymir.Box3D/Box3DRuntime.cs +++ b/src/Ymir.Box3D/Box3DRuntime.cs @@ -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 { diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index 18a9b06..57e867c 100644 --- a/src/Ymir.Box3D/Box3DSoaWorld.cs +++ b/src/Ymir.Box3D/Box3DSoaWorld.cs @@ -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; /// Metres per second, always positive. public readonly float ApproachSpeed; - /// 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. - public readonly float ImpulseEstimate; + /// 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. + public readonly float Impulse; public readonly ulong MaterialA; public readonly ulong MaterialB; } @@ -371,8 +371,8 @@ public readonly unsafe struct Box3DSoaView public Span TorqueZ => Write(_v.TorqueZ, Count); // Contact lanes, read-only, refreshed by every Step. - /// 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); + /// 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. + public ReadOnlySpan ContactImpulse => Lane(_v.ContactImpulse, Count); /// Number of touching contacts. public ReadOnlySpan ContactCount => Lane(_v.ContactCount, Count); /// The material id the body was added with (0 by default). @@ -417,7 +417,7 @@ public readonly unsafe struct Box3DSoaView /// public sealed class Box3DSoaWorld : IDisposable { - private const uint ExpectedAbi = 6; + private const uint ExpectedAbi = 7; private nint _handle; /// diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs index 550492a..c1c9deb 100644 --- a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs +++ b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaContactTests.cs @@ -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(); - 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(); diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs index 96b2313..317cf08 100644 --- a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs +++ b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs @@ -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(() => Box3DSoaWorld.RequireNativeAbi(5)); // an older native under this managed build - Assert.Throws(() => Box3DSoaWorld.RequireNativeAbi(7)); // a newer one + Assert.Equal(7u, Box3DSoaNative.ymir_box3d_soa_get_abi_version()); + Box3DSoaWorld.RequireNativeAbi(7); + Assert.Throws(() => Box3DSoaWorld.RequireNativeAbi(6)); // an older native under this managed build + Assert.Throws(() => Box3DSoaWorld.RequireNativeAbi(8)); // a newer one } [Fact] diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaImpulseTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaImpulseTests.cs new file mode 100644 index 0000000..0443544 --- /dev/null +++ b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaImpulseTests.cs @@ -0,0 +1,215 @@ +using Ymir.Box3D; + +namespace Ymir.Box3D.Soa.Tests; + +/// +/// 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. +/// +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; + + /// Momentum along Y of the given lanes with their masses. + 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; + } + + /// + /// 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. + /// + 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); + } + + /// 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. + 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); + } +}