SoA world: release a grab (destroy_joint tombstones), anchors hold still after arriving

destroy_joint releases a motor joint only; its lane reads Removed and is reused
after a step has passed. free_joint_slots lets a caller check before adding an
anchor. After every step anchor velocity is zeroed so a skipped frame flings
nothing. Fixes Soul s1 findings (no release, anchor keeps moving).

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
This commit is contained in:
Meta Vens 2026-11-20 02:28:47 +01:00
commit 83ea2b8fa4
3 changed files with 242 additions and 8 deletions

View file

@ -53,7 +53,8 @@ enum
YMIR_SOA_JOINT_REVOLUTE = 0,
YMIR_SOA_JOINT_SPHERICAL = 1,
YMIR_SOA_JOINT_WELD = 2,
YMIR_SOA_JOINT_MOTOR = 3
YMIR_SOA_JOINT_MOTOR = 3,
YMIR_SOA_JOINT_REMOVED = 4 // a destroyed joint's lane: never accepted by add_joint
};
#define YMIR_SOA_ABI_VERSION 5u
@ -90,6 +91,8 @@ typedef struct ymir_box3d_soa_body_desc
// frame origin onto body A's (so B's anchor point onto A's), the angular spring pulls B's frame orientation onto
// A's. `*_hertz` 0 turns that spring off; `max_force` (N) and `max_torque` (N m) bound what it may apply, so a
// weak grab hangs below its target. All six are finite and >= 0 and are ignored by the other kinds.
// `ymir_box3d_soa_destroy_joint` releases a motor joint: its lane becomes a tombstone (kind REMOVED = 4, bodies -1,
// every lane 0) that add_joint may reuse only once a step has completed since.
typedef struct ymir_box3d_soa_joint_desc
{
uint32_t kind;
@ -263,6 +266,7 @@ typedef struct ymir_box3d_soa_world
float* lanes; // YMIR_SOA_BODY_FLOAT_LANES arrays of `capacity` floats, lane-major
b3JointId* joints;
uint32_t* joint_kind;
uint64_t* joint_removed_at; // step_count when the slot was destroyed; reusable once a step has passed
int32_t* joint_body_a;
int32_t* joint_body_b;
b3Quat* joint_local_a; // joint frame in body A, kept so the state is read without asking Box3D
@ -407,6 +411,7 @@ static void ymir_soa_free(ymir_box3d_soa_world* world)
free(world->lanes);
free(world->joints);
free(world->joint_kind);
free(world->joint_removed_at);
free(world->joint_body_a);
free(world->joint_body_b);
free(world->joint_local_a);
@ -455,6 +460,7 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_create(
world->lanes = (float*)calloc(n * YMIR_SOA_BODY_FLOAT_LANES, sizeof(float));
world->joints = (b3JointId*)calloc(m, sizeof(b3JointId));
world->joint_kind = (uint32_t*)calloc(m, sizeof(uint32_t));
world->joint_removed_at = (uint64_t*)calloc(m, sizeof(uint64_t));
world->joint_body_a = (int32_t*)calloc(m, sizeof(int32_t));
world->joint_body_b = (int32_t*)calloc(m, sizeof(int32_t));
world->joint_local_a = (b3Quat*)calloc(m, sizeof(b3Quat));
@ -470,7 +476,7 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_create(
world->bodies == NULL || world->stable_id == NULL || world->kind == NULL || world->parent == NULL ||
world->limb == NULL || world->size == NULL || world->lanes == NULL || world->joints == NULL || world->joint_kind == NULL ||
world->joint_body_a == NULL || world->joint_body_b == NULL || world->joint_local_a == NULL ||
world->joint_local_b == NULL || world->joint_lanes == NULL)
world->joint_local_b == NULL || world->joint_lanes == NULL || world->joint_removed_at == NULL)
{
ymir_soa_free(world);
return YMIR_SOA_OUT_OF_MEMORY;
@ -666,6 +672,36 @@ static b3Transform ymir_soa_body_transform(b3BodyId body)
return t;
}
// The lane a new joint takes: the lowest tombstone whose destroying step has completed, else the next fresh
// slot, else -1. A destroyed lane is not reused within the step that destroyed it, so lanes a caller read
// that step stay meaningful.
static int ymir_soa_joint_slot(const ymir_box3d_soa_world* world)
{
for (int j = 0; j < world->joint_count; ++j)
{
if (world->joint_kind[j] == YMIR_SOA_JOINT_REMOVED && world->step_count > world->joint_removed_at[j])
{
return j;
}
}
return world->joint_count < world->joint_capacity ? world->joint_count : -1;
}
// Joints that add_joint could create right now.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_free_joint_slots(const ymir_box3d_soa_world* world)
{
if (world == NULL)
{
return 0;
}
int32_t free_slots = world->joint_capacity - world->joint_count;
for (int j = 0; j < world->joint_count; ++j)
{
free_slots += world->joint_kind[j] == YMIR_SOA_JOINT_REMOVED && world->step_count > world->joint_removed_at[j];
}
return free_slots;
}
// Creates a joint between two existing bodies. The joint frame is built in world space (origin at the
// anchor, z along the axis) and expressed in each body's local frame, so the creation pose is zero.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_joint(
@ -688,7 +724,8 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_joint(
{
return YMIR_SOA_INVALID_ARGUMENT;
}
if (world->joint_count >= world->joint_capacity)
int slot = ymir_soa_joint_slot(world);
if (slot < 0)
{
return YMIR_SOA_FULL;
}
@ -754,7 +791,11 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_joint(
return YMIR_SOA_INTERNAL_ERROR;
}
int j = world->joint_count++;
int j = slot;
if (j == world->joint_count)
{
world->joint_count++;
}
world->joints[j] = joint;
world->joint_kind[j] = d->kind;
world->joint_body_a[j] = d->body_a;
@ -769,6 +810,27 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_joint(
return YMIR_SOA_OK;
}
// Releases a motor joint (a grab). Only motor joints: general joint removal is another cut's. The lane becomes a
// tombstone, kind REMOVED, bodies -1 and every lane 0, until a later step lets add_joint reuse it.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_destroy_joint(ymir_box3d_soa_world* world, int32_t joint)
{
if (world == NULL || joint < 0 || joint >= world->joint_count || world->joint_kind[joint] != YMIR_SOA_JOINT_MOTOR)
{
return YMIR_SOA_INVALID_ARGUMENT;
}
b3DestroyJoint(world->joints[joint], true);
world->joints[joint] = (b3JointId){0};
world->joint_kind[joint] = YMIR_SOA_JOINT_REMOVED;
world->joint_body_a[joint] = -1;
world->joint_body_b[joint] = -1;
world->joint_removed_at[joint] = world->step_count;
for (int lane = 0; lane < YMIR_SOA_JOINT_FLOAT_LANES; ++lane)
{
ymir_soa_joint_lane(world, lane)[joint] = 0.0f;
}
return YMIR_SOA_OK;
}
// Applies the caller's force and torque lanes to the dynamic bodies, then clears them: a lane holds one
// step's intent, so a stale force is never applied twice. A non-finite value refuses the whole step
// before any body is touched.
@ -931,6 +993,16 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f
return status;
}
b3World_Step(world->world_id, time_step, sub_step_count);
// An anchor moves only on a target: the one just applied has been reached, so it holds still until the
// next target. Otherwise its velocity would carry it on past the pose (and fling what it holds).
for (int i = 0; i < world->count; ++i)
{
if (ymir_soa_is_anchor(world->kind[i]))
{
b3Body_SetLinearVelocity(world->bodies[i], b3Vec3_zero);
b3Body_SetAngularVelocity(world->bodies[i], b3Vec3_zero);
}
}
ymir_soa_collect_facts(world);
ymir_soa_collect_contact_lanes(world, time_step, sub_step_count);
for (int i = 0; i < world->count; ++i)
@ -939,7 +1011,10 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f
}
for (int j = 0; j < world->joint_count; ++j)
{
ymir_soa_refresh_joint(world, j);
if (world->joint_kind[j] != YMIR_SOA_JOINT_REMOVED)
{
ymir_soa_refresh_joint(world, j);
}
}
world->step_count += 1;
return YMIR_SOA_OK;
@ -948,7 +1023,8 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f
// Grab and poke input, each an immediate command on the caller's say-so. Ymir decides nothing about grabbing.
// Sets an anchor's pose for the coming step: Box3D gives it the velocity that reaches the pose after
// `time_step` seconds, so pass the step's own dt and the anchor arrives at the end of that step.
// `time_step` seconds, so pass the step's own dt and the anchor arrives at the end of that step, where `step`
// stops it (velocity 0) until the next target.
YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_set_anchor_target(
ymir_box3d_soa_world* world, int32_t lane, const float* position, const float* rotation, float time_step)
{

View file

@ -18,7 +18,9 @@ public enum Box3DSoaJointKind : uint
Spherical = 1,
Weld = 2,
/// <summary>Position springs pulling body B's frame onto body A's, bounded by a maximum force and torque.</summary>
Motor = 3
Motor = 3,
/// <summary>The kind lane of a destroyed joint (a tombstone): bodies read -1 and every other lane 0.</summary>
Removed = 4
}
/// <summary>Native layout of <c>ymir_box3d_soa_body_desc</c>.</summary>
@ -267,6 +269,14 @@ internal static unsafe partial class Box3DSoaNative
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
internal static partial int ymir_box3d_soa_get_facts(nint world, out Box3DSoaFactsNative facts);
[LibraryImport(Library)]
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
internal static partial int ymir_box3d_soa_destroy_joint(nint world, int joint);
[LibraryImport(Library)]
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
internal static partial int ymir_box3d_soa_free_joint_slots(nint world);
[LibraryImport(Library)]
[UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
internal static partial int ymir_box3d_soa_set_anchor_target(nint world, int lane, float* position, float* rotation, float timeStep);
@ -530,13 +540,47 @@ public sealed class Box3DSoaWorld : IDisposable
return index;
}
/// <summary>
/// Releases a motor joint (a grab); the anchor and the grabbed body stay. The joint's lane becomes a tombstone:
/// kind <see cref="Box3DSoaJointKind.Removed"/>, bodies -1, every other lane 0. It is reused by a later AddJoint only
/// once a <see cref="Step"/> has completed, so lanes read this step stay valid. Throws for any other kind of joint
/// (general removal is another cut's) or a lane that is not a live joint.
/// </summary>
public void DestroyJoint(int jointLane)
{
ObjectDisposedException.ThrowIf(_handle == 0, this);
Check(Box3DSoaNative.ymir_box3d_soa_destroy_joint(_handle, jointLane));
}
/// <summary>Joints AddJoint could create right now: unused capacity plus tombstones whose step has completed.
/// Check it before adding an anchor for a grab, so a full world leaves no orphan anchor.</summary>
public int FreeJointSlots
{
get
{
ObjectDisposedException.ThrowIf(_handle == 0, this);
return Box3DSoaNative.ymir_box3d_soa_free_joint_slots(_handle);
}
}
/// <summary>Bodies that can still be added. Bodies are never removed here; keep one anchor per cursor and reuse it.</summary>
public int FreeBodySlots
{
get
{
var v = View;
return v.Capacity - v.Count;
}
}
/// <summary>Adds a kinematic anchor with no shape (rotation as for <see cref="AddCapsule"/>); returns its lane index.</summary>
public int AddAnchor(ulong stableId, (float X, float Y, float Z) position, (float X, float Y, float Z, float W) rotation = default) =>
AddBody(Describe(Box3DSoaKind.Anchor, stableId, position, rotation, default, 0f, default, 0f, 0f, -1, 0));
/// <summary>
/// Sets an anchor's pose for the coming step. Box3D gives the anchor the velocity that reaches it after
/// <paramref name="timeStep"/>, so pass the same dt as the next <see cref="Step"/> and the anchor arrives at its end.
/// <paramref name="timeStep"/>, so pass the same dt as the next <see cref="Step"/> and the anchor arrives at its end,
/// where Step stops it: with no new target it holds still (its velocity lanes read 0).
/// Throws for a lane that is not an anchor, a non-finite or non-unit input, or a non-positive time step.
/// </summary>
public unsafe void SetAnchorTarget(

View file

@ -60,6 +60,120 @@ public sealed class Box3DSoaGrabTests
Assert.Equal(2f, view.PositionY[anchor], 1e-3f);
}
[Fact]
public void An_anchor_holds_still_once_it_arrives_and_a_skipped_frame_flings_nothing()
{
using var world = new Box3DSoaWorld(capacity: 3, gravityY: 0f, jointCapacity: 1);
var sphere = world.AddSphere(1, (0f, 0f, 0f), Radius, Density);
var anchor = world.AddAnchor(2, (0f, 0f, 0f));
world.AddMotorJoint(anchor, sphere, (0f, 0f, 0f), (5f, 0.7f, 1e6f), (0f, 0f, 0f));
var view = world.View;
world.SetAnchorTarget(anchor, (1f, 0f, 0f), default, Dt); // one target, 1 m away, then the caller sets no more
for (var i = 0; i < 150; i++) world.Step(Dt);
Assert.Equal(1f, view.PositionX[anchor], 1e-4f);
Assert.Equal(0f, view.VelocityX[anchor]);
Assert.Equal(1f, view.PositionX[sphere], 0.02f);
Assert.True(MathF.Abs(view.VelocityX[sphere]) < 0.05f, "the grabbed body is not flung");
}
[Fact]
public void A_grab_can_be_released_and_repeated_without_the_slots_growing()
{
using var world = new Box3DSoaWorld(capacity: 4, gravityY: -Gravity, jointCapacity: 1);
var a = world.AddSphere(1, (0f, 5f, 0f), Radius, Density);
var b = world.AddSphere(2, (3f, 5f, 0f), Radius, Density);
var anchor = world.AddAnchor(3, (0f, 5f, 0f));
var view = world.View;
var strong = 20f * Mass * Gravity;
for (var cycle = 0; cycle < 1000; cycle++)
{
var grabbed = cycle % 2 == 0 ? a : b;
var at = (view.PositionX[grabbed], view.PositionY[grabbed], view.PositionZ[grabbed]);
world.SetAnchorTarget(anchor, at, default, Dt);
world.Step(Dt); // the anchor is on the body before the joint is made
Assert.Equal(1, world.FreeJointSlots);
var joint = world.AddMotorJoint(anchor, grabbed, at, (5f, 0.7f, strong), (0f, 0f, 0f));
Assert.Equal(0, joint);
world.Step(Dt);
world.DestroyJoint(joint);
var jv = world.View; // joint count is as of when the view was taken
// A destroyed lane is a tombstone this step and is not reused within it.
Assert.Equal((uint)Box3DSoaJointKind.Removed, jv.JointKind[joint]);
Assert.Equal((-1, -1), (jv.JointBodyA[joint], jv.JointBodyB[joint]));
Assert.Equal(0f, jv.JointFrameW[joint]);
Assert.Equal(0, world.FreeJointSlots);
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(anchor, grabbed, at, (5f, 0.7f, strong), (0f, 0f, 0f)));
world.Step(Dt);
}
Assert.Equal(3, world.View.Count);
Assert.Equal(1, world.View.JointCount);
// Released, the sphere is free: it fell.
Assert.True(view.PositionY[b] < 5f - 0.01f, "a released body falls");
}
[Fact]
public void A_regrab_after_release_follows_the_anchor()
{
using var world = new Box3DSoaWorld(capacity: 3, gravityY: 0f, jointCapacity: 1);
var sphere = world.AddSphere(1, (0f, 5f, 0f), Radius, Density);
var anchor = world.AddAnchor(2, (0f, 5f, 0f));
var view = world.View;
var strong = 20f * Mass * Gravity;
for (var round = 0; round < 3; round++)
{
var at = (view.PositionX[sphere], view.PositionY[sphere], view.PositionZ[sphere]);
world.SetAnchorTarget(anchor, at, default, Dt);
world.Step(Dt);
var joint = world.AddMotorJoint(anchor, sphere, at, (5f, 0.7f, strong), (0f, 0f, 0f));
var target = (1f + round, at.Item2, 0f);
for (var i = 0; i < 90; i++) { world.SetAnchorTarget(anchor, target, default, Dt); world.Step(Dt); }
Assert.True(Distance(view, sphere, target) < 0.02f, $"round {round}: {Distance(view, sphere, target)} m from the target");
world.DestroyJoint(joint);
world.Step(Dt);
// Released: the anchor can go elsewhere and the sphere stays where it was left.
var away = (target.Item1 + 5f, target.Item2, 0f);
for (var i = 0; i < 30; i++) { world.SetAnchorTarget(anchor, away, default, Dt); world.Step(Dt); }
Assert.True(MathF.Abs(view.PositionX[sphere] - target.Item1) < 0.1f, "the released sphere did not follow the anchor");
}
}
[Fact]
public void Only_a_live_motor_joint_can_be_destroyed()
{
using var world = new Box3DSoaWorld(capacity: 4, gravityY: 0f, jointCapacity: 3);
var pin = world.AddStaticBox(1, (0f, 5f, 0f), (0.1f, 0.1f, 0.1f));
var sphere = world.AddSphere(2, (1f, 5f, 0f), Radius, Density);
var anchor = world.AddAnchor(3, (1f, 5f, 0f));
var hinge = world.AddJoint(Box3DSoaJointKind.Revolute, pin, sphere, (0f, 5f, 0f), (0f, 0f, 1f));
var weld = world.AddJoint(Box3DSoaJointKind.Weld, anchor, sphere, (1f, 5f, 0f), (0f, 0f, 1f));
var motor = world.AddMotorJoint(anchor, sphere, (1f, 5f, 0f), (5f, 0.7f, 100f), (0f, 0f, 0f));
Assert.Throws<InvalidOperationException>(() => world.DestroyJoint(hinge));
Assert.Throws<InvalidOperationException>(() => world.DestroyJoint(weld));
Assert.Throws<InvalidOperationException>(() => world.DestroyJoint(-1));
Assert.Throws<InvalidOperationException>(() => world.DestroyJoint(3));
var view = world.View;
Assert.Equal((uint)Box3DSoaJointKind.Revolute, view.JointKind[hinge]);
world.DestroyJoint(motor);
Assert.Throws<InvalidOperationException>(() => world.DestroyJoint(motor)); // already gone
world.Step(Dt); // the live joints still step
Assert.Equal((uint)Box3DSoaJointKind.Weld, view.JointKind[weld]);
}
[Fact]
public void A_world_with_no_joint_slot_is_seen_before_an_anchor_is_added()
{
using var world = new Box3DSoaWorld(capacity: 4, jointCapacity: 0);
var sphere = world.AddSphere(1, (0f, 5f, 0f), Radius, Density);
Assert.Equal(0, world.FreeJointSlots);
Assert.Equal(3, world.FreeBodySlots);
var anchorLane = -1;
if (world.FreeJointSlots > 0) anchorLane = world.AddAnchor(2, (0f, 5f, 0f)); // the caller's check: no grab, no anchor
Assert.Equal(-1, anchorLane);
Assert.Equal(3, world.FreeBodySlots);
Assert.Throws<InvalidOperationException>(() => world.AddMotorJoint(sphere, sphere, (0f, 5f, 0f), (5f, 0.7f, 1f), (0f, 0f, 0f)));
Assert.Equal(1, world.View.Count);
}
[Fact]
public void The_angular_spring_turns_the_grabbed_body_to_the_anchors_rotation_and_off_leaves_it_free()
{