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()
{