From 83ea2b8fa445acc8c9a63fb411385b1024d9d067 Mon Sep 17 00:00:00 2001 From: Meta Vens Date: Fri, 20 Nov 2026 02:28:47 +0100 Subject: [PATCH] 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 --- native/Ymir.Box3D/ymir_box3d_soa.c | 88 +++++++++++++- src/Ymir.Box3D/Box3DSoaWorld.cs | 48 +++++++- .../Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs | 114 ++++++++++++++++++ 3 files changed, 242 insertions(+), 8 deletions(-) diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index 2abef67..abb4efb 100644 --- a/native/Ymir.Box3D/ymir_box3d_soa.c +++ b/native/Ymir.Box3D/ymir_box3d_soa.c @@ -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) { diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index d6cc9b1..e9dad3f 100644 --- a/src/Ymir.Box3D/Box3DSoaWorld.cs +++ b/src/Ymir.Box3D/Box3DSoaWorld.cs @@ -18,7 +18,9 @@ public enum Box3DSoaJointKind : uint Spherical = 1, Weld = 2, /// Position springs pulling body B's frame onto body A's, bounded by a maximum force and torque. - Motor = 3 + Motor = 3, + /// The kind lane of a destroyed joint (a tombstone): bodies read -1 and every other lane 0. + Removed = 4 } /// Native layout of ymir_box3d_soa_body_desc. @@ -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; } + /// + /// Releases a motor joint (a grab); the anchor and the grabbed body stay. The joint's lane becomes a tombstone: + /// kind , bodies -1, every other lane 0. It is reused by a later AddJoint only + /// once a 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. + /// + public void DestroyJoint(int jointLane) + { + ObjectDisposedException.ThrowIf(_handle == 0, this); + Check(Box3DSoaNative.ymir_box3d_soa_destroy_joint(_handle, jointLane)); + } + + /// 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. + public int FreeJointSlots + { + get + { + ObjectDisposedException.ThrowIf(_handle == 0, this); + return Box3DSoaNative.ymir_box3d_soa_free_joint_slots(_handle); + } + } + + /// Bodies that can still be added. Bodies are never removed here; keep one anchor per cursor and reuse it. + public int FreeBodySlots + { + get + { + var v = View; + return v.Capacity - v.Count; + } + } + /// Adds a kinematic anchor with no shape (rotation as for ); returns its lane index. 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)); /// /// Sets an anchor's pose for the coming step. Box3D gives the anchor the velocity that reaches it after - /// , so pass the same dt as the next and the anchor arrives at its end. + /// , so pass the same dt as the next 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. /// public unsafe void SetAnchorTarget( diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs index fc887f7..a4134db 100644 --- a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs +++ b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs @@ -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(() => 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(() => world.DestroyJoint(hinge)); + Assert.Throws(() => world.DestroyJoint(weld)); + Assert.Throws(() => world.DestroyJoint(-1)); + Assert.Throws(() => world.DestroyJoint(3)); + var view = world.View; + Assert.Equal((uint)Box3DSoaJointKind.Revolute, view.JointKind[hinge]); + world.DestroyJoint(motor); + Assert.Throws(() => 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(() => 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() {