docs/soa-world.md

@@ -12,27 +12,45 @@ separate native object (`ymir_box3d_soa.c`, ABI 1) so the Aetheria session's con
 
 ## Contract
 
-- Native: `ymir_box3d_soa_create(capacity, gravity) / add_sphere / add_static_box / step / get_view / destroy`.
-  Capacity is fixed at creation, so lane pointers never move.
-- Lanes (`ymir_box3d_soa_view`): `stable_id`, `kind`, position xyz, rotation quaternion xyzw, linear
-  velocity xyz, angular velocity xyz. One contiguous array per component, indexed by body.
-- Lane contents change only inside `step`. Between steps a caller may read any lane in place.
-- Managed (`src/Ymir.Box3D/Box3DSoaWorld.cs`, internal): `Box3DSoaView` exposes each lane as a
-  `ReadOnlySpan<T>` over native memory. Spans are invalid after `Dispose`. `Count` is as of when the
-  view was taken, so take it after the last add.
+- Native (ABI 2): `ymir_box3d_soa_create(capacity, joint_capacity, gravity) / add_body / add_joint / step /
+  get_view / destroy`. `add_body` takes one descriptor (sphere, capsule, box, static box; position,
+  rotation, velocity, size, density, friction, restitution, rolling resistance, parent, limb). Capacities are fixed at creation,
+  so lane pointers never move.
+- Body lanes (`ymir_box3d_soa_view`): `stable_id`, `kind`, `parent` and `limb` (set at creation, not read by
+  Box3D: the limb tree for the consumer), position xyz, rotation quaternion xyzw, linear and angular
+  velocity xyz. One contiguous array per component, indexed by body.
+- Write lanes: per-body world `force` (at the centre of mass) and `torque`, xyz, mutable through the view.
+  `step` applies them to the dynamic bodies and clears them, so a lane holds one step's intent. A
+  non-finite value refuses the whole step before any body is touched.
+- Joints: `add_joint` takes two body indices (B is the child), a world anchor and axis, and a kind
+  (revolute, spherical, weld). The joint frame has its z axis on the axis (the hinge axis); the creation
+  pose is the joint's zero. Optional limits: revolute angle, spherical swing cone and twist. Joint lanes:
+  `joint_kind`, `joint_body_a/b`, `joint_angle` (Box3D's revolute angle or spherical twist), `joint_frame`
+  (world orientation of frame A, quaternion), `joint_rotation` (frame B relative to frame A, quaternion,
+  w >= 0) and `joint_angular_velocity` (B minus A, in frame A). A consumer can compute a joint torque from
+  these and write it as equal and opposite torques on the two body lanes.
+- State lanes change only inside `step`. Between steps a caller may read any state lane in place.
+- Managed (`src/Ymir.Box3D/Box3DSoaWorld.cs`, internal): `Box3DSoaView` exposes each lane as a span over
+  native memory (`ReadOnlySpan` for state, `Span` for write lanes). Spans are invalid after `Dispose`.
+  `Count` and `JointCount` are as of when the view was taken, so take it after the last add.
 - Determinism: one Box3D worker, bodies stepped in insertion order, same inputs give identical lane
   bytes on the same build. It is not claimed across platforms or CPUs.
+- Concurrency: creating Box3D worlds from several threads at once aborts the process (seen in tests);
+  create worlds one at a time.
 
 ## Tests
 
-`tests/Ymir.Box3D.Soa.Tests`: gravity and rest read through one held view, address stability across
-steps, two runs byte-equal at six checkpoints (25 spheres colliding on a slab), a one-ulp input
-difference diverging (so the equality check can fail), argument refusal, dispose.
+`tests/Ymir.Box3D.Soa.Tests`, run serially: gravity and rest read through one held view, address stability
+across steps, two runs byte-equal at six checkpoints (25 spheres colliding on a slab), a one-ulp input
+difference diverging, argument refusal, dispose; and for joints: hinge angle and the relative-rotation
+lanes agreeing, an off-axis hinge, a limit and a weld, a spherical joint leaving the swing plane, force and
+torque lanes (exact acceleration, cleared after the step, static bodies ignoring them, NaN refusing the
+step), capsule and box rest heights with parent and limb lanes, refusals, and bit-identical repeat runs of
+a torque-driven jointed body.
 
-## Needed next for Lodur
+## Not done
 
-Recorded as a mind follow-up; summary: joints (Box3D has revolute, spherical, weld, motor), a body
-layout beyond spheres and static boxes (capsules, boxes, compound, per-shape density), a torque/force
-write path (per-body lane written before the step, as the planar session's `apply_torque`), joint
-state lanes, contact facts for the SoA world, and a public surface in `Ymir.Core` rather than an
-internal wrapper.
+Contact facts for the SoA world (Begin/Hit/End), body removal with a stable ordering rule, compound bodies
+and per-shape material, joint motors and springs (Lodur drives joints from torque lanes instead), and a
+public typed surface in `Ymir.Core` (the wrapper is internal, visible to the test assembly). Windows build
+and Unity consumption are unrun; cross-platform bit-identity is unclaimed.

native/Ymir.Box3D/ymir_box3d_soa.c

@@ -1,8 +1,9 @@
 // Headless 3D Box3D world with a structure-of-arrays state view. Box3D owns every physical
-// decision (integration, collision, contact); this file only creates bodies, steps the world and
-// copies each body's state into contiguous lanes once per step. A caller reads the lanes in place
-// through `ymir_box3d_soa_view`: the pointers are stable for the world's whole life (capacity is
-// fixed at creation) and the lane contents change only inside `ymir_box3d_soa_step`.
+// decision (integration, collision, contact, joints); this file creates bodies and joints, applies
+// the caller's force and torque lanes, steps the world and copies each body's and joint's state into
+// contiguous lanes once per step. A caller reads the state lanes and writes the force/torque lanes in
+// place through `ymir_box3d_soa_get_view`: the pointers are stable for the world's whole life
+// (capacities are fixed at creation) and the state lanes change only inside `ymir_box3d_soa_step`.
 //
 // This world is not the retained planar session in ymir_box3d.c: that one locks Y, zeroes gravity
 // and hands out an array of structs by copy. See docs/soa-world.md.
@@ -39,21 +40,74 @@ enum
 enum
 {
     YMIR_SOA_KIND_SPHERE = 0,
-    YMIR_SOA_KIND_STATIC_BOX = 1
+    YMIR_SOA_KIND_STATIC_BOX = 1,
+    YMIR_SOA_KIND_CAPSULE = 2,
+    YMIR_SOA_KIND_BOX = 3
 };
 
-#define YMIR_SOA_ABI_VERSION 1u
+enum
+{
+    YMIR_SOA_JOINT_REVOLUTE = 0,
+    YMIR_SOA_JOINT_SPHERICAL = 1,
+    YMIR_SOA_JOINT_WELD = 2
+};
 
-// Pointers first and in lane order, so the layout is the same on every 64-bit target.
+#define YMIR_SOA_ABI_VERSION 2u
+
+// 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
+// or -1; `limb` is the caller's label. Neither is used by Box3D: they are lanes for the consumer.
+typedef struct ymir_box3d_soa_body_desc
+{
+    uint64_t stable_id;
+    uint32_t kind;
+    int32_t parent;
+    int32_t limb;
+    uint32_t reserved;
+    float position[3];
+    float rotation[4]; // x y z w, unit
+    float velocity[3];
+    float size[3];
+    float density;
+    float friction;
+    float restitution;
+    float rolling_resistance; // Box3D's surface rolling resistance, >= 0; spheres and capsules roll without it
+} ymir_box3d_soa_body_desc;
+
+// A joint between two bodies, given in world space at creation: the frame has its origin at `anchor`
+// and its z axis along `axis`; the creation pose is the joint's zero. Revolute turns about that z axis.
+// `enable_limit` applies `lower`/`upper` (revolute angle, spherical twist), and `cone_angle` (spherical
+// swing) when it is positive. Weld ignores limits.
+typedef struct ymir_box3d_soa_joint_desc
+{
+    uint32_t kind;
+    int32_t body_a;
+    int32_t body_b;
+    uint32_t enable_limit;
+    float anchor[3];
+    float axis[3];
+    float lower;
+    float upper;
+    float cone_angle;
+    float reserved_float;
+} ymir_box3d_soa_joint_desc;
+
+// Pointers first and in lane order, so the layout is the same on every 64-bit target. Lanes the caller
+// writes (force and torque per body) are mutable; every other lane is read-only to the caller.
 typedef struct ymir_box3d_soa_view
 {
     uint32_t abi_version;
     int32_t count;
     int32_t capacity;
+    int32_t joint_count;
+    int32_t joint_capacity;
     uint32_t reserved;
     uint64_t step_count;
+    // bodies
     const uint64_t* stable_id;
     const uint32_t* kind;
+    const int32_t* parent;
+    const int32_t* limb;
     const float* position_x;
     const float* position_y;
     const float* position_z;
@@ -67,20 +121,56 @@ typedef struct ymir_box3d_soa_view
     const float* angular_velocity_x;
     const float* angular_velocity_y;
     const float* angular_velocity_z;
+    float* force_x;
+    float* force_y;
+    float* force_z;
+    float* torque_x;
+    float* torque_y;
+    float* torque_z;
+    // joints
+    const uint32_t* joint_kind;
+    const int32_t* joint_body_a;
+    const int32_t* joint_body_b;
+    const float* joint_angle;
+    const float* joint_frame_x;
+    const float* joint_frame_y;
+    const float* joint_frame_z;
+    const float* joint_frame_w;
+    const float* joint_rotation_x;
+    const float* joint_rotation_y;
+    const float* joint_rotation_z;
+    const float* joint_rotation_w;
+    const float* joint_angular_velocity_x;
+    const float* joint_angular_velocity_y;
+    const float* joint_angular_velocity_z;
 } ymir_box3d_soa_view;
 
-#define YMIR_SOA_FLOAT_LANES 13
+#define YMIR_SOA_STATE_LANES 13
+#define YMIR_SOA_WRITE_LANES 6
+#define YMIR_SOA_BODY_FLOAT_LANES (YMIR_SOA_STATE_LANES + YMIR_SOA_WRITE_LANES)
+#define YMIR_SOA_JOINT_FLOAT_LANES 12
 
 typedef struct ymir_box3d_soa_world
 {
     b3WorldId world_id;
     int32_t count;
     int32_t capacity;
+    int32_t joint_count;
+    int32_t joint_capacity;
     uint64_t step_count;
     b3BodyId* bodies;
     uint64_t* stable_id;
     uint32_t* kind;
-    float* lanes; // YMIR_SOA_FLOAT_LANES arrays of `capacity` floats, lane-major
+    int32_t* parent;
+    int32_t* limb;
+    float* lanes; // YMIR_SOA_BODY_FLOAT_LANES arrays of `capacity` floats, lane-major
+    b3JointId* joints;
+    uint32_t* joint_kind;
+    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
+    b3Quat* joint_local_b;
+    float* joint_lanes; // YMIR_SOA_JOINT_FLOAT_LANES arrays of `joint_capacity` floats, lane-major
 } ymir_box3d_soa_world;
 
 static float* ymir_soa_lane(const ymir_box3d_soa_world* world, int lane)
@@ -88,11 +178,26 @@ static float* ymir_soa_lane(const ymir_box3d_soa_world* world, int lane)
     return world->lanes + (size_t)lane * (size_t)world->capacity;
 }
 
+static float* ymir_soa_joint_lane(const ymir_box3d_soa_world* world, int lane)
+{
+    return world->joint_lanes + (size_t)lane * (size_t)world->joint_capacity;
+}
+
 static int ymir_soa_finite(float v)
 {
     return isfinite(v);
 }
 
+static int ymir_soa_finite3(const float* v)
+{
+    return ymir_soa_finite(v[0]) && ymir_soa_finite(v[1]) && ymir_soa_finite(v[2]);
+}
+
+static int ymir_soa_is_dynamic(uint32_t kind)
+{
+    return kind != YMIR_SOA_KIND_STATIC_BOX;
+}
+
 static void ymir_soa_refresh(ymir_box3d_soa_world* world, int index)
 {
     b3BodyId body = world->bodies[index];
@@ -100,14 +205,47 @@ static void ymir_soa_refresh(ymir_box3d_soa_world* world, int index)
     b3Quat q = b3Body_GetRotation(body);
     b3Vec3 v = b3Body_GetLinearVelocity(body);
     b3Vec3 w = b3Body_GetAngularVelocity(body);
-    const float values[YMIR_SOA_FLOAT_LANES] = {
+    const float values[YMIR_SOA_STATE_LANES] = {
         p.x, p.y, p.z, q.v.x, q.v.y, q.v.z, q.s, v.x, v.y, v.z, w.x, w.y, w.z};
-    for (int lane = 0; lane < YMIR_SOA_FLOAT_LANES; ++lane)
+    for (int lane = 0; lane < YMIR_SOA_STATE_LANES; ++lane)
     {
         ymir_soa_lane(world, lane)[index] = values[lane];
     }
 }
 
+// Joint state from the bodies: the world orientation of frame A, frame B relative to frame A (as a
+// quaternion with w >= 0, the shorter arc) and the relative angular velocity in frame A. The angle
+// lane is Box3D's own: the revolute angle or the spherical twist angle, zero for a weld.
+static void ymir_soa_refresh_joint(ymir_box3d_soa_world* world, int j)
+{
+    int a = world->joint_body_a[j];
+    int b = world->joint_body_b[j];
+    b3Quat qa = b3MulQuat(b3Body_GetRotation(world->bodies[a]), world->joint_local_a[j]);
+    b3Quat qb = b3MulQuat(b3Body_GetRotation(world->bodies[b]), world->joint_local_b[j]);
+    b3Quat rel = b3InvMulQuat(qa, qb);
+    if (rel.s < 0.0f)
+    {
+        rel = b3NegateQuat(rel);
+    }
+    b3Vec3 w = b3InvRotateVector(
+        qa, b3Sub(b3Body_GetAngularVelocity(world->bodies[b]), b3Body_GetAngularVelocity(world->bodies[a])));
+    float angle = 0.0f;
+    if (world->joint_kind[j] == YMIR_SOA_JOINT_REVOLUTE)
+    {
+        angle = b3RevoluteJoint_GetAngle(world->joints[j]);
+    }
+    else if (world->joint_kind[j] == YMIR_SOA_JOINT_SPHERICAL)
+    {
+        angle = b3SphericalJoint_GetTwistAngle(world->joints[j]);
+    }
+    const float values[YMIR_SOA_JOINT_FLOAT_LANES] = {
+        angle, qa.v.x, qa.v.y, qa.v.z, qa.s, rel.v.x, rel.v.y, rel.v.z, rel.s, w.x, w.y, w.z};
+    for (int lane = 0; lane < YMIR_SOA_JOINT_FLOAT_LANES; ++lane)
+    {
+        ymir_soa_joint_lane(world, lane)[j] = values[lane];
+    }
+}
+
 YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_abi_version(void)
 {
     return YMIR_SOA_ABI_VERSION;
@@ -118,15 +256,45 @@ YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_view_size(void)
     return (uint32_t)sizeof(ymir_box3d_soa_view);
 }
 
+YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_body_desc_size(void)
+{
+    return (uint32_t)sizeof(ymir_box3d_soa_body_desc);
+}
+
+YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_joint_desc_size(void)
+{
+    return (uint32_t)sizeof(ymir_box3d_soa_joint_desc);
+}
+
+static void ymir_soa_free(ymir_box3d_soa_world* world)
+{
+    free(world->bodies);
+    free(world->stable_id);
+    free(world->kind);
+    free(world->parent);
+    free(world->limb);
+    free(world->lanes);
+    free(world->joints);
+    free(world->joint_kind);
+    free(world->joint_body_a);
+    free(world->joint_body_b);
+    free(world->joint_local_a);
+    free(world->joint_local_b);
+    free(world->joint_lanes);
+    free(world);
+}
+
 YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_create(
-    int32_t capacity, float gravity_x, float gravity_y, float gravity_z, ymir_box3d_soa_world** output)
+    int32_t capacity, int32_t joint_capacity, float gravity_x, float gravity_y, float gravity_z,
+    ymir_box3d_soa_world** output)
 {
     if (output == NULL)
     {
         return YMIR_SOA_INVALID_ARGUMENT;
     }
     *output = NULL;
-    if (capacity <= 0 || !ymir_soa_finite(gravity_x) || !ymir_soa_finite(gravity_y) || !ymir_soa_finite(gravity_z))
+    if (capacity <= 0 || joint_capacity < 0 || !ymir_soa_finite(gravity_x) || !ymir_soa_finite(gravity_y) ||
+        !ymir_soa_finite(gravity_z))
     {
         return YMIR_SOA_INVALID_ARGUMENT;
     }
@@ -137,17 +305,28 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_create(
         return YMIR_SOA_OUT_OF_MEMORY;
     }
     world->capacity = capacity;
-    world->bodies = (b3BodyId*)calloc((size_t)capacity, sizeof(b3BodyId));
-    world->stable_id = (uint64_t*)calloc((size_t)capacity, sizeof(uint64_t));
-    world->kind = (uint32_t*)calloc((size_t)capacity, sizeof(uint32_t));
-    world->lanes = (float*)calloc((size_t)capacity * YMIR_SOA_FLOAT_LANES, sizeof(float));
-    if (world->bodies == NULL || world->stable_id == NULL || world->kind == NULL || world->lanes == NULL)
+    world->joint_capacity = joint_capacity;
+    size_t n = (size_t)capacity;
+    size_t m = (size_t)joint_capacity > 0 ? (size_t)joint_capacity : 1; // calloc(0) may return NULL
+    world->bodies = (b3BodyId*)calloc(n, sizeof(b3BodyId));
+    world->stable_id = (uint64_t*)calloc(n, sizeof(uint64_t));
+    world->kind = (uint32_t*)calloc(n, sizeof(uint32_t));
+    world->parent = (int32_t*)calloc(n, sizeof(int32_t));
+    world->limb = (int32_t*)calloc(n, sizeof(int32_t));
+    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_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));
+    world->joint_local_b = (b3Quat*)calloc(m, sizeof(b3Quat));
+    world->joint_lanes = (float*)calloc(m * YMIR_SOA_JOINT_FLOAT_LANES, sizeof(float));
+    if (world->bodies == NULL || world->stable_id == NULL || world->kind == NULL || world->parent == NULL ||
+        world->limb == 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)
     {
-        free(world->bodies);
-        free(world->stable_id);
-        free(world->kind);
-        free(world->lanes);
-        free(world);
+        ymir_soa_free(world);
         return YMIR_SOA_OUT_OF_MEMORY;
     }
 
@@ -158,11 +337,7 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_create(
     world->world_id = b3CreateWorld(&def);
     if (B3_IS_NULL(world->world_id))
     {
-        free(world->bodies);
-        free(world->stable_id);
-        free(world->kind);
-        free(world->lanes);
-        free(world);
+        ymir_soa_free(world);
         return YMIR_SOA_INTERNAL_ERROR;
     }
 
@@ -180,22 +355,67 @@ YMIR_EXPORT void YMIR_CALL ymir_box3d_soa_destroy(ymir_box3d_soa_world* world)
     {
         b3DestroyWorld(world->world_id);
     }
-    free(world->bodies);
-    free(world->stable_id);
-    free(world->kind);
-    free(world->lanes);
-    free(world);
+    ymir_soa_free(world);
 }
 
-static int32_t ymir_soa_reserve(ymir_box3d_soa_world* world, uint64_t stable_id)
+static int32_t ymir_soa_check_body(const ymir_box3d_soa_world* world, const ymir_box3d_soa_body_desc* d)
 {
+    if (d->kind > YMIR_SOA_KIND_BOX || !ymir_soa_finite3(d->position) || !ymir_soa_finite3(d->velocity) ||
+        !ymir_soa_finite3(d->size) || !ymir_soa_finite(d->friction) || !ymir_soa_finite(d->restitution) ||
+        !ymir_soa_finite(d->density) || !ymir_soa_finite(d->rolling_resistance) || d->friction < 0.0f ||
+        d->restitution < 0.0f || d->rolling_resistance < 0.0f)
+    {
+        return YMIR_SOA_INVALID_ARGUMENT;
+    }
+    float q2 = 0.0f;
+    for (int i = 0; i < 4; ++i)
+    {
+        if (!ymir_soa_finite(d->rotation[i]))
+        {
+            return YMIR_SOA_INVALID_ARGUMENT;
+        }
+        q2 += d->rotation[i] * d->rotation[i];
+    }
+    if (fabsf(q2 - 1.0f) > 1e-3f)
+    {
+        return YMIR_SOA_INVALID_ARGUMENT;
+    }
+    if (d->parent < -1 || d->parent >= world->count)
+    {
+        return YMIR_SOA_INVALID_ARGUMENT;
+    }
+    if (ymir_soa_is_dynamic(d->kind) && d->density <= 0.0f)
+    {
+        return YMIR_SOA_INVALID_ARGUMENT;
+    }
+    switch (d->kind)
+    {
+        case YMIR_SOA_KIND_SPHERE:
+            if (d->size[0] <= 0.0f)
+            {
+                return YMIR_SOA_INVALID_ARGUMENT;
+            }
+            break;
+        case YMIR_SOA_KIND_CAPSULE:
+            if (d->size[0] <= 0.0f || d->size[1] <= 0.0f)
+            {
+                return YMIR_SOA_INVALID_ARGUMENT;
+            }
+            break;
+        default:
+            if (d->size[0] <= 0.0f || d->size[1] <= 0.0f || d->size[2] <= 0.0f)
+            {
+                return YMIR_SOA_INVALID_ARGUMENT;
+            }
+            break;
+    }
     if (world->count >= world->capacity)
     {
         return YMIR_SOA_FULL;
     }
     for (int i = 0; i < world->count; ++i)
     {
-        if (world->stable_id[i] == stable_id)
+        if (world->stable_id[i] == d->stable_id)
         {
             return YMIR_SOA_DUPLICATE_ID;
         }
@@ -203,13 +423,69 @@ static int32_t ymir_soa_reserve(ymir_box3d_soa_world* world, uint64_t stable_id)
     return YMIR_SOA_OK;
 }
 
-static int32_t ymir_soa_commit(
-    ymir_box3d_soa_world* world, uint64_t stable_id, uint32_t kind, b3BodyId body, int32_t* index)
+// Creates one body from a descriptor. Box3D owns mass: it follows from `density` and the shape.
+YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body(
+    ymir_box3d_soa_world* world, const ymir_box3d_soa_body_desc* d, int32_t* index)
 {
+    if (world == NULL || d == NULL)
+    {
+        return YMIR_SOA_INVALID_ARGUMENT;
+    }
+    int32_t status = ymir_soa_check_body(world, d);
+    if (status != YMIR_SOA_OK)
+    {
+        return status;
+    }
+
+    b3BodyDef body_def = b3DefaultBodyDef();
+    body_def.type = ymir_soa_is_dynamic(d->kind) ? b3_dynamicBody : b3_staticBody;
+    body_def.position = (b3Vec3){d->position[0], d->position[1], d->position[2]};
+    body_def.rotation = (b3Quat){{d->rotation[0], d->rotation[1], d->rotation[2]}, d->rotation[3]};
+    body_def.linearVelocity = (b3Vec3){d->velocity[0], d->velocity[1], d->velocity[2]};
+    body_def.enableSleep = false;
+    b3BodyId body = b3CreateBody(world->world_id, &body_def);
+    if (B3_IS_NULL(body))
+    {
+        return YMIR_SOA_INTERNAL_ERROR;
+    }
+    b3ShapeDef shape_def = b3DefaultShapeDef();
+    shape_def.userData = (void*)(uintptr_t)d->stable_id;
+    if (ymir_soa_is_dynamic(d->kind))
+    {
+        shape_def.density = d->density;
+    }
+    shape_def.baseMaterial.friction = d->friction;
+    shape_def.baseMaterial.restitution = d->restitution;
+    shape_def.baseMaterial.rollingResistance = d->rolling_resistance;
+
+    b3ShapeId shape;
+    if (d->kind == YMIR_SOA_KIND_SPHERE)
+    {
+        b3Sphere sphere = {{0.0f, 0.0f, 0.0f}, d->size[0]};
+        shape = b3CreateSphereShape(body, &shape_def, &sphere);
+    }
+    else if (d->kind == YMIR_SOA_KIND_CAPSULE)
+    {
+        b3Capsule capsule = {{0.0f, -d->size[1], 0.0f}, {0.0f, d->size[1], 0.0f}, d->size[0]};
+        shape = b3CreateCapsuleShape(body, &shape_def, &capsule);
+    }
+    else
+    {
+        b3BoxHull box = b3MakeBoxHull(d->size[0], d->size[1], d->size[2]);
+        shape = b3CreateHullShape(body, &shape_def, &box.base);
+    }
+    if (B3_IS_NULL(shape))
+    {
+        b3DestroyBody(body);
+        return YMIR_SOA_INTERNAL_ERROR;
+    }
+
     int i = world->count++;
     world->bodies[i] = body;
-    world->stable_id[i] = stable_id;
-    world->kind[i] = kind;
+    world->stable_id[i] = d->stable_id;
+    world->kind[i] = d->kind;
+    world->parent[i] = d->parent;
+    world->limb[i] = d->limb;
     ymir_soa_refresh(world, i);
     if (index != NULL)
     {
@@ -218,86 +494,135 @@ static int32_t ymir_soa_commit(
     return YMIR_SOA_OK;
 }
 
-// A dynamic sphere. `density` is Box3D's shape density; mass follows from it.
-YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_sphere(
-    ymir_box3d_soa_world* world, uint64_t stable_id, float position_x, float position_y, float position_z,
-    float velocity_x, float velocity_y, float velocity_z, float radius, float density, float friction,
-    float restitution, int32_t* index)
+static b3Transform ymir_soa_body_transform(b3BodyId body)
 {
-    if (world == NULL || radius <= 0.0f || density <= 0.0f || !ymir_soa_finite(position_x) ||
-        !ymir_soa_finite(position_y) || !ymir_soa_finite(position_z) || !ymir_soa_finite(velocity_x) ||
-        !ymir_soa_finite(velocity_y) || !ymir_soa_finite(velocity_z) || !ymir_soa_finite(radius) ||
-        !ymir_soa_finite(density) || !ymir_soa_finite(friction) || !ymir_soa_finite(restitution))
-    {
-        return YMIR_SOA_INVALID_ARGUMENT;
-    }
-    int32_t status = ymir_soa_reserve(world, stable_id);
-    if (status != YMIR_SOA_OK)
-    {
-        return status;
-    }
-
-    b3BodyDef body_def = b3DefaultBodyDef();
-    body_def.type = b3_dynamicBody;
-    body_def.position = (b3Vec3){position_x, position_y, position_z};
-    body_def.linearVelocity = (b3Vec3){velocity_x, velocity_y, velocity_z};
-    body_def.enableSleep = false;
-    b3BodyId body = b3CreateBody(world->world_id, &body_def);
-    if (B3_IS_NULL(body))
-    {
-        return YMIR_SOA_INTERNAL_ERROR;
-    }
-    b3ShapeDef shape_def = b3DefaultShapeDef();
-    shape_def.userData = (void*)(uintptr_t)stable_id;
-    shape_def.density = density;
-    shape_def.baseMaterial.friction = friction;
-    shape_def.baseMaterial.restitution = restitution;
-    b3Sphere sphere = {{0.0f, 0.0f, 0.0f}, radius};
-    if (B3_IS_NULL(b3CreateSphereShape(body, &shape_def, &sphere)))
-    {
-        b3DestroyBody(body);
-        return YMIR_SOA_INTERNAL_ERROR;
-    }
-    return ymir_soa_commit(world, stable_id, YMIR_SOA_KIND_SPHERE, body, index);
+    b3Pos p = b3Body_GetPosition(body);
+    b3Transform t = {{p.x, p.y, p.z}, b3Body_GetRotation(body)};
+    return t;
 }
 
-// A static box (ground, walls) centred at the position, half extents along the world axes.
-YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_static_box(
-    ymir_box3d_soa_world* world, uint64_t stable_id, float position_x, float position_y, float position_z,
-    float half_x, float half_y, float half_z, float friction, float restitution, int32_t* index)
+// 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(
+    ymir_box3d_soa_world* world, const ymir_box3d_soa_joint_desc* d, int32_t* index)
 {
-    if (world == NULL || half_x <= 0.0f || half_y <= 0.0f || half_z <= 0.0f || !ymir_soa_finite(position_x) ||
-        !ymir_soa_finite(position_y) || !ymir_soa_finite(position_z) || !ymir_soa_finite(half_x) ||
-        !ymir_soa_finite(half_y) || !ymir_soa_finite(half_z) || !ymir_soa_finite(friction) ||
-        !ymir_soa_finite(restitution))
+    if (world == NULL || d == NULL || d->kind > YMIR_SOA_JOINT_WELD || d->body_a < 0 || d->body_a >= world->count ||
+        d->body_b < 0 || d->body_b >= world->count || d->body_a == d->body_b || !ymir_soa_finite3(d->anchor) ||
+        !ymir_soa_finite3(d->axis) || !ymir_soa_finite(d->lower) || !ymir_soa_finite(d->upper) ||
+        !ymir_soa_finite(d->cone_angle) || d->cone_angle < 0.0f || d->cone_angle > B3_PI)
     {
         return YMIR_SOA_INVALID_ARGUMENT;
     }
-    int32_t status = ymir_soa_reserve(world, stable_id);
-    if (status != YMIR_SOA_OK)
+    b3Vec3 axis = {d->axis[0], d->axis[1], d->axis[2]};
+    float length = sqrtf(b3Dot(axis, axis));
+    if (length < 1e-6f || (d->enable_limit != 0 && d->kind != YMIR_SOA_JOINT_WELD && d->lower > d->upper))
     {
-        return status;
+        return YMIR_SOA_INVALID_ARGUMENT;
+    }
+    if (world->joint_count >= world->joint_capacity)
+    {
+        return YMIR_SOA_FULL;
+    }
+    axis = b3MulSV(1.0f / length, axis);
+
+    b3Transform frame = {{d->anchor[0], d->anchor[1], d->anchor[2]}, b3ComputeQuatBetweenUnitVectors((b3Vec3){0.0f, 0.0f, 1.0f}, axis)};
+    b3Transform local_a = b3InvMulTransforms(ymir_soa_body_transform(world->bodies[d->body_a]), frame);
+    b3Transform local_b = b3InvMulTransforms(ymir_soa_body_transform(world->bodies[d->body_b]), frame);
+
+    b3JointId joint;
+    if (d->kind == YMIR_SOA_JOINT_REVOLUTE)
+    {
+        b3RevoluteJointDef def = b3DefaultRevoluteJointDef();
+        def.base.bodyIdA = world->bodies[d->body_a];
+        def.base.bodyIdB = world->bodies[d->body_b];
+        def.base.localFrameA = local_a;
+        def.base.localFrameB = local_b;
+        def.enableLimit = d->enable_limit != 0;
+        def.lowerAngle = d->lower;
+        def.upperAngle = d->upper;
+        joint = b3CreateRevoluteJoint(world->world_id, &def);
+    }
+    else if (d->kind == YMIR_SOA_JOINT_SPHERICAL)
+    {
+        b3SphericalJointDef def = b3DefaultSphericalJointDef();
+        def.base.bodyIdA = world->bodies[d->body_a];
+        def.base.bodyIdB = world->bodies[d->body_b];
+        def.base.localFrameA = local_a;
+        def.base.localFrameB = local_b;
+        def.enableConeLimit = d->enable_limit != 0 && d->cone_angle > 0.0f;
+        def.coneAngle = d->cone_angle;
+        def.enableTwistLimit = d->enable_limit != 0;
+        def.lowerTwistAngle = d->lower;
+        def.upperTwistAngle = d->upper;
+        joint = b3CreateSphericalJoint(world->world_id, &def);
+    }
+    else
+    {
+        b3WeldJointDef def = b3DefaultWeldJointDef();
+        def.base.bodyIdA = world->bodies[d->body_a];
+        def.base.bodyIdB = world->bodies[d->body_b];
+        def.base.localFrameA = local_a;
+        def.base.localFrameB = local_b;
+        joint = b3CreateWeldJoint(world->world_id, &def);
+    }
+    if (B3_IS_NULL(joint))
+    {
+        return YMIR_SOA_INTERNAL_ERROR;
     }
 
-    b3BodyDef body_def = b3DefaultBodyDef();
-    body_def.type = b3_staticBody;
-    body_def.position = (b3Vec3){position_x, position_y, position_z};
-    b3BodyId body = b3CreateBody(world->world_id, &body_def);
-    if (B3_IS_NULL(body))
+    int j = world->joint_count++;
+    world->joints[j] = joint;
+    world->joint_kind[j] = d->kind;
+    world->joint_body_a[j] = d->body_a;
+    world->joint_body_b[j] = d->body_b;
+    world->joint_local_a[j] = local_a.q;
+    world->joint_local_b[j] = local_b.q;
+    ymir_soa_refresh_joint(world, j);
+    if (index != NULL)
     {
-        return YMIR_SOA_INTERNAL_ERROR;
+        *index = j;
     }
-    b3ShapeDef shape_def = b3DefaultShapeDef();
-    shape_def.userData = (void*)(uintptr_t)stable_id;
-    shape_def.baseMaterial.friction = friction;
-    shape_def.baseMaterial.restitution = restitution;
-    b3BoxHull box = b3MakeBoxHull(half_x, half_y, half_z);
-    if (B3_IS_NULL(b3CreateHullShape(body, &shape_def, &box.base)))
+    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.
+static int32_t ymir_soa_apply_write_lanes(ymir_box3d_soa_world* world)
+{
+    float* lane[YMIR_SOA_WRITE_LANES];
+    for (int k = 0; k < YMIR_SOA_WRITE_LANES; ++k)
     {
-        b3DestroyBody(body);
-        return YMIR_SOA_INTERNAL_ERROR;
+        lane[k] = ymir_soa_lane(world, YMIR_SOA_STATE_LANES + k);
+        for (int i = 0; i < world->count; ++i)
+        {
+            if (!ymir_soa_finite(lane[k][i]))
+            {
+                return YMIR_SOA_INVALID_ARGUMENT;
+            }
+        }
     }
-    return ymir_soa_commit(world, stable_id, YMIR_SOA_KIND_STATIC_BOX, body, index);
+    for (int i = 0; i < world->count; ++i)
+    {
+        b3Vec3 force = {lane[0][i], lane[1][i], lane[2][i]};
+        b3Vec3 torque = {lane[3][i], lane[4][i], lane[5][i]};
+        if (ymir_soa_is_dynamic(world->kind[i]))
+        {
+            if (force.x != 0.0f || force.y != 0.0f || force.z != 0.0f)
+            {
+                b3Body_ApplyForceToCenter(world->bodies[i], force, true);
+            }
+            if (torque.x != 0.0f || torque.y != 0.0f || torque.z != 0.0f)
+            {
+                b3Body_ApplyTorque(world->bodies[i], torque, true);
+            }
+        }
+        for (int k = 0; k < YMIR_SOA_WRITE_LANES; ++k)
+        {
+            lane[k][i] = 0.0f;
+        }
+    }
+    return YMIR_SOA_OK;
 }
 
 YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, float time_step, int32_t sub_step_count)
@@ -306,11 +631,20 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f
     {
         return YMIR_SOA_INVALID_ARGUMENT;
     }
+    int32_t status = ymir_soa_apply_write_lanes(world);
+    if (status != YMIR_SOA_OK)
+    {
+        return status;
+    }
     b3World_Step(world->world_id, time_step, sub_step_count);
     for (int i = 0; i < world->count; ++i)
     {
         ymir_soa_refresh(world, i);
     }
+    for (int j = 0; j < world->joint_count; ++j)
+    {
+        ymir_soa_refresh_joint(world, j);
+    }
     world->step_count += 1;
     return YMIR_SOA_OK;
 }
@@ -321,15 +655,21 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_get_view(const ymir_box3d_soa_world
     {
         return YMIR_SOA_INVALID_ARGUMENT;
     }
-    const float* base = world->lanes;
+    float* base = world->lanes;
     size_t n = (size_t)world->capacity;
+    float* jb = world->joint_lanes;
+    size_t m = (size_t)world->joint_capacity;
     *view = (ymir_box3d_soa_view){
         .abi_version = YMIR_SOA_ABI_VERSION,
         .count = world->count,
         .capacity = world->capacity,
+        .joint_count = world->joint_count,
+        .joint_capacity = world->joint_capacity,
         .step_count = world->step_count,
         .stable_id = world->stable_id,
         .kind = world->kind,
+        .parent = world->parent,
+        .limb = world->limb,
         .position_x = base + 0 * n,
         .position_y = base + 1 * n,
         .position_z = base + 2 * n,
@@ -343,6 +683,27 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_get_view(const ymir_box3d_soa_world
         .angular_velocity_x = base + 10 * n,
         .angular_velocity_y = base + 11 * n,
         .angular_velocity_z = base + 12 * n,
+        .force_x = base + 13 * n,
+        .force_y = base + 14 * n,
+        .force_z = base + 15 * n,
+        .torque_x = base + 16 * n,
+        .torque_y = base + 17 * n,
+        .torque_z = base + 18 * n,
+        .joint_kind = world->joint_kind,
+        .joint_body_a = world->joint_body_a,
+        .joint_body_b = world->joint_body_b,
+        .joint_angle = jb + 0 * m,
+        .joint_frame_x = jb + 1 * m,
+        .joint_frame_y = jb + 2 * m,
+        .joint_frame_z = jb + 3 * m,
+        .joint_frame_w = jb + 4 * m,
+        .joint_rotation_x = jb + 5 * m,
+        .joint_rotation_y = jb + 6 * m,
+        .joint_rotation_z = jb + 7 * m,
+        .joint_rotation_w = jb + 8 * m,
+        .joint_angular_velocity_x = jb + 9 * m,
+        .joint_angular_velocity_y = jb + 10 * m,
+        .joint_angular_velocity_z = jb + 11 * m,
     };
     return YMIR_SOA_OK;
 }

src/Ymir.Box3D/Box3DSoaWorld.cs

@@ -5,7 +5,45 @@ namespace Ymir.Box3D;
 internal enum Box3DSoaKind : uint
 {
     Sphere = 0,
-    StaticBox = 1
+    StaticBox = 1,
+    Capsule = 2,
+    Box = 3
+}
+
+internal enum Box3DSoaJointKind : uint
+{
+    Revolute = 0,
+    Spherical = 1,
+    Weld = 2
+}
+
+/// <summary>Native layout of <c>ymir_box3d_soa_body_desc</c>.</summary>
+[StructLayout(LayoutKind.Sequential)]
+internal struct Box3DSoaBodyDesc
+{
+    public ulong StableId;
+    public uint Kind;
+    public int Parent;
+    public int Limb;
+    public uint Reserved;
+    public float PositionX, PositionY, PositionZ;
+    public float RotationX, RotationY, RotationZ, RotationW;
+    public float VelocityX, VelocityY, VelocityZ;
+    public float Size0, Size1, Size2;
+    public float Density, Friction, Restitution, RollingResistance;
+}
+
+/// <summary>Native layout of <c>ymir_box3d_soa_joint_desc</c>.</summary>
+[StructLayout(LayoutKind.Sequential)]
+internal struct Box3DSoaJointDesc
+{
+    public uint Kind;
+    public int BodyA;
+    public int BodyB;
+    public uint EnableLimit;
+    public float AnchorX, AnchorY, AnchorZ;
+    public float AxisX, AxisY, AxisZ;
+    public float Lower, Upper, ConeAngle, ReservedFloat;
 }
 
 /// <summary>Native layout of <c>ymir_box3d_soa_view</c>: counts, then one pointer per lane.</summary>
@@ -15,10 +53,14 @@ internal unsafe struct Box3DSoaViewNative
     public uint AbiVersion;
     public int Count;
     public int Capacity;
+    public int JointCount;
+    public int JointCapacity;
     public uint Reserved;
     public ulong StepCount;
     public ulong* StableId;
     public uint* Kind;
+    public int* Parent;
+    public int* Limb;
     public float* PositionX;
     public float* PositionY;
     public float* PositionZ;
@@ -32,6 +74,27 @@ internal unsafe struct Box3DSoaViewNative
     public float* AngularVelocityX;
     public float* AngularVelocityY;
     public float* AngularVelocityZ;
+    public float* ForceX;
+    public float* ForceY;
+    public float* ForceZ;
+    public float* TorqueX;
+    public float* TorqueY;
+    public float* TorqueZ;
+    public uint* JointKind;
+    public int* JointBodyA;
+    public int* JointBodyB;
+    public float* JointAngle;
+    public float* JointFrameX;
+    public float* JointFrameY;
+    public float* JointFrameZ;
+    public float* JointFrameW;
+    public float* JointRotationX;
+    public float* JointRotationY;
+    public float* JointRotationZ;
+    public float* JointRotationW;
+    public float* JointAngularVelocityX;
+    public float* JointAngularVelocityY;
+    public float* JointAngularVelocityZ;
 }
 
 internal static unsafe partial class Box3DSoaNative
@@ -48,7 +111,15 @@ internal static unsafe partial class Box3DSoaNative
 
     [LibraryImport(Library)]
     [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
-    internal static partial int ymir_box3d_soa_create(int capacity, float gx, float gy, float gz, out nint world);
+    internal static partial uint ymir_box3d_soa_get_body_desc_size();
+
+    [LibraryImport(Library)]
+    [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
+    internal static partial uint ymir_box3d_soa_get_joint_desc_size();
+
+    [LibraryImport(Library)]
+    [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
+    internal static partial int ymir_box3d_soa_create(int capacity, int jointCapacity, float gx, float gy, float gz, out nint world);
 
     [LibraryImport(Library)]
     [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
@@ -56,15 +127,11 @@ internal static unsafe partial class Box3DSoaNative
 
     [LibraryImport(Library)]
     [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
-    internal static partial int ymir_box3d_soa_add_sphere(
-        nint world, ulong stableId, float px, float py, float pz, float vx, float vy, float vz,
-        float radius, float density, float friction, float restitution, out int index);
+    internal static partial int ymir_box3d_soa_add_body(nint world, in Box3DSoaBodyDesc desc, out int index);
 
     [LibraryImport(Library)]
     [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
-    internal static partial int ymir_box3d_soa_add_static_box(
-        nint world, ulong stableId, float px, float py, float pz, float hx, float hy, float hz,
-        float friction, float restitution, out int index);
+    internal static partial int ymir_box3d_soa_add_joint(nint world, in Box3DSoaJointDesc desc, out int index);
 
     [LibraryImport(Library)]
     [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])]
@@ -93,6 +160,10 @@ internal readonly unsafe struct Box3DSoaView
 
     public ReadOnlySpan<ulong> StableId => new(_v.StableId, Count);
     public ReadOnlySpan<uint> Kind => new(_v.Kind, Count);
+    /// <summary>Lane index of the body's parent in the limb tree, or -1 for a root. Set at creation, not used by Box3D.</summary>
+    public ReadOnlySpan<int> Parent => new(_v.Parent, Count);
+    /// <summary>The caller's limb label for the body. Set at creation, not used by Box3D.</summary>
+    public ReadOnlySpan<int> Limb => new(_v.Limb, Count);
     public ReadOnlySpan<float> PositionX => new(_v.PositionX, Count);
     public ReadOnlySpan<float> PositionY => new(_v.PositionY, Count);
     public ReadOnlySpan<float> PositionZ => new(_v.PositionZ, Count);
@@ -108,6 +179,39 @@ internal readonly unsafe struct Box3DSoaView
     public ReadOnlySpan<float> AngularVelocityY => new(_v.AngularVelocityY, Count);
     public ReadOnlySpan<float> AngularVelocityZ => new(_v.AngularVelocityZ, Count);
 
+    // Write lanes: the caller's per-body world force (N, at the centre of mass) and torque (N m), written
+    // in place before a step. Step applies them to the dynamic bodies and clears them, so a lane holds one
+    // step's intent. A non-finite value makes Step throw before any body is touched.
+    public Span<float> ForceX => new(_v.ForceX, Count);
+    public Span<float> ForceY => new(_v.ForceY, Count);
+    public Span<float> ForceZ => new(_v.ForceZ, Count);
+    public Span<float> TorqueX => new(_v.TorqueX, Count);
+    public Span<float> TorqueY => new(_v.TorqueY, Count);
+    public Span<float> TorqueZ => new(_v.TorqueZ, Count);
+
+    /// <summary>Joints at the moment this view was taken: take it after the last AddJoint.</summary>
+    public int JointCount => _v.JointCount;
+    public ReadOnlySpan<uint> JointKind => new(_v.JointKind, JointCount);
+    /// <summary>Lane indices of the joint's two bodies; the second is the child.</summary>
+    public ReadOnlySpan<int> JointBodyA => new(_v.JointBodyA, JointCount);
+    public ReadOnlySpan<int> JointBodyB => new(_v.JointBodyB, JointCount);
+    /// <summary>Box3D's own angle: revolute angle or spherical twist angle (radians), zero for a weld.</summary>
+    public ReadOnlySpan<float> JointAngle => new(_v.JointAngle, JointCount);
+    /// <summary>World orientation of the joint frame on body A (quaternion x y z w).</summary>
+    public ReadOnlySpan<float> JointFrameX => new(_v.JointFrameX, JointCount);
+    public ReadOnlySpan<float> JointFrameY => new(_v.JointFrameY, JointCount);
+    public ReadOnlySpan<float> JointFrameZ => new(_v.JointFrameZ, JointCount);
+    public ReadOnlySpan<float> JointFrameW => new(_v.JointFrameW, JointCount);
+    /// <summary>Joint frame B relative to frame A (quaternion x y z w, w >= 0); identity at the creation pose.</summary>
+    public ReadOnlySpan<float> JointRotationX => new(_v.JointRotationX, JointCount);
+    public ReadOnlySpan<float> JointRotationY => new(_v.JointRotationY, JointCount);
+    public ReadOnlySpan<float> JointRotationZ => new(_v.JointRotationZ, JointCount);
+    public ReadOnlySpan<float> JointRotationW => new(_v.JointRotationW, JointCount);
+    /// <summary>Body B's angular velocity minus body A's, expressed in frame A (rad/s).</summary>
+    public ReadOnlySpan<float> JointAngularVelocityX => new(_v.JointAngularVelocityX, JointCount);
+    public ReadOnlySpan<float> JointAngularVelocityY => new(_v.JointAngularVelocityY, JointCount);
+    public ReadOnlySpan<float> JointAngularVelocityZ => new(_v.JointAngularVelocityZ, JointCount);
+
     /// <summary>Address of a lane, for tests and interop that need to prove nothing was copied.</summary>
     public nint PositionYAddress => (nint)_v.PositionY;
 }
@@ -118,18 +222,20 @@ internal readonly unsafe struct Box3DSoaView
 /// </summary>
 internal sealed class Box3DSoaWorld : IDisposable
 {
-    private const uint ExpectedAbi = 1;
+    private const uint ExpectedAbi = 2;
     private nint _handle;
 
-    public Box3DSoaWorld(int capacity, float gravityX = 0f, float gravityY = -10f, float gravityZ = 0f)
+    public Box3DSoaWorld(int capacity, float gravityX = 0f, float gravityY = -10f, float gravityZ = 0f, int jointCapacity = 0)
     {
         if (Box3DSoaNative.ymir_box3d_soa_get_abi_version() != ExpectedAbi ||
-            Box3DSoaNative.ymir_box3d_soa_get_view_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaViewNative>())
+            Box3DSoaNative.ymir_box3d_soa_get_view_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaViewNative>() ||
+            Box3DSoaNative.ymir_box3d_soa_get_body_desc_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaBodyDesc>() ||
+            Box3DSoaNative.ymir_box3d_soa_get_joint_desc_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf<Box3DSoaJointDesc>())
         {
             throw new InvalidOperationException("The native Ymir Box3D SoA ABI does not match this build.");
         }
 
-        Check(Box3DSoaNative.ymir_box3d_soa_create(capacity, gravityX, gravityY, gravityZ, out _handle));
+        Check(Box3DSoaNative.ymir_box3d_soa_create(capacity, jointCapacity, gravityX, gravityY, gravityZ, out _handle));
     }
 
     /// <summary>The live lanes. Count and step count are read from native memory each call.</summary>
@@ -157,24 +263,80 @@ internal sealed class Box3DSoaWorld : IDisposable
     /// <summary>Adds a dynamic sphere; returns its lane index.</summary>
     public int AddSphere(
         ulong stableId, (float X, float Y, float Z) position, float radius, float density,
-        (float X, float Y, float Z) velocity = default, float friction = 0.6f, float restitution = 0f)
-    {
-        ObjectDisposedException.ThrowIf(_handle == 0, this);
-        Check(Box3DSoaNative.ymir_box3d_soa_add_sphere(
-            _handle, stableId, position.X, position.Y, position.Z, velocity.X, velocity.Y, velocity.Z,
-            radius, density, friction, restitution, out var index));
-        return index;
-    }
+        (float X, float Y, float Z) velocity = default, float friction = 0.6f, float restitution = 0f,
+        int parent = -1, int limb = 0, float rollingResistance = 0f) =>
+        AddBody(Describe(Box3DSoaKind.Sphere, stableId, position, default, (radius, 0f, 0f), density,
+            velocity, friction, restitution, parent, limb, rollingResistance));
 
     /// <summary>Adds a static axis-aligned box centred at <paramref name="position"/>.</summary>
     public int AddStaticBox(
         ulong stableId, (float X, float Y, float Z) position, (float X, float Y, float Z) halfExtents,
-        float friction = 0.6f, float restitution = 0f)
+        float friction = 0.6f, float restitution = 0f) =>
+        AddBody(Describe(Box3DSoaKind.StaticBox, stableId, position, default, halfExtents, 0f,
+            default, friction, restitution, -1, 0));
+
+    /// <summary>
+    /// Adds a dynamic capsule: a segment of <paramref name="halfLength"/> either side of the centre along
+    /// the body's local Y, swept by <paramref name="radius"/>. <paramref name="rotation"/> is a unit quaternion
+    /// x y z w; the default (all zero) means identity.
+    /// </summary>
+    public int AddCapsule(
+        ulong stableId, (float X, float Y, float Z) position, float radius, float halfLength, float density,
+        (float X, float Y, float Z, float W) rotation = default, float friction = 0.6f, float restitution = 0f,
+        int parent = -1, int limb = 0, float rollingResistance = 0f) =>
+        AddBody(Describe(Box3DSoaKind.Capsule, stableId, position, rotation, (radius, halfLength, 0f), density,
+            default, friction, restitution, parent, limb, rollingResistance));
+
+    /// <summary>Adds a dynamic box with the given half extents; rotation as for <see cref="AddCapsule"/>.</summary>
+    public int AddBox(
+        ulong stableId, (float X, float Y, float Z) position, (float X, float Y, float Z) halfExtents, float density,
+        (float X, float Y, float Z, float W) rotation = default, float friction = 0.6f, float restitution = 0f,
+        int parent = -1, int limb = 0) =>
+        AddBody(Describe(Box3DSoaKind.Box, stableId, position, rotation, halfExtents, density,
+            default, friction, restitution, parent, limb));
+
+    /// <summary>
+    /// Joins two bodies at a world-space <paramref name="anchor"/>. The joint frame's z axis is
+    /// <paramref name="axis"/> (the hinge axis of a revolute joint); the pose at creation is the joint's zero.
+    /// Limits are radians: the revolute angle, or the spherical twist with <paramref name="coneAngle"/> as swing.
+    /// A weld takes no limits. Returns the joint's lane index. Body B is the child.
+    /// </summary>
+    public int AddJoint(
+        Box3DSoaJointKind kind, int bodyA, int bodyB, (float X, float Y, float Z) anchor,
+        (float X, float Y, float Z) axis, bool limit = false, float lower = 0f, float upper = 0f, float coneAngle = 0f)
     {
         ObjectDisposedException.ThrowIf(_handle == 0, this);
-        Check(Box3DSoaNative.ymir_box3d_soa_add_static_box(
-            _handle, stableId, position.X, position.Y, position.Z, halfExtents.X, halfExtents.Y, halfExtents.Z,
-            friction, restitution, out var index));
+        var desc = new Box3DSoaJointDesc
+        {
+            Kind = (uint)kind, BodyA = bodyA, BodyB = bodyB, EnableLimit = limit ? 1u : 0u,
+            AnchorX = anchor.X, AnchorY = anchor.Y, AnchorZ = anchor.Z,
+            AxisX = axis.X, AxisY = axis.Y, AxisZ = axis.Z, Lower = lower, Upper = upper, ConeAngle = coneAngle,
+        };
+        Check(Box3DSoaNative.ymir_box3d_soa_add_joint(_handle, desc, out var index));
+        return index;
+    }
+
+    private static Box3DSoaBodyDesc Describe(
+        Box3DSoaKind kind, ulong stableId, (float X, float Y, float Z) position,
+        (float X, float Y, float Z, float W) rotation, (float, float, float) size, float density,
+        (float X, float Y, float Z) velocity, float friction, float restitution, int parent, int limb, float rollingResistance = 0f)
+    {
+        if (rotation == default) rotation = (0f, 0f, 0f, 1f);
+        return new Box3DSoaBodyDesc
+        {
+            StableId = stableId, Kind = (uint)kind, Parent = parent, Limb = limb,
+            PositionX = position.X, PositionY = position.Y, PositionZ = position.Z,
+            RotationX = rotation.X, RotationY = rotation.Y, RotationZ = rotation.Z, RotationW = rotation.W,
+            VelocityX = velocity.X, VelocityY = velocity.Y, VelocityZ = velocity.Z,
+            Size0 = size.Item1, Size1 = size.Item2, Size2 = size.Item3,
+            Density = density, Friction = friction, Restitution = restitution, RollingResistance = rollingResistance,
+        };
+    }
+
+    private int AddBody(Box3DSoaBodyDesc desc)
+    {
+        ObjectDisposedException.ThrowIf(_handle == 0, this);
+        Check(Box3DSoaNative.ymir_box3d_soa_add_body(_handle, desc, out var index));
         return index;
     }
 

src/Ymir.Box3D/Properties/AssemblyInfo.cs

@@ -2,3 +2,5 @@ using System.Runtime.CompilerServices;
 
 [assembly: InternalsVisibleTo("Ymir.Core")]
 [assembly: InternalsVisibleTo("Ymir.Box3D.Soa.Tests")]
+[assembly: InternalsVisibleTo("Lodur.Body.Ymir")]
+[assembly: InternalsVisibleTo("Lodur.Tests")]

tests/Ymir.Box3D.Soa.Tests/AssemblyInfo.cs

@@ -0,0 +1,4 @@
+// Box3D world creation is not safe to run concurrently in one process: two threads calling
+// b3CreateWorld abort the host (found when two test classes created worlds at once). Tests that create
+// worlds run one at a time.
+[assembly: CollectionBehavior(DisableTestParallelization = true)]

tests/Ymir.Box3D.Soa.Tests/Box3DSoaJointTests.cs

@@ -0,0 +1,228 @@
+using System.Runtime.InteropServices;
+using Ymir.Box3D;
+
+namespace Ymir.Box3D.Soa.Tests;
+
+public sealed class Box3DSoaJointTests
+{
+    private const float Dt = 1f / 60f;
+    private static readonly (float, float, float, float) QuarterTurnZ = (0f, 0f, MathF.Sin(MathF.PI / 4), MathF.Cos(MathF.PI / 4));
+
+    /// <summary>A static anchor at the origin and a capsule hinged at x = 0, lying along -X, free to swing about Z.</summary>
+    private static (Box3DSoaWorld World, int Anchor, int Capsule, int Joint) Pendulum(Box3DSoaJointKind kind = Box3DSoaJointKind.Revolute)
+    {
+        var world = new Box3DSoaWorld(capacity: 4, jointCapacity: 2);
+        var anchor = world.AddStaticBox(1, (0f, 5f, 0f), (0.1f, 0.1f, 0.1f));
+        var capsule = world.AddCapsule(2, (-0.5f, 5f, 0f), radius: 0.1f, halfLength: 0.5f, density: 1f,
+            rotation: QuarterTurnZ, parent: anchor, limb: 7);
+        var joint = world.AddJoint(kind, anchor, capsule, anchor: (0f, 5f, 0f), axis: (0f, 0f, 1f));
+        return (world, anchor, capsule, joint);
+    }
+
+    [Fact]
+    public void A_revolute_joint_starts_at_zero_and_its_state_lanes_follow_the_swing()
+    {
+        var (world, anchor, capsule, joint) = Pendulum();
+        using var _ = world;
+        var view = world.View;
+        Assert.Equal(1, view.JointCount);
+        Assert.Equal((uint)Box3DSoaJointKind.Revolute, view.JointKind[joint]);
+        Assert.Equal(anchor, view.JointBodyA[joint]);
+        Assert.Equal(capsule, view.JointBodyB[joint]);
+        Assert.Equal(0f, view.JointAngle[joint], 1e-5f);
+        Assert.Equal(1f, view.JointRotationW[joint], 1e-5f); // the creation pose is the joint's zero
+
+        for (var i = 0; i < 30; i++) world.Step(Dt);
+        var angle = view.JointAngle[joint];
+        Assert.True(MathF.Abs(angle) > 0.5f, "gravity swings the capsule down about the hinge");
+        // Box3D's own angle and the angle derived from the relative-rotation lanes are the same quantity.
+        Assert.Equal(angle, 2f * MathF.Atan2(view.JointRotationZ[joint], view.JointRotationW[joint]), 1e-3f);
+        // The hinge permits only Z: the off-axis part of the relative rotation stays small.
+        Assert.True(MathF.Abs(view.JointRotationX[joint]) < 0.02f && MathF.Abs(view.JointRotationY[joint]) < 0.02f);
+        Assert.True(MathF.Abs(view.JointAngularVelocityZ[joint]) > 0.5f);
+        // The anchor never moves; the capsule's end stays on the pin (distance from the hinge is the arm length 1 - radius-free).
+        var dx = view.PositionX[capsule]; var dy = view.PositionY[capsule] - 5f;
+        Assert.Equal(0.5f, MathF.Sqrt(dx * dx + dy * dy), 0.02f);
+    }
+
+    [Fact]
+    public void Joint_frame_lane_is_the_world_orientation_of_frame_a()
+    {
+        var (world, anchor, _, joint) = Pendulum();
+        using var _ = world;
+        var view = world.View;
+        // Anchor is unrotated and the joint axis is Z, so frame A is identity in world space.
+        Assert.Equal(0f, view.JointFrameX[joint], 1e-5f);
+        Assert.Equal(0f, view.JointFrameY[joint], 1e-5f);
+        Assert.Equal(0f, view.JointFrameZ[joint], 1e-5f);
+        Assert.Equal(1f, MathF.Abs(view.JointFrameW[joint]), 1e-5f);
+        Assert.Equal(-1, view.Parent[anchor]);
+    }
+
+    [Fact]
+    public void An_off_axis_hinge_gets_a_frame_rotated_onto_its_axis()
+    {
+        using var world = new Box3DSoaWorld(capacity: 2, jointCapacity: 1);
+        var a = world.AddStaticBox(1, (0f, 5f, 0f), (0.1f, 0.1f, 0.1f));
+        var b = world.AddBox(2, (0f, 5f, 1f), (0.2f, 0.2f, 0.2f), 1f, parent: a); // level with the hinge, so it falls
+        // Hinge about world X: frame z = X, so frame A is the quarter turn about Y that takes Z to X.
+        var j = world.AddJoint(Box3DSoaJointKind.Revolute, a, b, (0f, 5f, 0f), (1f, 0f, 0f));
+        var v = world.View;
+        Assert.Equal(MathF.Sin(MathF.PI / 4), v.JointFrameY[j], 1e-4f);
+        Assert.Equal(MathF.Cos(MathF.PI / 4), v.JointFrameW[j], 1e-4f);
+        for (var i = 0; i < 40; i++) world.Step(Dt);
+        // It swings about world X only: the box's rotation quaternion has no Y or Z part.
+        Assert.True(MathF.Abs(v.RotationX[b]) > 0.05f);
+        Assert.True(MathF.Abs(v.RotationY[b]) < 0.02f && MathF.Abs(v.RotationZ[b]) < 0.02f);
+    }
+
+    [Fact]
+    public void A_revolute_limit_stops_the_swing_and_a_weld_holds_the_pose()
+    {
+        using var world = new Box3DSoaWorld(capacity: 6, jointCapacity: 3);
+        var a = world.AddStaticBox(1, (0f, 5f, 0f), (0.1f, 0.1f, 0.1f));
+        var limited = world.AddCapsule(2, (-0.5f, 5f, 0f), 0.1f, 0.5f, 1f, rotation: QuarterTurnZ, parent: a);
+        var j = world.AddJoint(Box3DSoaJointKind.Revolute, a, limited, (0f, 5f, 0f), (0f, 0f, 1f), limit: true, lower: -0.3f, upper: 0.3f);
+        var welded = world.AddCapsule(3, (0.5f, 5f, 0f), 0.1f, 0.5f, 1f, rotation: QuarterTurnZ, parent: a);
+        var w = world.AddJoint(Box3DSoaJointKind.Weld, a, welded, (0f, 5f, 0f), (0f, 0f, 1f));
+        for (var i = 0; i < 180; i++) world.Step(Dt);
+        var v = world.View;
+        Assert.InRange(v.JointAngle[j], -0.32f, 0.32f);
+        Assert.True(MathF.Abs(v.JointAngle[j]) > 0.25f, "it reached the stop rather than hanging free");
+        Assert.Equal((uint)Box3DSoaJointKind.Weld, v.JointKind[w]);
+        Assert.Equal(0.5f, v.PositionX[welded], 0.05f); // a cantilevered weld sags only a little
+        Assert.Equal(5f, v.PositionY[welded], 0.1f);
+    }
+
+    [Fact]
+    public void A_spherical_joint_swings_in_every_direction_and_reports_a_relative_rotation()
+    {
+        var (world, _, capsule, joint) = Pendulum(Box3DSoaJointKind.Spherical);
+        using var _ = world;
+        var v = world.View;
+        for (var i = 0; i < 20; i++) world.Step(Dt);
+        // A push along Z leaves the plane of the swing, which a hinge would refuse.
+        v.ForceZ[capsule] = 60f;
+        world.Step(Dt);
+        for (var i = 0; i < 20; i++) world.Step(Dt);
+        Assert.True(MathF.Abs(v.JointRotationX[joint]) > 0.02f || MathF.Abs(v.JointRotationY[joint]) > 0.02f);
+        Assert.True(MathF.Abs(v.PositionZ[capsule]) > 0.005f);
+        var n = v.JointRotationX[joint] * v.JointRotationX[joint] + v.JointRotationY[joint] * v.JointRotationY[joint]
+              + v.JointRotationZ[joint] * v.JointRotationZ[joint] + v.JointRotationW[joint] * v.JointRotationW[joint];
+        Assert.Equal(1f, n, 1e-3f);
+        Assert.True(v.JointRotationW[joint] >= 0f);
+    }
+
+    [Fact]
+    public void Force_and_torque_lanes_act_before_the_step_and_clear_after()
+    {
+        using var world = new Box3DSoaWorld(capacity: 3, gravityY: 0f);
+        var ball = world.AddSphere(1, (0f, 0f, 0f), 0.5f, 1f);
+        var ground = world.AddStaticBox(2, (0f, -10f, 0f), (1f, 1f, 1f));
+        var v = world.View;
+        var mass = 4f / 3f * MathF.PI * 0.5f * 0.5f * 0.5f;
+        var inertia = 0.4f * mass * 0.25f;
+
+        v.ForceX[ball] = 6f;
+        v.TorqueZ[ball] = 2f;
+        v.ForceX[ground] = 1000f; // a static body ignores its lanes, and they clear all the same
+        world.Step(Dt);
+        Assert.Equal(6f / mass * Dt, v.VelocityX[ball], 6f / mass * Dt * 0.02f);
+        Assert.Equal(2f / inertia * Dt, v.AngularVelocityZ[ball], 2f / inertia * Dt * 0.02f);
+        Assert.Equal(0f, v.VelocityX[ground]);
+        Assert.Equal(0f, v.ForceX[ball]);
+        Assert.Equal(0f, v.TorqueZ[ball]);
+        Assert.Equal(0f, v.ForceX[ground]);
+
+        var vx = v.VelocityX[ball];
+        world.Step(Dt);
+        Assert.Equal(vx, v.VelocityX[ball], 1e-6f); // nothing was applied twice
+    }
+
+    [Fact]
+    public void A_non_finite_write_lane_refuses_the_step_and_touches_nothing()
+    {
+        using var world = new Box3DSoaWorld(capacity: 3, gravityY: 0f);
+        var a = world.AddSphere(1, (0f, 0f, 0f), 0.5f, 1f);
+        var b = world.AddSphere(2, (5f, 0f, 0f), 0.5f, 1f);
+        var v = world.View;
+        v.ForceX[a] = 10f;
+        v.TorqueY[b] = float.NaN;
+        Assert.Throws<InvalidOperationException>(() => world.Step(Dt));
+        Assert.Equal(0ul, world.StepCount);
+        Assert.Equal(10f, v.ForceX[a]); // not applied, not cleared
+        Assert.Equal(0f, v.VelocityX[a]);
+        v.TorqueY[b] = 0f;
+        world.Step(Dt);
+        Assert.True(v.VelocityX[a] > 0f);
+    }
+
+    [Fact]
+    public void Capsules_and_boxes_rest_on_the_ground_at_their_own_heights_with_parent_and_limb_lanes()
+    {
+        using var world = new Box3DSoaWorld(capacity: 4);
+        var ground = world.AddStaticBox(1, (0f, -0.5f, 0f), (20f, 0.5f, 20f));
+        var capsule = world.AddCapsule(2, (-2f, 2f, 0f), 0.2f, 0.5f, 1f, rotation: QuarterTurnZ, parent: ground, limb: 3);
+        var box = world.AddBox(3, (2f, 2f, 0f), (0.3f, 0.4f, 0.5f), 1f, parent: capsule, limb: 9);
+        for (var i = 0; i < 300; i++) world.Step(Dt);
+        var v = world.View;
+        Assert.Equal((uint)Box3DSoaKind.Capsule, v.Kind[capsule]);
+        Assert.Equal((uint)Box3DSoaKind.Box, v.Kind[box]);
+        Assert.Equal(0.2f, v.PositionY[capsule], 0.02f); // lying down: centre one radius up
+        Assert.Equal(0.4f, v.PositionY[box], 0.02f);     // half height
+        Assert.Equal(-1, v.Parent[ground]);
+        Assert.Equal(ground, v.Parent[capsule]);
+        Assert.Equal(capsule, v.Parent[box]);
+        Assert.Equal(3, v.Limb[capsule]);
+        Assert.Equal(9, v.Limb[box]);
+    }
+
+    [Fact]
+    public void Refuses_bad_bodies_and_bad_joints()
+    {
+        using var world = new Box3DSoaWorld(capacity: 3, jointCapacity: 1);
+        var a = world.AddSphere(1, (0f, 5f, 0f), 0.5f, 1f);
+        var b = world.AddSphere(2, (1f, 5f, 0f), 0.5f, 1f);
+        Assert.Throws<InvalidOperationException>(() => world.AddCapsule(3, (0f, 0f, 0f), 0.2f, 0f, 1f));            // no length
+        Assert.Throws<InvalidOperationException>(() => world.AddBox(3, (0f, 0f, 0f), (1f, 0f, 1f), 1f));           // flat box
+        Assert.Throws<InvalidOperationException>(() => world.AddBox(3, (0f, 0f, 0f), (1f, 1f, 1f), 0f));           // massless
+        Assert.Throws<InvalidOperationException>(() => world.AddBox(3, (0f, 0f, 0f), (1f, 1f, 1f), 1f, rotation: (0f, 0f, 0.5f, 0.5f))); // not unit
+        Assert.Throws<InvalidOperationException>(() => world.AddSphere(3, (0f, 0f, 0f), 0.5f, 1f, parent: 2));     // parent not yet created
+        Assert.Throws<InvalidOperationException>(() => world.AddSphere(3, (0f, 0f, 0f), 0.5f, 1f, friction: -1f));
+        Assert.Throws<InvalidOperationException>(() => world.AddJoint(Box3DSoaJointKind.Weld, a, a, (0f, 0f, 0f), (0f, 0f, 1f)));
+        Assert.Throws<InvalidOperationException>(() => world.AddJoint(Box3DSoaJointKind.Weld, a, 9, (0f, 0f, 0f), (0f, 0f, 1f)));
+        Assert.Throws<InvalidOperationException>(() => world.AddJoint(Box3DSoaJointKind.Weld, a, b, (0f, 0f, 0f), (0f, 0f, 0f)));
+        Assert.Throws<InvalidOperationException>(() => world.AddJoint(Box3DSoaJointKind.Revolute, a, b, (0f, 0f, 0f), (0f, 0f, 1f), limit: true, lower: 1f, upper: -1f));
+        world.AddJoint(Box3DSoaJointKind.Weld, a, b, (0.5f, 5f, 0f), (0f, 0f, 1f));
+        Assert.Throws<InvalidOperationException>(() => world.AddJoint(Box3DSoaJointKind.Weld, a, b, (0.5f, 5f, 0f), (0f, 0f, 1f))); // joint capacity
+        Assert.Equal(2, world.View.Count); // refused bodies left no trace
+        Assert.Equal(1, world.View.JointCount);
+    }
+
+    [Fact]
+    public void A_jointed_body_driven_by_torque_lanes_repeats_bit_for_bit()
+    {
+        static byte[] Run()
+        {
+            var (world, anchor, capsule, joint) = Pendulum(Box3DSoaJointKind.Spherical);
+            using var _ = world;
+            var v = world.View;
+            for (var i = 0; i < 200; i++)
+            {
+                v.TorqueZ[capsule] = 3f * MathF.Sin(i * 0.1f);
+                v.TorqueX[capsule] = 1f * MathF.Cos(i * 0.07f);
+                world.Step(Dt);
+            }
+            var floats = new List<float>();
+            void Add(ReadOnlySpan<float> lane) => floats.AddRange(lane.ToArray());
+            Add(v.PositionX); Add(v.PositionY); Add(v.PositionZ);
+            Add(v.RotationX); Add(v.RotationY); Add(v.RotationZ); Add(v.RotationW);
+            Add(v.JointAngle); Add(v.JointRotationX); Add(v.JointRotationY); Add(v.JointRotationZ); Add(v.JointRotationW);
+            Add(v.JointAngularVelocityX); Add(v.JointAngularVelocityY); Add(v.JointAngularVelocityZ);
+            return MemoryMarshal.AsBytes(floats.ToArray().AsSpan()).ToArray();
+        }
+        var first = Run();
+        Assert.True(first.AsSpan().SequenceEqual(Run()));
+        Assert.Contains(first, b => b != 0);
+    }
+}