From 7efcefc8a011f26826e563a500ba2d8393c58231 Mon Sep 17 00:00:00 2001 From: Meta Vens Date: Fri, 20 Nov 2026 02:02:14 +0100 Subject: [PATCH] SoA world: kinematic anchors, motor joint spring, point impulses, closest-hit ray (ABI 5) Anchor body kind (kinematic, no shape) with set_anchor_target; Motor joint kind with position springs bounded by max force and torque, taking the slot of the joint desc's reserved float; apply_impulse at a world point; cast_ray_closest. Ymir executes the commands and decides nothing about grabbing. Co-Authored-By: Claude Sonnet 5.5 --- native/Ymir.Box3D/ymir_box3d_soa.c | 226 +++++++++++++---- src/Ymir.Box3D/Box3DSoaWorld.cs | 116 ++++++++- .../Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs | 233 ++++++++++++++++++ 3 files changed, 528 insertions(+), 47 deletions(-) create mode 100644 tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs diff --git a/native/Ymir.Box3D/ymir_box3d_soa.c b/native/Ymir.Box3D/ymir_box3d_soa.c index 135cc41..2abef67 100644 --- a/native/Ymir.Box3D/ymir_box3d_soa.c +++ b/native/Ymir.Box3D/ymir_box3d_soa.c @@ -44,21 +44,26 @@ enum YMIR_SOA_KIND_SPHERE = 0, YMIR_SOA_KIND_STATIC_BOX = 1, YMIR_SOA_KIND_CAPSULE = 2, - YMIR_SOA_KIND_BOX = 3 + YMIR_SOA_KIND_BOX = 3, + YMIR_SOA_KIND_ANCHOR = 4 }; enum { YMIR_SOA_JOINT_REVOLUTE = 0, YMIR_SOA_JOINT_SPHERICAL = 1, - YMIR_SOA_JOINT_WELD = 2 + YMIR_SOA_JOINT_WELD = 2, + YMIR_SOA_JOINT_MOTOR = 3 }; -#define YMIR_SOA_ABI_VERSION 4u +#define YMIR_SOA_ABI_VERSION 5u // 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. +// An anchor (kind 4) is a kinematic body with no shape: it is never hit and never collides, and size, +// density, friction, restitution, rolling resistance and material are ignored. Its pose is set with +// `ymir_box3d_soa_set_anchor_target`; the force and torque lanes never move it. typedef struct ymir_box3d_soa_body_desc { uint64_t stable_id; @@ -81,6 +86,10 @@ typedef struct ymir_box3d_soa_body_desc // 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. +// A motor joint (kind 3) is a pair of position springs, no velocity target: the linear spring pulls body B's +// frame origin onto body A's (so B's anchor point onto A's), the angular spring pulls B's frame orientation onto +// A's. `*_hertz` 0 turns that spring off; `max_force` (N) and `max_torque` (N m) bound what it may apply, so a +// weak grab hangs below its target. All six are finite and >= 0 and are ignored by the other kinds. typedef struct ymir_box3d_soa_joint_desc { uint32_t kind; @@ -92,7 +101,12 @@ typedef struct ymir_box3d_soa_joint_desc float lower; float upper; float cone_angle; - float reserved_float; + float linear_hertz; + float linear_damping; + float max_force; + float angular_hertz; + float angular_damping; + float max_torque; } 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 @@ -211,6 +225,19 @@ typedef struct ymir_box3d_soa_facts const ymir_box3d_soa_hit_fact* hit; } ymir_box3d_soa_facts; +// The closest hit of a ray. `hit` is 0 for a miss and every other field is then 0. `lane` and `stable_id` name +// the body and shape hit; `fraction` is of the ray's translation. +typedef struct ymir_box3d_soa_ray_hit +{ + int32_t hit; + int32_t lane; + uint64_t stable_id; + float point[3]; + float normal[3]; + float fraction; + uint32_t reserved; +} ymir_box3d_soa_ray_hit; + #define YMIR_SOA_STATE_LANES 13 #define YMIR_SOA_WRITE_LANES 6 #define YMIR_SOA_LAST_SUBSTEP_FORCE_LANE (YMIR_SOA_STATE_LANES + YMIR_SOA_WRITE_LANES) @@ -269,9 +296,14 @@ 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_anchor(uint32_t kind) +{ + return kind == YMIR_SOA_KIND_ANCHOR; +} + static int ymir_soa_is_dynamic(uint32_t kind) { - return kind != YMIR_SOA_KIND_STATIC_BOX; + return kind != YMIR_SOA_KIND_STATIC_BOX && !ymir_soa_is_anchor(kind); } static void ymir_soa_refresh(ymir_box3d_soa_world* world, int index) @@ -342,6 +374,11 @@ YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_joint_desc_size(void) return (uint32_t)sizeof(ymir_box3d_soa_joint_desc); } +YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_ray_hit_size(void) +{ + return (uint32_t)sizeof(ymir_box3d_soa_ray_hit); +} + YMIR_EXPORT uint32_t YMIR_CALL ymir_box3d_soa_get_touch_fact_size(void) { return (uint32_t)sizeof(ymir_box3d_soa_touch_fact); @@ -470,7 +507,7 @@ YMIR_EXPORT void YMIR_CALL ymir_box3d_soa_destroy(ymir_box3d_soa_world* world) 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) || + if (d->kind > YMIR_SOA_KIND_ANCHOR || !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) @@ -500,6 +537,8 @@ static int32_t ymir_soa_check_body(const ymir_box3d_soa_world* world, const ymir } switch (d->kind) { + case YMIR_SOA_KIND_ANCHOR: + break; // no shape: nothing to size case YMIR_SOA_KIND_SPHERE: if (d->size[0] <= 0.0f) { @@ -548,7 +587,9 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body( } b3BodyDef body_def = b3DefaultBodyDef(); - body_def.type = ymir_soa_is_dynamic(d->kind) ? b3_dynamicBody : b3_staticBody; + body_def.type = ymir_soa_is_anchor(d->kind) ? b3_kinematicBody + : 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]}; @@ -559,39 +600,42 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body( { return YMIR_SOA_INTERNAL_ERROR; } - b3ShapeDef shape_def = b3DefaultShapeDef(); - shape_def.userData = (void*)(uintptr_t)d->stable_id; - shape_def.enableContactEvents = true; // off by default in Box3D - shape_def.enableHitEvents = true; - shape_def.baseMaterial.userMaterialId = d->material_id; - if (ymir_soa_is_dynamic(d->kind)) + if (!ymir_soa_is_anchor(d->kind)) // an anchor has no shape { - shape_def.density = d->density; - } - shape_def.baseMaterial.friction = d->friction; - shape_def.baseMaterial.restitution = d->restitution; - shape_def.baseMaterial.rollingResistance = d->rolling_resistance; + b3ShapeDef shape_def = b3DefaultShapeDef(); + shape_def.userData = (void*)(uintptr_t)d->stable_id; + shape_def.enableContactEvents = true; // off by default in Box3D + shape_def.enableHitEvents = true; + shape_def.baseMaterial.userMaterialId = d->material_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; + 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++; @@ -600,9 +644,9 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_body( world->kind[i] = d->kind; world->parent[i] = d->parent; world->limb[i] = d->limb; - world->material_id[i] = d->material_id; + world->material_id[i] = ymir_soa_is_anchor(d->kind) ? 0 : d->material_id; /* Publish only the components the kind uses; unused ones read 0 whatever the caller passed. */ - int used = d->kind == YMIR_SOA_KIND_SPHERE ? 1 : d->kind == YMIR_SOA_KIND_CAPSULE ? 2 : 3; + int used = ymir_soa_is_anchor(d->kind) ? 0 : d->kind == YMIR_SOA_KIND_SPHERE ? 1 : d->kind == YMIR_SOA_KIND_CAPSULE ? 2 : 3; for (int k = 0; k < 3; k++) { world->size[(size_t)k * (size_t)world->capacity + (size_t)i] = k < used ? d->size[k] : 0.0f; @@ -627,10 +671,14 @@ static b3Transform ymir_soa_body_transform(b3BodyId body) 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 || d == NULL || d->kind > YMIR_SOA_JOINT_WELD || d->body_a < 0 || d->body_a >= world->count || + if (world == NULL || d == NULL || d->kind > YMIR_SOA_JOINT_MOTOR || 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) + !ymir_soa_finite(d->cone_angle) || d->cone_angle < 0.0f || d->cone_angle > B3_PI || + !ymir_soa_finite(d->linear_hertz) || !ymir_soa_finite(d->linear_damping) || !ymir_soa_finite(d->max_force) || + !ymir_soa_finite(d->angular_hertz) || !ymir_soa_finite(d->angular_damping) || !ymir_soa_finite(d->max_torque) || + d->linear_hertz < 0.0f || d->linear_damping < 0.0f || d->max_force < 0.0f || d->angular_hertz < 0.0f || + d->angular_damping < 0.0f || d->max_torque < 0.0f) { return YMIR_SOA_INVALID_ARGUMENT; } @@ -677,6 +725,21 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_add_joint( def.upperTwistAngle = d->upper; joint = b3CreateSphericalJoint(world->world_id, &def); } + else if (d->kind == YMIR_SOA_JOINT_MOTOR) + { + b3MotorJointDef def = b3DefaultMotorJointDef(); + 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.linearHertz = d->linear_hertz; + def.linearDampingRatio = d->linear_damping; + def.maxSpringForce = d->max_force; + def.angularHertz = d->angular_hertz; + def.angularDampingRatio = d->angular_damping; + def.maxSpringTorque = d->max_torque; + joint = b3CreateMotorJoint(world->world_id, &def); + } else { b3WeldJointDef def = b3DefaultWeldJointDef(); @@ -882,6 +945,85 @@ YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_step(ymir_box3d_soa_world* world, f return YMIR_SOA_OK; } +// Grab and poke input, each an immediate command on the caller's say-so. Ymir decides nothing about grabbing. + +// Sets an anchor's pose for the coming step: Box3D gives it the velocity that reaches the pose after +// `time_step` seconds, so pass the step's own dt and the anchor arrives at the end of that step. +YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_set_anchor_target( + ymir_box3d_soa_world* world, int32_t lane, const float* position, const float* rotation, float time_step) +{ + if (world == NULL || position == NULL || rotation == NULL || lane < 0 || lane >= world->count || + !ymir_soa_is_anchor(world->kind[lane]) || !ymir_soa_finite3(position) || !ymir_soa_finite(time_step) || + time_step <= 0.0f) + { + return YMIR_SOA_INVALID_ARGUMENT; + } + float q2 = 0.0f; + for (int i = 0; i < 4; ++i) + { + if (!ymir_soa_finite(rotation[i])) + { + return YMIR_SOA_INVALID_ARGUMENT; + } + q2 += rotation[i] * rotation[i]; + } + if (fabsf(q2 - 1.0f) > 1e-3f) + { + return YMIR_SOA_INVALID_ARGUMENT; + } + b3WorldTransform target = { + {position[0], position[1], position[2]}, {{rotation[0], rotation[1], rotation[2]}, rotation[3]}}; + b3Body_SetTargetTransform(world->bodies[lane], target, time_step, true); + return YMIR_SOA_OK; +} + +// A one-shot world impulse (N s) applied at a world point, so it turns the body when the point is off the +// centre of mass. Dynamic bodies only. +YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_apply_impulse( + ymir_box3d_soa_world* world, int32_t lane, const float* impulse, const float* point) +{ + if (world == NULL || impulse == NULL || point == NULL || lane < 0 || lane >= world->count || + !ymir_soa_is_dynamic(world->kind[lane]) || !ymir_soa_finite3(impulse) || !ymir_soa_finite3(point)) + { + return YMIR_SOA_INVALID_ARGUMENT; + } + b3Body_ApplyLinearImpulse( + world->bodies[lane], (b3Vec3){impulse[0], impulse[1], impulse[2]}, (b3Pos){point[0], point[1], point[2]}, true); + return YMIR_SOA_OK; +} + +// The closest shape along origin + t * translation, t in [0, 1], with Box3D's default filter. A miss is a +// success with `hit` 0. Refuses a non-finite or zero-length translation. +YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_cast_ray_closest( + const ymir_box3d_soa_world* world, const float* origin, const float* translation, ymir_box3d_soa_ray_hit* out) +{ + if (world == NULL || origin == NULL || translation == NULL || out == NULL || !ymir_soa_finite3(origin) || + !ymir_soa_finite3(translation) || + translation[0] * translation[0] + translation[1] * translation[1] + translation[2] * translation[2] <= 0.0f) + { + return YMIR_SOA_INVALID_ARGUMENT; + } + memset(out, 0, sizeof(*out)); + b3RayResult r = b3World_CastRayClosest( + world->world_id, (b3Pos){origin[0], origin[1], origin[2]}, + (b3Vec3){translation[0], translation[1], translation[2]}, b3DefaultQueryFilter()); + if (!r.hit) + { + return YMIR_SOA_OK; + } + out->hit = 1; + out->lane = ymir_soa_shape_lane(r.shapeId); + out->stable_id = (uint64_t)(uintptr_t)b3Shape_GetUserData(r.shapeId); + out->point[0] = (float)r.point.x; + out->point[1] = (float)r.point.y; + out->point[2] = (float)r.point.z; + out->normal[0] = r.normal.x; + out->normal[1] = r.normal.y; + out->normal[2] = r.normal.z; + out->fraction = r.fraction; + return YMIR_SOA_OK; +} + YMIR_EXPORT int32_t YMIR_CALL ymir_box3d_soa_get_view(const ymir_box3d_soa_world* world, ymir_box3d_soa_view* view) { if (world == NULL || view == NULL) diff --git a/src/Ymir.Box3D/Box3DSoaWorld.cs b/src/Ymir.Box3D/Box3DSoaWorld.cs index 6c7b7a0..d6cc9b1 100644 --- a/src/Ymir.Box3D/Box3DSoaWorld.cs +++ b/src/Ymir.Box3D/Box3DSoaWorld.cs @@ -7,14 +7,18 @@ public enum Box3DSoaKind : uint Sphere = 0, StaticBox = 1, Capsule = 2, - Box = 3 + Box = 3, + /// A kinematic body with no shape: it never collides or is hit, and only moves it. + Anchor = 4 } public enum Box3DSoaJointKind : uint { Revolute = 0, Spherical = 1, - Weld = 2 + Weld = 2, + /// Position springs pulling body B's frame onto body A's, bounded by a maximum force and torque. + Motor = 3 } /// Native layout of ymir_box3d_soa_body_desc. @@ -44,9 +48,27 @@ internal struct Box3DSoaJointDesc public uint EnableLimit; public float AnchorX, AnchorY, AnchorZ; public float AxisX, AxisY, AxisZ; - public float Lower, Upper, ConeAngle, ReservedFloat; + public float Lower, Upper, ConeAngle; + public float LinearHertz, LinearDamping, MaxForce, AngularHertz, AngularDamping, MaxTorque; } +/// Native layout of ymir_box3d_soa_ray_hit. +[StructLayout(LayoutKind.Sequential)] +internal struct Box3DSoaRayHitNative +{ + public int Hit; + public int Lane; + public ulong StableId; + public float PointX, PointY, PointZ; + public float NormalX, NormalY, NormalZ; + public float Fraction; + public uint Reserved; +} + +/// The closest hit of a ray. false is a miss and every other field is then default. +public readonly record struct Box3DSoaRayHit( + bool Hit, int Lane, ulong StableId, (float X, float Y, float Z) Point, (float X, float Y, float Z) Normal, float Fraction); + /// Native layout of ymir_box3d_soa_view: counts, then one pointer per lane. [StructLayout(LayoutKind.Sequential)] internal unsafe struct Box3DSoaViewNative @@ -209,6 +231,10 @@ internal static unsafe partial class Box3DSoaNative [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])] internal static partial uint ymir_box3d_soa_get_touch_fact_size(); + [LibraryImport(Library)] + [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])] + internal static partial uint ymir_box3d_soa_get_ray_hit_size(); + [LibraryImport(Library)] [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])] internal static partial uint ymir_box3d_soa_get_hit_fact_size(); @@ -240,6 +266,18 @@ internal static unsafe partial class Box3DSoaNative [LibraryImport(Library)] [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])] internal static partial int ymir_box3d_soa_get_facts(nint world, out Box3DSoaFactsNative facts); + + [LibraryImport(Library)] + [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])] + internal static partial int ymir_box3d_soa_set_anchor_target(nint world, int lane, float* position, float* rotation, float timeStep); + + [LibraryImport(Library)] + [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])] + internal static partial int ymir_box3d_soa_apply_impulse(nint world, int lane, float* impulse, float* point); + + [LibraryImport(Library)] + [UnmanagedCallConv(CallConvs = [typeof(System.Runtime.CompilerServices.CallConvCdecl)])] + internal static partial int ymir_box3d_soa_cast_ray_closest(nint world, float* origin, float* translation, out Box3DSoaRayHitNative hit); } /// @@ -355,7 +393,7 @@ public readonly unsafe struct Box3DSoaView /// public sealed class Box3DSoaWorld : IDisposable { - private const uint ExpectedAbi = 4; + private const uint ExpectedAbi = 5; private nint _handle; /// @@ -372,7 +410,8 @@ public sealed class Box3DSoaWorld : IDisposable Box3DSoaNative.ymir_box3d_soa_get_joint_desc_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf() || Box3DSoaNative.ymir_box3d_soa_get_facts_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf() || Box3DSoaNative.ymir_box3d_soa_get_touch_fact_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf() || - Box3DSoaNative.ymir_box3d_soa_get_hit_fact_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf()) + Box3DSoaNative.ymir_box3d_soa_get_hit_fact_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf() || + Box3DSoaNative.ymir_box3d_soa_get_ray_hit_size() != (uint)System.Runtime.CompilerServices.Unsafe.SizeOf()) { throw new InvalidOperationException("The native Ymir Box3D SoA ABI does not match this build."); } @@ -469,6 +508,73 @@ public sealed class Box3DSoaWorld : IDisposable return index; } + /// + /// A motor joint: position springs that pull body B's frame onto body A's. The frame sits at the world-space + /// at creation (axes aligned with the world), so the spring pulls B's anchor point onto A's. + /// Hertz 0 turns that spring off; MaxForce (N) and MaxTorque (N m) bound it. All are finite and + /// not negative. Pair an anchor (body A) with the body to grab (body B). Returns the joint's lane index. + /// + public int AddMotorJoint( + int bodyA, int bodyB, (float X, float Y, float Z) anchor, + (float Hertz, float Damping, float MaxForce) linear, (float Hertz, float Damping, float MaxTorque) angular) + { + ObjectDisposedException.ThrowIf(_handle == 0, this); + var desc = new Box3DSoaJointDesc + { + Kind = (uint)Box3DSoaJointKind.Motor, BodyA = bodyA, BodyB = bodyB, + AnchorX = anchor.X, AnchorY = anchor.Y, AnchorZ = anchor.Z, AxisZ = 1f, + LinearHertz = linear.Hertz, LinearDamping = linear.Damping, MaxForce = linear.MaxForce, + AngularHertz = angular.Hertz, AngularDamping = angular.Damping, MaxTorque = angular.MaxTorque, + }; + Check(Box3DSoaNative.ymir_box3d_soa_add_joint(_handle, desc, out var index)); + return index; + } + + /// Adds a kinematic anchor with no shape (rotation as for ); returns its lane index. + public int AddAnchor(ulong stableId, (float X, float Y, float Z) position, (float X, float Y, float Z, float W) rotation = default) => + AddBody(Describe(Box3DSoaKind.Anchor, stableId, position, rotation, default, 0f, default, 0f, 0f, -1, 0)); + + /// + /// Sets an anchor's pose for the coming step. Box3D gives the anchor the velocity that reaches it after + /// , so pass the same dt as the next and the anchor arrives at its end. + /// Throws for a lane that is not an anchor, a non-finite or non-unit input, or a non-positive time step. + /// + public unsafe void SetAnchorTarget( + int lane, (float X, float Y, float Z) position, (float X, float Y, float Z, float W) rotation, float timeStep) + { + ObjectDisposedException.ThrowIf(_handle == 0, this); + if (rotation == default) rotation = (0f, 0f, 0f, 1f); + var p = stackalloc float[] { position.X, position.Y, position.Z }; + var q = stackalloc float[] { rotation.X, rotation.Y, rotation.Z, rotation.W }; + Check(Box3DSoaNative.ymir_box3d_soa_set_anchor_target(_handle, lane, p, q, timeStep)); + } + + /// + /// A one-shot world impulse (N s) at a world : it adds impulse / mass to the linear + /// velocity and turns the body by (point - centre of mass) x impulse. Dynamic bodies only; throws otherwise + /// or for non-finite input. + /// + public unsafe void ApplyImpulse(int lane, (float X, float Y, float Z) impulse, (float X, float Y, float Z) point) + { + ObjectDisposedException.ThrowIf(_handle == 0, this); + var j = stackalloc float[] { impulse.X, impulse.Y, impulse.Z }; + var p = stackalloc float[] { point.X, point.Y, point.Z }; + Check(Box3DSoaNative.ymir_box3d_soa_apply_impulse(_handle, lane, j, p)); + } + + /// + /// The closest shape along + t * , t in [0, 1], with Box3D's + /// default filter; anchors have no shape and are never hit. Throws for non-finite input or a zero translation. + /// + public unsafe Box3DSoaRayHit CastRayClosest((float X, float Y, float Z) origin, (float X, float Y, float Z) translation) + { + ObjectDisposedException.ThrowIf(_handle == 0, this); + var o = stackalloc float[] { origin.X, origin.Y, origin.Z }; + var t = stackalloc float[] { translation.X, translation.Y, translation.Z }; + Check(Box3DSoaNative.ymir_box3d_soa_cast_ray_closest(_handle, o, t, out var h)); + return new Box3DSoaRayHit(h.Hit != 0, h.Lane, h.StableId, (h.PointX, h.PointY, h.PointZ), (h.NormalX, h.NormalY, h.NormalZ), h.Fraction); + } + 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, diff --git a/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs new file mode 100644 index 0000000..b564c38 --- /dev/null +++ b/tests/Ymir.Box3D.Soa.Tests/Box3DSoaGrabTests.cs @@ -0,0 +1,233 @@ +using System.Runtime.InteropServices; +using Ymir.Box3D; + +namespace Ymir.Box3D.Soa.Tests; + +/// Kinematic anchors, the motor joint, point impulses and the closest-hit ray: the input plumbing for grab and poke. +public sealed class Box3DSoaGrabTests +{ + private const float Dt = 1f / 60f; + private const float Gravity = 10f; + private const float Radius = 0.2f; + private const float Density = 500f; + private static readonly float Mass = Density * 4f / 3f * MathF.PI * Radius * Radius * Radius; + + private static float Distance(Box3DSoaView v, int lane, (float X, float Y, float Z) p) + { + var dx = v.PositionX[lane] - p.X; var dy = v.PositionY[lane] - p.Y; var dz = v.PositionZ[lane] - p.Z; + return MathF.Sqrt(dx * dx + dy * dy + dz * dz); + } + + /// A sphere hung from an anchor at the same point by a motor joint with the given linear spring. + private static (Box3DSoaWorld World, int Anchor, int Sphere, int Joint) Grab(float maxForce, float hertz = 5f) + { + var world = new Box3DSoaWorld(capacity: 4, gravityY: -Gravity, jointCapacity: 2); + var sphere = world.AddSphere(1, (0f, 5f, 0f), Radius, Density); + var anchor = world.AddAnchor(2, (0f, 5f, 0f)); + var joint = world.AddMotorJoint(anchor, sphere, (0f, 5f, 0f), (hertz, 0.7f, maxForce), (0f, 0f, 0f)); + return (world, anchor, sphere, joint); + } + + [Fact] + public void A_strong_grab_follows_its_target_and_the_anchor_shows_its_pose() + { + var (world, anchor, sphere, joint) = Grab(maxForce: 20f * Mass * Gravity); + using var _ = world; + var view = world.View; + Assert.Equal((uint)Box3DSoaKind.Anchor, view.Kind[anchor]); + Assert.Equal((uint)Box3DSoaJointKind.Motor, view.JointKind[joint]); + var target = (1f, 5f, 0f); + for (var i = 0; i < 60; i++) + { + world.SetAnchorTarget(anchor, target, default, Dt); + world.Step(Dt); + } + Assert.True(Distance(view, sphere, target) < 0.02f, $"sphere {Distance(view, sphere, target)} m from the target"); + Assert.Equal(target.Item1, view.PositionX[anchor], 1e-3f); // the anchor reaches the pose at the end of the step + Assert.Equal(0f, view.JointAngle[joint]); + Assert.Equal(0f, view.Size0[anchor]); // no shape, so no size + } + + [Fact] + public void A_weak_grab_is_bounded_by_its_max_force() + { + var maxForce = 0.5f * Mass * Gravity; + var (world, anchor, sphere, _) = Grab(maxForce); + using var _ = world; + var view = world.View; + var target = (0f, 6f, 0f); + for (var i = 0; i < 60; i++) + { + world.SetAnchorTarget(anchor, target, default, Dt); + world.Step(Dt); + } + // Whatever the spring does, it adds at most maxForce / m upward: the fall is at least that of a body + // under g - F/m = 5 m/s^2, 2.5 m in a second. A spring that exceeded its bound would hold it higher. + Assert.True(5f - view.PositionY[sphere] >= 2.4f, $"fell only {5f - view.PositionY[sphere]} m"); + Assert.True(view.PositionY[sphere] < target.Item2 - 1f, "the sphere hangs below the target and never reaches it"); + } + + [Fact] + public void Anchors_do_not_collide_and_are_never_hit_by_a_ray() + { + static float DropY(bool withAnchor) + { + using var world = new Box3DSoaWorld(capacity: 3, gravityY: -Gravity); + var sphere = world.AddSphere(1, (0f, 3f, 0f), Radius, Density); + if (withAnchor) world.AddAnchor(2, (0f, 1.5f, 0f)); + var view = world.View; + for (var i = 0; i < 60; i++) world.Step(Dt); + return view.PositionY[sphere]; + } + + Assert.True(DropY(false) < 1f, "the sphere fell past y = 1.5"); + Assert.Equal(BitConverter.SingleToInt32Bits(DropY(false)), BitConverter.SingleToInt32Bits(DropY(true))); + + using var w = new Box3DSoaWorld(capacity: 2); + w.AddAnchor(2, (0f, 1.5f, 0f)); + Assert.False(w.CastRayClosest((0f, 3f, 0f), (0f, -3f, 0f)).Hit); + } + + [Fact] + public void The_force_lanes_never_move_an_anchor() + { + using var world = new Box3DSoaWorld(capacity: 2, gravityY: -Gravity); + var anchor = world.AddAnchor(1, (1f, 2f, 3f)); + var view = world.View; + for (var i = 0; i < 30; i++) + { + view.ForceY[anchor] = 1e6f; + view.TorqueZ[anchor] = 1e6f; + world.Step(Dt); + } + Assert.Equal((1f, 2f, 3f), (view.PositionX[anchor], view.PositionY[anchor], view.PositionZ[anchor])); + Assert.Equal(1f, view.RotationW[anchor]); + } + + [Fact] + public void An_off_centre_impulse_turns_the_body() + { + const float r = 0.25f, density = 100f; + var mass = density * 4f / 3f * MathF.PI * r * r * r; + using var world = new Box3DSoaWorld(capacity: 2, gravityY: 0f); + var sphere = world.AddSphere(1, (0f, 0f, 0f), r, density); + var centred = world.AddSphere(2, (5f, 0f, 0f), r, density); + var view = world.View; + const float j = 2f; + world.ApplyImpulse(sphere, (j, 0f, 0f), (0f, 0.1f, 0f)); + world.ApplyImpulse(centred, (j, 0f, 0f), (5f, 0f, 0f)); + world.Step(Dt); + Assert.Equal(j / mass, view.VelocityX[sphere], 1e-4f); + Assert.Equal(0f, view.VelocityY[sphere], 1e-4f); + // r x J = (0, 0.1, 0) x (j, 0, 0) = (0, 0, -0.1 j): the spin is about -z, I = 2/5 m r^2. + Assert.True(view.AngularVelocityZ[sphere] < 0f); + Assert.Equal(-0.1f * j / (0.4f * mass * r * r), view.AngularVelocityZ[sphere], 0.02f); + Assert.Equal(0f, view.AngularVelocityX[sphere], 1e-4f); + // The same impulse through the centre of mass gives no spin. + Assert.Equal(j / mass, view.VelocityX[centred], 1e-4f); + Assert.Equal(0f, view.AngularVelocityZ[centred], 1e-4f); + } + + [Fact] + public void The_ray_finds_the_nearest_body_with_its_lane_and_stable_id() + { + using var world = new Box3DSoaWorld(capacity: 4, gravityY: 0f); + var far = world.AddSphere(12, (6f, 0f, 0f), 0.5f, 1f); + var near = world.AddSphere(11, (3f, 0f, 0f), 0.5f, 1f); + var hit = world.CastRayClosest((0f, 0f, 0f), (10f, 0f, 0f)); + Assert.True(hit.Hit); + Assert.Equal(near, hit.Lane); + Assert.NotEqual(far, hit.Lane); + Assert.Equal(11ul, hit.StableId); + Assert.Equal(2.5f, hit.Point.X, 1e-3f); + Assert.Equal(0.25f, hit.Fraction, 1e-4f); + Assert.Equal(-1f, hit.Normal.X, 1e-3f); + Assert.False(world.CastRayClosest((0f, 0f, 0f), (0f, 10f, 0f)).Hit); + Assert.Equal(default, world.CastRayClosest((0f, 0f, 0f), (0f, 10f, 0f))); + } + + [Fact] + public void Refused_calls_throw_and_change_nothing() + { + static Box3DSoaWorld Build(out int anchor, out int sphere, out int box) + { + var w = new Box3DSoaWorld(capacity: 5, gravityY: -Gravity, jointCapacity: 2); + sphere = w.AddSphere(1, (0f, 2f, 0f), Radius, Density); + box = w.AddStaticBox(2, (0f, -1f, 0f), (5f, 1f, 5f)); + anchor = w.AddAnchor(3, (0f, 2f, 0f)); + return w; + } + + using var world = Build(out var anchor, out var sphere, out var box); + using var control = Build(out _, out _, out _); + var nan = float.NaN; + Assert.Throws(() => world.SetAnchorTarget(sphere, (1f, 1f, 1f), default, Dt)); + Assert.Throws(() => world.SetAnchorTarget(box, (1f, 1f, 1f), default, Dt)); + Assert.Throws(() => world.SetAnchorTarget(9, (1f, 1f, 1f), default, Dt)); + Assert.Throws(() => world.SetAnchorTarget(anchor, (nan, 1f, 1f), default, Dt)); + Assert.Throws(() => world.SetAnchorTarget(anchor, (1f, 1f, 1f), (0f, 0f, nan, 1f), Dt)); + Assert.Throws(() => world.SetAnchorTarget(anchor, (1f, 1f, 1f), (0f, 0f, 0f, 2f), Dt)); + Assert.Throws(() => world.SetAnchorTarget(anchor, (1f, 1f, 1f), default, 0f)); + Assert.Throws(() => world.ApplyImpulse(box, (1f, 0f, 0f), (0f, 0f, 0f))); + Assert.Throws(() => world.ApplyImpulse(anchor, (1f, 0f, 0f), (0f, 2f, 0f))); + Assert.Throws(() => world.ApplyImpulse(sphere, (nan, 0f, 0f), (0f, 2f, 0f))); + Assert.Throws(() => world.ApplyImpulse(sphere, (1f, 0f, 0f), (0f, float.PositiveInfinity, 0f))); + Assert.Throws(() => world.CastRayClosest((nan, 0f, 0f), (1f, 0f, 0f))); + Assert.Throws(() => world.CastRayClosest((0f, 0f, 0f), (0f, 0f, 0f))); + foreach (var bad in new[] { -1f, nan, float.PositiveInfinity }) + { + Assert.Throws(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (bad, 0.7f, 100f), (0f, 0f, 0f))); + Assert.Throws(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, bad, 100f), (0f, 0f, 0f))); + Assert.Throws(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, bad), (0f, 0f, 0f))); + Assert.Throws(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, 100f), (bad, 0f, 0f))); + Assert.Throws(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, 100f), (1f, bad, 0f))); + Assert.Throws(() => world.AddMotorJoint(anchor, sphere, (0f, 2f, 0f), (5f, 0.7f, 100f), (1f, 0f, bad))); + } + Assert.Equal(0, world.View.JointCount); + + for (var i = 0; i < 30; i++) { world.Step(Dt); control.Step(Dt); } + Assert.Equal(Snapshot(control), Snapshot(world)); + } + + [Fact] + public void Two_identical_worlds_with_the_same_grab_and_impulses_match_bit_for_bit() + { + var a = Snapshot(RunGrab(0f)); + Assert.Equal(a, Snapshot(RunGrab(0f))); + Assert.NotEqual(a, Snapshot(RunGrab(MathF.BitIncrement(1f) - 1f))); // one ulp of impulse is seen + } + + private static Box3DSoaWorld RunGrab(float impulseNudge) + { + var world = new Box3DSoaWorld(capacity: 5, gravityY: -Gravity, jointCapacity: 2); + var slab = world.AddStaticBox(1, (0f, -1f, 0f), (10f, 1f, 10f)); + var ball = world.AddSphere(2, (0f, 1f, 0f), Radius, Density); + var other = world.AddSphere(3, (1.5f, 0.2f, 0f), Radius, Density); + var anchor = world.AddAnchor(4, (0f, 1f, 0f)); + world.AddMotorJoint(anchor, ball, (0f, 1f, 0f), (4f, 0.5f, 8f * Mass * Gravity), (2f, 0.5f, 5f)); + for (var i = 0; i < 300; i++) + { + var t = i * Dt; + world.SetAnchorTarget(anchor, (MathF.Sin(t) * 1.5f, 1f + 0.5f * MathF.Cos(2f * t), 0.3f * t), default, Dt); + if (i == 100) world.ApplyImpulse(other, (0.5f + impulseNudge, 0.2f, 0f), (1.5f, 0.3f, 0f)); + world.Step(Dt); + } + _ = slab; + return world; + } + + /// Every state lane of the world as bytes, disposing the world. + private static byte[] Snapshot(Box3DSoaWorld world) + { + using var _ = world; + var v = world.View; + var bytes = new List(); + void Add(ReadOnlySpan lane) => bytes.AddRange(MemoryMarshal.AsBytes(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.VelocityX); Add(v.VelocityY); Add(v.VelocityZ); + Add(v.AngularVelocityX); Add(v.AngularVelocityY); Add(v.AngularVelocityZ); + for (var j = 0; j < v.JointCount; j++) bytes.AddRange(BitConverter.GetBytes(v.JointRotationW[j])); + return [.. bytes]; + } +}