diff --git a/docs/soa-world.md b/docs/soa-world.md index 4258840..f62d76e 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, 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. diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index cc4c29a..af2fe78 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 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; } diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index 2af40d3..c6eee77 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, RollingResistance; +} + +/// 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, float rollingResistance = 0f) => + AddBody(Describe(Box3DSoaKind.Sphere, stableId, position, default, (radius, 0f, 0f), density, + velocity, friction, restitution, parent, limb, rollingResistance)); /// 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, float rollingResistance = 0f) => + AddBody(Describe(Box3DSoaKind.Capsule, stableId, position, rotation, (radius, halfLength, 0f), density, + default, friction, restitution, parent, limb, rollingResistance)); + + /// 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, 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; } diff --git a/src/Ymir.Box3D/Properties/AssemblyInfo.cs b/src/Ymir.Box3D/Properties/AssemblyInfo.cs index 93c6e82..263a317 100644 --- a/src/Ymir.Box3D/Properties/AssemblyInfo.cs +++ b/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")] 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); + } +}