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:
parent
72e9d3e71e
commit
99f1d0bfaf
7 changed files with 273 additions and 183 deletions
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
|
|
|||
215
tests/Ymir.Box3D.Soa.Tests/Box3DSoaImpulseTests.cs
Normal file
215
tests/Ymir.Box3D.Soa.Tests/Box3DSoaImpulseTests.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue