From 8bb0d0b1de011091ddc3b51918539e9d4ee9ead8 Mon Sep 17 00:00:00 2001 From: Nibu Date: Sat, 7 Nov 2026 03:55:49 +0000 Subject: [PATCH 1/4] SoA world: joints, force/torque write lanes, capsules and boxes, parent/limb lanes ABI 2. One body descriptor replaces add_sphere/add_static_box (kinds: sphere, capsule, box, static box). Revolute, spherical and weld joints created from a world anchor and axis, with joint state lanes (angle, frame A orientation, relative rotation, relative angular velocity). Per-body force and torque write lanes, applied before the step then cleared; non-finite values refuse the step. Parent and limb index lanes. Tests run serially: concurrent b3CreateWorld aborts the host. Co-Authored-By: Claude Sonnet 5.5 --- docs/soa-world.md | 52 +- native/Ymir.Box3D/ymir_box3d_soa.c | 571 ++++++++++++++---- src/Ymir.Box3D/Box3DSoaWorld.cs | 210 ++++++- tests/Ymir.Box3D.Soa.Tests/AssemblyInfo.cs | 4 + .../Box3DSoaJointTests.cs | 228 +++++++ 5 files changed, 918 insertions(+), 147 deletions(-) create mode 100644 tests/Ymir.Box3D.Soa.Tests/AssemblyInfo.cs create mode 100644 tests/Ymir.Box3D.Soa.Tests/Box3DSoaJointTests.cs diff --git a/docs/soa-world.md b/docs/soa-world.md index 4258840..264aba5 100644 --- a/docs/soa-world.md +++ b/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` 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, 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. diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index cc4c29a..861d15e 100644 --- a/native/Ymir.Box3D/ymir_box3d_soa.c +++ b/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 reserved_float; +} 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,66 @@ 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) || d->friction < 0.0f || d->restitution < 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 +422,68 @@ 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; + + 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 +492,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 +629,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 +653,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 +681,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; } diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index 2af40d3..17817b8 100644 --- a/src/Ymir.Box3D/Box3DSoaWorld.cs +++ b/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 +} + +/// Native layout of ymir_box3d_soa_body_desc. +[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, ReservedFloat; +} + +/// Native layout of ymir_box3d_soa_joint_desc. +[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; } /// Native layout of ymir_box3d_soa_view: counts, then one pointer per lane. @@ -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 StableId => new(_v.StableId, Count); public ReadOnlySpan Kind => new(_v.Kind, Count); + /// Lane index of the body's parent in the limb tree, or -1 for a root. Set at creation, not used by Box3D. + public ReadOnlySpan Parent => new(_v.Parent, Count); + /// The caller's limb label for the body. Set at creation, not used by Box3D. + public ReadOnlySpan Limb => new(_v.Limb, Count); public ReadOnlySpan PositionX => new(_v.PositionX, Count); public ReadOnlySpan PositionY => new(_v.PositionY, Count); public ReadOnlySpan PositionZ => new(_v.PositionZ, Count); @@ -108,6 +179,39 @@ internal readonly unsafe struct Box3DSoaView public ReadOnlySpan AngularVelocityY => new(_v.AngularVelocityY, Count); public ReadOnlySpan 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 ForceX => new(_v.ForceX, Count); + public Span ForceY => new(_v.ForceY, Count); + public Span ForceZ => new(_v.ForceZ, Count); + public Span TorqueX => new(_v.TorqueX, Count); + public Span TorqueY => new(_v.TorqueY, Count); + public Span TorqueZ => new(_v.TorqueZ, Count); + + /// Joints at the moment this view was taken: take it after the last AddJoint. + public int JointCount => _v.JointCount; + public ReadOnlySpan JointKind => new(_v.JointKind, JointCount); + /// Lane indices of the joint's two bodies; the second is the child. + public ReadOnlySpan JointBodyA => new(_v.JointBodyA, JointCount); + public ReadOnlySpan JointBodyB => new(_v.JointBodyB, JointCount); + /// Box3D's own angle: revolute angle or spherical twist angle (radians), zero for a weld. + public ReadOnlySpan JointAngle => new(_v.JointAngle, JointCount); + /// World orientation of the joint frame on body A (quaternion x y z w). + public ReadOnlySpan JointFrameX => new(_v.JointFrameX, JointCount); + public ReadOnlySpan JointFrameY => new(_v.JointFrameY, JointCount); + public ReadOnlySpan JointFrameZ => new(_v.JointFrameZ, JointCount); + public ReadOnlySpan JointFrameW => new(_v.JointFrameW, JointCount); + /// Joint frame B relative to frame A (quaternion x y z w, w >= 0); identity at the creation pose. + public ReadOnlySpan JointRotationX => new(_v.JointRotationX, JointCount); + public ReadOnlySpan JointRotationY => new(_v.JointRotationY, JointCount); + public ReadOnlySpan JointRotationZ => new(_v.JointRotationZ, JointCount); + public ReadOnlySpan JointRotationW => new(_v.JointRotationW, JointCount); + /// Body B's angular velocity minus body A's, expressed in frame A (rad/s). + public ReadOnlySpan JointAngularVelocityX => new(_v.JointAngularVelocityX, JointCount); + public ReadOnlySpan JointAngularVelocityY => new(_v.JointAngularVelocityY, JointCount); + public ReadOnlySpan JointAngularVelocityZ => new(_v.JointAngularVelocityZ, JointCount); + /// Address of a lane, for tests and interop that need to prove nothing was copied. public nint PositionYAddress => (nint)_v.PositionY; } @@ -118,18 +222,20 @@ internal readonly unsafe struct Box3DSoaView /// 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()) + Box3DSoaNative.ymir_box3d_soa_get_view_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf() || + Box3DSoaNative.ymir_box3d_soa_get_body_desc_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf() || + Box3DSoaNative.ymir_box3d_soa_get_joint_desc_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf()) { 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)); } /// The live lanes. Count and step count are read from native memory each call. @@ -157,24 +263,80 @@ internal sealed class Box3DSoaWorld : IDisposable /// Adds a dynamic sphere; returns its lane index. 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) => + AddBody(Describe(Box3DSoaKind.Sphere, stableId, position, default, (radius, 0f, 0f), density, + velocity, friction, restitution, parent, limb)); /// Adds a static axis-aligned box centred at . 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)); + + /// + /// Adds a dynamic capsule: a segment of either side of the centre along + /// the body's local Y, swept by . is a unit quaternion + /// x y z w; the default (all zero) means identity. + /// + 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) => + AddBody(Describe(Box3DSoaKind.Capsule, stableId, position, rotation, (radius, halfLength, 0f), density, + default, friction, restitution, parent, limb)); + + /// Adds a dynamic box with the given half extents; rotation as for . + 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)); + + /// + /// Joins two bodies at a world-space . The joint frame's z axis is + /// (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 as swing. + /// A weld takes no limits. Returns the joint's lane index. Body B is the child. + /// + 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) + { + 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, + }; + } + + private int AddBody(Box3DSoaBodyDesc desc) + { + ObjectDisposedException.ThrowIf(_handle == 0, this); + Check(Box3DSoaNative.ymir_box3d_soa_add_body(_handle, desc, out var index)); return index; } diff --git a/tests/Ymir.Box3D.Soa.Tests/AssemblyInfo.cs b/tests/Ymir.Box3D.Soa.Tests/AssemblyInfo.cs new file mode 100644 index 0000000..8e22081 --- /dev/null +++ b/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)] diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaJointTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaJointTests.cs new file mode 100644 index 0000000..7370a72 --- /dev/null +++ b/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)); + + /// A static anchor at the origin and a capsule hinged at x = 0, lying along -X, free to swing about Z. + 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(() => 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(() => world.AddCapsule(3, (0f, 0f, 0f), 0.2f, 0f, 1f)); // no length + Assert.Throws(() => world.AddBox(3, (0f, 0f, 0f), (1f, 0f, 1f), 1f)); // flat box + Assert.Throws(() => world.AddBox(3, (0f, 0f, 0f), (1f, 1f, 1f), 0f)); // massless + Assert.Throws(() => world.AddBox(3, (0f, 0f, 0f), (1f, 1f, 1f), 1f, rotation: (0f, 0f, 0.5f, 0.5f))); // not unit + Assert.Throws(() => world.AddSphere(3, (0f, 0f, 0f), 0.5f, 1f, parent: 2)); // parent not yet created + Assert.Throws(() => world.AddSphere(3, (0f, 0f, 0f), 0.5f, 1f, friction: -1f)); + Assert.Throws(() => world.AddJoint(Box3DSoaJointKind.Weld, a, a, (0f, 0f, 0f), (0f, 0f, 1f))); + Assert.Throws(() => world.AddJoint(Box3DSoaJointKind.Weld, a, 9, (0f, 0f, 0f), (0f, 0f, 1f))); + Assert.Throws(() => world.AddJoint(Box3DSoaJointKind.Weld, a, b, (0f, 0f, 0f), (0f, 0f, 0f))); + Assert.Throws(() => 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(() => 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(); + void Add(ReadOnlySpan 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); + } +} -- 2.47.3 From 02276363fc917c11cd78ce7da1992102cec182ab Mon Sep 17 00:00:00 2001 From: Nibu Date: Sat, 7 Nov 2026 03:56:51 +0000 Subject: [PATCH 2/4] Ymir.Box3D: let Lodur.Body.Ymir see the SoA world until Ymir.Core has a public surface Co-Authored-By: Claude Sonnet 5.5 --- src/Ymir.Box3D/Properties/AssemblyInfo.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Ymir.Box3D/Properties/AssemblyInfo.cs b/src/Ymir.Box3D/Properties/AssemblyInfo.cs index 93c6e82..bd44821 100644 --- a/src/Ymir.Box3D/Properties/AssemblyInfo.cs +++ b/src/Ymir.Box3D/Properties/AssemblyInfo.cs @@ -2,3 +2,4 @@ using System.Runtime.CompilerServices; [assembly: InternalsVisibleTo("Ymir.Core")] [assembly: InternalsVisibleTo("Ymir.Box3D.Soa.Tests")] +[assembly: InternalsVisibleTo("Lodur.Body.Ymir")] -- 2.47.3 From b96ad34b0bb33b457cd60b55ae187583850d02aa Mon Sep 17 00:00:00 2001 From: Nibu Date: Sat, 7 Nov 2026 04:07:38 +0000 Subject: [PATCH 3/4] SoA world: rolling resistance on the body descriptor A bare sphere or capsule rolls on without it, so a fallen head never settles. Co-Authored-By: Claude Sonnet 5.5 --- native/Ymir.Box3D/ymir_box3d_soa.c | 6 ++++-- src/Ymir.Box3D/Box3DSoaWorld.cs | 14 +++++++------- src/Ymir.Box3D/Properties/AssemblyInfo.cs | 1 + 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index 861d15e..af2fe78 100644 --- a/native/Ymir.Box3D/ymir_box3d_soa.c +++ b/native/Ymir.Box3D/ymir_box3d_soa.c @@ -71,7 +71,7 @@ typedef struct ymir_box3d_soa_body_desc float density; float friction; float restitution; - float reserved_float; + 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` @@ -362,7 +362,8 @@ static int32_t ymir_soa_check_body(const ymir_box3d_soa_world* world, const ymir { 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) || d->friction < 0.0f || d->restitution < 0.0f) + !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; } @@ -455,6 +456,7 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body( } 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) diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index 17817b8..c6eee77 100644 --- a/src/Ymir.Box3D/Box3DSoaWorld.cs +++ b/src/Ymir.Box3D/Box3DSoaWorld.cs @@ -30,7 +30,7 @@ internal struct Box3DSoaBodyDesc public float RotationX, RotationY, RotationZ, RotationW; public float VelocityX, VelocityY, VelocityZ; public float Size0, Size1, Size2; - public float Density, Friction, Restitution, ReservedFloat; + public float Density, Friction, Restitution, RollingResistance; } /// Native layout of ymir_box3d_soa_joint_desc. @@ -264,9 +264,9 @@ internal sealed class Box3DSoaWorld : IDisposable 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, - int parent = -1, int limb = 0) => + 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)); + velocity, friction, restitution, parent, limb, rollingResistance)); /// Adds a static axis-aligned box centred at . public int AddStaticBox( @@ -283,9 +283,9 @@ internal sealed class Box3DSoaWorld : IDisposable 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) => + 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)); + default, friction, restitution, parent, limb, rollingResistance)); /// Adds a dynamic box with the given half extents; rotation as for . public int AddBox( @@ -319,7 +319,7 @@ internal sealed class Box3DSoaWorld : IDisposable 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 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 @@ -329,7 +329,7 @@ internal sealed class Box3DSoaWorld : IDisposable 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, + Density = density, Friction = friction, Restitution = restitution, RollingResistance = rollingResistance, }; } diff --git a/src/Ymir.Box3D/Properties/AssemblyInfo.cs b/src/Ymir.Box3D/Properties/AssemblyInfo.cs index bd44821..263a317 100644 --- a/src/Ymir.Box3D/Properties/AssemblyInfo.cs +++ b/src/Ymir.Box3D/Properties/AssemblyInfo.cs @@ -3,3 +3,4 @@ using System.Runtime.CompilerServices; [assembly: InternalsVisibleTo("Ymir.Core")] [assembly: InternalsVisibleTo("Ymir.Box3D.Soa.Tests")] [assembly: InternalsVisibleTo("Lodur.Body.Ymir")] +[assembly: InternalsVisibleTo("Lodur.Tests")] -- 2.47.3 From ea7b3f3d7fa3bc2ee25fef242da57906321122d1 Mon Sep 17 00:00:00 2001 From: Nibu Date: Sat, 7 Nov 2026 04:08:18 +0000 Subject: [PATCH 4/4] soa-world doc: rolling resistance in the descriptor Co-Authored-By: Claude Sonnet 5.5 --- docs/soa-world.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/soa-world.md b/docs/soa-world.md index 264aba5..f62d76e 100644 --- a/docs/soa-world.md +++ b/docs/soa-world.md @@ -14,7 +14,7 @@ separate native object (`ymir_box3d_soa.c`, ABI 1) so the Aetheria session's con - 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, parent, limb). Capacities are fixed at creation, + 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 -- 2.47.3